From 547b4ed55b193a2373c188217cb061ef98c836e2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 11 Mar 2012 19:36:12 +1100 Subject: [PATCH] AHRS: adapt ArduCopter for new AHRS framework --- ArduCopter/ArduCopter.pde | 28 ++++++++++++---------------- ArduCopter/Attitude.pde | 14 +++++++------- ArduCopter/Camera.pde | 10 +++++----- ArduCopter/GCS_Mavlink.pde | 38 +++++++++++++++++++------------------- ArduCopter/Log.pde | 23 ++++++++++------------- ArduCopter/defines.h | 2 +- ArduCopter/sensors.pde | 2 +- ArduCopter/system.pde | 2 +- ArduCopter/test.pde | 2 +- 9 files changed, 57 insertions(+), 64 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index bdfe8cf8bd..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 diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index edd033826e..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 @@ -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..81380432de 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); } } @@ -1559,8 +1559,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 809ee4d4d1..1debcdef96 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -565,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,