From 232f2678396f25b4b1b5379d65e36963b2fba350 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Wed, 7 Nov 2012 16:28:20 +0900 Subject: [PATCH] APMRover: fixes to incorporate accelerometer calibration --- APMrover2/APMrover2.pde | 17 ++++++-------- APMrover2/GCS_Mavlink.pde | 16 ++++++------- APMrover2/Log.pde | 8 +++---- APMrover2/Parameters.h | 7 +++--- APMrover2/Parameters.pde | 7 +++++- APMrover2/config.h | 8 +++---- APMrover2/defines.h | 4 ++-- APMrover2/setup.pde | 48 ++++++++++++++++++++++++++------------- APMrover2/system.pde | 15 ++++++------ APMrover2/test.pde | 18 +++++++-------- 10 files changed, 83 insertions(+), 65 deletions(-) diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde index 8279332bf8..cafb54f5a0 100644 --- a/APMrover2/APMrover2.pde +++ b/APMrover2/APMrover2.pde @@ -93,12 +93,12 @@ version 2.1 of the License, or (at your option) any later version. #include // ArduPilot Mega Magnetometer Library #include // ArduPilot Mega Vector/Matrix math Library #include // Inertial Sensor (uncalibated IMU) Library -#include // ArduPilot Mega IMU Library #include // ArduPilot Mega DCM Library #include // PID library #include // RC Channel Library #include // Range finder library #include // Filter library +#include // FIFO buffer library #include // Mode Filter from Filter library #include // Mode Filter from Filter library #include // APM relay @@ -250,14 +250,13 @@ AP_GPS_None g_gps_driver(NULL); #error Unrecognised GPS_PROTOCOL setting. #endif // GPS PROTOCOL -# if CONFIG_IMU_TYPE == CONFIG_IMU_MPU6000 +# if CONFIG_INS_TYPE == CONFIG_INS_MPU6000 AP_InertialSensor_MPU6000 ins; # else AP_InertialSensor_Oilpan ins( &adc ); -#endif // CONFIG_IMU_TYPE -AP_IMU_INS imu( &ins ); +#endif // CONFIG_INS_TYPE -AP_AHRS_DCM ahrs(&imu, g_gps); +AP_AHRS_DCM ahrs(&ins, g_gps); #elif HIL_MODE == HIL_MODE_SENSORS // sensor emulators @@ -266,13 +265,11 @@ AP_Baro_BMP085_HIL barometer; AP_Compass_HIL compass; AP_GPS_HIL g_gps_driver(NULL); AP_InertialSensor_Oilpan ins( &adc ); -AP_IMU_Shim imu; -AP_AHRS_DCM ahrs(&imu, g_gps); +AP_AHRS_DCM ahrs(&ins, g_gps); #elif HIL_MODE == HIL_MODE_ATTITUDE AP_ADC_HIL adc; -AP_IMU_Shim imu; // never used -AP_AHRS_HIL ahrs(&imu, g_gps); +AP_AHRS_HIL ahrs(&ins, g_gps); AP_GPS_HIL g_gps_driver(NULL); AP_Compass_HIL compass; // never used 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.pitch_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.y, DEC); Serial.printf_P(PSTR("\t")); Serial.println(tempaccel.z, DEC); diff --git a/APMrover2/GCS_Mavlink.pde b/APMrover2/GCS_Mavlink.pde index 81c21db0da..95057941e4 100644 --- a/APMrover2/GCS_Mavlink.pde +++ b/APMrover2/GCS_Mavlink.pde @@ -367,8 +367,8 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) #if HIL_MODE != HIL_MODE_ATTITUDE static void NOINLINE send_raw_imu1(mavlink_channel_t chan) { - Vector3f accel = imu.get_accel(); - Vector3f gyro = imu.get_gyro(); + Vector3f accel = ins.get_accel(); + Vector3f gyro = ins.get_gyro(); mavlink_msg_raw_imu_send( chan, @@ -406,8 +406,8 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan) compass.get_declination(), 0, 0, - imu.gx(), imu.gy(), imu.gz(), - imu.ax(), imu.ay(), imu.az()); + ins.gx(), ins.gy(), ins.gz(), + ins.ax(), ins.ay(), ins.az()); } 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.z = (float)packet.zacc / 1000.0; - imu.set_gyro(gyros); + ins.set_gyro(gyros); - imu.set_accel(accels); + ins.set_accel(accels); #else @@ -1785,9 +1785,9 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) accels.y = (float)packet.yacc / 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); break; diff --git a/APMrover2/Log.pde b/APMrover2/Log.pde index 6500c4a10e..7df03bdde8 100644 --- a/APMrover2/Log.pde +++ b/APMrover2/Log.pde @@ -242,7 +242,7 @@ static void Log_Write_Performance() DataFlash.WriteInt((int16_t)mainLoop_count); DataFlash.WriteInt(G_Dt_max); DataFlash.WriteByte(0); - DataFlash.WriteByte(imu.adc_constraints); + DataFlash.WriteByte(0); // was adc_constraints DataFlash.WriteByte(ahrs.renorm_range_count); DataFlash.WriteByte(ahrs.renorm_blowup_count); DataFlash.WriteByte(gps_fix_count); @@ -295,7 +295,7 @@ static void Log_Write_Startup(byte type) #if HIL_MODE != HIL_MODE_ATTITUDE static void Log_Write_Control_Tuning() { - Vector3f accel = imu.get_accel(); + Vector3f accel = ins.get_accel(); DataFlash.WriteByte(HEAD_BYTE1); 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 static void Log_Write_Raw() { - Vector3f gyro = imu.get_gyro(); - Vector3f accel = imu.get_accel(); + Vector3f gyro = ins.get_gyro(); + Vector3f accel = ins.get_accel(); gyro *= t7; // Scale up for storage as long integers accel *= t7; DataFlash.WriteByte(HEAD_BYTE1); diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 03a34b9258..d105012f3b 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -49,7 +49,8 @@ public: k_param_num_resets, k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change k_param_reset_switch_chan, - k_param_manual_level, + k_param_manual_level, + k_param_ins, // libraries/AP_InertialSensor variables // 110: Telemetry control @@ -70,8 +71,8 @@ public: // // 130: Sensor parameters // - k_param_imu = 130, // sensor calibration - k_param_altitude_mix, + k_param_imu = 130, + k_param_altitude_mix, // sensor calibration k_param_airspeed_ratio, k_param_ground_temperature, k_param_ground_pressure, diff --git a/APMrover2/Parameters.pde b/APMrover2/Parameters.pde index c605d73b11..f95667f167 100644 --- a/APMrover2/Parameters.pde +++ b/APMrover2/Parameters.pde @@ -150,7 +150,12 @@ const AP_Param::Info var_info[] PROGMEM = { GOBJECT(compass, "COMPASS_", Compass), GOBJECT(gcs0, "SR0_", 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 }; diff --git a/APMrover2/config.h b/APMrover2/config.h index 81365c312f..0b689cbded 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -72,7 +72,7 @@ // #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_RELAY DISABLED # define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD @@ -130,15 +130,15 @@ ////////////////////////////////////////////////////////////////////////////// // IMU Selection // -#ifndef CONFIG_IMU_TYPE -# define CONFIG_IMU_TYPE CONFIG_IMU_OILPAN +#ifndef CONFIG_INS_TYPE +# define CONFIG_INS_TYPE CONFIG_INS_OILPAN #endif ////////////////////////////////////////////////////////////////////////////// // ADC Enable - used to eliminate for systems which don't have ADC. // #ifndef CONFIG_ADC -# if CONFIG_IMU_TYPE == CONFIG_IMU_OILPAN +# if CONFIG_INS_TYPE == CONFIG_INS_OILPAN # define CONFIG_ADC ENABLED # else # define CONFIG_ADC DISABLED diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 9f8bde3553..baa43a80cc 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -255,8 +255,8 @@ enum gcs_severity { // mark a function as not to be inlined #define NOINLINE __attribute__((noinline)) -#define CONFIG_IMU_OILPAN 1 -#define CONFIG_IMU_MPU6000 2 +#define CONFIG_INS_OILPAN 1 +#define CONFIG_INS_MPU6000 2 #define APM_HARDWARE_APM1 1 #define APM_HARDWARE_APM2 2 diff --git a/APMrover2/setup.pde b/APMrover2/setup.pde index d7777769a9..886624d1b8 100644 --- a/APMrover2/setup.pde +++ b/APMrover2/setup.pde @@ -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_factory (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_compass (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}, {"radio", setup_radio}, {"modes", setup_flightmodes}, + {"accel", setup_accel_scale}, {"compass", setup_compass}, {"declination", setup_declination}, {"battery", setup_batt_monitor}, @@ -60,7 +62,7 @@ setup_show(uint8_t argc, const Menu::arg *argv) report_xtrack(); report_throttle(); report_flight_modes(); - report_imu(); + report_ins(); report_compass(); Serial.printf_P(PSTR("Raw Values\n")); @@ -288,6 +290,15 @@ setup_erase(uint8_t argc, const Menu::arg *argv) 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 setup_compass(uint8_t argc, const Menu::arg *argv) { @@ -416,15 +427,15 @@ static void report_throttle() print_blanks(2); } -static void report_imu() +static void report_ins() { - //print_blanks(2); - Serial.printf_P(PSTR("IMU\n")); - print_divider(); + //print_blanks(2); + Serial.printf_P(PSTR("INS\n")); + print_divider(); - print_gyro_offsets(); - print_accel_offsets(); - print_blanks(2); + print_gyro_offsets(); + print_accel_offsets_and_scaling(); + print_blanks(2); } static void report_compass() @@ -583,21 +594,26 @@ static void print_enabled(bool b) } 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"), - (float)imu.ax(), - (float)imu.ay(), - (float)imu.az()); + Vector3f accel_offsets = ins.get_accel_offsets(); + Vector3f accel_scale = ins.get_accel_scale(); + Serial.printf_P(PSTR("Accel offsets: %4.2f, %4.2f, %4.2f\tscale: %4.2f, %4.2f, %4.2f\n"), + (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 print_gyro_offsets(void) { Serial.printf_P(PSTR("Gyro offsets: %4.2f, %4.2f, %4.2f\n"), - (float)imu.gx(), - (float)imu.gy(), - (float)imu.gz()); + (float)ins.gx(), + (float)ins.gy(), + (float)ins.gz()); } diff --git a/APMrover2/system.pde b/APMrover2/system.pde index c61aa1f4a6..70af98a413 100644 --- a/APMrover2/system.pde +++ b/APMrover2/system.pde @@ -333,7 +333,7 @@ static void startup_ground(void) //------------------------ // - startup_IMU_ground(false); + startup_INS_ground(false); #endif // read the radio to set trims // --------------------------- @@ -447,24 +447,24 @@ static void check_short_failsafe() } #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 gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Warming up ADC...")); 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); - 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); - 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) { // when MANUAL_LEVEL is set to 1 we don't do accelerometer // levelling on each boot, and instead rely on the user to do // 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.reset(); @@ -484,7 +484,7 @@ static void startup_IMU_ground(bool force_accel_level) #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(C_LED_PIN, LED_OFF); } @@ -521,7 +521,6 @@ static void update_GPS_light(void) static void resetPerfData(void) { mainLoop_count = 0; G_Dt_max = 0; - imu.adc_constraints = 0; ahrs.renorm_range_count = 0; ahrs.renorm_blowup_count = 0; gps_fix_count = 0; diff --git a/APMrover2/test.pde b/APMrover2/test.pde index c60f291e04..d03da5579f 100644 --- a/APMrover2/test.pde +++ b/APMrover2/test.pde @@ -12,7 +12,7 @@ static int8_t test_gps(uint8_t argc, const Menu::arg *argv); #if CONFIG_ADC == ENABLED static int8_t test_adc(uint8_t argc, const Menu::arg *argv); #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_relay(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 {"gps", test_gps}, {"rawgps", test_rawgps}, - {"imu", test_imu}, + {"ins", test_ins}, {"airspeed", test_airspeed}, {"airpressure", test_pressure}, #if CONFIG_SONAR == ENABLED @@ -68,7 +68,7 @@ static const struct Menu::command test_menu_commands[] PROGMEM = { #elif HIL_MODE == HIL_MODE_SENSORS {"adc", test_adc}, {"gps", test_gps}, - {"imu", test_imu}, + {"ins", test_ins}, {"compass", test_mag}, #elif HIL_MODE == HIL_MODE_ATTITUDE #endif @@ -519,10 +519,10 @@ test_gps(uint8_t argc, const Menu::arg *argv) } 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.")); - imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler); + ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler); ahrs.reset(); 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 fast_loopTimer = millis(); - // IMU + // INS // --- ahrs.update(); @@ -549,8 +549,8 @@ test_imu(uint8_t argc, const Menu::arg *argv) // We are using the IMU // --------------------- - Vector3f gyros = imu.get_gyro(); - Vector3f accels = imu.get_accel(); + Vector3f gyros = ins.get_gyro(); + 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"), (int)ahrs.roll_sensor / 100, (int)ahrs.pitch_sensor / 100, @@ -583,7 +583,7 @@ test_mag(uint8_t argc, const Menu::arg *argv) report_compass(); // 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(); int counter = 0;