diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 78f8ce07c4..aa765f5273 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -73,8 +73,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // Parent header of Timer // (only included for makefile libpath to work) #include // TimerProcess is the scheduler for MPU6000 reads. -#include // Madgwick quaternion system -#include // ArduPilot Mega DCM Library +#include #include // PI library #include // PID library #include // RC Channel Library @@ -235,12 +234,9 @@ AP_IMU_INS imu(&ins); static GPS *g_gps_null; #if QUATERNION_ENABLE == ENABLED - // this shouldn't be called dcm of course, but until we - // decide to actually use something else, I don't want the patch - // size to be huge - AP_Quaternion dcm(&imu, g_gps_null); + AP_AHRS_Quaternion ahrs(&imu, g_gps_null); #else - AP_DCM dcm(&imu, g_gps_null); + AP_AHRS_DCM ahrs(&imu, g_gps_null); #endif AP_TimerProcess timer_scheduler; @@ -251,7 +247,7 @@ AP_TimerProcess timer_scheduler; AP_Compass_HIL compass; AP_GPS_HIL g_gps_driver(NULL); AP_IMU_Shim imu; - AP_DCM dcm(&imu, g_gps); + AP_AHRS_DCM ahrs(&imu, g_gps); AP_PeriodicProcessStub timer_scheduler; AP_InertialSensor_Stub ins; @@ -259,11 +255,11 @@ AP_TimerProcess timer_scheduler; #elif HIL_MODE == HIL_MODE_ATTITUDE AP_ADC_HIL adc; - AP_DCM_HIL dcm; + AP_IMU_Shim imu; // never used + AP_AHRS_HIL ahrs(&imu, g_gps); AP_GPS_HIL g_gps_driver(NULL); AP_Compass_HIL compass; // never used AP_Baro_BMP085_HIL barometer; - AP_IMU_Shim imu; // never used AP_InertialSensor_Stub ins; AP_PeriodicProcessStub timer_scheduler; #ifdef OPTFLOW_ENABLED @@ -984,7 +980,7 @@ static void medium_loop() if(g.compass_enabled){ if (compass.read()) { // Calculate heading - Matrix3f m = dcm.get_dcm_matrix(); + Matrix3f m = ahrs.get_dcm_matrix(); compass.calculate(m); compass.null_offsets(m); } @@ -1297,7 +1293,7 @@ static void update_optical_flow(void) static int log_counter = 0; optflow.update(); - optflow.update_position(dcm.roll, dcm.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow + optflow.update_position(ahrs.roll, ahrs.pitch, cos_yaw_x, sin_yaw_y, current_loc.alt); // updates internal lon and lat with estimation based on optical flow // write to log log_counter++; @@ -1555,7 +1551,7 @@ void update_simple_mode(void) // which improves speed of function simple_counter++; - int delta = wrap_360(dcm.yaw_sensor - initial_simple_bearing)/100; + int delta = wrap_360(ahrs.yaw_sensor - initial_simple_bearing)/100; if (simple_counter == 1){ // roll @@ -1863,17 +1859,17 @@ static void read_AHRS(void) // Perform IMU calculations and get attitude info //----------------------------------------------- #if HIL_MODE != HIL_MODE_DISABLED - // update hil before dcm update + // update hil before ahrs update gcs_update(); #endif - dcm.update_DCM(); + ahrs.update(); omega = imu.get_gyro(); } static void update_trig(void){ Vector2f yawvector; - Matrix3f temp = dcm.get_dcm_matrix(); + Matrix3f temp = ahrs.get_dcm_matrix(); yawvector.x = temp.a.x; // sin yawvector.y = temp.b.x; // cos @@ -2210,6 +2206,9 @@ static void update_nav_wp() nav_lon = g.pid_loiter_rate_lon.get_integrator(); nav_lat = g.pid_loiter_rate_lon.get_integrator(); + nav_lon = constrain(nav_lon, -2000, 2000); // 20° + nav_lat = constrain(nav_lat, -2000, 2000); // 20° + // rotate pitch and roll to the copter frame of reference calc_loiter_pitch_roll(); } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 5188a57a32..ac7f272dda 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -4,7 +4,7 @@ static int16_t get_stabilize_roll(int32_t target_angle) { // angle error - target_angle = wrap_180(target_angle - dcm.roll_sensor); + target_angle = wrap_180(target_angle - ahrs.roll_sensor); #if FRAME_CONFIG == HELI_FRAME @@ -33,7 +33,7 @@ static int16_t get_stabilize_pitch(int32_t target_angle) { // angle error - target_angle = wrap_180(target_angle - dcm.pitch_sensor); + target_angle = wrap_180(target_angle - ahrs.pitch_sensor); #if FRAME_CONFIG == HELI_FRAME // limit the error we're feeding to the PID @@ -60,7 +60,7 @@ static int16_t get_stabilize_yaw(int32_t target_angle) { // angle error - target_angle = wrap_180(target_angle - dcm.yaw_sensor); + target_angle = wrap_180(target_angle - ahrs.yaw_sensor); #if FRAME_CONFIG == HELI_FRAME // cannot use rate control for helicopters // limit the error we're feeding to the PID @@ -255,7 +255,6 @@ static void reset_I_all(void) { reset_rate_I(); reset_stability_I(); - reset_nav_I(); reset_wind_I(); reset_throttle_I(); reset_optflow_I(); @@ -282,13 +281,14 @@ static void reset_optflow_I(void) static void reset_wind_I(void) { // Wind Compensation + // this i is not currently being used, but we reset it anyway + // because someone may modify it and not realize it, causing a bug g.pi_loiter_lat.reset_I(); g.pi_loiter_lon.reset_I(); -} -static void reset_nav_I(void) -{ - // Rate control for WP navigation + g.pid_loiter_rate_lat.reset_I(); + g.pid_loiter_rate_lon.reset_I(); + g.pid_nav_lat.reset_I(); g.pid_nav_lon.reset_I(); } @@ -320,13 +320,13 @@ get_nav_yaw_offset(int yaw_input, int reset) if(reset == 0){ // we are on the ground - return dcm.yaw_sensor; + return ahrs.yaw_sensor; }else{ // re-define nav_yaw if we have stick input if(yaw_input != 0){ // set nav_yaw + or - the current location - _yaw = yaw_input + dcm.yaw_sensor; + _yaw = yaw_input + ahrs.yaw_sensor; // we need to wrap our value so we can be 0 to 360 (*100) return wrap_360(_yaw); @@ -364,7 +364,7 @@ static int get_z_damping() float get_world_Z_accel() { - accels_rot = dcm.get_dcm_matrix() * imu.get_accel(); + accels_rot = ahrs.get_dcm_matrix() * imu.get_accel(); //Serial.printf("z %1.4f\n", accels_rot.z); return accels_rot.z; } @@ -412,7 +412,7 @@ static float fullDampP = 0.100; float get_world_Z_accel() { - accels_rot = dcm.get_dcm_matrix() * imu.get_accel(); + accels_rot = ahrs.get_dcm_matrix() * imu.get_accel(); return accels_rot.z; } diff --git a/ArduCopter/Camera.pde b/ArduCopter/Camera.pde index 8341f2b40b..36b3e16aa9 100644 --- a/ArduCopter/Camera.pde +++ b/ArduCopter/Camera.pde @@ -23,10 +23,10 @@ camera_stabilization() // Allow user to control camera pitch with channel 6 (mixed with pitch DCM) if(g.radio_tuning == 0) { g.rc_camera_pitch.set_pwm(APM_RC.InputCh(CH_6)); - g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(dcm.pitch_sensor); + g.rc_camera_pitch.servo_out = g.rc_camera_pitch.control_mix(ahrs.pitch_sensor); }else{ // unless channel 6 is already being used for tuning - g.rc_camera_pitch.servo_out = dcm.pitch_sensor * -1; + g.rc_camera_pitch.servo_out = ahrs.pitch_sensor * -1; } g.rc_camera_pitch.servo_out = (float)g.rc_camera_pitch.servo_out * g.camera_pitch_gain; @@ -39,14 +39,14 @@ camera_stabilization() // allow control mixing /* g.rc_camera_roll.set_pwm(APM_RC.InputCh(CH_6)); // I'm using CH 6 input here. - g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-dcm.roll_sensor); + g.rc_camera_roll.servo_out = g.rc_camera_roll.control_mix(-ahrs.roll_sensor); */ // dont allow control mixing - g.rc_camera_roll.servo_out = (float)-dcm.roll_sensor * g.camera_roll_gain; + g.rc_camera_roll.servo_out = (float)-ahrs.roll_sensor * g.camera_roll_gain; // limit - //g.rc_camera_roll.servo_out = constrain(-dcm.roll_sensor, -4500, 4500); + //g.rc_camera_roll.servo_out = constrain(-ahrs.roll_sensor, -4500, 4500); // Output // ------ diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index aac172ba6a..d97f89adc1 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -38,9 +38,9 @@ static NOINLINE void send_attitude(mavlink_channel_t chan) mavlink_msg_attitude_send( chan, micros(), - dcm.roll, - dcm.pitch, - dcm.yaw, + ahrs.roll, + ahrs.pitch, + ahrs.yaw, omega.x, omega.y, omega.z); @@ -99,7 +99,7 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan) static void NOINLINE send_location(mavlink_channel_t chan) { - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now mavlink_msg_global_position_int_send( chan, current_loc.lat, @@ -125,18 +125,18 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) } #if HIL_MODE != HIL_MODE_ATTITUDE -static void NOINLINE send_dcm(mavlink_channel_t chan) +static void NOINLINE send_ahrs(mavlink_channel_t chan) { - Vector3f omega_I = dcm.get_gyro_drift(); - mavlink_msg_dcm_send( + Vector3f omega_I = ahrs.get_gyro_drift(); + mavlink_msg_ahrs_send( chan, omega_I.x, omega_I.y, omega_I.z, - dcm.get_accel_weight(), - dcm.get_renorm_val(), - dcm.get_error_rp(), - dcm.get_error_yaw()); + 1, + 0, + ahrs.get_error_rp(), + ahrs.get_error_yaw()); } #endif // HIL_MODE != HIL_MODE_ATTITUDE @@ -275,7 +275,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) chan, (float)g_gps->ground_speed / 100.0, (float)g_gps->ground_speed / 100.0, - (dcm.yaw_sensor / 100) % 360, + (ahrs.yaw_sensor / 100) % 360, g.rc_3.servo_out/10, current_loc.alt / 100.0, climb_rate); @@ -471,16 +471,16 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, send_statustext(chan); break; - case MSG_DCM: + case MSG_AHRS: #if HIL_MODE != HIL_MODE_ATTITUDE - CHECK_PAYLOAD_SIZE(DCM); - send_dcm(chan); + CHECK_PAYLOAD_SIZE(AHRS); + send_ahrs(chan); #endif break; case MSG_SIMSTATE: #ifdef DESKTOP_BUILD - CHECK_PAYLOAD_SIZE(DCM); + CHECK_PAYLOAD_SIZE(SIMSTATE); send_simstate(chan); #endif break; @@ -736,7 +736,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) } if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ - send_message(MSG_DCM); + send_message(MSG_AHRS); send_message(MSG_HWSTATUS); } } @@ -1470,19 +1470,25 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) Log_Write_Data(1, ((AP_Int32 *)vp)->get()); #endif if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); + float v = packet.param_value+rounding_addition; + v = constrain(v, -2147483648, 2147483647); + ((AP_Int32 *)vp)->set_and_save(v); } else if (var_type == AP_PARAM_INT16) { #if LOGGING_ENABLED == ENABLED Log_Write_Data(3, (int32_t)((AP_Int16 *)vp)->get()); #endif if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); + float v = packet.param_value+rounding_addition; + v = constrain(v, -32768, 32767); + ((AP_Int16 *)vp)->set_and_save(v); } else if (var_type == AP_PARAM_INT8) { #if LOGGING_ENABLED == ENABLED Log_Write_Data(4, (int32_t)((AP_Int8 *)vp)->get()); #endif if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); + float v = packet.param_value+rounding_addition; + v = constrain(v, -128, 127); + ((AP_Int8 *)vp)->set_and_save(v); } else { // we don't support mavlink set on this parameter break; @@ -1559,8 +1565,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_attitude_t packet; mavlink_msg_attitude_decode(msg, &packet); - // set dcm hil sensor - dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + // set AHRS hil sensor + ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, packet.pitchspeed,packet.yawspeed); // rad/sec diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 64b43bd7b4..4727884dc8 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -617,11 +617,10 @@ static void Log_Write_Performance() DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_PERFORMANCE_MSG); - DataFlash.WriteByte( dcm.gyro_sat_count); //1 - DataFlash.WriteByte( imu.adc_constraints); //2 - DataFlash.WriteByte( dcm.renorm_range_count); //3 - DataFlash.WriteByte( dcm.renorm_blowup_count); //4 - DataFlash.WriteByte( gps_fix_count); //5 + DataFlash.WriteByte( imu.adc_constraints); //1 + DataFlash.WriteByte( ahrs.renorm_range_count); //2 + DataFlash.WriteByte( ahrs.renorm_blowup_count); //3 + DataFlash.WriteByte( gps_fix_count); //4 DataFlash.WriteByte(END_BYTE); } @@ -632,15 +631,13 @@ static void Log_Read_Performance() int8_t temp2 = DataFlash.ReadByte(); int8_t temp3 = DataFlash.ReadByte(); int8_t temp4 = DataFlash.ReadByte(); - int8_t temp5 = DataFlash.ReadByte(); - //1 2 3 4 5 - Serial.printf_P(PSTR("PM, %d, %d, %d, %d, %d\n"), + //1 2 3 4 + Serial.printf_P(PSTR("PM, %d, %d, %d, %d\n"), (int)temp1, (int)temp2, (int)temp3, - (int)temp4, - (int)temp5); + (int)temp4); } // Write a command processing packet. Total length : 21 bytes @@ -696,11 +693,11 @@ static void Log_Write_Attitude() DataFlash.WriteByte(LOG_ATTITUDE_MSG); DataFlash.WriteInt(g.rc_1.control_in); // 1 - DataFlash.WriteInt((int)dcm.roll_sensor); // 2 + DataFlash.WriteInt((int)ahrs.roll_sensor); // 2 DataFlash.WriteInt(g.rc_2.control_in); // 3 - DataFlash.WriteInt((int)dcm.pitch_sensor); // 4 + DataFlash.WriteInt((int)ahrs.pitch_sensor); // 4 DataFlash.WriteInt(g.rc_4.control_in); // 5 - DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); // 6 + DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor); // 6 DataFlash.WriteInt((uint16_t)(wrap_360(ToDeg(compass.heading)*100))); // 7 DataFlash.WriteByte(END_BYTE); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index c0645ead8a..2ddca06d3f 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -236,7 +236,7 @@ enum ap_message { MSG_NEXT_WAYPOINT, MSG_NEXT_PARAM, MSG_STATUSTEXT, - MSG_DCM, + MSG_AHRS, MSG_SIMSTATE, MSG_HWSTATUS, MSG_RETRY_DEFERRED // this must be last diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 5745b7208c..7cf21bcd88 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -93,7 +93,7 @@ static void init_compass() Serial.println_P(PSTR("COMPASS INIT ERROR")); return; } - dcm.set_compass(&compass); + ahrs.set_compass(&compass); compass.null_offsets_enable(); } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index b0810b0b92..1debcdef96 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -532,8 +532,6 @@ static void set_mode(byte mode) reset_nav_params(); // remove the wind compenstaion reset_wind_I(); - // Clears the WP navigation speed compensation - reset_nav_I(); // Clears the alt hold compensation reset_throttle_I(); } @@ -567,7 +565,7 @@ static void set_failsafe(boolean mode) static void init_simple_bearing() { - initial_simple_bearing = dcm.yaw_sensor; + initial_simple_bearing = ahrs.yaw_sensor; } static void update_throttle_cruise() diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index f828dabf3c..2ea0a9318c 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -935,7 +935,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) while(1){ delay(100); if (compass.read()) { - compass.calculate(dcm.get_dcm_matrix()); + compass.calculate(ahrs.get_dcm_matrix()); Vector3f maggy = compass.get_offsets(); Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"), (wrap_360(ToDeg(compass.heading) * 100)) /100, diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 22d5d5cf86..676a8720bc 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -40,8 +40,7 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega Vector/Matrix math Library #include // Inertial Sensor (uncalibated IMU) Library #include // ArduPilot Mega IMU Library -#include // ArduPilot Mega DCM Library -#include // Madgwick quaternion system +#include // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library #include // Range finder library @@ -194,12 +193,9 @@ AP_GPS_None g_gps_driver(NULL); AP_IMU_INS imu( &ins ); #if QUATERNION_ENABLE == ENABLED - // this shouldn't be called dcm of course, but until we - // decide to actually use something else, I don't want the patch - // size to be huge - AP_Quaternion dcm(&imu, g_gps); + AP_AHRS_Quaternion ahrs(&imu, g_gps); #else - AP_DCM dcm(&imu, g_gps); + AP_AHRS_DCM ahrs(&imu, g_gps); #endif #elif HIL_MODE == HIL_MODE_SENSORS @@ -210,14 +206,14 @@ AP_Compass_HIL compass; AP_GPS_HIL g_gps_driver(NULL); AP_InertialSensor_Oilpan ins( &adc ); AP_IMU_Shim imu; -AP_DCM dcm(&imu, g_gps); +AP_AHRS_DCM ahrs(&imu, g_gps); #elif HIL_MODE == HIL_MODE_ATTITUDE AP_ADC_HIL adc; -AP_DCM_HIL dcm; +AP_IMU_Shim imu; // never used +AP_AHRS_HIL ahrs(&imu, g_gps); AP_GPS_HIL g_gps_driver(NULL); AP_Compass_HIL compass; // never used -AP_IMU_Shim imu; // never used AP_Baro_BMP085_HIL barometer; #else @@ -261,7 +257,7 @@ AP_Relay relay; // Camera/Antenna mount tracking and stabilisation stuff // -------------------------------------- #if MOUNT == ENABLED -AP_Mount camera_mount(g_gps, &dcm); +AP_Mount camera_mount(g_gps, &ahrs); #endif @@ -719,18 +715,18 @@ static void fast_loop() } #if HIL_MODE == HIL_MODE_SENSORS - // update hil before dcm update + // update hil before AHRS update gcs_update(); #endif - dcm.update_DCM(); + ahrs.update(); // uses the yaw from the DCM to give more accurate turns calc_bearing_error(); # if HIL_MODE == HIL_MODE_DISABLED if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) - Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor); + Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor); if (g.log_bitmask & MASK_LOG_RAW) Log_Write_Raw(); @@ -784,19 +780,19 @@ static void medium_loop() #if HIL_MODE != HIL_MODE_ATTITUDE if (g.compass_enabled && compass.read()) { - dcm.set_compass(&compass); + ahrs.set_compass(&compass); // Calculate heading - Matrix3f m = dcm.get_dcm_matrix(); + Matrix3f m = ahrs.get_dcm_matrix(); compass.calculate(m); compass.null_offsets(m); } else { - dcm.set_compass(NULL); + ahrs.set_compass(NULL); } #endif /*{ -Serial.print(dcm.roll_sensor, DEC); Serial.printf_P(PSTR("\t")); -Serial.print(dcm.pitch_sensor, DEC); Serial.printf_P(PSTR("\t")); -Serial.print(dcm.yaw_sensor, DEC); Serial.printf_P(PSTR("\t")); +Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t")); +Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t")); +Serial.print(ahrs.yaw_sensor, DEC); Serial.printf_P(PSTR("\t")); Vector3f tempaccel = imu.get_accel(); Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t")); Serial.print(tempaccel.y, DEC); Serial.printf_P(PSTR("\t")); @@ -850,7 +846,7 @@ Serial.println(tempaccel.z, DEC); #if HIL_MODE != HIL_MODE_ATTITUDE if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) - Log_Write_Attitude((int)dcm.roll_sensor, (int)dcm.pitch_sensor, (uint16_t)dcm.yaw_sensor); + Log_Write_Attitude((int)ahrs.roll_sensor, (int)ahrs.pitch_sensor, (uint16_t)ahrs.yaw_sensor); if (g.log_bitmask & MASK_LOG_CTUN) Log_Write_Control_Tuning(); diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde index e8b0e70216..93c1db0bff 100644 --- a/ArduPlane/Attitude.pde +++ b/ArduPlane/Attitude.pde @@ -38,7 +38,7 @@ static void stabilize() // handle this is to ensure both go in the same direction from // zero nav_roll += 18000; - if (dcm.roll_sensor < 0) nav_roll -= 36000; + if (ahrs.roll_sensor < 0) nav_roll -= 36000; } // For Testing Only @@ -48,11 +48,11 @@ static void stabilize() // Calculate dersired servo output for the roll // --------------------------------------------- - g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - dcm.roll_sensor), delta_ms_fast_loop, speed_scaler); + g.channel_roll.servo_out = g.pidServoRoll.get_pid((nav_roll - ahrs.roll_sensor), delta_ms_fast_loop, speed_scaler); long tempcalc = nav_pitch + - fabs(dcm.roll_sensor * g.kff_pitch_compensation) + + fabs(ahrs.roll_sensor * g.kff_pitch_compensation) + (g.channel_throttle.servo_out * g.kff_throttle_to_pitch) - - (dcm.pitch_sensor - g.pitch_trim); + (ahrs.pitch_sensor - g.pitch_trim); if (inverted_flight) { // when flying upside down the elevator control is inverted tempcalc = -tempcalc; @@ -120,7 +120,7 @@ static void stabilize() static void crash_checker() { - if(dcm.pitch_sensor < -4500){ + if(ahrs.pitch_sensor < -4500){ crash_timer = 255; } if(crash_timer > 0) @@ -215,7 +215,7 @@ static void calc_nav_roll() nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get()); Vector3f omega; - omega = dcm.get_gyro(); + omega = ahrs.get_gyro(); // rate limiter long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index f1d4cda07d..dd80bae476 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -108,13 +108,13 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) static NOINLINE void send_attitude(mavlink_channel_t chan) { - Vector3f omega = dcm.get_gyro(); + Vector3f omega = ahrs.get_gyro(); mavlink_msg_attitude_send( chan, micros(), - dcm.roll, - dcm.pitch - radians(g.pitch_trim*0.01), - dcm.yaw, + ahrs.roll, + ahrs.pitch - radians(g.pitch_trim*0.01), + ahrs.yaw, omega.x, omega.y, omega.z); @@ -306,7 +306,7 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan) static void NOINLINE send_location(mavlink_channel_t chan) { - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + Matrix3f rot = ahrs.get_dcm_matrix(); // neglecting angle of attack for now mavlink_msg_global_position_int_send( chan, millis(), @@ -434,7 +434,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) chan, (float)airspeed / 100.0, (float)g_gps->ground_speed / 100.0, - (dcm.yaw_sensor / 100) % 360, + (ahrs.yaw_sensor / 100) % 360, (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100 current_loc.alt / 100.0, 0); @@ -486,18 +486,18 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan) imu.ax(), imu.ay(), imu.az()); } -static void NOINLINE send_dcm(mavlink_channel_t chan) +static void NOINLINE send_ahrs(mavlink_channel_t chan) { - Vector3f omega_I = dcm.get_gyro_drift(); - mavlink_msg_dcm_send( + Vector3f omega_I = ahrs.get_gyro_drift(); + mavlink_msg_ahrs_send( chan, omega_I.x, omega_I.y, omega_I.z, - dcm.get_accel_weight(), - dcm.get_renorm_val(), - dcm.get_error_rp(), - dcm.get_error_yaw()); + 0, + 0, + ahrs.get_error_rp(), + ahrs.get_error_yaw()); } #endif // HIL_MODE != HIL_MODE_ATTITUDE @@ -679,16 +679,16 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, break; #endif - case MSG_DCM: + case MSG_AHRS: #if HIL_MODE != HIL_MODE_ATTITUDE - CHECK_PAYLOAD_SIZE(DCM); - send_dcm(chan); + CHECK_PAYLOAD_SIZE(AHRS); + send_ahrs(chan); #endif break; case MSG_SIMSTATE: #ifdef DESKTOP_BUILD - CHECK_PAYLOAD_SIZE(DCM); + CHECK_PAYLOAD_SIZE(SIMSTATE); send_simstate(chan); #endif break; @@ -932,7 +932,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) } if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ - send_message(MSG_DCM); + send_message(MSG_AHRS); send_message(MSG_HWSTATUS); } } @@ -1791,13 +1791,19 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) ((AP_Float *)vp)->set_and_save(packet.param_value); } else if (var_type == AP_PARAM_INT32) { if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int32 *)vp)->set_and_save(packet.param_value+rounding_addition); + float v = packet.param_value+rounding_addition; + v = constrain(v, -2147483648, 2147483647); + ((AP_Int32 *)vp)->set_and_save(v); } else if (var_type == AP_PARAM_INT16) { if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int16 *)vp)->set_and_save(packet.param_value+rounding_addition); + float v = packet.param_value+rounding_addition; + v = constrain(v, -32768, 32767); + ((AP_Int16 *)vp)->set_and_save(v); } else if (var_type == AP_PARAM_INT8) { if (packet.param_value < 0) rounding_addition = -rounding_addition; - ((AP_Int8 *)vp)->set_and_save(packet.param_value+rounding_addition); + float v = packet.param_value+rounding_addition; + v = constrain(v, -128, 127); + ((AP_Int8 *)vp)->set_and_save(v); } else { // we don't support mavlink set on this parameter break; @@ -1928,8 +1934,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #else - // set dcm hil sensor - dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + // set AHRS hil sensor + ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, packet.pitchspeed,packet.yawspeed); #endif @@ -1945,8 +1951,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) mavlink_attitude_t packet; mavlink_msg_attitude_decode(msg, &packet); - // set dcm hil sensor - dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + // set AHRS hil sensor + ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, packet.pitchspeed,packet.yawspeed); break; } diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index ece99ea6c2..069f0220f7 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -239,15 +239,15 @@ static void Log_Write_Performance() DataFlash.WriteLong(millis()- perf_mon_timer); DataFlash.WriteInt((int16_t)mainLoop_count); DataFlash.WriteInt(G_Dt_max); - DataFlash.WriteByte(dcm.gyro_sat_count); + DataFlash.WriteByte(0); DataFlash.WriteByte(imu.adc_constraints); - DataFlash.WriteByte(dcm.renorm_range_count); - DataFlash.WriteByte(dcm.renorm_blowup_count); + DataFlash.WriteByte(ahrs.renorm_range_count); + DataFlash.WriteByte(ahrs.renorm_blowup_count); DataFlash.WriteByte(gps_fix_count); - DataFlash.WriteInt((int)(dcm.get_health() * 1000)); - DataFlash.WriteInt((int)(dcm.get_gyro_drift().x * 1000)); - DataFlash.WriteInt((int)(dcm.get_gyro_drift().y * 1000)); - DataFlash.WriteInt((int)(dcm.get_gyro_drift().z * 1000)); + DataFlash.WriteInt(1); // AHRS health + DataFlash.WriteInt((int)(ahrs.get_gyro_drift().x * 1000)); + DataFlash.WriteInt((int)(ahrs.get_gyro_drift().y * 1000)); + DataFlash.WriteInt((int)(ahrs.get_gyro_drift().z * 1000)); DataFlash.WriteInt(pmTest1); DataFlash.WriteByte(END_BYTE); } @@ -300,10 +300,10 @@ static void Log_Write_Control_Tuning() DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); DataFlash.WriteInt((int)(g.channel_roll.servo_out)); DataFlash.WriteInt((int)nav_roll); - DataFlash.WriteInt((int)dcm.roll_sensor); + DataFlash.WriteInt((int)ahrs.roll_sensor); DataFlash.WriteInt((int)(g.channel_pitch.servo_out)); DataFlash.WriteInt((int)nav_pitch); - DataFlash.WriteInt((int)dcm.pitch_sensor); + DataFlash.WriteInt((int)ahrs.pitch_sensor); DataFlash.WriteInt((int)(g.channel_throttle.servo_out)); DataFlash.WriteInt((int)(g.channel_rudder.servo_out)); DataFlash.WriteInt((int)(accel.y * 10000)); @@ -317,7 +317,7 @@ static void Log_Write_Nav_Tuning() DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_NAV_TUNING_MSG); - DataFlash.WriteInt((uint16_t)dcm.yaw_sensor); + DataFlash.WriteInt((uint16_t)ahrs.yaw_sensor); DataFlash.WriteInt((int)wp_distance); DataFlash.WriteInt((uint16_t)target_bearing); DataFlash.WriteInt((uint16_t)nav_bearing); diff --git a/ArduPlane/commands_logic.pde b/ArduPlane/commands_logic.pde index 2156a0c2f6..e9e9de4a20 100644 --- a/ArduPlane/commands_logic.pde +++ b/ArduPlane/commands_logic.pde @@ -279,7 +279,7 @@ static bool verify_takeoff() if (hold_course == -1) { // save our current course to take off if(g.compass_enabled) { - hold_course = dcm.yaw_sensor; + hold_course = ahrs.yaw_sensor; } else { hold_course = g_gps->ground_course; } @@ -326,7 +326,7 @@ static bool verify_land() // be quite large at this point, and that could induce a // sudden large roll correction which is very nasty at // this point in the landing. - hold_course = dcm.yaw_sensor; + hold_course = ahrs.yaw_sensor; } } diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index 8161638686..9144bf3cfd 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -123,7 +123,7 @@ enum ap_message { MSG_NEXT_PARAM, MSG_STATUSTEXT, MSG_FENCE_STATUS, - MSG_DCM, + MSG_AHRS, MSG_SIMSTATE, MSG_HWSTATUS, MSG_RETRY_DEFERRED // this must be last diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index 4aa68f3ad9..df95caab05 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -115,7 +115,7 @@ static void calc_bearing_error() correction using the GPS typically takes 10 seconds or so for a 180 degree correction. */ - bearing_error = nav_bearing - dcm.yaw_sensor; + bearing_error = nav_bearing - ahrs.yaw_sensor; } else { // TODO: we need to use the Yaw gyro for in between GPS reads, diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde index 9bcc101e4f..0dc50c54d9 100644 --- a/ArduPlane/system.pde +++ b/ArduPlane/system.pde @@ -183,7 +183,7 @@ static void init_ardupilot() Serial.println_P(PSTR("Compass initialisation failed!")); g.compass_enabled = false; } else { - dcm.set_compass(&compass); + ahrs.set_compass(&compass); compass.null_offsets_enable(); } } @@ -263,7 +263,7 @@ static void init_ardupilot() //read_EEPROM_airstart_critical(); #if HIL_MODE != HIL_MODE_ATTITUDE imu.init(IMU::WARM_START, mavlink_delay, flash_leds, &timer_scheduler); - dcm.set_centripetal(1); + ahrs.set_centripetal(1); #endif // This delay is important for the APM_RC library to work. @@ -455,8 +455,8 @@ static void startup_IMU_ground(void) imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler); imu.init_accel(mavlink_delay, flash_leds); - dcm.set_centripetal(1); - dcm.matrix_reset(); + ahrs.set_centripetal(1); + ahrs.reset(); // read Baro pressure at ground //----------------------------- @@ -510,10 +510,9 @@ static void update_GPS_light(void) static void resetPerfData(void) { mainLoop_count = 0; G_Dt_max = 0; - dcm.gyro_sat_count = 0; imu.adc_constraints = 0; - dcm.renorm_range_count = 0; - dcm.renorm_blowup_count = 0; + ahrs.renorm_range_count = 0; + ahrs.renorm_blowup_count = 0; gps_fix_count = 0; pmTest1 = 0; perf_mon_timer = millis(); diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index 85c8e4ae3f..ec988bbdd3 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -514,7 +514,7 @@ test_imu(uint8_t argc, const Menu::arg *argv) { //Serial.printf_P(PSTR("Calibrating.")); imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler); - dcm.matrix_reset(); + ahrs.reset(); print_hit_enter(); delay(1000); @@ -528,14 +528,14 @@ test_imu(uint8_t argc, const Menu::arg *argv) // IMU // --- - dcm.update_DCM(); + ahrs.update(); if(g.compass_enabled) { medium_loopCounter++; if(medium_loopCounter == 5){ if (compass.read()) { // Calculate heading - compass.calculate(dcm.get_dcm_matrix()); + compass.calculate(ahrs.get_dcm_matrix()); } medium_loopCounter = 0; } @@ -546,9 +546,9 @@ test_imu(uint8_t argc, const Menu::arg *argv) Vector3f gyros = imu.get_gyro(); Vector3f accels = imu.get_accel(); Serial.printf_P(PSTR("r:%4d p:%4d y:%3d g=(%5.1f %5.1f %5.1f) a=(%5.1f %5.1f %5.1f)\n"), - (int)dcm.roll_sensor / 100, - (int)dcm.pitch_sensor / 100, - (uint16_t)dcm.yaw_sensor / 100, + (int)ahrs.roll_sensor / 100, + (int)ahrs.pitch_sensor / 100, + (uint16_t)ahrs.yaw_sensor / 100, gyros.x, gyros.y, gyros.z, accels.x, accels.y, accels.z); } @@ -574,12 +574,12 @@ test_mag(uint8_t argc, const Menu::arg *argv) return 0; } compass.null_offsets_enable(); - dcm.set_compass(&compass); + ahrs.set_compass(&compass); report_compass(); - // we need the DCM initialised for this test + // we need the AHRS initialised for this test imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler); - dcm.matrix_reset(); + ahrs.reset(); int counter = 0; //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); @@ -595,13 +595,13 @@ test_mag(uint8_t argc, const Menu::arg *argv) // IMU // --- - dcm.update_DCM(); + ahrs.update(); medium_loopCounter++; if(medium_loopCounter == 5){ if (compass.read()) { // Calculate heading - Matrix3f m = dcm.get_dcm_matrix(); + Matrix3f m = ahrs.get_dcm_matrix(); compass.calculate(m); compass.null_offsets(m); } diff --git a/Tools/VARTest/VARTest.pde b/Tools/VARTest/VARTest.pde index 9c4b5de900..e8b07b862c 100644 --- a/Tools/VARTest/VARTest.pde +++ b/Tools/VARTest/VARTest.pde @@ -15,7 +15,7 @@ #include // Arduino SPI lib #include #include -#include +#include #include #include #include @@ -25,6 +25,7 @@ #include // ArduPilot Mega IMU Library #include #include +#include #include #include @@ -43,7 +44,7 @@ AP_GPS_Auto g_gps_driver(&Serial1, &g_gps); AP_InertialSensor_Oilpan ins( &adc ); #endif // CONFIG_IMU_TYPE AP_IMU_INS imu( &ins ); -AP_DCM dcm(&imu, g_gps); +AP_AHRS_DCM ahrs(&imu, g_gps); Arduino_Mega_ISR_Registry isr_registry; #ifdef DESKTOP_BUILD diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h new file mode 100644 index 0000000000..1cd685d557 --- /dev/null +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -0,0 +1,109 @@ +#ifndef AP_AHRS_H +#define AP_AHRS_H +/* + AHRS (Attitude Heading Reference System) interface for ArduPilot + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. +*/ + +#include +#include +#include +#include +#include + +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +class AP_AHRS +{ +public: + // Constructor + AP_AHRS(IMU *imu, GPS *&gps): + _imu(imu), + _gps(gps) + { + // base the ki values by the sensors maximum drift + // rate. The APM2 has gyros which are much less drift + // prone than the APM1, so we should have a lower ki, + // which will make us less prone to increasing omegaI + // incorrectly due to sensor noise + _gyro_drift_limit = imu->get_gyro_drift_rate(); + } + + // Accessors + void set_centripetal(bool b) { _centripetal = b; } + bool get_centripetal(void) { return _centripetal; } + void set_compass(Compass *compass) { _compass = compass; } + + // Methods + virtual void update(void) = 0; + + // Euler angles (radians) + float roll; + float pitch; + float yaw; + + // integer Euler angles (Degrees * 100) + int32_t roll_sensor; + int32_t pitch_sensor; + int32_t yaw_sensor; + + // return a smoothed and corrected gyro vector + virtual Vector3f get_gyro(void) = 0; + + // return the current estimate of the gyro drift + virtual Vector3f get_gyro_drift(void) = 0; + + // reset the current attitude, used on new IMU calibration + virtual void reset(bool recover_eulers=false) = 0; + + // how often our attitude representation has gone out of range + uint8_t renorm_range_count; + + // how often our attitude representation has blown up completely + uint8_t renorm_blowup_count; + + // return the average size of the roll/pitch error estimate + // since last call + virtual float get_error_rp(void) = 0; + + // return the average size of the yaw error estimate + // since last call + virtual float get_error_yaw(void) = 0; + + // return a DCM rotation matrix representing our current + // attitude + virtual Matrix3f get_dcm_matrix(void) = 0; + +protected: + // pointer to compass object, if enabled + Compass * _compass; + + // time in microseconds of last compass update + uint32_t _compass_last_update; + + // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing + // IMU under us without our noticing. + GPS *&_gps; + IMU *_imu; + + // true if we are doing centripetal acceleration correction + bool _centripetal; + + // the limit of the gyro drift claimed by the sensors, in + // radians/s/s + float _gyro_drift_limit; +}; + +#include +#include +#include + +#endif // AP_AHRS_H diff --git a/libraries/AP_DCM/AP_DCM.cpp b/libraries/AP_AHRS/AP_AHRS_DCM.cpp similarity index 80% rename from libraries/AP_DCM/AP_DCM.cpp rename to libraries/AP_AHRS/AP_AHRS_DCM.cpp index 5d08c9b810..323d1bb338 100644 --- a/libraries/AP_DCM/AP_DCM.cpp +++ b/libraries/AP_AHRS/AP_AHRS_DCM.cpp @@ -1,22 +1,19 @@ /* - APM_DCM.cpp - DCM AHRS Library, fixed wing version, for Ardupilot Mega - Code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com + APM_AHRS_DCM.cpp - This library works with the ArduPilot Mega and "Oilpan" + AHRS system using DCM matrices + + Based on DCM code by Doug Weibel, Jordi Muñoz and Jose Julio. DIYDrones.com + + Adapted for the general ArduPilot AHRS interface by Andrew Tridgell This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - Methods: - update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data - get_gyro() : Returns gyro vector corrected for bias - get_accel() : Returns accelerometer vector - get_dcm_matrix() : Returns dcm matrix - + modify it under the terms of the GNU Lesser General Public License + as published by the Free Software Foundation; either version 2.1 + of the License, or (at your option) any later version. */ -#include +#include +#include // this is the speed in cm/s above which we first get a yaw lock with // the GPS @@ -26,21 +23,11 @@ // from the GPS and wait for the ground speed to get above GPS_SPEED_MIN #define GPS_SPEED_RESET 100 -void -AP_DCM::set_compass(Compass *compass) -{ - _compass = compass; -} - - // run a full DCM update round -// the drift_correction_frequency is how many steps of the algorithm -// to run before doing an accelerometer and yaw drift correction step void -AP_DCM::update_DCM(uint8_t drift_correction_frequency) +AP_AHRS_DCM::update(void) { float delta_t; - Vector3f accel; // tell the IMU to grab some data _imu->update(); @@ -50,43 +37,16 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency) // Get current values for gyros _gyro_vector = _imu->get_gyro(); - - // accumulate some more accelerometer data - accel = _imu->get_accel(); - _accel_sum += accel; - _drift_correction_time += delta_t; + _accel_vector = _imu->get_accel(); // Integrate the DCM matrix using gyro inputs matrix_update(delta_t); - // add up the omega vector so we pass a value to the drift - // correction averaged over the same time period as the - // accelerometers - _omega_sum += _omega_integ_corr; - // Normalize the DCM matrix normalize(); - // see if we will perform drift correction on this call - _drift_correction_count++; - - if (_drift_correction_count >= drift_correction_frequency) { - // calculate the average accelerometer vector over - // since the last drift correction call - float scale = 1.0 / _drift_correction_count; - _accel_vector = _accel_sum * scale; - _accel_sum.zero(); - - // calculate the average omega value over this time - _omega_smoothed = _omega_sum * scale; - _omega_sum.zero(); - - // Perform drift correction - drift_correction(_drift_correction_time); - - _drift_correction_time = 0; - _drift_correction_count = 0; - } + // Perform drift correction + drift_correction(delta_t); // paranoid check for bad values in the DCM matrix check_matrix(); @@ -97,7 +57,7 @@ AP_DCM::update_DCM(uint8_t drift_correction_frequency) // update the DCM matrix using only the gyros void -AP_DCM::matrix_update(float _G_Dt) +AP_AHRS_DCM::matrix_update(float _G_Dt) { // _omega_integ_corr is used for _centripetal correction // (theoretically better than _omega) @@ -106,33 +66,22 @@ AP_DCM::matrix_update(float _G_Dt) // Equation 16, adding proportional and integral correction terms _omega = _omega_integ_corr + _omega_P + _omega_yaw_P; - // this is an expansion of the DCM matrix multiply (equation + // this is a replacement of the DCM matrix multiply (equation // 17), with known zero elements removed and the matrix // operations inlined. This runs much faster than the original // version of this code, as the compiler was doing a terrible // job of realising that so many of the factors were in common // or zero. It also uses much less stack, as we no longer need - // additional local matrices + // two additional local matrices - float tmpx = _G_Dt * _omega.x; - float tmpy = _G_Dt * _omega.y; - float tmpz = _G_Dt * _omega.z; - - _dcm_matrix.a.x += _dcm_matrix.a.y * tmpz - _dcm_matrix.a.z * tmpy; - _dcm_matrix.a.y += _dcm_matrix.a.z * tmpx - _dcm_matrix.a.x * tmpz; - _dcm_matrix.a.z += _dcm_matrix.a.x * tmpy - _dcm_matrix.a.y * tmpx; - _dcm_matrix.b.x += _dcm_matrix.b.y * tmpz - _dcm_matrix.b.z * tmpy; - _dcm_matrix.b.y += _dcm_matrix.b.z * tmpx - _dcm_matrix.b.x * tmpz; - _dcm_matrix.b.z += _dcm_matrix.b.x * tmpy - _dcm_matrix.b.y * tmpx; - _dcm_matrix.c.x += _dcm_matrix.c.y * tmpz - _dcm_matrix.c.z * tmpy; - _dcm_matrix.c.y += _dcm_matrix.c.z * tmpx - _dcm_matrix.c.x * tmpz; - _dcm_matrix.c.z += _dcm_matrix.c.x * tmpy - _dcm_matrix.c.y * tmpx; + Vector3f r = _omega * _G_Dt; + _dcm_matrix.rotate(r); } // adjust an accelerometer vector for known acceleration forces void -AP_DCM::accel_adjust(Vector3f &accel) +AP_AHRS_DCM::accel_adjust(Vector3f &accel) { float veloc; // compensate for linear acceleration. This makes a @@ -149,8 +98,8 @@ AP_DCM::accel_adjust(Vector3f &accel) // direction as positive // Equation 26 broken up into separate pieces - accel.y -= _omega_smoothed.z * veloc; - accel.z += _omega_smoothed.y * veloc; + accel.y -= _omega_integ_corr.z * veloc; + accel.z += _omega_integ_corr.y * veloc; } /* @@ -158,10 +107,10 @@ AP_DCM::accel_adjust(Vector3f &accel) extreme errors in the matrix */ void -AP_DCM::matrix_reset(bool recover_eulers) +AP_AHRS_DCM::reset(bool recover_eulers) { if (_compass != NULL) { - _compass->null_offsets_disable(); + _compass->null_offsets_disable(); } // reset the integration terms @@ -169,7 +118,6 @@ AP_DCM::matrix_reset(bool recover_eulers) _omega_P.zero(); _omega_yaw_P.zero(); _omega_integ_corr.zero(); - _omega_smoothed.zero(); _omega.zero(); // if the caller wants us to try to recover to the current @@ -193,13 +141,13 @@ AP_DCM::matrix_reset(bool recover_eulers) check the DCM matrix for pathological values */ void -AP_DCM::check_matrix(void) +AP_AHRS_DCM::check_matrix(void) { if (_dcm_matrix.is_nan()) { //Serial.printf("ERROR: DCM matrix NAN\n"); SITL_debug("ERROR: DCM matrix NAN\n"); renorm_blowup_count++; - matrix_reset(true); + reset(true); return; } // some DCM matrix values can lead to an out of range error in @@ -221,7 +169,7 @@ AP_DCM::check_matrix(void) SITL_debug("ERROR: DCM matrix error. _dcm_matrix.c.x=%f\n", _dcm_matrix.c.x); renorm_blowup_count++; - matrix_reset(true); + reset(true); } } } @@ -229,7 +177,7 @@ AP_DCM::check_matrix(void) // renormalise one vector component of the DCM matrix // this will return false if renormalization fails bool -AP_DCM::renorm(Vector3f const &a, Vector3f &result) +AP_AHRS_DCM::renorm(Vector3f const &a, Vector3f &result) { float renorm_val; @@ -289,7 +237,7 @@ simple matter to stay ahead of it. We call the process of enforcing the orthogonality conditions ÒrenormalizationÓ. */ void -AP_DCM::normalize(void) +AP_AHRS_DCM::normalize(void) { float error; Vector3f t0, t1, t2; @@ -305,7 +253,7 @@ AP_DCM::normalize(void) !renorm(t2, _dcm_matrix.c)) { // Our solution is blowing up and we will force back // to last euler angles - matrix_reset(true); + reset(true); } } @@ -319,7 +267,7 @@ AP_DCM::normalize(void) // This function also updates _omega_yaw_P with a yaw correction term // from our yaw reference vector void -AP_DCM::drift_correction(float deltat) +AP_AHRS_DCM::drift_correction(float deltat) { float error_course = 0; Vector3f accel; @@ -530,7 +478,7 @@ AP_DCM::drift_correction(float deltat) // calculate the euler angles which will be used for high level // navigation control void -AP_DCM::euler_angles(void) +AP_AHRS_DCM::euler_angles(void) { _dcm_matrix.to_euler(&roll, &pitch, &yaw); @@ -544,27 +492,8 @@ AP_DCM::euler_angles(void) /* reporting of DCM state for MAVLink */ -// average accel_weight since last call -float AP_DCM::get_accel_weight(void) -{ - return 1.0; -} - -// average renorm_val since last call -float AP_DCM::get_renorm_val(void) -{ - float ret; - if (_renorm_val_count == 0) { - return 0; - } - ret = _renorm_val_sum / _renorm_val_count; - _renorm_val_sum = 0; - _renorm_val_count = 0; - return ret; -} - // average error_roll_pitch since last call -float AP_DCM::get_error_rp(void) +float AP_AHRS_DCM::get_error_rp(void) { float ret; if (_error_rp_count == 0) { @@ -577,7 +506,7 @@ float AP_DCM::get_error_rp(void) } // average error_yaw since last call -float AP_DCM::get_error_yaw(void) +float AP_AHRS_DCM::get_error_yaw(void) { float ret; if (_error_yaw_count == 0) { diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h new file mode 100644 index 0000000000..ee5b3403ac --- /dev/null +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -0,0 +1,85 @@ +#ifndef AP_AHRS_DCM_H +#define AP_AHRS_DCM_H +/* + DCM based AHRS (Attitude Heading Reference System) interface for + ArduPilot + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. +*/ + +class AP_AHRS_DCM : public AP_AHRS +{ +public: + // Constructors + AP_AHRS_DCM(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps) + { + _kp_roll_pitch = 0.13; + _kp_yaw = 0.4; + _dcm_matrix(Vector3f(1, 0, 0), + Vector3f(0, 1, 0), + Vector3f(0, 0, 1)); + + // base the ki values on the sensors drift rate + _ki_roll_pitch = _gyro_drift_limit * 5; + _ki_yaw = _gyro_drift_limit * 8; + } + + // return the smoothed gyro vector corrected for drift + Vector3f get_gyro(void) {return _omega; } + Matrix3f get_dcm_matrix(void) {return _dcm_matrix; } + + // return the current drift correction integrator value + Vector3f get_gyro_drift(void) {return _omega_I; } + + // Methods + void update(void); + void reset(bool recover_eulers = false); + + // status reporting + float get_error_rp(void); + float get_error_yaw(void); + +private: + float _kp_roll_pitch; + float _ki_roll_pitch; + float _kp_yaw; + float _ki_yaw; + bool _have_initial_yaw; + + // Methods + void accel_adjust(Vector3f &accel); + void matrix_update(float _G_Dt); + void normalize(void); + void check_matrix(void); + bool renorm(Vector3f const &a, Vector3f &result); + void drift_correction(float deltat); + void euler_angles(void); + + // primary representation of attitude + Matrix3f _dcm_matrix; + + Vector3f _gyro_vector; // Store the gyros turn rate in a vector + Vector3f _accel_vector; // current accel vector + + Vector3f _omega_P; // accel Omega Proportional correction + Vector3f _omega_yaw_P; // yaw Omega Proportional correction + Vector3f _omega_I; // Omega Integrator correction + Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction + Vector3f _omega; // Corrected Gyro_Vector data + + // state to support status reporting + float _renorm_val_sum; + uint16_t _renorm_val_count; + float _error_rp_sum; + uint16_t _error_rp_count; + float _error_yaw_sum; + uint16_t _error_yaw_count; + + // time in millis when we last got a GPS heading + uint32_t _gps_last_update; +}; + +#endif // AP_AHRS_DCM_H diff --git a/libraries/AP_AHRS/AP_AHRS_HIL.cpp b/libraries/AP_AHRS/AP_AHRS_HIL.cpp new file mode 100644 index 0000000000..6f263137f9 --- /dev/null +++ b/libraries/AP_AHRS/AP_AHRS_HIL.cpp @@ -0,0 +1,30 @@ +/* + APM_AHRS_HIL.cpp + + Hardware in the loop AHRS object + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later + version. +*/ + +#include +#include + +/**************************************************/ +void +AP_AHRS_HIL::setHil(float _roll, float _pitch, float _yaw, + float _rollRate, float _pitchRate, float _yawRate) +{ + roll = _roll; + pitch = _pitch; + yaw = _yaw; + + _omega(_rollRate, _pitchRate, _yawRate); + + roll_sensor = ToDeg(roll)*100; + pitch_sensor = ToDeg(pitch)*100; + yaw_sensor = ToDeg(yaw)*100; +} diff --git a/libraries/AP_AHRS/AP_AHRS_HIL.h b/libraries/AP_AHRS/AP_AHRS_HIL.h new file mode 100644 index 0000000000..ea46cbe92a --- /dev/null +++ b/libraries/AP_AHRS/AP_AHRS_HIL.h @@ -0,0 +1,39 @@ +#ifndef AP_AHRS_HIL_H +#define AP_AHRS_HIL_H + +class AP_AHRS_HIL : public AP_AHRS +{ +public: + // Constructors + AP_AHRS_HIL(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps) + { + } + + // Accessors + Vector3f get_gyro(void) {return _omega; } + + Matrix3f get_dcm_matrix(void) { + Matrix3f m; + m.from_euler(roll, pitch, yaw); + return m; + } + + // Methods + void update(void) {} + void setHil(float roll, float pitch, float yaw, + float rollRate, float pitchRate, float yawRate); + + // return the current estimate of the gyro drift + Vector3f get_gyro_drift(void) { return Vector3f(0,0,0); } + + // reset the current attitude, used on new IMU calibration + void reset(bool recover_eulers=false) {} + + float get_error_rp(void) { return 0; } + float get_error_yaw(void) { return 0; } + +private: + Vector3f _omega; +}; + +#endif diff --git a/libraries/AP_Quaternion/AP_Quaternion.cpp b/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp similarity index 61% rename from libraries/AP_Quaternion/AP_Quaternion.cpp rename to libraries/AP_AHRS/AP_AHRS_Quaternion.cpp index 25f82d303a..03fe02869d 100644 --- a/libraries/AP_Quaternion/AP_Quaternion.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Quaternion.cpp @@ -1,5 +1,5 @@ /* - AP_Quaternion code, based on quaternion code from Jeb Madgwick + AP_AHRS_Quaternion code, based on quaternion code from Jeb Madgwick See http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf @@ -13,24 +13,7 @@ version. */ #include -#include - -#define ToRad(x) (x*0.01745329252) // *pi/180 -#define ToDeg(x) (x*57.2957795131) // *180/pi - -// this is the speed in cm/s above which we first get a yaw lock with -// the GPS -#define GPS_SPEED_MIN 300 - -// this is the speed in cm/s at which we stop using drift correction -// from the GPS and wait for the ground speed to get above GPS_SPEED_MIN -#define GPS_SPEED_RESET 100 - -void -AP_Quaternion::set_compass(Compass *compass) -{ - _compass = compass; -} +#include // to keep the code as close to the original as possible, we use these // macros for quaternion access @@ -40,7 +23,7 @@ AP_Quaternion::set_compass(Compass *compass) #define SEq_4 q.q4 // Function to compute one quaternion iteration without magnetometer -void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) +void AP_AHRS_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) { // Local system variables float norm; // vector norm @@ -134,7 +117,7 @@ void AP_Quaternion::update_IMU(float deltat, Vector3f &gyro, Vector3f &accel) // Function to compute one quaternion iteration including magnetometer -void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag) +void AP_AHRS_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag) { // local system variables float norm; // vector norm @@ -253,7 +236,7 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V // don't allow the drift rate to be exceeded. This prevents a // sudden drift change coming from a outage in the compass - float max_change = gyroMeasDrift * deltat; + float max_change = _gyro_drift_limit * deltat; drift_delta.x = constrain(drift_delta.x, -max_change, max_change); drift_delta.y = constrain(drift_delta.y, -max_change, max_change); drift_delta.z = constrain(drift_delta.z, -max_change, max_change); @@ -308,161 +291,8 @@ void AP_Quaternion::update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, V } -// Function to update our gyro_bias drift estimate using the accelerometer -// and magnetometer. This is a cut-down version of update_MARG(), but -// without the quaternion update. -void AP_Quaternion::update_drift(float deltat, Vector3f &mag) -{ - // local system variables - float norm; // vector norm - float f_4, f_5, f_6; // objective function elements - float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33, // objective function Jacobian elements - J_41, J_42, J_43, J_44, J_51, J_52, J_53, J_54, J_61, J_62, J_63, J_64; // - float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4; // estimated direction of the gyroscope error - - // computed flux in the earth frame - Vector3f flux; - - // estimated direction of the gyroscope error (radians) - Vector3f w_err; - - // normalise the magnetometer measurement - mag.normalize(); - if (mag.is_inf()) { - // discard this data point - renorm_range_count++; - return; - } - - // auxiliary variables to avoid repeated calculations - float twoSEq_1 = 2.0f * SEq_1; - float twoSEq_2 = 2.0f * SEq_2; - float twoSEq_3 = 2.0f * SEq_3; - float twoSEq_4 = 2.0f * SEq_4; - float twob_x = 2.0f * b_x; - float twob_z = 2.0f * b_z; - float twob_xSEq_1 = 2.0f * b_x * SEq_1; - float twob_xSEq_2 = 2.0f * b_x * SEq_2; - float twob_xSEq_3 = 2.0f * b_x * SEq_3; - float twob_xSEq_4 = 2.0f * b_x * SEq_4; - float twob_zSEq_1 = 2.0f * b_z * SEq_1; - float twob_zSEq_2 = 2.0f * b_z * SEq_2; - float twob_zSEq_3 = 2.0f * b_z * SEq_3; - float twob_zSEq_4 = 2.0f * b_z * SEq_4; - float SEq_1SEq_2; - float SEq_1SEq_3 = SEq_1 * SEq_3; - float SEq_1SEq_4; - float SEq_2SEq_3; - float SEq_2SEq_4 = SEq_2 * SEq_4; - float SEq_3SEq_4; - Vector3f twom = mag * 2.0; - - // compute the objective function and Jacobian - f_4 = twob_x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twob_z * (SEq_2SEq_4 - SEq_1SEq_3) - mag.x; - f_5 = twob_x * (SEq_2 * SEq_3 - SEq_1 * SEq_4) + twob_z * (SEq_1 * SEq_2 + SEq_3 * SEq_4) - mag.y; - f_6 = twob_x * (SEq_1SEq_3 + SEq_2SEq_4) + twob_z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3) - mag.z; - J_11or24 = twoSEq_3; // J_11 negated in matrix multiplication - J_12or23 = 2.0f * SEq_4; - J_13or22 = twoSEq_1; // J_12 negated in matrix multiplication - J_14or21 = twoSEq_2; - J_32 = 2.0f * J_14or21; // negated in matrix multiplication - J_33 = 2.0f * J_11or24; // negated in matrix multiplication - J_41 = twob_zSEq_3; // negated in matrix multiplication - J_42 = twob_zSEq_4; - J_43 = 2.0f * twob_xSEq_3 + twob_zSEq_1; // negated in matrix multiplication - J_44 = 2.0f * twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication - J_51 = twob_xSEq_4 - twob_zSEq_2; // negated in matrix multiplication - J_52 = twob_xSEq_3 + twob_zSEq_1; - J_53 = twob_xSEq_2 + twob_zSEq_4; - J_54 = twob_xSEq_1 - twob_zSEq_3; // negated in matrix multiplication - J_61 = twob_xSEq_3; - J_62 = twob_xSEq_4 - 2.0f * twob_zSEq_2; - J_63 = twob_xSEq_1 - 2.0f * twob_zSEq_3; - J_64 = twob_xSEq_2; - - // compute the gradient (matrix multiplication) - SEqHatDot_1 = - J_41 * f_4 - J_51 * f_5 + J_61 * f_6; - SEqHatDot_2 = + J_42 * f_4 + J_52 * f_5 + J_62 * f_6; - SEqHatDot_3 = - J_43 * f_4 + J_53 * f_5 + J_63 * f_6; - SEqHatDot_4 = - J_44 * f_4 - J_54 * f_5 + J_64 * f_6; - - // normalise the gradient to estimate direction of the gyroscope error - norm = 1.0 / safe_sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4); - if (isinf(norm)) { - // discard this data point - renorm_range_count++; - return; - } - SEqHatDot_1 *= norm; - SEqHatDot_2 *= norm; - SEqHatDot_3 *= norm; - SEqHatDot_4 *= norm; - - // compute angular estimated direction of the gyroscope error - w_err.x = twoSEq_1 * SEqHatDot_2 - twoSEq_2 * SEqHatDot_1 - twoSEq_3 * SEqHatDot_4 + twoSEq_4 * SEqHatDot_3; - w_err.y = twoSEq_1 * SEqHatDot_3 + twoSEq_2 * SEqHatDot_4 - twoSEq_3 * SEqHatDot_1 - twoSEq_4 * SEqHatDot_2; - w_err.z = twoSEq_1 * SEqHatDot_4 - twoSEq_2 * SEqHatDot_3 + twoSEq_3 * SEqHatDot_2 - twoSEq_4 * SEqHatDot_1; - - // keep track of the error rates - _error_rp_sum += 0.5*(fabs(w_err.x) + fabs(w_err.y)); - _error_yaw_sum += fabs(w_err.z); - _error_rp_count++; - _error_yaw_count++; - - // compute the gyroscope bias delta - Vector3f drift_delta = w_err * (deltat * zeta); - - // don't allow the drift rate to be exceeded. This prevents a - // sudden drift change coming from a outage in the compass - float max_change = gyroMeasDrift * deltat; - drift_delta.x = constrain(drift_delta.x, -max_change, max_change); - drift_delta.y = constrain(drift_delta.y, -max_change, max_change); - drift_delta.z = constrain(drift_delta.z, -max_change, max_change); - gyro_bias += drift_delta; - - // compute then integrate the estimated quaternion rate - SEq_1 -= (beta * SEqHatDot_1) * deltat; - SEq_2 -= (beta * SEqHatDot_2) * deltat; - SEq_3 -= (beta * SEqHatDot_3) * deltat; - SEq_4 -= (beta * SEqHatDot_4) * deltat; - - // normalise quaternion - norm = 1.0/safe_sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4); - if (isinf(norm)) { - // our quaternion is bad! Reset based on roll/pitch/yaw - // and hope for the best ... - renorm_blowup_count++; - _compass->null_offsets_disable(); - q.from_euler(roll, pitch, yaw); - _compass->null_offsets_disable(); - return; - } - SEq_1 *= norm; - SEq_2 *= norm; - SEq_3 *= norm; - SEq_4 *= norm; - - // compute flux in the earth frame - // recompute axulirary variables - SEq_1SEq_2 = SEq_1 * SEq_2; - SEq_1SEq_3 = SEq_1 * SEq_3; - SEq_1SEq_4 = SEq_1 * SEq_4; - SEq_3SEq_4 = SEq_3 * SEq_4; - SEq_2SEq_3 = SEq_2 * SEq_3; - SEq_2SEq_4 = SEq_2 * SEq_4; - flux.x = twom.x * (0.5f - SEq_3 * SEq_3 - SEq_4 * SEq_4) + twom.y * (SEq_2SEq_3 - SEq_1SEq_4) + twom.z * (SEq_2SEq_4 + SEq_1SEq_3); - flux.y = twom.x * (SEq_2SEq_3 + SEq_1SEq_4) + twom.y * (0.5f - SEq_2 * SEq_2 - SEq_4 * SEq_4) + twom.z * (SEq_3SEq_4 - SEq_1SEq_2); - flux.z = twom.x * (SEq_2SEq_4 - SEq_1SEq_3) + twom.y * (SEq_3SEq_4 + SEq_1SEq_2) + twom.z * (0.5f - SEq_2 * SEq_2 - SEq_3 * SEq_3); - - // normalise the flux vector to have only components in the x and z - b_x = sqrt((flux.x * flux.x) + (flux.y * flux.y)); - b_z = flux.z; -} - - - // Function to compute one quaternion iteration -void AP_Quaternion::update(void) +void AP_AHRS_Quaternion::update(void) { Vector3f gyro, accel; float deltat; @@ -491,22 +321,16 @@ void AP_Quaternion::update(void) // get current IMU state gyro = _imu->get_gyro(); - accel = _imu->get_accel(); - // Quaternion code uses opposite x and y gyro sense from the - // rest of APM - gyro.x = -gyro.x; - gyro.y = -gyro.y; - - // Quaternion code uses opposite z accel as well - accel.z = -accel.z; + // the quaternion system uses opposite sign for accel + accel = - _imu->get_accel(); if (_centripetal && _gps && _gps->status() == GPS::GPS_OK) { // compensate for linear acceleration. This makes a // surprisingly large difference in the pitch estimate when // turning, plus on takeoff and landing float acceleration = _gps->acceleration(); - accel.x -= acceleration; + accel.x += acceleration; // compensate for centripetal acceleration float veloc; @@ -514,46 +338,19 @@ void AP_Quaternion::update(void) // be careful of the signs in this calculation. the // quaternion system uses different signs than the // rest of APM - accel.y -= (gyro.z - gyro_bias.z) * veloc; - accel.z += (gyro.y - gyro_bias.y) * veloc; + accel.y += (gyro.z - gyro_bias.z) * veloc; + accel.z -= (gyro.y - gyro_bias.y) * veloc; } -#define SEPARATE_DRIFT 0 -#if SEPARATE_DRIFT - /* - The full Madgwick quaternion 'MARG' system assumes you get - gyro, accel and magnetometer updates at the same rate. On - APM we get them at very different rates, and re-calculating - our drift due to the magnetometer in the fast loop is very - wasteful of CPU. - - Instead, we only update the gyro_bias vector when we get a - new magnetometer point, and use the much simpler - update_IMU() as the main quaternion update function. - */ - _gyro_sum += gyro; - _accel_sum += accel; - _sum_count++; - - if (_compass != NULL && _compass->use_for_yaw() && - _compass->last_update != _compass_last_update && - _sum_count != 0) { - // use new compass sample for drift correction - float mag_deltat = 1.0e-6*(_compass->last_update - _compass_last_update); - Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z); - _sum_count = 0; - update_drift(mag_deltat, mag); - _compass_last_update = _compass->last_update; + if (_compass != NULL && _compass->use_for_yaw()) { + Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, _compass->mag_z); + update_MARG(deltat, gyro, accel, mag); + } else { + // step the quaternion solution using just gyros and accels + gyro -= gyro_bias; + update_IMU(deltat, gyro, accel); } - // step the quaternion solution using just gyros and accels - gyro -= gyro_bias; - update_IMU(deltat, gyro, accel); -#else - Vector3f mag = Vector3f(_compass->mag_x, _compass->mag_y, - _compass->mag_z); - update_MARG(deltat, gyro, accel, mag); -#endif - #ifdef DESKTOP_BUILD if (q.is_nan()) { SITL_debug("QUAT NAN: deltat=%f roll=%f pitch=%f yaw=%f q=[%f %f %f %f] a=[%f %f %f] g=(%f %f %f)\n", @@ -587,7 +384,7 @@ void AP_Quaternion::update(void) } // average error in roll/pitch since last call -float AP_Quaternion::get_error_rp(void) +float AP_AHRS_Quaternion::get_error_rp(void) { float ret; if (_error_rp_count == 0) { @@ -600,7 +397,7 @@ float AP_Quaternion::get_error_rp(void) } // average error in yaw since last call -float AP_Quaternion::get_error_yaw(void) +float AP_AHRS_Quaternion::get_error_yaw(void) { float ret; if (_error_yaw_count == 0) { @@ -611,3 +408,18 @@ float AP_Quaternion::get_error_yaw(void) _error_yaw_count = 0; return ret; } + +// reset attitude system +void AP_AHRS_Quaternion::reset(bool recover_eulers) +{ + if (recover_eulers) { + q.from_euler(roll, pitch, yaw); + } else { + q(1, 0, 0, 0); + } + gyro_bias.zero(); + + // reference direction of flux in earth frame + b_x = 0; + b_z = -1; +} diff --git a/libraries/AP_AHRS/AP_AHRS_Quaternion.h b/libraries/AP_AHRS/AP_AHRS_Quaternion.h new file mode 100644 index 0000000000..a5b6583903 --- /dev/null +++ b/libraries/AP_AHRS/AP_AHRS_Quaternion.h @@ -0,0 +1,94 @@ +#ifndef AP_Quaternion_h +#define AP_Quaternion_h + +#include +#include +#include +#include +#include + +#if defined(ARDUINO) && ARDUINO >= 100 + #include "Arduino.h" +#else + #include "WProgram.h" +#endif + +class AP_AHRS_Quaternion : public AP_AHRS +{ +public: + // Constructor + AP_AHRS_Quaternion(IMU *imu, GPS *&gps) : AP_AHRS(imu, gps) + { + // scaled gyro drift limits + beta = sqrt(3.0f / 4.0f) * gyroMeasError; + zeta = sqrt(3.0f / 4.0f) * _gyro_drift_limit; + + // reset attitude + reset(); + } + + // Methods + void update(void); + void reset(bool recover_eulers=false); + + // get corrected gyro vector + Vector3f get_gyro(void) { + // notice the sign reversals here + return Vector3f(_gyro_corrected.x, _gyro_corrected.y, _gyro_corrected.z); + } + + Vector3f get_gyro_drift(void) { + // notice the sign reversals here. The quaternion + // system uses a -ve gyro bias, DCM uses a +ve + return Vector3f(-gyro_bias.x, -gyro_bias.y, -gyro_bias.z); + } + + float get_error_rp(void); + float get_error_yaw(void); + + // convert quaternion to a DCM matrix, used by compass + // null offsets code + Matrix3f get_dcm_matrix(void) { + Matrix3f ret; + q.rotation_matrix(ret); + return ret; + } + +private: + void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel); + void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag); + + bool _have_initial_yaw; + + // Methods + void accel_adjust(void); + + // maximum gyroscope measurement error in rad/s (set to 7 degrees/second) + static const float gyroMeasError = 20.0 * (M_PI/180.0); + + // scaled tuning constants + float beta; + float zeta; + + // quaternion elements + Quaternion q; + + // magnetic flux estimates. These are used for the automatic + // magnetometer calibration + float b_x; + float b_z; + + // estimate gyroscope biases error + Vector3f gyro_bias; + + // the current corrected gyro vector + Vector3f _gyro_corrected; + + // estimate of error + float _error_rp_sum; + uint16_t _error_rp_count; + float _error_yaw_sum; + uint16_t _error_yaw_count; +}; + +#endif diff --git a/libraries/AP_Quaternion/examples/Quaternion/Quaternion.pde b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde similarity index 92% rename from libraries/AP_Quaternion/examples/Quaternion/Quaternion.pde rename to libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde index b24cfd8c5b..0a557f3340 100644 --- a/libraries/AP_Quaternion/examples/Quaternion/Quaternion.pde +++ b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.pde @@ -1,7 +1,7 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- // -// Simple test for the AP_Quaternion library +// Simple test for the AP_AHRS interface // #include @@ -13,8 +13,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -24,7 +23,7 @@ #include // uncomment this for a APM2 board -//#define APM2_HARDWARE +#define APM2_HARDWARE FastSerialPort(Serial, 0); @@ -42,13 +41,15 @@ AP_Compass_HMC5843 compass; # else AP_ADC_ADS7844 adc; AP_InertialSensor_Oilpan ins( &adc ); -#endif // CONFIG_IMU_TYPE +#endif static GPS *g_gps; AP_IMU_INS imu(&ins); -AP_Quaternion ahrs(&imu, g_gps); -//AP_DCM ahrs(&imu, g_gps); + +// choose which AHRS system to use +//AP_AHRS_DCM ahrs(&imu, g_gps); +AP_AHRS_Quaternion ahrs(&imu, g_gps); AP_Baro_BMP085_HIL barometer; @@ -122,6 +123,7 @@ void loop(void) if (now - last_compass > 100*1000UL && compass.read()) { + compass.calculate(ahrs.get_dcm_matrix()); // read compass at 10Hz last_compass = now; } diff --git a/libraries/AP_DCM/examples/DCM_Test/Makefile b/libraries/AP_AHRS/examples/AHRS_Test/Makefile similarity index 100% rename from libraries/AP_DCM/examples/DCM_Test/Makefile rename to libraries/AP_AHRS/examples/AHRS_Test/Makefile diff --git a/libraries/AP_DCM/examples/DCM_Test/DCM_Test.pde b/libraries/AP_AHRS/examples/DCM_Test/DCM_Test.pde similarity index 100% rename from libraries/AP_DCM/examples/DCM_Test/DCM_Test.pde rename to libraries/AP_AHRS/examples/DCM_Test/DCM_Test.pde diff --git a/libraries/AP_Quaternion/examples/Quaternion/Makefile b/libraries/AP_AHRS/examples/DCM_Test/Makefile similarity index 100% rename from libraries/AP_Quaternion/examples/Quaternion/Makefile rename to libraries/AP_AHRS/examples/DCM_Test/Makefile diff --git a/libraries/AP_DCM/AP_DCM.h b/libraries/AP_DCM/AP_DCM.h deleted file mode 100644 index 7c7b2a4217..0000000000 --- a/libraries/AP_DCM/AP_DCM.h +++ /dev/null @@ -1,157 +0,0 @@ -#ifndef AP_DCM_h -#define AP_DCM_h - -// temporarily include all other classes here -// since this naming is a bit off from the -// convention and the AP_DCM should be the top -// header file -#include "AP_DCM_HIL.h" - -#include "../FastSerial/FastSerial.h" -#include "../AP_Math/AP_Math.h" -#include -#include "../AP_Compass/AP_Compass.h" -#include "../AP_ADC/AP_ADC.h" -#include "../AP_GPS/AP_GPS.h" -#include "../AP_IMU/AP_IMU.h" -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - -class AP_DCM -{ -public: - // Constructors - AP_DCM(IMU *imu, GPS *&gps) : - _kp_roll_pitch(0.13), - _kp_yaw(0.4), - _gps(gps), - _imu(imu), - _dcm_matrix(1, 0, 0, - 0, 1, 0, - 0, 0, 1), - _health(1.), - _toggle(0) - { - // base the ki values by the sensors maximum drift - // rate. The APM2 has gyros which are much less drift - // prone than the APM1, so we should have a lower ki, - // which will make us less prone to increasing omegaI - // incorrectly due to sensor noise - _gyro_drift_limit = imu->get_gyro_drift_rate(); - _ki_roll_pitch = _gyro_drift_limit * 5; - _ki_yaw = _gyro_drift_limit * 8; - } - - // Accessors - - // return the smoothed gyro vector corrected for drift - Vector3f get_gyro(void) {return _omega_smoothed; } - Matrix3f get_dcm_matrix(void) {return _dcm_matrix; } - Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();} - - // return the current drift correction integrator value - Vector3f get_gyro_drift(void) {return _omega_I; } - - float get_health(void) {return _health;} - void set_centripetal(bool b) {_centripetal = b;} - bool get_centripetal(void) {return _centripetal;} - void set_compass(Compass *compass); - - // Methods - void update_DCM(uint8_t drift_correction_frequency=1); - void update(void) { update_DCM(); } - void matrix_reset(bool recover_eulers = false); - - long roll_sensor; // Degrees * 100 - long pitch_sensor; // Degrees * 100 - long yaw_sensor; // Degrees * 100 - - float roll; // Radians - float pitch; // Radians - float yaw; // Radians - - uint8_t gyro_sat_count; - uint8_t renorm_range_count; - uint8_t renorm_blowup_count; - - // status reporting - float get_accel_weight(void); - float get_renorm_val(void); - float get_error_rp(void); - float get_error_yaw(void); - -private: - float _kp_roll_pitch; - float _ki_roll_pitch; - float _kp_yaw; - float _ki_yaw; - float _gyro_drift_limit; // radians/s/s - bool _have_initial_yaw; - - // Methods - void read_adc_raw(void); - void accel_adjust(Vector3f &accel); - float read_adc(int select); - void matrix_update(float _G_Dt); - void normalize(void); - void check_matrix(void); - bool renorm(Vector3f const &a, Vector3f &result); - void drift_correction(float deltat); - void euler_angles(void); - - // members - Compass * _compass; - - // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing - // IMU under us without our noticing. - GPS *&_gps; // note: this is a reference to a pointer owned by the caller - - IMU *_imu; - - Matrix3f _dcm_matrix; - - // sum of accel vectors between drift_correction() calls - // this allows the drift correction to run at a different rate - // to the main DCM update code - Vector3f _accel_vector; - Vector3f _accel_sum; - - Vector3f _gyro_vector; // Store the gyros turn rate in a vector - Vector3f _omega_P; // accel Omega Proportional correction - Vector3f _omega_yaw_P; // yaw Omega Proportional correction - Vector3f _omega_I; // Omega Integrator correction - Vector3f _omega_integ_corr; // Partially corrected Gyro_Vector data - used for centrepetal correction - Vector3f _omega; // Corrected Gyro_Vector data - Vector3f _omega_sum; - Vector3f _omega_smoothed; - float _health; - bool _centripetal; - uint8_t _toggle; - - // state to support status reporting - float _renorm_val_sum; - uint16_t _renorm_val_count; - float _error_rp_sum; - uint16_t _error_rp_count; - float _error_yaw_sum; - uint16_t _error_yaw_count; - - // time in micros when we last got a compass fix - uint32_t _compass_last_update; - - // time in millis when we last got a GPS heading - uint32_t _gps_last_update; - - // counter of calls to update_DCM() without drift correction - uint8_t _drift_correction_count; - float _drift_correction_time; - -}; - -#endif - - - diff --git a/libraries/AP_DCM/AP_DCM_HIL.cpp b/libraries/AP_DCM/AP_DCM_HIL.cpp deleted file mode 100644 index ae7aa3ff56..0000000000 --- a/libraries/AP_DCM/AP_DCM_HIL.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/* - APM_DCM_HIL.cpp - DCM AHRS Library, for Ardupilot Mega, Hardware in the Loop Model - Code by James Goppert. DIYDrones.com - - This library is free software; you can redistribute it and/or - modify it under the terms of the GNU Lesser General Public - License as published by the Free Software Foundation; either - version 2.1 of the License, or (at your option) any later version. - - Methods: - update_DCM() : Updates the AHRS by integrating the rotation matrix over time using the IMU object data - get_gyro() : Returns gyro vector corrected for bias - get_accel() : Returns accelerometer vector - get_dcm_matrix() : Returns dcm matrix - -*/ -#include - -#define ToRad(x) (x*0.01745329252) // *pi/180 -#define ToDeg(x) (x*57.2957795131) // *180/pi - -/**************************************************/ -void -AP_DCM_HIL::setHil(float _roll, float _pitch, float _yaw, - float _rollRate, float _pitchRate, float _yawRate) -{ - roll = _roll; - pitch = _pitch; - yaw = _yaw; - - _omega_integ_corr.x = _rollRate; - _omega_integ_corr.y = _pitchRate; - _omega_integ_corr.z = _yawRate; - - roll_sensor =ToDeg(roll)*100; - pitch_sensor =ToDeg(pitch)*100; - yaw_sensor =ToDeg(yaw)*100; - - // Need the standard C_body<-nav dcm from navigation frame to body frame - // Strapdown Inertial Navigation Technology / Titterton/ pg. 41 - float sRoll = sin(roll), cRoll = cos(roll); - float sPitch = sin(pitch), cPitch = cos(pitch); - float sYaw = sin(yaw), cYaw = cos(yaw); - _dcm_matrix.a.x = cPitch*cYaw; - _dcm_matrix.a.y = -cRoll*sYaw+sRoll*sPitch*cYaw; - _dcm_matrix.a.z = sRoll*sYaw+cRoll*sPitch*cYaw; - _dcm_matrix.b.x = cPitch*sYaw; - _dcm_matrix.b.y = cRoll*cYaw+sRoll*sPitch*sYaw; - _dcm_matrix.b.z = -sRoll*cYaw+cRoll*sPitch*sYaw; - _dcm_matrix.c.x = -sPitch; - _dcm_matrix.c.y = sRoll*cPitch; - _dcm_matrix.c.z = cRoll*cPitch; -} diff --git a/libraries/AP_DCM/AP_DCM_HIL.h b/libraries/AP_DCM/AP_DCM_HIL.h deleted file mode 100644 index 6ebdf13893..0000000000 --- a/libraries/AP_DCM/AP_DCM_HIL.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef AP_DCM_HIL_H -#define AP_DCM_HIL_H - -#include "../FastSerial/FastSerial.h" -#include "../AP_Math/AP_Math.h" -#include -#include "../AP_Compass/AP_Compass.h" -#include "../AP_ADC/AP_ADC.h" -#include "../AP_GPS/AP_GPS.h" -#include "../AP_IMU/AP_IMU.h" -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - - -class AP_DCM_HIL -{ -public: - // Constructors - AP_DCM_HIL() : - _dcm_matrix(1, 0, 0, - 0, 1, 0, - 0, 0, 1) - {} - - // Accessors - Vector3f get_gyro(void) {return _omega_integ_corr; } - Vector3f get_accel(void) { return _accel_vector; } - Matrix3f get_dcm_matrix(void) {return _dcm_matrix; } - Matrix3f get_dcm_transposed(void) {Matrix3f temp = _dcm_matrix; return temp.transpose();} - - void set_centripetal(bool b) {} - void set_compass(Compass *compass) {} - - // Methods - void update_DCM(void) {} - void update_DCM_fast(void) {} - - float get_health(void) { return 1.0; } - - long roll_sensor; // Degrees * 100 - long pitch_sensor; // Degrees * 100 - long yaw_sensor; // Degrees * 100 - - float roll; // Radians - float pitch; // Radians - float yaw; // Radians - - uint8_t gyro_sat_count; - uint8_t renorm_range_count; - uint8_t renorm_blowup_count; - - - float kp_roll_pitch() { return 0; } - void kp_roll_pitch(float v) { } - - float ki_roll_pitch() { return 0; } - void ki_roll_pitch(float v) { } - - float kp_yaw() { return 0; } - void kp_yaw(float v) { } - - float ki_yaw() { return 0; } - void ki_yaw(float v) { } - - - void setHil(float roll, float pitch, float yaw, - float rollRate, float pitchRate, float yawRate); -private: - // Methods - Matrix3f _dcm_matrix; - Vector3f _omega_integ_corr; - Vector3f _accel_vector; -}; - -#endif - - - diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp index ab16fbe833..abb8a360c8 100644 --- a/libraries/AP_Math/AP_Math.cpp +++ b/libraries/AP_Math/AP_Math.cpp @@ -30,3 +30,40 @@ float safe_sqrt(float v) } return ret; } + + +// find a rotation that is the combination of two other +// rotations. This is used to allow us to add an overall board +// rotation to an existing rotation of a sensor such as the compass +// Note that this relies the set of rotations being complete. The +// optional 'found' parameter is for the test suite to ensure that it is. +enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found) +{ + Vector3f tv1, tv2; + enum Rotation r; + tv1(1,2,3); + tv1.rotate(r1); + tv1.rotate(r2); + + for (r=ROTATION_NONE; r #include #include +#include +#include #include #include #include @@ -25,6 +27,10 @@ AP_Baro_BMP085_HIL barometer; AP_Compass_HIL compass; #endif +#if 0 +#include +#endif + static float rad_diff(float rad1, float rad2) { float diff = rad1 - rad2; @@ -206,6 +212,52 @@ void test_frame_transforms(void) printf("%f %f %f\n", v2.x, v2.y, v2.z); } +// generate a random float between -1 and 1 +static float rand_num(void) +{ + float ret = ((unsigned)random()) % 2000000; + return (ret - 1.0e6) / 1.0e6; +} + +void test_matrix_rotate(void) +{ + Matrix3f m1, m2, diff; + Vector3f r; + + m1.identity(); + m2.identity(); + r.x = rand_num(); + r.y = rand_num(); + r.z = rand_num(); + + for (uint16_t i = 0; i<1000; i++) { + // old method + Matrix3f temp_matrix; + temp_matrix.a.x = 0; + temp_matrix.a.y = -r.z; + temp_matrix.a.z = r.y; + temp_matrix.b.x = r.z; + temp_matrix.b.y = 0; + temp_matrix.b.z = -r.x; + temp_matrix.c.x = -r.y; + temp_matrix.c.y = r.x; + temp_matrix.c.z = 0; + temp_matrix = m1 * temp_matrix; + m1 += temp_matrix; + + // new method + m2.rotate(r); + + // check they behave in the same way + diff = m1 - m2; + float err = diff.a.length() + diff.b.length() + diff.c.length(); + + if (err > 0) { + Serial.printf("ERROR: i=%u err=%f\n", (unsigned)i, err); + } + } +} + /* euler angle tests */ @@ -220,6 +272,7 @@ void setup(void) test_conversions(); test_quaternion_eulers(); test_matrix_eulers(); + test_matrix_rotate(); } void diff --git a/libraries/AP_Math/examples/rotations/rotations.pde b/libraries/AP_Math/examples/rotations/rotations.pde index 7095446794..2d27a1f8ad 100644 --- a/libraries/AP_Math/examples/rotations/rotations.pde +++ b/libraries/AP_Math/examples/rotations/rotations.pde @@ -25,6 +25,11 @@ AP_Baro_BMP085_HIL barometer; AP_Compass_HIL compass; #endif +#if AUTOMATIC_DECLINATION == ENABLED +// this is in an #if to avoid the static data +#include // ArduPilot Mega Declination Helper Library +#endif + // standard rotation matrices (these are the originals from the old code) #define MATRIX_ROTATION_NONE Matrix3f(1, 0, 0, 0, 1, 0, 0 ,0, 1) @@ -162,6 +167,28 @@ static void test_vectors(void) } +// test combinations of rotations +static void test_combinations(void) +{ + enum Rotation r1, r2, r3; + bool found; + + for (r1=ROTATION_NONE; r1 %u\n", + (unsigned)r1, (unsigned)r2, (unsigned)r3); + } else { + Serial.printf("ERROR rotation: no combination for %u + %u\n", + (unsigned)r1, (unsigned)r2); + } + } + } +} + /* rotation tests */ @@ -171,6 +198,7 @@ void setup(void) Serial.println("rotation unit tests\n"); test_matrices(); test_vectors(); + test_combinations(); Serial.println("rotation unit tests done\n"); } diff --git a/libraries/AP_Math/matrix3.cpp b/libraries/AP_Math/matrix3.cpp index 9122be5316..d819a37263 100644 --- a/libraries/AP_Math/matrix3.cpp +++ b/libraries/AP_Math/matrix3.cpp @@ -44,6 +44,7 @@ void Matrix3::rotation(enum Rotation r) { switch (r) { case ROTATION_NONE: + case ROTATION_MAX: *this = MATRIX_ROTATION_NONE; break; case ROTATION_YAW_45: @@ -133,7 +134,27 @@ void Matrix3::to_euler(float *roll, float *pitch, float *yaw) } } +// apply an additional rotation from a body frame gyro vector +// to a rotation matrix. +template +void Matrix3::rotate(const Vector3 &g) +{ + Matrix3f temp_matrix; + temp_matrix.a.x = a.y * g.z - a.z * g.y; + temp_matrix.a.y = a.z * g.x - a.x * g.z; + temp_matrix.a.z = a.x * g.y - a.y * g.x; + temp_matrix.b.x = b.y * g.z - b.z * g.y; + temp_matrix.b.y = b.z * g.x - b.x * g.z; + temp_matrix.b.z = b.x * g.y - b.y * g.x; + temp_matrix.c.x = c.y * g.z - c.z * g.y; + temp_matrix.c.y = c.z * g.x - c.x * g.z; + temp_matrix.c.z = c.x * g.y - c.y * g.x; + + (*this) += temp_matrix; +} + // only define for float template void Matrix3::rotation(enum Rotation); +template void Matrix3::rotate(const Vector3 &g); template void Matrix3::from_euler(float roll, float pitch, float yaw); template void Matrix3::to_euler(float *roll, float *pitch, float *yaw); diff --git a/libraries/AP_Math/matrix3.h b/libraries/AP_Math/matrix3.h index 85861d6783..db5cbe088e 100644 --- a/libraries/AP_Math/matrix3.h +++ b/libraries/AP_Math/matrix3.h @@ -124,6 +124,21 @@ public: Matrix3 transpose(void) { return *this = transposed(); } + // zero the matrix + void zero(void) { + a.x = a.y = a.z = 0; + b.x = b.y = b.z = 0; + c.x = c.y = c.z = 0; + } + + // setup the identity matrix + void identity(void) { + a.x = b.y = c.z = 1; + a.y = a.z = 0; + b.x = b.z = 0; + c.x = c.y = 0; + } + // check if any elements are NAN bool is_nan(void) { return a.is_nan() || b.is_nan() || c.is_nan(); } @@ -136,6 +151,10 @@ public: // create eulers from a rotation matrix void to_euler(float *roll, float *pitch, float *yaw); + + // apply an additional rotation from a body frame gyro vector + // to a rotation matrix. + void rotate(const Vector3 &g); }; typedef Matrix3 Matrix3i; diff --git a/libraries/AP_Math/quaternion.cpp b/libraries/AP_Math/quaternion.cpp index aae3cada05..014662a632 100644 --- a/libraries/AP_Math/quaternion.cpp +++ b/libraries/AP_Math/quaternion.cpp @@ -34,12 +34,12 @@ void Quaternion::rotation_matrix(Matrix3f &m) m.a.x = 1-2*(q3q3 + q4q4); m.a.y = 2*(q2q3 - q1q4); - m.a.z = - 2*(q2q4 + q1q3); + m.a.z = 2*(q2q4 + q1q3); m.b.x = 2*(q2q3 + q1q4); m.b.y = 1-2*(q2q2 + q4q4); - m.b.z = -2*(q3q4 - q1q2); - m.c.x = -2*(q2q4 - q1q3); - m.c.y = -2*(q3q4 + q1q2); + m.b.z = 2*(q3q4 - q1q2); + m.c.x = 2*(q2q4 - q1q3); + m.c.y = 2*(q3q4 + q1q2); m.c.z = 1-2*(q2q2 + q3q3); } @@ -61,10 +61,8 @@ void Quaternion::from_euler(float roll, float pitch, float yaw) float cr2 = cos(roll*0.5); float cp2 = cos(pitch*0.5); float cy2 = cos(yaw*0.5); - // the sign reversal here is due to the different conventions - // in the madgwick quaternion code and the rest of APM - float sr2 = -sin(roll*0.5); - float sp2 = -sin(pitch*0.5); + float sr2 = sin(roll*0.5); + float sp2 = sin(pitch*0.5); float sy2 = sin(yaw*0.5); q1 = cr2*cp2*cy2 + sr2*sp2*sy2; @@ -77,12 +75,12 @@ void Quaternion::from_euler(float roll, float pitch, float yaw) void Quaternion::to_euler(float *roll, float *pitch, float *yaw) { if (roll) { - *roll = -(atan2(2.0*(q1*q2 + q3*q4), - 1 - 2.0*(q2*q2 + q3*q3))); + *roll = (atan2(2.0*(q1*q2 + q3*q4), + 1 - 2.0*(q2*q2 + q3*q3))); } if (pitch) { // we let safe_asin() handle the singularities near 90/-90 in pitch - *pitch = -safe_asin(2.0*(q1*q3 - q4*q2)); + *pitch = safe_asin(2.0*(q1*q3 - q4*q2)); } if (yaw) { *yaw = atan2(2.0*(q1*q4 + q2*q3), diff --git a/libraries/AP_Math/rotations.h b/libraries/AP_Math/rotations.h index 839119b9e8..ca648bde54 100644 --- a/libraries/AP_Math/rotations.h +++ b/libraries/AP_Math/rotations.h @@ -17,6 +17,12 @@ * with this program. If not, see . */ + +// these rotations form a full set - every rotation in the following +// list when combined with another in the list forms an entry which is +// also in the list. This is an important property. Please run the +// rotations test suite if you add to the list. + // these rotation values are stored to EEPROM, so be careful not to // change the numbering of any existing entry when adding a new entry. enum Rotation { @@ -35,5 +41,6 @@ enum Rotation { ROTATION_PITCH_180, ROTATION_ROLL_180_YAW_225, ROTATION_ROLL_180_YAW_270, - ROTATION_ROLL_180_YAW_315 + ROTATION_ROLL_180_YAW_315, + ROTATION_MAX }; diff --git a/libraries/AP_Math/vector3.cpp b/libraries/AP_Math/vector3.cpp index 9e0be0630c..cd373488df 100644 --- a/libraries/AP_Math/vector3.cpp +++ b/libraries/AP_Math/vector3.cpp @@ -29,6 +29,7 @@ void Vector3::rotate(enum Rotation rotation) T tmp; switch (rotation) { case ROTATION_NONE: + case ROTATION_MAX: return; case ROTATION_YAW_45: { tmp = HALF_SQRT_2*(x - y); diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index b5b27b1b83..9861395107 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/libraries/AP_Mount/AP_Mount.cpp @@ -4,10 +4,9 @@ extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function -AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm) +AP_Mount::AP_Mount(GPS *gps, AP_AHRS *ahrs) { - _dcm=dcm; - _dcm_hil=NULL; + _ahrs=ahrs; _gps=gps; //set_mode(MAV_MOUNT_MODE_RETRACT); //set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink @@ -18,20 +17,6 @@ AP_Mount::AP_Mount(GPS *gps, AP_DCM *dcm) _retract_angles.z=0; } -AP_Mount::AP_Mount(GPS *gps, AP_DCM_HIL *dcm) -{ - _dcm=NULL; - _dcm_hil=dcm; - _gps=gps; - //set_mode(MAV_MOUNT_MODE_RETRACT); - //set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink - set_mode(MAV_MOUNT_MODE_MAVLINK_TARGETING); // FIXME: this is to test ONLY targeting - - _retract_angles.x=0; - _retract_angles.y=0; - _retract_angles.z=0; -} - //sets the servo angles for retraction, note angles are * 100 void AP_Mount::set_retract_angles(int roll, int pitch, int yaw) { @@ -88,7 +73,8 @@ void AP_Mount::update_mount_position() aux_vec.x = _mavlink_angles.x; aux_vec.y = _mavlink_angles.y; aux_vec.z = _mavlink_angles.z; - m = _dcm?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed(); + m = _ahrs->get_dcm_matrix(); + m.transpose(); //rotate vector targ = m*aux_vec; // TODO The next three lines are probably not correct yet @@ -101,17 +87,11 @@ void AP_Mount::update_mount_position() case MAV_MOUNT_MODE_RC_TARGETING: // radio manual control { // TODO It does work, but maybe is a good idea to replace this simplified implementation with a proper one - if (_dcm) + if (_ahrs) { - roll_angle = -_dcm->roll_sensor; - pitch_angle = -_dcm->pitch_sensor; - yaw_angle = -_dcm->yaw_sensor; - } - if (_dcm_hil) - { - roll_angle = -_dcm_hil->roll_sensor; - pitch_angle = -_dcm_hil->pitch_sensor; - yaw_angle = -_dcm_hil->yaw_sensor; + roll_angle = -_ahrs->roll_sensor; + pitch_angle = -_ahrs->pitch_sensor; + yaw_angle = -_ahrs->yaw_sensor; } if (g_rc_function[RC_Channel_aux::k_mount_roll]) roll_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_roll]); @@ -128,7 +108,8 @@ void AP_Mount::update_mount_position() { calc_GPS_target_vector(&_target_GPS_location); } - m = (_dcm)?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed(); + m = _ahrs->get_dcm_matrix(); + m.transpose(); targ = m*_GPS_vector; /* disable stabilization for now, this will help debug */ _stab_roll = 0;_stab_pitch=0;_stab_yaw=0; diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h index b8333ea339..2b64185608 100644 --- a/libraries/AP_Mount/AP_Mount.h +++ b/libraries/AP_Mount/AP_Mount.h @@ -24,7 +24,7 @@ #include #include #include -#include +#include #include #include <../RC_Channel/RC_Channel_aux.h> @@ -32,8 +32,7 @@ class AP_Mount { public: //Constructors - AP_Mount(GPS *gps, AP_DCM *dcm); - AP_Mount(GPS *gps, AP_DCM_HIL *dcm); // constructor for HIL usage + AP_Mount(GPS *gps, AP_AHRS *ahrs); //enums enum MountType{ @@ -72,8 +71,7 @@ private: long rc_map(RC_Channel_aux* rc_ch); //members - AP_DCM *_dcm; - AP_DCM_HIL *_dcm_hil; + AP_AHRS *_ahrs; GPS *_gps; int roll_angle; ///< degrees*100 diff --git a/libraries/AP_Quaternion/AP_Quaternion.h b/libraries/AP_Quaternion/AP_Quaternion.h deleted file mode 100644 index 4d8b061765..0000000000 --- a/libraries/AP_Quaternion/AP_Quaternion.h +++ /dev/null @@ -1,139 +0,0 @@ -#ifndef AP_Quaternion_h -#define AP_Quaternion_h - -#include -#include -#include -#include -#include - -#if defined(ARDUINO) && ARDUINO >= 100 - #include "Arduino.h" -#else - #include "WProgram.h" -#endif - -class AP_Quaternion -{ -public: - // Constructor - AP_Quaternion(IMU *imu, GPS *&gps) : - _imu(imu), - _gps(gps) - { - // reference direction of flux in earth frame - b_x = 0; - b_z = -1; - - // limit the drift to the drift rate reported by the - // sensor driver - gyroMeasDrift = imu->get_gyro_drift_rate(); - - // scaled gyro drift limits - beta = sqrt(3.0f / 4.0f) * gyroMeasError; - zeta = sqrt(3.0f / 4.0f) * gyroMeasDrift; - } - - // Accessors - void set_centripetal(bool b) {_centripetal = b;} - bool get_centripetal(void) {return _centripetal;} - void set_compass(Compass *compass); - - // Methods - void update(void); - - // Euler angles (radians) - float roll; - float pitch; - float yaw; - - // integer Euler angles (Degrees * 100) - int32_t roll_sensor; - int32_t pitch_sensor; - int32_t yaw_sensor; - - - // compatibility methods with DCM - void update_DCM(void) { update(); } - void update_DCM_fast(void) { update(); } - Vector3f get_gyro(void) { - // notice the sign reversals here - return Vector3f(-_gyro_corrected.x, -_gyro_corrected.y, _gyro_corrected.z); - } - Vector3f get_gyro_drift(void) { - // notice the sign reversals here - return Vector3f(-gyro_bias.x, -gyro_bias.y, gyro_bias.z); - } - float get_accel_weight(void) { return 0; } - float get_renorm_val(void) { return 0; } - float get_health(void) { return 0; } - void matrix_reset(void) { } - uint8_t gyro_sat_count; - uint8_t renorm_range_count; - uint8_t renorm_blowup_count; - float get_error_rp(void); - float get_error_yaw(void); - Matrix3f get_dcm_matrix(void) { - Matrix3f ret; - q.rotation_matrix(ret); - return ret; - } - -private: - void update_IMU(float deltat, Vector3f &gyro, Vector3f &accel); - void update_MARG(float deltat, Vector3f &gyro, Vector3f &accel, Vector3f &mag); - void update_drift(float deltat, Vector3f &mag); - - bool _have_initial_yaw; - - // Methods - void accel_adjust(void); - - // members - Compass * _compass; - // time in microseconds of last compass update - uint32_t _compass_last_update; - - // note: we use ref-to-pointer here so that our caller can change the GPS without our noticing - // IMU under us without our noticing. - GPS *&_gps; - IMU *_imu; - - // true if we are doing centripetal acceleration correction - bool _centripetal; - - // maximum gyroscope measurement error in rad/s (set to 7 degrees/second) - static const float gyroMeasError = 20.0 * (M_PI/180.0); - - float gyroMeasDrift; - - float beta; - float zeta; - - // quaternion elements - Quaternion q; - - // magnetic flux estimates. These are used for the automatic - // magnetometer calibration - float b_x; - float b_z; - - // estimate gyroscope biases error - Vector3f gyro_bias; - - // the current corrected gyro vector - Vector3f _gyro_corrected; - - // accel and gyro accumulators for drift correction - Vector3f _gyro_sum; - Vector3f _accel_sum; - uint32_t _sum_count; - - // estimate of error - float _error_rp_sum; - uint16_t _error_rp_count; - float _error_yaw_sum; - uint16_t _error_yaw_count; -}; - -#endif diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index 99a5b60d8e..3e5c53c1d9 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -16,11 +16,11 @@ extern "C" { #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 205, 42, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 127, 42, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_DCM, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {NULL}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {NULL}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {NULL}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, {NULL}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} #endif #include "../protocol.h" @@ -143,7 +143,7 @@ enum FENCE_BREACH #include "./mavlink_msg_fence_point.h" #include "./mavlink_msg_fence_fetch_point.h" #include "./mavlink_msg_fence_status.h" -#include "./mavlink_msg_dcm.h" +#include "./mavlink_msg_ahrs.h" #include "./mavlink_msg_simstate.h" #include "./mavlink_msg_hwstatus.h" diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ahrs.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ahrs.h new file mode 100644 index 0000000000..c82afe2f23 --- /dev/null +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink_msg_ahrs.h @@ -0,0 +1,276 @@ +// MESSAGE AHRS PACKING + +#define MAVLINK_MSG_ID_AHRS 163 + +typedef struct __mavlink_ahrs_t +{ + float omegaIx; ///< X gyro drift estimate rad/s + float omegaIy; ///< Y gyro drift estimate rad/s + float omegaIz; ///< Z gyro drift estimate rad/s + float accel_weight; ///< average accel_weight + float renorm_val; ///< average renormalisation value + float error_rp; ///< average error_roll_pitch value + float error_yaw; ///< average error_yaw value +} mavlink_ahrs_t; + +#define MAVLINK_MSG_ID_AHRS_LEN 28 +#define MAVLINK_MSG_ID_163_LEN 28 + + + +#define MAVLINK_MESSAGE_INFO_AHRS { \ + "AHRS", \ + 7, \ + { { "omegaIx", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_ahrs_t, omegaIx) }, \ + { "omegaIy", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_ahrs_t, omegaIy) }, \ + { "omegaIz", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_ahrs_t, omegaIz) }, \ + { "accel_weight", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_ahrs_t, accel_weight) }, \ + { "renorm_val", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_ahrs_t, renorm_val) }, \ + { "error_rp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_ahrs_t, error_rp) }, \ + { "error_yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_ahrs_t, error_yaw) }, \ + } \ +} + + +/** + * @brief Pack a ahrs message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param omegaIx X gyro drift estimate rad/s + * @param omegaIy Y gyro drift estimate rad/s + * @param omegaIz Z gyro drift estimate rad/s + * @param accel_weight average accel_weight + * @param renorm_val average renormalisation value + * @param error_rp average error_roll_pitch value + * @param error_yaw average error_yaw value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 28); +#else + mavlink_ahrs_t packet; + packet.omegaIx = omegaIx; + packet.omegaIy = omegaIy; + packet.omegaIz = omegaIz; + packet.accel_weight = accel_weight; + packet.renorm_val = renorm_val; + packet.error_rp = error_rp; + packet.error_yaw = error_yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS; + return mavlink_finalize_message(msg, system_id, component_id, 28); +} + +/** + * @brief Pack a ahrs message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param omegaIx X gyro drift estimate rad/s + * @param omegaIy Y gyro drift estimate rad/s + * @param omegaIz Z gyro drift estimate rad/s + * @param accel_weight average accel_weight + * @param renorm_val average renormalisation value + * @param error_rp average error_roll_pitch value + * @param error_yaw average error_yaw value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 28); +#else + mavlink_ahrs_t packet; + packet.omegaIx = omegaIx; + packet.omegaIy = omegaIy; + packet.omegaIz = omegaIz; + packet.accel_weight = accel_weight; + packet.renorm_val = renorm_val; + packet.error_rp = error_rp; + packet.error_yaw = error_yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28); +} + +/** + * @brief Encode a ahrs struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ahrs C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) +{ + return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); +} + +/** + * @brief Send a ahrs message + * @param chan MAVLink channel to send the message + * + * @param omegaIx X gyro drift estimate rad/s + * @param omegaIy Y gyro drift estimate rad/s + * @param omegaIz Z gyro drift estimate rad/s + * @param accel_weight average accel_weight + * @param renorm_val average renormalisation value + * @param error_rp average error_roll_pitch value + * @param error_yaw average error_yaw value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, 28); +#else + mavlink_ahrs_t packet; + packet.omegaIx = omegaIx; + packet.omegaIy = omegaIy; + packet.omegaIz = omegaIz; + packet.accel_weight = accel_weight; + packet.renorm_val = renorm_val; + packet.error_rp = error_rp; + packet.error_yaw = error_yaw; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, 28); +#endif +} + +#endif + +// MESSAGE AHRS UNPACKING + + +/** + * @brief Get field omegaIx from ahrs message + * + * @return X gyro drift estimate rad/s + */ +static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field omegaIy from ahrs message + * + * @return Y gyro drift estimate rad/s + */ +static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field omegaIz from ahrs message + * + * @return Z gyro drift estimate rad/s + */ +static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field accel_weight from ahrs message + * + * @return average accel_weight + */ +static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field renorm_val from ahrs message + * + * @return average renormalisation value + */ +static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field error_rp from ahrs message + * + * @return average error_roll_pitch value + */ +static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field error_yaw from ahrs message + * + * @return average error_yaw value + */ +static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a ahrs message into a struct + * + * @param msg The message to decode + * @param ahrs C-struct to decode the message contents into + */ +static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs) +{ +#if MAVLINK_NEED_BYTE_SWAP + ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg); + ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg); + ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg); + ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg); + ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg); + ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg); + ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg); +#else + memcpy(ahrs, _MAV_PAYLOAD(msg), 28); +#endif +} diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h index 957bd99ca5..27485947c3 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/testsuite.h @@ -668,12 +668,12 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } -static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +static void mavlink_test_ahrs(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_dcm_t packet_in = { + mavlink_ahrs_t packet_in = { 17.0, 45.0, 73.0, @@ -682,7 +682,7 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me 157.0, 185.0, }; - mavlink_dcm_t packet1, packet2; + mavlink_ahrs_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.omegaIx = packet_in.omegaIx; packet1.omegaIy = packet_in.omegaIy; @@ -695,18 +695,18 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_dcm_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_dcm_decode(&msg, &packet2); + mavlink_msg_ahrs_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ahrs_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_dcm_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw ); - mavlink_msg_dcm_decode(&msg, &packet2); + mavlink_msg_ahrs_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw ); + mavlink_msg_ahrs_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_dcm_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw ); - mavlink_msg_dcm_decode(&msg, &packet2); + mavlink_msg_ahrs_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw ); + mavlink_msg_ahrs_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); @@ -714,12 +714,12 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me for (i=0; imsgid = MAVLINK_MSG_ID_AHRS; + return mavlink_finalize_message(msg, system_id, component_id, 28, 127); +} + +/** + * @brief Pack a ahrs message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param omegaIx X gyro drift estimate rad/s + * @param omegaIy Y gyro drift estimate rad/s + * @param omegaIz Z gyro drift estimate rad/s + * @param accel_weight average accel_weight + * @param renorm_val average renormalisation value + * @param error_rp average error_roll_pitch value + * @param error_yaw average error_yaw value + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ahrs_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + float omegaIx,float omegaIy,float omegaIz,float accel_weight,float renorm_val,float error_rp,float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + + memcpy(_MAV_PAYLOAD(msg), buf, 28); +#else + mavlink_ahrs_t packet; + packet.omegaIx = omegaIx; + packet.omegaIy = omegaIy; + packet.omegaIz = omegaIz; + packet.accel_weight = accel_weight; + packet.renorm_val = renorm_val; + packet.error_rp = error_rp; + packet.error_yaw = error_yaw; + + memcpy(_MAV_PAYLOAD(msg), &packet, 28); +#endif + + msg->msgid = MAVLINK_MSG_ID_AHRS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 28, 127); +} + +/** + * @brief Encode a ahrs struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ahrs C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ahrs_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ahrs_t* ahrs) +{ + return mavlink_msg_ahrs_pack(system_id, component_id, msg, ahrs->omegaIx, ahrs->omegaIy, ahrs->omegaIz, ahrs->accel_weight, ahrs->renorm_val, ahrs->error_rp, ahrs->error_yaw); +} + +/** + * @brief Send a ahrs message + * @param chan MAVLink channel to send the message + * + * @param omegaIx X gyro drift estimate rad/s + * @param omegaIy Y gyro drift estimate rad/s + * @param omegaIz Z gyro drift estimate rad/s + * @param accel_weight average accel_weight + * @param renorm_val average renormalisation value + * @param error_rp average error_roll_pitch value + * @param error_yaw average error_yaw value + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ahrs_send(mavlink_channel_t chan, float omegaIx, float omegaIy, float omegaIz, float accel_weight, float renorm_val, float error_rp, float error_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[28]; + _mav_put_float(buf, 0, omegaIx); + _mav_put_float(buf, 4, omegaIy); + _mav_put_float(buf, 8, omegaIz); + _mav_put_float(buf, 12, accel_weight); + _mav_put_float(buf, 16, renorm_val); + _mav_put_float(buf, 20, error_rp); + _mav_put_float(buf, 24, error_yaw); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, buf, 28, 127); +#else + mavlink_ahrs_t packet; + packet.omegaIx = omegaIx; + packet.omegaIy = omegaIy; + packet.omegaIz = omegaIz; + packet.accel_weight = accel_weight; + packet.renorm_val = renorm_val; + packet.error_rp = error_rp; + packet.error_yaw = error_yaw; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AHRS, (const char *)&packet, 28, 127); +#endif +} + +#endif + +// MESSAGE AHRS UNPACKING + + +/** + * @brief Get field omegaIx from ahrs message + * + * @return X gyro drift estimate rad/s + */ +static inline float mavlink_msg_ahrs_get_omegaIx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field omegaIy from ahrs message + * + * @return Y gyro drift estimate rad/s + */ +static inline float mavlink_msg_ahrs_get_omegaIy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field omegaIz from ahrs message + * + * @return Z gyro drift estimate rad/s + */ +static inline float mavlink_msg_ahrs_get_omegaIz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field accel_weight from ahrs message + * + * @return average accel_weight + */ +static inline float mavlink_msg_ahrs_get_accel_weight(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field renorm_val from ahrs message + * + * @return average renormalisation value + */ +static inline float mavlink_msg_ahrs_get_renorm_val(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field error_rp from ahrs message + * + * @return average error_roll_pitch value + */ +static inline float mavlink_msg_ahrs_get_error_rp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field error_yaw from ahrs message + * + * @return average error_yaw value + */ +static inline float mavlink_msg_ahrs_get_error_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a ahrs message into a struct + * + * @param msg The message to decode + * @param ahrs C-struct to decode the message contents into + */ +static inline void mavlink_msg_ahrs_decode(const mavlink_message_t* msg, mavlink_ahrs_t* ahrs) +{ +#if MAVLINK_NEED_BYTE_SWAP + ahrs->omegaIx = mavlink_msg_ahrs_get_omegaIx(msg); + ahrs->omegaIy = mavlink_msg_ahrs_get_omegaIy(msg); + ahrs->omegaIz = mavlink_msg_ahrs_get_omegaIz(msg); + ahrs->accel_weight = mavlink_msg_ahrs_get_accel_weight(msg); + ahrs->renorm_val = mavlink_msg_ahrs_get_renorm_val(msg); + ahrs->error_rp = mavlink_msg_ahrs_get_error_rp(msg); + ahrs->error_yaw = mavlink_msg_ahrs_get_error_yaw(msg); +#else + memcpy(ahrs, _MAV_PAYLOAD(msg), 28); +#endif +} diff --git a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h index 5cda49612f..9bc89d25bb 100644 --- a/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include_v1.0/ardupilotmega/testsuite.h @@ -668,12 +668,12 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } -static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +static void mavlink_test_ahrs(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { mavlink_message_t msg; uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; - mavlink_dcm_t packet_in = { + mavlink_ahrs_t packet_in = { 17.0, 45.0, 73.0, @@ -682,7 +682,7 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me 157.0, 185.0, }; - mavlink_dcm_t packet1, packet2; + mavlink_ahrs_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.omegaIx = packet_in.omegaIx; packet1.omegaIy = packet_in.omegaIy; @@ -695,18 +695,18 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_dcm_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_dcm_decode(&msg, &packet2); + mavlink_msg_ahrs_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ahrs_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_dcm_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw ); - mavlink_msg_dcm_decode(&msg, &packet2); + mavlink_msg_ahrs_pack(system_id, component_id, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw ); + mavlink_msg_ahrs_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_dcm_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw ); - mavlink_msg_dcm_decode(&msg, &packet2); + mavlink_msg_ahrs_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.omegaIx , packet1.omegaIy , packet1.omegaIz , packet1.accel_weight , packet1.renorm_val , packet1.error_rp , packet1.error_yaw ); + mavlink_msg_ahrs_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); @@ -714,12 +714,12 @@ static void mavlink_test_dcm(uint8_t system_id, uint8_t component_id, mavlink_me for (i=0; itime of last breach in milliseconds since boot - + Status of DCM attitude estimator X gyro drift estimate rad/s Y gyro drift estimate rad/s diff --git a/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml b/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml index d43cffec96..f1bbfd6ce6 100644 --- a/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml +++ b/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml @@ -226,7 +226,7 @@ time of last breach in milliseconds since boot - + Status of DCM attitude estimator X gyro drift estimate rad/s Y gyro drift estimate rad/s