Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APMRover: fixes to incorporate accelerometer calibration

This commit is contained in:
rmackay9 2012-11-07 16:28:20 +09:00
parent e2b1cb7e8d
commit 232f267839
10 changed files with 83 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
APMrover2/APMrover2.pde
View File

@ -93,12 +93,12 @@ version 2.1 of the License, or (at your option) any later version.
#include <AP_Compass.h> // ArduPilot Mega Magnetometer Library #include <AP_Compass.h> // ArduPilot Mega Magnetometer Library
#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_AHRS.h> // ArduPilot Mega DCM Library #include <AP_AHRS.h> // ArduPilot Mega DCM Library
#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
#include <Filter.h> // Filter library #include <Filter.h> // Filter library
#include <AP_Buffer.h> // FIFO buffer library
#include <ModeFilter.h> // Mode Filter from Filter library #include <ModeFilter.h> // Mode Filter from Filter library
#include <AverageFilter.h> // Mode Filter from Filter library #include <AverageFilter.h> // Mode Filter from Filter library
#include <AP_Relay.h> // APM relay #include <AP_Relay.h> // APM relay
@ -250,14 +250,13 @@ AP_GPS_None g_gps_driver(NULL);
#error Unrecognised GPS_PROTOCOL setting. #error Unrecognised GPS_PROTOCOL setting.
#endif // GPS PROTOCOL #endif // GPS PROTOCOL
# if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 # if CONFIG_INS_TYPE == CONFIG_INS_MPU6000
AP_InertialSensor_MPU6000 ins; AP_InertialSensor_MPU6000 ins;
# else # else
AP_InertialSensor_Oilpan ins( &adc ); AP_InertialSensor_Oilpan ins( &adc );
#endif // CONFIG_IMU_TYPE #endif // CONFIG_INS_TYPE
AP_IMU_INS imu( &ins );
AP_AHRS_DCM ahrs(&imu, g_gps); AP_AHRS_DCM ahrs(&ins, g_gps);
#elif HIL_MODE == HIL_MODE_SENSORS #elif HIL_MODE == HIL_MODE_SENSORS
// sensor emulators // sensor emulators
@ -266,13 +265,11 @@ AP_Baro_BMP085_HIL barometer;
AP_Compass_HIL compass; 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_AHRS_DCM ahrs(&ins, 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_IMU_Shim imu; // never used AP_AHRS_HIL ahrs(&ins, g_gps);
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_Baro_BMP085_HIL barometer; AP_Baro_BMP085_HIL barometer;
@ -903,7 +900,7 @@ static void medium_loop()
Serial.print(ahrs.roll_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.pitch_sensor, DEC); Serial.printf_P(PSTR("\t"));
Serial.print(ahrs.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 = ins.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"));
Serial.println(tempaccel.z, DEC); Serial.println(tempaccel.z, DEC);

16
APMrover2/GCS_Mavlink.pde
View File

@ -367,8 +367,8 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan)
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
static void NOINLINE send_raw_imu1(mavlink_channel_t chan) static void NOINLINE send_raw_imu1(mavlink_channel_t chan)
{ {
Vector3f accel = imu.get_accel(); Vector3f accel = ins.get_accel();
Vector3f gyro = imu.get_gyro(); Vector3f gyro = ins.get_gyro();
mavlink_msg_raw_imu_send( mavlink_msg_raw_imu_send(
chan, chan,
@ -406,8 +406,8 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan)
compass.get_declination(), compass.get_declination(),
0, 0,
0, 0,
imu.gx(), imu.gy(), imu.gz(), ins.gx(), ins.gy(), ins.gz(),
imu.ax(), imu.ay(), imu.az()); ins.ax(), ins.ay(), ins.az());
} }
static void NOINLINE send_ahrs(mavlink_channel_t chan) static void NOINLINE send_ahrs(mavlink_channel_t chan)
@ -1731,9 +1731,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
accels.y = (float)packet.yacc / 1000.0; accels.y = (float)packet.yacc / 1000.0;
accels.z = (float)packet.zacc / 1000.0; accels.z = (float)packet.zacc / 1000.0;
imu.set_gyro(gyros); ins.set_gyro(gyros);
imu.set_accel(accels); ins.set_accel(accels);
#else #else
@ -1785,9 +1785,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
accels.y = (float)packet.yacc / 1000.0; accels.y = (float)packet.yacc / 1000.0;
accels.z = (float)packet.zacc / 1000.0; accels.z = (float)packet.zacc / 1000.0;
imu.set_gyro(gyros); ins.set_gyro(gyros);
imu.set_accel(accels); ins.set_accel(accels);
compass.setHIL(packet.xmag,packet.ymag,packet.zmag); compass.setHIL(packet.xmag,packet.ymag,packet.zmag);
break; break;

8
APMrover2/Log.pde
View File

@ -242,7 +242,7 @@ static void Log_Write_Performance()
DataFlash.WriteInt((int16_t)mainLoop_count); DataFlash.WriteInt((int16_t)mainLoop_count);
DataFlash.WriteInt(G_Dt_max); DataFlash.WriteInt(G_Dt_max);
DataFlash.WriteByte(0); DataFlash.WriteByte(0);
DataFlash.WriteByte(imu.adc_constraints); DataFlash.WriteByte(0); // was adc_constraints
DataFlash.WriteByte(ahrs.renorm_range_count); DataFlash.WriteByte(ahrs.renorm_range_count);
DataFlash.WriteByte(ahrs.renorm_blowup_count); DataFlash.WriteByte(ahrs.renorm_blowup_count);
DataFlash.WriteByte(gps_fix_count); DataFlash.WriteByte(gps_fix_count);
@ -295,7 +295,7 @@ static void Log_Write_Startup(byte type)
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Control_Tuning() static void Log_Write_Control_Tuning()
{ {
Vector3f accel = imu.get_accel(); Vector3f accel = ins.get_accel();
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);
DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(HEAD_BYTE2);
@ -366,8 +366,8 @@ static void Log_Write_GPS( int32_t log_Time, int32_t log_Lattitude, int32_t log_
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
static void Log_Write_Raw() static void Log_Write_Raw()
{ {
Vector3f gyro = imu.get_gyro(); Vector3f gyro = ins.get_gyro();
Vector3f accel = imu.get_accel(); Vector3f accel = ins.get_accel();
gyro *= t7; // Scale up for storage as long integers gyro *= t7; // Scale up for storage as long integers
accel *= t7; accel *= t7;
DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE1);

5
APMrover2/Parameters.h
View File

@ -50,6 +50,7 @@ public:
k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change
k_param_reset_switch_chan, k_param_reset_switch_chan,
k_param_manual_level, k_param_manual_level,
k_param_ins, // libraries/AP_InertialSensor variables
// 110: Telemetry control // 110: Telemetry control
@ -70,8 +71,8 @@ public:
// //
// 130: Sensor parameters // 130: Sensor parameters
// //
k_param_imu = 130, // sensor calibration k_param_imu = 130,
k_param_altitude_mix, k_param_altitude_mix, // sensor calibration
k_param_airspeed_ratio, k_param_airspeed_ratio,
k_param_ground_temperature, k_param_ground_temperature,
k_param_ground_pressure, k_param_ground_pressure,

7
APMrover2/Parameters.pde
View File

@ -150,7 +150,12 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(compass, "COMPASS_", Compass), GOBJECT(compass, "COMPASS_", Compass),
GOBJECT(gcs0, "SR0_", GCS_MAVLINK), GOBJECT(gcs0, "SR0_", GCS_MAVLINK),
GOBJECT(gcs3, "SR3_", GCS_MAVLINK), GOBJECT(gcs3, "SR3_", GCS_MAVLINK),
GOBJECT(imu, "IMU_", IMU),
#if HIL_MODE == HIL_MODE_DISABLED
// @Group: INS_
// @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp
GOBJECT(ins, "INS_", AP_InertialSensor),
#endif
AP_VAREND AP_VAREND
}; };

8
APMrover2/config.h
View File

@ -72,7 +72,7 @@
// //
#if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2
# define CONFIG_IMU_TYPE CONFIG_IMU_MPU6000 # define CONFIG_INS_TYPE CONFIG_INS_MPU6000
# define CONFIG_PUSHBUTTON DISABLED # define CONFIG_PUSHBUTTON DISABLED
# define CONFIG_RELAY DISABLED # define CONFIG_RELAY DISABLED
# define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD # define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD
@ -130,15 +130,15 @@
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// IMU Selection // IMU Selection
// //
#ifndef CONFIG_IMU_TYPE #ifndef CONFIG_INS_TYPE
# define CONFIG_IMU_TYPE CONFIG_IMU_OILPAN # define CONFIG_INS_TYPE CONFIG_INS_OILPAN
#endif #endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// ADC Enable - used to eliminate for systems which don't have ADC. // ADC Enable - used to eliminate for systems which don't have ADC.
// //
#ifndef CONFIG_ADC #ifndef CONFIG_ADC
# if CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN # if CONFIG_INS_TYPE == CONFIG_INS_OILPAN
# define CONFIG_ADC ENABLED # define CONFIG_ADC ENABLED
# else # else
# define CONFIG_ADC DISABLED # define CONFIG_ADC DISABLED

4
APMrover2/defines.h
View File

@ -255,8 +255,8 @@ enum gcs_severity {
// mark a function as not to be inlined // mark a function as not to be inlined
#define NOINLINE __attribute__((noinline)) #define NOINLINE __attribute__((noinline))
#define CONFIG_IMU_OILPAN 1 #define CONFIG_INS_OILPAN 1
#define CONFIG_IMU_MPU6000 2 #define CONFIG_INS_MPU6000 2
#define APM_HARDWARE_APM1 1 #define APM_HARDWARE_APM1 1
#define APM_HARDWARE_APM2 2 #define APM_HARDWARE_APM2 2

40
APMrover2/setup.pde
View File

@ -7,6 +7,7 @@ static int8_t setup_radio (uint8_t argc, const Menu::arg *argv);
static int8_t setup_show (uint8_t argc, const Menu::arg *argv); static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
static int8_t setup_factory (uint8_t argc, const Menu::arg *argv); static int8_t setup_factory (uint8_t argc, const Menu::arg *argv);
static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv); static int8_t setup_flightmodes (uint8_t argc, const Menu::arg *argv);
static int8_t setup_accel_scale (uint8_t argc, const Menu::arg *argv);
static int8_t setup_erase (uint8_t argc, const Menu::arg *argv); static int8_t setup_erase (uint8_t argc, const Menu::arg *argv);
static int8_t setup_compass (uint8_t argc, const Menu::arg *argv); static int8_t setup_compass (uint8_t argc, const Menu::arg *argv);
static int8_t setup_declination (uint8_t argc, const Menu::arg *argv); static int8_t setup_declination (uint8_t argc, const Menu::arg *argv);
@ -19,6 +20,7 @@ static const struct Menu::command setup_menu_commands[] PROGMEM = {
{"reset", setup_factory}, {"reset", setup_factory},
{"radio", setup_radio}, {"radio", setup_radio},
{"modes", setup_flightmodes}, {"modes", setup_flightmodes},
{"accel", setup_accel_scale},
{"compass", setup_compass}, {"compass", setup_compass},
{"declination", setup_declination}, {"declination", setup_declination},
{"battery", setup_batt_monitor}, {"battery", setup_batt_monitor},
@ -60,7 +62,7 @@ setup_show(uint8_t argc, const Menu::arg *argv)
report_xtrack(); report_xtrack();
report_throttle(); report_throttle();
report_flight_modes(); report_flight_modes();
report_imu(); report_ins();
report_compass(); report_compass();
Serial.printf_P(PSTR("Raw Values\n")); Serial.printf_P(PSTR("Raw Values\n"));
@ -288,6 +290,15 @@ setup_erase(uint8_t argc, const Menu::arg *argv)
return 0; return 0;
} }
static int8_t
setup_accel_scale(uint8_t argc, const Menu::arg *argv)
{
ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
ins.calibrate_accel(delay, flash_leds, gcs_send_text_fmt);
report_ins();
return(0);
}
static int8_t static int8_t
setup_compass(uint8_t argc, const Menu::arg *argv) setup_compass(uint8_t argc, const Menu::arg *argv)
{ {
@ -416,14 +427,14 @@ static void report_throttle()
print_blanks(2); print_blanks(2);
} }
static void report_imu() static void report_ins()
{ {
//print_blanks(2); //print_blanks(2);
Serial.printf_P(PSTR("IMU\n")); Serial.printf_P(PSTR("INS\n"));
print_divider(); print_divider();
print_gyro_offsets(); print_gyro_offsets();
print_accel_offsets(); print_accel_offsets_and_scaling();
print_blanks(2); print_blanks(2);
} }
@ -583,21 +594,26 @@ static void print_enabled(bool b)
} }
static void static void
print_accel_offsets(void) print_accel_offsets_and_scaling(void)
{ {
Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\n"), Vector3f accel_offsets = ins.get_accel_offsets();
(float)imu.ax(), Vector3f accel_scale = ins.get_accel_scale();
(float)imu.ay(), Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"),
(float)imu.az()); (float)accel_offsets.x, // Pitch
(float)accel_offsets.y, // Roll
(float)accel_offsets.z, // YAW
(float)accel_scale.x, // Pitch
(float)accel_scale.y, // Roll
(float)accel_scale.z); // YAW
} }
static void static void
print_gyro_offsets(void) print_gyro_offsets(void)
{ {
Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"),
(float)imu.gx(), (float)ins.gx(),
(float)imu.gy(), (float)ins.gy(),
(float)imu.gz()); (float)ins.gz());
} }

15
APMrover2/system.pde
View File

@ -333,7 +333,7 @@ static void startup_ground(void)
//------------------------ //------------------------
// //
startup_IMU_ground(false); startup_INS_ground(false);
#endif #endif
// read the radio to set trims // read the radio to set trims
// --------------------------- // ---------------------------
@ -447,24 +447,24 @@ static void check_short_failsafe()
} }
#if LITE == DISABLED #if LITE == DISABLED
static void startup_IMU_ground(bool force_accel_level) static void startup_INS_ground(bool force_accel_level)
{ {
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC..."));
mavlink_delay(500); mavlink_delay(500);
// Makes the servos wiggle twice - about to begin IMU calibration - HOLD LEVEL AND STILL!! // Makes the servos wiggle twice - about to begin INS calibration - HOLD LEVEL AND STILL!!
// ----------------------- // -----------------------
demo_servos(2); demo_servos(2);
gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning IMU calibration; do not move plane")); gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Beginning INS calibration; do not move plane"));
mavlink_delay(1000); mavlink_delay(1000);
imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler); ins.init(AP_InertialSensor::COLD_START, mavlink_delay, flash_leds, &timer_scheduler);
if (force_accel_level || g.manual_level == 0) { if (force_accel_level || g.manual_level == 0) {
// when MANUAL_LEVEL is set to 1 we don't do accelerometer // when MANUAL_LEVEL is set to 1 we don't do accelerometer
// levelling on each boot, and instead rely on the user to do // levelling on each boot, and instead rely on the user to do
// it once via the ground station // it once via the ground station
imu.init_accel(mavlink_delay, flash_leds); ins.init_accel(mavlink_delay, flash_leds);
} }
ahrs.set_fly_forward(true); ahrs.set_fly_forward(true);
ahrs.reset(); ahrs.reset();
@ -484,7 +484,7 @@ static void startup_IMU_ground(bool force_accel_level)
#endif // HIL_MODE_ATTITUDE #endif // HIL_MODE_ATTITUDE
digitalWrite(B_LED_PIN, LED_ON); // Set LED B high to indicate IMU ready digitalWrite(B_LED_PIN, LED_ON); // Set LED B high to indicate INS ready
digitalWrite(A_LED_PIN, LED_OFF); digitalWrite(A_LED_PIN, LED_OFF);
digitalWrite(C_LED_PIN, LED_OFF); digitalWrite(C_LED_PIN, LED_OFF);
} }
@ -521,7 +521,6 @@ 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;
imu.adc_constraints = 0;
ahrs.renorm_range_count = 0; ahrs.renorm_range_count = 0;
ahrs.renorm_blowup_count = 0; ahrs.renorm_blowup_count = 0;
gps_fix_count = 0; gps_fix_count = 0;

18
APMrover2/test.pde
View File

@ -12,7 +12,7 @@ static int8_t test_gps(uint8_t argc, const Menu::arg *argv);
#if CONFIG_ADC == ENABLED #if CONFIG_ADC == ENABLED
static int8_t test_adc(uint8_t argc, const Menu::arg *argv); static int8_t test_adc(uint8_t argc, const Menu::arg *argv);
#endif #endif
static int8_t test_imu(uint8_t argc, const Menu::arg *argv); static int8_t test_ins(uint8_t argc, const Menu::arg *argv);
static int8_t test_battery(uint8_t argc, const Menu::arg *argv); static int8_t test_battery(uint8_t argc, const Menu::arg *argv);
static int8_t test_relay(uint8_t argc, const Menu::arg *argv); static int8_t test_relay(uint8_t argc, const Menu::arg *argv);
static int8_t test_wp(uint8_t argc, const Menu::arg *argv); static int8_t test_wp(uint8_t argc, const Menu::arg *argv);
@ -58,7 +58,7 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
#endif #endif
{"gps", test_gps}, {"gps", test_gps},
{"rawgps", test_rawgps}, {"rawgps", test_rawgps},
{"imu", test_imu}, {"ins", test_ins},
{"airspeed", test_airspeed}, {"airspeed", test_airspeed},
{"airpressure", test_pressure}, {"airpressure", test_pressure},
#if CONFIG_SONAR == ENABLED #if CONFIG_SONAR == ENABLED
@ -68,7 +68,7 @@ static const struct Menu::command test_menu_commands[] PROGMEM = {
#elif HIL_MODE == HIL_MODE_SENSORS #elif HIL_MODE == HIL_MODE_SENSORS
{"adc", test_adc}, {"adc", test_adc},
{"gps", test_gps}, {"gps", test_gps},
{"imu", test_imu}, {"ins", test_ins},
{"compass", test_mag}, {"compass", test_mag},
#elif HIL_MODE == HIL_MODE_ATTITUDE #elif HIL_MODE == HIL_MODE_ATTITUDE
#endif #endif
@ -519,10 +519,10 @@ test_gps(uint8_t argc, const Menu::arg *argv)
} }
static int8_t static int8_t
test_imu(uint8_t argc, const Menu::arg *argv) test_ins(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); ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
ahrs.reset(); ahrs.reset();
print_hit_enter(); print_hit_enter();
@ -535,7 +535,7 @@ test_imu(uint8_t argc, const Menu::arg *argv)
G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator G_Dt = (float)delta_ms_fast_loop / 1000.f; // used by DCM integrator
fast_loopTimer = millis(); fast_loopTimer = millis();
// IMU // INS
// --- // ---
ahrs.update(); ahrs.update();
@ -549,8 +549,8 @@ test_imu(uint8_t argc, const Menu::arg *argv)
// We are using the IMU // We are using the IMU
// --------------------- // ---------------------
Vector3f gyros = imu.get_gyro(); Vector3f gyros = ins.get_gyro();
Vector3f accels = imu.get_accel(); Vector3f accels = ins.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)ahrs.roll_sensor / 100, (int)ahrs.roll_sensor / 100,
(int)ahrs.pitch_sensor / 100, (int)ahrs.pitch_sensor / 100,
@ -583,7 +583,7 @@ test_mag(uint8_t argc, const Menu::arg *argv)
report_compass(); report_compass();
// we need the AHRS initialised for this test // we need the AHRS initialised for this test
imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler); ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler);
ahrs.reset(); ahrs.reset();
int counter = 0; int counter = 0;