예제 #1
0
파일: AHRS.cs 프로젝트: chopshop-166/WPILib
            public void SetAHRSData(AHRSProtocol.AHRSUpdate ahrsUpdate)
            {

                /* Update base IMU class variables */

                m_parent.m_yaw = ahrsUpdate.yaw;
                m_parent.m_pitch = ahrsUpdate.pitch;
                m_parent.m_roll = ahrsUpdate.roll;
                m_parent.m_compassHeading = ahrsUpdate.compass_heading;
                m_parent.m_yawOffsetTracker.UpdateHistory(ahrsUpdate.yaw);

                /* Update AHRS class variables */

                // 9-axis data
                m_parent.m_fusedHeading = ahrsUpdate.fused_heading;

                // Gravity-corrected linear acceleration (world-frame)
                m_parent.m_worldLinearAccelX = ahrsUpdate.linear_accel_x;
                m_parent.m_worldLinearAccelY = ahrsUpdate.linear_accel_y;
                m_parent.m_worldLinearAccelZ = ahrsUpdate.linear_accel_z;

                // Gyro/Accelerometer Die Temperature
                m_parent.m_mpuTempC = ahrsUpdate.mpu_temp;

                // Barometric Pressure/Altitude
                m_parent.m_altitude = ahrsUpdate.altitude;
                m_parent.m_baroPressure = ahrsUpdate.barometric_pressure;

                // Magnetometer Data
                m_parent.m_calMagX = ahrsUpdate.cal_mag_x;
                m_parent.m_calMagY = ahrsUpdate.cal_mag_y;
                m_parent.m_calMagZ = ahrsUpdate.cal_mag_z;

                // Status/Motion Detection
                m_parent.m_isMoving =
                        (((ahrsUpdate.sensor_status &
                                AHRSProtocol.NAVX_SENSOR_STATUS_MOVING) != 0)
                                ? true : false);
                m_parent.m_isRotating =
                        (((ahrsUpdate.sensor_status &
                                AHRSProtocol.NAVX_SENSOR_STATUS_YAW_STABLE) != 0)
                                ? false : true);
                m_parent.m_altitudeValid =
                        (((ahrsUpdate.sensor_status &
                                AHRSProtocol.NAVX_SENSOR_STATUS_ALTITUDE_VALID) != 0)
                                ? true : false);
                m_parent.m_isMagnetometerCalibrated =
                        (((ahrsUpdate.cal_status &
                                AHRSProtocol.NAVX_CAL_STATUS_MAG_CAL_COMPLETE) != 0)
                                ? true : false);
                m_parent.m_magneticDisturbance =
                        (((ahrsUpdate.sensor_status &
                                AHRSProtocol.NAVX_SENSOR_STATUS_MAG_DISTURBANCE) != 0)
                                ? true : false);

                m_parent.m_quaternionW = ahrsUpdate.quat_w;
                m_parent.m_quaternionX = ahrsUpdate.quat_x;
                m_parent.m_quaternionY = ahrsUpdate.quat_y;
                m_parent.m_quaternionZ = ahrsUpdate.quat_z;

                m_parent.UpdateDisplacement(m_parent.m_worldLinearAccelX,
                        m_parent.m_worldLinearAccelY,
                        m_parent.m_updateRateHz,
                        m_parent.m_isMoving);

                m_parent.m_yawAngleTracker.NextAngle(m_parent.GetYaw());

            }
예제 #2
0
파일: AHRS.cs 프로젝트: chopshop-166/WPILib
            public void SetBoardID(AHRSProtocol.BoardID boardId)
            {
                m_parent.m_boardType = boardId.type;
                m_parent.m_hwRev = boardId.hw_rev;
                m_parent.m_fwVerMajor = boardId.fw_ver_major;
                m_parent.m_fwVerMinor = boardId.fw_ver_minor;

            }
예제 #3
0
파일: AHRS.cs 프로젝트: chopshop-166/WPILib
            public void SetRawData(AHRSProtocol.GyroUpdate rawDataUpdate)
            {
                m_parent.m_rawGyroX = rawDataUpdate.gyro_x;
                m_parent.m_rawGyroY = rawDataUpdate.gyro_y;
                m_parent.m_rawGyroZ = rawDataUpdate.gyro_z;
                m_parent.m_rawAccelX = rawDataUpdate.accel_x;
                m_parent.m_rawAccelY = rawDataUpdate.accel_y;
                m_parent.m_rawAccelZ = rawDataUpdate.accel_z;
                m_parent.m_calMagX = rawDataUpdate.mag_x;
                m_parent.m_calMagY = rawDataUpdate.mag_y;
                m_parent.m_calMagZ = rawDataUpdate.mag_z;
                m_parent.m_mpuTempC = rawDataUpdate.temp_c;

            }