aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

APM: adapt ArduPlane for AHRS framework

Browse Source
This commit is contained in:
Andrew Tridgell 2012-03-11 19:13:31 +11:00
parent 8ae0ea7e37
commit 8afd196907
9 changed files with 76 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
ArduPlane/ArduPlane.pde
View File

@ -40,8 +40,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library #include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library #include <AP_InertialSensor.h> // Inertial Sensor (uncalibated IMU) Library
#include <AP_IMU.h> // ArduPilot Mega IMU Library #include <AP_IMU.h> // ArduPilot Mega IMU Library
#include <AP_DCM.h> // ArduPilot Mega DCM Library #include <AP_AHRS.h> // ArduPilot Mega DCM Library
#include <AP_Quaternion.h> // Madgwick quaternion system
#include <PID.h> // PID library #include <PID.h> // PID library
#include <RC_Channel.h> // RC Channel Library #include <RC_Channel.h> // RC Channel Library
#include <AP_RangeFinder.h> // Range finder library #include <AP_RangeFinder.h> // Range finder library
@ -194,12 +193,9 @@ AP_GPS_None g_gps_driver(NULL);
AP_IMU_INS imu( &ins ); AP_IMU_INS imu( &ins );
#if QUATERNION_ENABLE == ENABLED #if QUATERNION_ENABLE == ENABLED
// this shouldn't be called dcm of course, but until we AP_AHRS_Quaternion ahrs(&imu, g_gps);
// decide to actually use something else, I don't want the patch
// size to be huge
AP_Quaternion dcm(&imu, g_gps);
#else #else
AP_DCM dcm(&imu, g_gps); AP_AHRS_DCM ahrs(&imu, g_gps);
#endif #endif
#elif HIL_MODE == HIL_MODE_SENSORS #elif HIL_MODE == HIL_MODE_SENSORS
@ -210,14 +206,14 @@ AP_Compass_HIL compass;
AP_GPS_HIL g_gps_driver(NULL); AP_GPS_HIL g_gps_driver(NULL);
AP_InertialSensor_Oilpan ins( &adc ); AP_InertialSensor_Oilpan ins( &adc );
AP_IMU_Shim imu; AP_IMU_Shim imu;
AP_DCM dcm(&imu, g_gps); AP_AHRS_DCM ahrs(&imu, g_gps);
#elif HIL_MODE == HIL_MODE_ATTITUDE #elif HIL_MODE == HIL_MODE_ATTITUDE
AP_ADC_HIL adc; 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_GPS_HIL g_gps_driver(NULL);
AP_Compass_HIL compass; // never used AP_Compass_HIL compass; // never used
AP_IMU_Shim imu; // never used
AP_Baro_BMP085_HIL barometer; AP_Baro_BMP085_HIL barometer;
#else #else
@ -261,7 +257,7 @@ AP_Relay relay;
// Camera/Antenna mount tracking and stabilisation stuff // Camera/Antenna mount tracking and stabilisation stuff
// -------------------------------------- // --------------------------------------
#if MOUNT == ENABLED #if MOUNT == ENABLED
AP_Mount camera_mount(g_gps, &dcm); AP_Mount camera_mount(g_gps, &ahrs);
#endif #endif
@ -719,18 +715,18 @@ static void fast_loop()
} }
#if HIL_MODE == HIL_MODE_SENSORS #if HIL_MODE == HIL_MODE_SENSORS
// update hil before dcm update // update hil before AHRS update
gcs_update(); gcs_update();
#endif #endif
dcm.update_DCM(); ahrs.update();
// uses the yaw from the DCM to give more accurate turns // uses the yaw from the DCM to give more accurate turns
calc_bearing_error(); calc_bearing_error();
# if HIL_MODE == HIL_MODE_DISABLED # if HIL_MODE == HIL_MODE_DISABLED
if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST) 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) if (g.log_bitmask & MASK_LOG_RAW)
Log_Write_Raw(); Log_Write_Raw();
@ -784,19 +780,19 @@ static void medium_loop()
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
if (g.compass_enabled && compass.read()) { if (g.compass_enabled && compass.read()) {
dcm.set_compass(&compass); ahrs.set_compass(&compass);
// Calculate heading // Calculate heading
Matrix3f m = dcm.get_dcm_matrix(); Matrix3f m = ahrs.get_dcm_matrix();
compass.calculate(m); compass.calculate(m);
compass.null_offsets(m); compass.null_offsets(m);
} else { } else {
dcm.set_compass(NULL); ahrs.set_compass(NULL);
} }
#endif #endif
/*{ /*{
Serial.print(dcm.roll_sensor, DEC); Serial.printf_P(PSTR("\t")); Serial.print(ahrs.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(dcm.pitch_sensor, DEC); Serial.printf_P(PSTR("\t")); Serial.print(ahrs.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(dcm.yaw_sensor, DEC); Serial.printf_P(PSTR("\t")); Serial.print(ahrs.yaw_sensor, DEC); Serial.printf_P(PSTR("\t"));
Vector3f tempaccel = imu.get_accel(); Vector3f tempaccel = imu.get_accel();
Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t")); Serial.print(tempaccel.x, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(tempaccel.y, 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 HIL_MODE != HIL_MODE_ATTITUDE
if ((g.log_bitmask & MASK_LOG_ATTITUDE_MED) && !(g.log_bitmask & MASK_LOG_ATTITUDE_FAST)) 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) if (g.log_bitmask & MASK_LOG_CTUN)
Log_Write_Control_Tuning(); Log_Write_Control_Tuning();

12
ArduPlane/Attitude.pde
View File

@ -38,7 +38,7 @@ static void stabilize()
// handle this is to ensure both go in the same direction from // handle this is to ensure both go in the same direction from
// zero // zero
nav_roll += 18000; nav_roll += 18000;
if (dcm.roll_sensor < 0) nav_roll -= 36000; if (ahrs.roll_sensor < 0) nav_roll -= 36000;
} }
// For Testing Only // For Testing Only
@ -48,11 +48,11 @@ static void stabilize()
// Calculate dersired servo output for the roll // 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 + 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) - (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) { if (inverted_flight) {
// when flying upside down the elevator control is inverted // when flying upside down the elevator control is inverted
tempcalc = -tempcalc; tempcalc = -tempcalc;
@ -120,7 +120,7 @@ static void stabilize()
static void crash_checker() static void crash_checker()
{ {
if(dcm.pitch_sensor < -4500){ if(ahrs.pitch_sensor < -4500){
crash_timer = 255; crash_timer = 255;
} }
if(crash_timer > 0) 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()); nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
Vector3f omega; Vector3f omega;
omega = dcm.get_gyro(); omega = ahrs.get_gyro();
// rate limiter // rate limiter
long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377 long rate = degrees(omega.z) * 100; // 3rad = 17188 , 6rad = 34377

44
ArduPlane/GCS_Mavlink.pde
View File

@ -108,13 +108,13 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan)
static NOINLINE void send_attitude(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( mavlink_msg_attitude_send(
chan, chan,
micros(), micros(),
dcm.roll, ahrs.roll,
dcm.pitch - radians(g.pitch_trim*0.01), ahrs.pitch - radians(g.pitch_trim*0.01),
dcm.yaw, ahrs.yaw,
omega.x, omega.x,
omega.y, omega.y,
omega.z); omega.z);
@ -306,7 +306,7 @@ static void NOINLINE send_meminfo(mavlink_channel_t chan)
static void NOINLINE send_location(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( mavlink_msg_global_position_int_send(
chan, chan,
millis(), millis(),
@ -434,7 +434,7 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
chan, chan,
(float)airspeed / 100.0, (float)airspeed / 100.0,
(float)g_gps->ground_speed / 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 (uint16_t)(100 * (g.channel_throttle.norm_output() / 2.0 + 0.5)), // scale -1,1 to 0-100
current_loc.alt / 100.0, current_loc.alt / 100.0,
0); 0);
@ -486,18 +486,18 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
imu.ax(), imu.ay(), imu.az()); 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(); Vector3f omega_I = ahrs.get_gyro_drift();
mavlink_msg_dcm_send( mavlink_msg_ahrs_send(
chan, chan,
omega_I.x, omega_I.x,
omega_I.y, omega_I.y,
omega_I.z, omega_I.z,
dcm.get_accel_weight(), 0,
dcm.get_renorm_val(), 0,
dcm.get_error_rp(), ahrs.get_error_rp(),
dcm.get_error_yaw()); ahrs.get_error_yaw());
} }
#endif // HIL_MODE != HIL_MODE_ATTITUDE #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; break;
#endif #endif
case MSG_DCM: case MSG_AHRS:
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
CHECK_PAYLOAD_SIZE(DCM); CHECK_PAYLOAD_SIZE(AHRS);
send_dcm(chan); send_ahrs(chan);
#endif #endif
break; break;
case MSG_SIMSTATE: case MSG_SIMSTATE:
#ifdef DESKTOP_BUILD #ifdef DESKTOP_BUILD
CHECK_PAYLOAD_SIZE(DCM); CHECK_PAYLOAD_SIZE(SIMSTATE);
send_simstate(chan); send_simstate(chan);
#endif #endif
break; break;
@ -932,7 +932,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
} }
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
send_message(MSG_DCM); send_message(MSG_AHRS);
send_message(MSG_HWSTATUS); send_message(MSG_HWSTATUS);
} }
} }
@ -1928,8 +1928,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
#else #else
// set dcm hil sensor // set AHRS hil sensor
dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed); packet.pitchspeed,packet.yawspeed);
#endif #endif
@ -1945,8 +1945,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
mavlink_attitude_t packet; mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet); mavlink_msg_attitude_decode(msg, &packet);
// set dcm hil sensor // set AHRS hil sensor
dcm.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed,
packet.pitchspeed,packet.yawspeed); packet.pitchspeed,packet.yawspeed);
break; break;
} }

20
ArduPlane/Log.pde
View File

@ -239,15 +239,15 @@ static void Log_Write_Performance()
DataFlash.WriteLong(millis()- perf_mon_timer); DataFlash.WriteLong(millis()- perf_mon_timer);
DataFlash.WriteInt((int16_t)mainLoop_count); DataFlash.WriteInt((int16_t)mainLoop_count);
DataFlash.WriteInt(G_Dt_max); DataFlash.WriteInt(G_Dt_max);
DataFlash.WriteByte(dcm.gyro_sat_count); DataFlash.WriteByte(0);
DataFlash.WriteByte(imu.adc_constraints); DataFlash.WriteByte(imu.adc_constraints);
DataFlash.WriteByte(dcm.renorm_range_count); DataFlash.WriteByte(ahrs.renorm_range_count);
DataFlash.WriteByte(dcm.renorm_blowup_count); DataFlash.WriteByte(ahrs.renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count); DataFlash.WriteByte(gps_fix_count);
DataFlash.WriteInt((int)(dcm.get_health() * 1000)); DataFlash.WriteInt(1); // AHRS health
DataFlash.WriteInt((int)(dcm.get_gyro_drift().x * 1000)); DataFlash.WriteInt((int)(ahrs.get_gyro_drift().x * 1000));
DataFlash.WriteInt((int)(dcm.get_gyro_drift().y * 1000)); DataFlash.WriteInt((int)(ahrs.get_gyro_drift().y * 1000));
DataFlash.WriteInt((int)(dcm.get_gyro_drift().z * 1000)); DataFlash.WriteInt((int)(ahrs.get_gyro_drift().z * 1000));
DataFlash.WriteInt(pmTest1); DataFlash.WriteInt(pmTest1);
DataFlash.WriteByte(END_BYTE); DataFlash.WriteByte(END_BYTE);
} }
@ -300,10 +300,10 @@ static void Log_Write_Control_Tuning()
DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG); DataFlash.WriteByte(LOG_CONTROL_TUNING_MSG);
DataFlash.WriteInt((int)(g.channel_roll.servo_out)); DataFlash.WriteInt((int)(g.channel_roll.servo_out));
DataFlash.WriteInt((int)nav_roll); 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)(g.channel_pitch.servo_out));
DataFlash.WriteInt((int)nav_pitch); 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_throttle.servo_out));
DataFlash.WriteInt((int)(g.channel_rudder.servo_out)); DataFlash.WriteInt((int)(g.channel_rudder.servo_out));
DataFlash.WriteInt((int)(accel.y * 10000)); DataFlash.WriteInt((int)(accel.y * 10000));
@ -317,7 +317,7 @@ static void Log_Write_Nav_Tuning()
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
DataFlash.WriteByte(LOG_NAV_TUNING_MSG); 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((int)wp_distance);
DataFlash.WriteInt((uint16_t)target_bearing); DataFlash.WriteInt((uint16_t)target_bearing);
DataFlash.WriteInt((uint16_t)nav_bearing); DataFlash.WriteInt((uint16_t)nav_bearing);

4
ArduPlane/commands_logic.pde
View File

@ -279,7 +279,7 @@ static bool verify_takeoff()
if (hold_course == -1) { if (hold_course == -1) {
// save our current course to take off // save our current course to take off
if(g.compass_enabled) { if(g.compass_enabled) {
hold_course = dcm.yaw_sensor; hold_course = ahrs.yaw_sensor;
} else { } else {
hold_course = g_gps->ground_course; 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 // be quite large at this point, and that could induce a
// sudden large roll correction which is very nasty at // sudden large roll correction which is very nasty at
// this point in the landing. // this point in the landing.
hold_course = dcm.yaw_sensor; hold_course = ahrs.yaw_sensor;
} }
} }

2
ArduPlane/defines.h
View File

@ -123,7 +123,7 @@ enum ap_message {
MSG_NEXT_PARAM, MSG_NEXT_PARAM,
MSG_STATUSTEXT, MSG_STATUSTEXT,
MSG_FENCE_STATUS, MSG_FENCE_STATUS,
MSG_DCM, MSG_AHRS,
MSG_SIMSTATE, MSG_SIMSTATE,
MSG_HWSTATUS, MSG_HWSTATUS,
MSG_RETRY_DEFERRED // this must be last MSG_RETRY_DEFERRED // this must be last

2
ArduPlane/navigation.pde
View File

@ -115,7 +115,7 @@ static void calc_bearing_error()
correction using the GPS typically takes 10 seconds or so correction using the GPS typically takes 10 seconds or so
for a 180 degree correction. for a 180 degree correction.
*/ */
bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = nav_bearing - ahrs.yaw_sensor;
} else { } else {
// TODO: we need to use the Yaw gyro for in between GPS reads, // TODO: we need to use the Yaw gyro for in between GPS reads,

13
ArduPlane/system.pde
View File

@ -183,7 +183,7 @@ static void init_ardupilot()
Serial.println_P(PSTR("Compass initialisation failed!")); Serial.println_P(PSTR("Compass initialisation failed!"));
g.compass_enabled = false; g.compass_enabled = false;
} else { } else {
dcm.set_compass(&compass); ahrs.set_compass(&compass);
compass.null_offsets_enable(); compass.null_offsets_enable();
} }
} }
@ -263,7 +263,7 @@ static void init_ardupilot()
//read_EEPROM_airstart_critical(); //read_EEPROM_airstart_critical();
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
imu.init(IMU::WARM_START, mavlink_delay, flash_leds, &timer_scheduler); imu.init(IMU::WARM_START, mavlink_delay, flash_leds, &timer_scheduler);
dcm.set_centripetal(1); ahrs.set_centripetal(1);
#endif #endif
// This delay is important for the APM_RC library to work. // 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(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
imu.init_accel(mavlink_delay, flash_leds); imu.init_accel(mavlink_delay, flash_leds);
dcm.set_centripetal(1); ahrs.set_centripetal(1);
dcm.matrix_reset(); ahrs.reset();
// read Baro pressure at ground // read Baro pressure at ground
//----------------------------- //-----------------------------
@ -510,10 +510,9 @@ static void update_GPS_light(void)
static void resetPerfData(void) { static void resetPerfData(void) {
mainLoop_count = 0; mainLoop_count = 0;
G_Dt_max = 0; G_Dt_max = 0;
dcm.gyro_sat_count = 0;
imu.adc_constraints = 0; imu.adc_constraints = 0;
dcm.renorm_range_count = 0; ahrs.renorm_range_count = 0;
dcm.renorm_blowup_count = 0; ahrs.renorm_blowup_count = 0;
gps_fix_count = 0; gps_fix_count = 0;
pmTest1 = 0; pmTest1 = 0;
perf_mon_timer = millis(); perf_mon_timer = millis();

22
ArduPlane/test.pde
View File

@ -514,7 +514,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
{ {
//Serial.printf_P(PSTR("Calibrating.")); //Serial.printf_P(PSTR("Calibrating."));
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler); imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
dcm.matrix_reset(); ahrs.reset();
print_hit_enter(); print_hit_enter();
delay(1000); delay(1000);
@ -528,14 +528,14 @@ test_imu(uint8_t argc, const Menu::arg *argv)
// IMU // IMU
// --- // ---
dcm.update_DCM(); ahrs.update();
if(g.compass_enabled) { if(g.compass_enabled) {
medium_loopCounter++; medium_loopCounter++;
if(medium_loopCounter == 5){ if(medium_loopCounter == 5){
if (compass.read()) { if (compass.read()) {
// Calculate heading // Calculate heading
compass.calculate(dcm.get_dcm_matrix()); compass.calculate(ahrs.get_dcm_matrix());
} }
medium_loopCounter = 0; medium_loopCounter = 0;
} }
@ -546,9 +546,9 @@ test_imu(uint8_t argc, const Menu::arg *argv)
Vector3f gyros = imu.get_gyro(); Vector3f gyros = imu.get_gyro();
Vector3f accels = imu.get_accel(); 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"), 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)ahrs.roll_sensor / 100,
(int)dcm.pitch_sensor / 100, (int)ahrs.pitch_sensor / 100,
(uint16_t)dcm.yaw_sensor / 100, (uint16_t)ahrs.yaw_sensor / 100,
gyros.x, gyros.y, gyros.z, gyros.x, gyros.y, gyros.z,
accels.x, accels.y, accels.z); accels.x, accels.y, accels.z);
} }
@ -574,12 +574,12 @@ test_mag(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
compass.null_offsets_enable(); compass.null_offsets_enable();
dcm.set_compass(&compass); ahrs.set_compass(&compass);
report_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); imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler);
dcm.matrix_reset(); ahrs.reset();
int counter = 0; int counter = 0;
//Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION); //Serial.printf_P(PSTR("MAG_ORIENTATION: %d\n"), MAG_ORIENTATION);
@ -595,13 +595,13 @@ test_mag(uint8_t argc, const Menu::arg *argv)
// IMU // IMU
// --- // ---
dcm.update_DCM(); ahrs.update();
medium_loopCounter++; medium_loopCounter++;
if(medium_loopCounter == 5){ if(medium_loopCounter == 5){
if (compass.read()) { if (compass.read()) {
// Calculate heading // Calculate heading
Matrix3f m = dcm.get_dcm_matrix(); Matrix3f m = ahrs.get_dcm_matrix();
compass.calculate(m); compass.calculate(m);
compass.null_offsets(m); compass.null_offsets(m);
} }