zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

APM: adapt ArduPlane for AHRS framework

master
Andrew Tridgell 13 years ago
parent
8ae0ea7e37
commit
8afd196907
9 changed files with 76 additions and 81 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 38
      ArduPlane/ArduPlane.pde
  2. 12
      ArduPlane/Attitude.pde
  3. 44
      ArduPlane/GCS_Mavlink.pde
  4. 20
      ArduPlane/Log.pde
  5. 4
      ArduPlane/commands_logic.pde
  6. 2
      ArduPlane/defines.h
  7. 2
      ArduPlane/navigation.pde
  8. 13
      ArduPlane/system.pde
  9. 22
      ArduPlane/test.pde

38
ArduPlane/ArduPlane.pde
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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
Unescape 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);
} }

Write Preview
Loading…
Cancel
Save