diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b8aa27879f..acf3cf2862 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -78,7 +78,6 @@ #include // ArduPilot Mega Vector/Matrix math Library #include // Curve used to linearlise throttle pwm to thrust #include // ArduPilot Mega Inertial Sensor (accel & gyro) Library -#include // ArduPilot Mega IMU Library #include // Parent header of Timer // (only included for makefile libpath to work) #include // TimerProcess is the scheduler for MPU6000 reads. @@ -98,6 +97,7 @@ #include // Range finder library #include // Optical Flow library #include // Filter library +#include // APM FIFO Buffer #include // Mode Filter from Filter library #include // Mode Filter from Filter library #include // GPS Lead filter @@ -268,21 +268,20 @@ AP_InertialSensor_MPU6000 ins; #else AP_InertialSensor_Oilpan ins(&adc); #endif -AP_IMU_INS imu(&ins); // we don't want to use gps for yaw correction on ArduCopter, so pass // a NULL GPS object pointer static GPS *g_gps_null; #if DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -AP_AHRS_MPU6000 ahrs(&imu, g_gps, &ins); // only works with APM2 +AP_AHRS_MPU6000 ahrs(&ins, g_gps); // only works with APM2 #else -AP_AHRS_DCM ahrs(&imu, g_gps); +AP_AHRS_DCM ahrs(&ins, g_gps); #endif // ahrs2 object is the secondary ahrs to allow running DMP in parallel with DCM #if SECONDARY_DMP_ENABLED == ENABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 -AP_AHRS_MPU6000 ahrs2(&imu, g_gps, &ins); // only works with APM2 +AP_AHRS_MPU6000 ahrs2(&ins, g_gps); // only works with APM2 #endif #elif HIL_MODE == HIL_MODE_SENSORS @@ -291,20 +290,19 @@ AP_ADC_HIL adc; AP_Baro_BMP085_HIL barometer; AP_Compass_HIL compass; AP_GPS_HIL g_gps_driver(NULL); -AP_IMU_Shim imu; -AP_AHRS_DCM ahrs(&imu, g_gps); AP_InertialSensor_Stub ins; +AP_AHRS_DCM ahrs(&ins, g_gps); + static int32_t gps_base_alt; #elif HIL_MODE == HIL_MODE_ATTITUDE AP_ADC_HIL adc; -AP_IMU_Shim imu; -AP_AHRS_HIL ahrs(&imu, g_gps); +AP_InertialSensor_Stub ins; +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; -AP_InertialSensor_Stub ins; #ifdef OPTFLOW_ENABLED #if CONFIG_APM_HARDWARE == APM_HARDWARE_APM2 AP_OpticalFlow_ADNS3080 optflow(&spi3_semaphore,OPTFLOW_CS_PIN); @@ -919,8 +917,8 @@ static uint32_t last_gps_time; // Set true if we have new PWM data to act on from the Radio static bool new_radio_frame; -// Used to auto exit the in-flight leveler -static int16_t auto_level_counter; +// Used to auto exit the roll_pitch_trim saving function +static uint8_t save_trim_counter; // Reference to the AP relay object - APM1 only AP_Relay relay; @@ -984,7 +982,7 @@ void loop() // We want this to execute fast // ---------------------------- - num_samples = imu.num_samples_available(); + num_samples = ins.num_samples_available(); if (num_samples >= NUM_IMU_SAMPLES_FOR_100HZ) { #if DEBUG_FAST_LOOP == ENABLED @@ -1148,8 +1146,8 @@ static void medium_loop() } #endif - // auto_trim, uses an auto_level algorithm - auto_trim(); + // save_trim - stores roll and pitch radio inputs to ahrs + save_trim(); // record throttle output // ------------------------------ @@ -2067,7 +2065,7 @@ static void read_AHRS(void) #endif ahrs.update(); - omega = imu.get_gyro(); + omega = ins.get_gyro(); #if SECONDARY_DMP_ENABLED == ENABLED ahrs2.update(); diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 6d551371e2..76bb3127b4 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -647,7 +647,7 @@ static int16_t get_z_damping() float get_world_Z_accel() { - accels_rot = ahrs.get_dcm_matrix() * imu.get_accel(); + accels_rot = ahrs.get_dcm_matrix() * ins.get_accel(); //Serial.printf("z %1.4f\n", accels_rot.z); return accels_rot.z; } @@ -695,7 +695,7 @@ static float fullDampP = 0.100; float get_world_Z_accel() { - accels_rot = ahrs.get_dcm_matrix() * imu.get_accel(); + accels_rot = ahrs.get_dcm_matrix() * ins.get_accel(); return accels_rot.z; } diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 3d13c3ed05..aae4f86cbb 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -395,8 +395,8 @@ static void NOINLINE send_vfr_hud(mavlink_channel_t chan) 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, micros(), @@ -432,8 +432,8 @@ static void NOINLINE send_raw_imu3(mavlink_channel_t chan) compass.get_declination(), barometer.get_raw_pressure(), barometer.get_raw_temp(), - 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_gps_status(mavlink_channel_t chan) @@ -1065,7 +1065,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) if (packet.param1 == 1 || packet.param2 == 1 || packet.param3 == 1) { - imu.init_accel(mavlink_delay, flash_leds); + ins.init_accel(mavlink_delay, flash_leds); } if (packet.param4 == 1) { trim_radio(); @@ -1748,9 +1748,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_offsets(gyros); - imu.set_accel(accels); + ins.set_accel_offsets(accels); // set AHRS hil sensor @@ -1814,9 +1814,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_offsets(gyros); - imu.set_accel(accels); + ins.set_accel_offsets(accels); compass.setHIL(packet.xmag,packet.ymag,packet.zmag); break; diff --git a/ArduCopter/Log.pde b/ArduCopter/Log.pde index 5c847839d4..689d96c037 100644 --- a/ArduCopter/Log.pde +++ b/ArduCopter/Log.pde @@ -292,7 +292,8 @@ static void Log_Read_GPS() #if INERTIAL_NAV == ENABLED static void Log_Write_Raw() { - Vector3f accel = imu.get_accel(); + Vector3f gyro = ins.get_gyro(); + Vector3f accel = ins.get_accel(); DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); @@ -345,8 +346,8 @@ static void Log_Read_Raw() #else 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(); DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); @@ -724,7 +725,7 @@ static void Log_Write_Performance() DataFlash.WriteByte(HEAD_BYTE1); DataFlash.WriteByte(HEAD_BYTE2); DataFlash.WriteByte(LOG_PERFORMANCE_MSG); - DataFlash.WriteByte( imu.adc_constraints); //1 + DataFlash.WriteByte( 0); //1 - was adc_constraints DataFlash.WriteByte( ahrs.renorm_range_count); //2 DataFlash.WriteByte( ahrs.renorm_blowup_count); //3 DataFlash.WriteByte( gps_fix_count); //4 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index e853273283..8a5494f90b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -99,8 +99,8 @@ public: // // 140: Sensor parameters // - k_param_imu = 140, // sensor calibration - k_param_battery_monitoring, + k_param_imu = 140, // deprecated - can be deleted + k_param_battery_monitoring = 141, k_param_volt_div_ratio, k_param_curr_amp_per_volt, k_param_input_voltage, diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index ab3b568640..5a54941600 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -363,18 +363,14 @@ const AP_Param::Info var_info[] PROGMEM = { GOBJECT(compass, "COMPASS_", Compass), // @Group: INS_ - // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor_Oilpan.cpp -#if HIL_MODE == HIL_MODE_DISABLED && CONFIG_APM_HARDWARE == APM_HARDWARE_APM1 - GOBJECT(ins, "INS_", AP_InertialSensor_Oilpan), + // @Path: ../libraries/AP_InertialSensor/AP_InertialSensor.cpp +#if HIL_MODE == HIL_MODE_DISABLED + GOBJECT(ins, "INS_", AP_InertialSensor), #endif GOBJECT(gcs0, "SR0_", GCS_MAVLINK), GOBJECT(gcs3, "SR3_", GCS_MAVLINK), - // @Group: IMU_ - // @Path: ../libraries/AP_IMU/IMU.cpp - GOBJECT(imu, "IMU_", IMU), - // @Group: AHRS_ // @Path: ../libraries/AP_AHRS/AP_AHRS.cpp GOBJECT(ahrs, "AHRS_", AP_AHRS), diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 0f3ada06df..b2052226a6 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -148,102 +148,38 @@ static void read_trim_switch() } } }else if (option == CH7_AUTO_TRIM) { - if (g.rc_7.radio_in > CH_7_PWM_TRIGGER) { - auto_level_counter = 10; + if(g.rc_7.radio_in > CH_7_PWM_TRIGGER && control_mode <= ACRO && g.rc_3.control_in == 0) { + save_trim_counter = 15; } } } -static void auto_trim() +// save_trim - adds roll and pitch trims from the radio to ahrs +static void save_trim() { - if(auto_level_counter > 0) { - //g.rc_1.dead_zone = 60; // 60 = .6 degrees - //g.rc_2.dead_zone = 60; - - auto_level_counter--; - - if( motors.armed() ) { - // set high AHRS gains to force accelerometers to impact attitude estimate - ahrs.set_fast_gains(true); -#if SECONDARY_DMP_ENABLED == ENABLED - ahrs2.set_fast_gains(true); -#endif - } + float roll_trim, pitch_trim; - trim_accel(); - led_mode = AUTO_TRIM_LEDS; - do_simple = false; + if(save_trim_counter > 0) { + save_trim_counter--; - if(auto_level_counter == 1) { - //g.rc_1.dead_zone = 0; // 60 = .6 degrees - //g.rc_2.dead_zone = 0; - led_mode = NORMAL_LEDS; - clear_leds(); - imu.save(); + // first few iterations we simply flash the leds + led_mode = SAVE_TRIM_LEDS; + + if(save_trim_counter == 1) { + // save roll trim + roll_trim = ToRad((float)g.rc_1.control_in/100.0); + pitch_trim = ToRad((float)g.rc_2.control_in/100.0); + ahrs.add_trim(roll_trim, pitch_trim); + Serial.printf_P(PSTR("\nTrim Roll:%4.2f Pitch:%4.2f\n"),ToDeg(roll_trim), ToDeg(pitch_trim)); reset_control_switch(); - // go back to normal AHRS gains - if( motors.armed() ) { - ahrs.set_fast_gains(false); -#if SECONDARY_DMP_ENABLED == ENABLED - ahrs2.set_fast_gains(false); -#endif - } + // switch back leds to normal + led_mode = NORMAL_LEDS; - //Serial.println("Done"); - auto_level_counter = 0; + save_trim_counter = 0; } } } -/* - * How this works: - * Level Example: - * A_off: -14.00, -20.59, -30.80 - * - * Right roll Example: - * A_off: -6.73, 89.05, -46.02 - * - * Left Roll Example: - * A_off: -18.11, -160.31, -56.42 - * - * Pitch Forward: - * A_off: -127.00, -22.16, -50.09 - * - * Pitch Back: - * A_off: 201.95, -24.00, -88.56 - */ - -static void trim_accel() -{ - reset_stability_I(); - - float trim_roll = (float)g.rc_1.control_in / 30000.0; - float trim_pitch = (float)g.rc_2.control_in / 30000.0; - - trim_roll = constrain(trim_roll, -1.5, 1.5); - trim_pitch = constrain(trim_pitch, -1.5, 1.5); - - if(g.rc_1.control_in > 200) { // Roll Right - imu.ay(imu.ay() - trim_roll); - }else if (g.rc_1.control_in < -200) { - imu.ay(imu.ay() - trim_roll); - } - - if(g.rc_2.control_in > 200) { // Pitch Back - imu.ax(imu.ax() + trim_pitch); - }else if (g.rc_2.control_in < -200) { - imu.ax(imu.ax() + trim_pitch); - } - - /* - * Serial.printf_P(PSTR("r:%1.2f %1.2f \t| p:%1.2f %1.2f\n"), - * trim_roll, - * (float)imu.ay(), - * trim_pitch, - * (float)imu.ax()); - * //*/ -} - diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 80257e6a61..23a29cdb60 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -67,7 +67,7 @@ // LED output #define NORMAL_LEDS 0 -#define AUTO_TRIM_LEDS 1 +#define SAVE_TRIM_LEDS 1 #define CH_7_PWM_TRIGGER 1800 diff --git a/ArduCopter/inertia.pde b/ArduCopter/inertia.pde index 99194eeab3..bda5952a33 100644 --- a/ArduCopter/inertia.pde +++ b/ArduCopter/inertia.pde @@ -6,7 +6,7 @@ void calc_inertia() { // rotate accels based on DCM // -------------------------- - accels_rotated = ahrs.get_dcm_matrix() * imu.get_accel(); + accels_rotated = ahrs.get_dcm_matrix() * ins.get_accel(); //accels_rotated += accels_offset; // skew accels to account for long term error using calibration accels_rotated.z += 9.805; // remove influence of gravity diff --git a/ArduCopter/leds.pde b/ArduCopter/leds.pde index b3b3b7cd62..e158d7660d 100644 --- a/ArduCopter/leds.pde +++ b/ArduCopter/leds.pde @@ -6,7 +6,7 @@ static void update_lights() update_GPS_light(); break; - case AUTO_TRIM_LEDS: + case SAVE_TRIM_LEDS: dancing_light(); break; } diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index 236c3c2294..b73e760c40 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -3,7 +3,6 @@ // 10 = 1 second #define ARM_DELAY 20 #define DISARM_DELAY 20 -#define LEVEL_DELAY 100 // called at 10hz @@ -12,7 +11,7 @@ static void arm_motors() static int16_t arming_counter; // don't allow arming/disarming in anything but manual - if ((g.rc_3.control_in > 0) || (arming_counter > LEVEL_DELAY)) { + if (g.rc_3.control_in > 0) { arming_counter = 0; return; } @@ -30,13 +29,7 @@ static void arm_motors() // full right if (tmp > 4000) { - if (arming_counter == LEVEL_DELAY) { - //Serial.printf("\nAL\n"); - // begin auto leveling - auto_level_counter = 250; - arming_counter = 0; - - }else if (arming_counter == ARM_DELAY) { + if (arming_counter == ARM_DELAY) { if(motors.armed() == false) { // arm the motors and configure for flight @@ -74,14 +67,7 @@ static void arm_motors() // full left }else if (tmp < -4000) { - if (arming_counter == LEVEL_DELAY) { - //Serial.printf("\nLEV\n"); - - // begin manual leveling - imu.init_accel(mavlink_delay, flash_leds); - arming_counter = 0; - - }else if (arming_counter == DISARM_DELAY) { + if (arming_counter == DISARM_DELAY) { if(motors.armed()) { // arm the motors and configure for flight init_disarm_motors(); diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index 642b2f225d..aad2284f9c 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -97,7 +97,7 @@ setup_show(uint8_t argc, const Menu::arg *argv) //report_xtrack(); //report_throttle(); report_flight_modes(); - report_imu(); + report_ins(); report_compass(); report_optflow(); @@ -247,10 +247,9 @@ setup_motors(uint8_t argc, const Menu::arg *argv) static int8_t setup_accel(uint8_t argc, const Menu::arg *argv) { - imu.init(IMU::COLD_START, delay, flash_leds, &timer_scheduler); - imu.init_accel(delay, flash_leds); - print_accel_offsets(); - report_imu(); + ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler); + ins.init_accel(delay, flash_leds); + report_ins(); return(0); } @@ -258,69 +257,10 @@ setup_accel(uint8_t argc, const Menu::arg *argv) static int8_t setup_accel_scale(uint8_t argc, const Menu::arg *argv) { - #if CONFIG_ADC == ENABLED && HIL_MODE == HIL_MODE_DISABLED - int8_t accel_num; - float accel_avg = 0; - - if (!strcmp_P(argv[1].str, PSTR("x"))) { - accel_num = 4; - }else if (!strcmp_P(argv[1].str, PSTR("y"))) { - accel_num = 5; - }else if (!strcmp_P(argv[1].str, PSTR("z"))) { - accel_num = 6; - }else{ - Serial.printf_P(PSTR("x, y, or z\n")); - return 0; - } - print_hit_enter(); - Serial.printf_P(PSTR("ADC\n")); - - adc.Init(&timer_scheduler); // APM ADC library initialization - - int16_t low, high; - - delay(1000); - accel_avg = adc.Ch(accel_num); - low = high = accel_avg; - - while(1) { - delay(50); - accel_avg = accel_avg * .95 + adc.Ch(accel_num) * .05; - - if(accel_avg > high) - high = ceil(accel_avg); - - if(accel_avg < low) - low = floor(accel_avg); - - Serial.printf_P(PSTR("%1.2f, %d, %d\n"), accel_avg, low, high); - - if(Serial.available() > 0) { - if(wait_for_yes()) { - if(accel_num == 4) { - ins._x_high = high; - ins._x_low = low; - ins._x_high.save(); - ins._x_low.save(); - }else if(accel_num == 5) { - ins._y_high = high; - ins._y_low = low; - ins._y_high.save(); - ins._y_low.save(); - }else{ - ins._z_high = high; - ins._z_low = low; - ins._z_high.save(); - ins._z_low.save(); - } - print_done(); - } - return (0); - } - } - #else - return 0; - #endif // CONFIG_ADC + ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler); + ins.calibrate_accel(delay, flash_leds); + report_ins(); + return(0); } static int8_t @@ -873,13 +813,13 @@ static void report_radio() print_blanks(2); } -static void report_imu() +static void report_ins() { - Serial.printf_P(PSTR("IMU\n")); + Serial.printf_P(PSTR("INS\n")); print_divider(); print_gyro_offsets(); - print_accel_offsets(); + print_accel_offsets_and_scaling(); print_blanks(2); } @@ -1029,21 +969,27 @@ static void zero_eeprom(void) } static void -print_accel_offsets(void) +print_accel_offsets_and_scaling(void) { - Serial.printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\n"), - (float)imu.ax(), // Pitch - (float)imu.ay(), // Roll - (float)imu.az()); // YAW + Vector3f accel_offsets = ins.get_accel_offsets(); + Vector3f accel_scale = ins.get_accel_scale(); + Serial.printf_P(PSTR("A_off: %4.2f, %4.2f, %4.2f\tA_scale: %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) { + Vector3f gyro_offsets = ins.get_gyro_offsets(); Serial.printf_P(PSTR("G_off: %4.2f, %4.2f, %4.2f\n"), - (float)imu.gx(), - (float)imu.gy(), - (float)imu.gz()); + (float)gyro_offsets.x, + (float)gyro_offsets.y, + (float)gyro_offsets.z); } #if FRAME_CONFIG == HELI_FRAME diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 805235418d..a8cf2eba9b 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -367,9 +367,9 @@ static void startup_ground(void) // Warm up and read Gyro offsets // ----------------------------- - imu.init(IMU::COLD_START, mavlink_delay, flash_leds, &timer_scheduler); + ins.init(AP_InertialSensor::COLD_START, mavlink_delay, flash_leds, &timer_scheduler); #if CLI_ENABLED == ENABLED - report_imu(); + report_ins(); #endif // initialise ahrs (may push imu calibration into the mpu6000 if using that device). diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index d5b3ff10ae..0608075a22 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -66,7 +66,6 @@ const struct Menu::command test_menu_commands[] PROGMEM = { {"gps", test_gps}, // {"adc", test_adc}, {"ins", test_ins}, -// {"imu", test_imu}, // {"dcm", test_dcm_eulers}, //{"omega", test_omega}, // {"stab_d", test_stab_d}, @@ -440,69 +439,6 @@ test_radio(uint8_t argc, const Menu::arg *argv) * #endif */ -/* - * static int8_t - * test_adc(uint8_t argc, const Menu::arg *argv) - * { - * ins.init(&timer_scheduler); - * - * int8_t mytimer = 0; - * startup_ground(); - * Serial.println("OK"); - * - * while(1){ - * // We want this to execute fast - * // ---------------------------- - * uint32_t timer = micros(); - * - * if ((timer - fast_loopTimer) >= 10000 && imu.new_data_available()) { - * G_Dt = (float)(timer - fast_loopTimer) / 1000000.f; // used by PI Loops - * fast_loopTimer = timer; - * - * read_AHRS(); - * - * //calc_inertia(); - * accels_rotated = ahrs.get_dcm_matrix() * imu.get_accel(); - * //accels_rotated += accels_offset; // skew accels to account for long term error using calibration - * - * mytimer++; - * - * if ((timer - fiftyhz_loopTimer) >= 20000) { - * fiftyhz_loopTimer = timer; - * //sensed_loc.lng = sensed_loc.lat = sensed_loc.alt = 0; - * - * // position fix - * //inertial_error_correction(); - * } - * - * if (mytimer >= 10){ - * float test = sqrt(sq(accels_rotated.x) + sq(accels_rotated.y) + sq(accels_rotated.z)) / 9.80665; - * - * Vector3f _accels = imu.get_accel(); - * mytimer = 0; - * - * - * Serial.printf("%1.4f, %1.4f, %1.4f | %1.4f, %1.4f, %1.4f | %d, %1.4f, %d, %1.4f \n", - * _accels.x, - * _accels.y, - * _accels.z, - * accels_rotated.x, - * accels_rotated.y, - * accels_rotated.z, - * test); - * - * - * } - * - * if(Serial.available() > 0){ - * return (0); - * } - * } - * } - * return (0); - * } - */ - static int8_t test_ins(uint8_t argc, const Menu::arg *argv) { @@ -510,37 +446,28 @@ test_ins(uint8_t argc, const Menu::arg *argv) print_test_disabled(); return (0); #else - float gyro[3], accel[3], temp; + Vector3f gyro, accel; + float temp; print_hit_enter(); - Serial.printf_P(PSTR("InertialSensor\n")); + Serial.printf_P(PSTR("INS\n")); delay(1000); - ins.init(&timer_scheduler); + ins.init(AP_InertialSensor::COLD_START, delay, flash_leds, &timer_scheduler); delay(50); while(1) { ins.update(); - ins.get_gyros(gyro); - ins.get_accels(accel); + gyro = ins.get_gyro(); + accel = ins.get_accel(); temp = ins.temperature(); - float test = sqrt(sq(accel[0]) + sq(accel[1]) + sq(accel[2])) / 9.80665; + float test = sqrt(sq(accel.x) + sq(accel.y) + sq(accel.z)) / 9.80665; - Serial.printf_P(PSTR("g")); - - for (int16_t i = 0; i < 3; i++) { - Serial.printf_P(PSTR(" %7.4f"), gyro[i]); - } - - Serial.printf_P(PSTR(" a")); - - for (int16_t i = 0; i < 3; i++) { - Serial.printf_P(PSTR(" %7.4f"),accel[i]); - } - - Serial.printf_P(PSTR(" t %7.4f "), temp); - Serial.printf_P(PSTR(" | %7.4f \n"), test); + Serial.printf_P(PSTR("a %7.4f %7.4f %7.4f g %7.4f %7.4f %7.4f t %74f | %7.4f\n"), + accel.x, accel.y, accel.z, + gyro.x, gyro.y, gyro.z, + temp, test); delay(40); if(Serial.available() > 0) { @@ -550,190 +477,6 @@ test_ins(uint8_t argc, const Menu::arg *argv) #endif } - -/* - * test the IMU interface - */ -/* - * static int8_t - * test_imu(uint8_t argc, const Menu::arg *argv) - * { - * #if defined( __AVR_ATmega1280__ ) // test disabled to save code size for 1280 - * print_test_disabled(); - * return (0); - * #else - * Vector3f gyro; - * Vector3f accel; - * - * imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); - * - * report_imu(); - * imu.init_gyro(delay, flash_leds); - * report_imu(); - * - * print_hit_enter(); - * delay(1000); - * - * while(1){ - * delay(40); - * - * imu.update(); - * gyro = imu.get_gyro(); - * accel = imu.get_accel(); - * - * Serial.printf_P(PSTR("g %8.4f %8.4f %8.4f"), gyro.x, gyro.y, gyro.z); - * Serial.printf_P(PSTR(" a %8.4f %8.4f %8.4f\n"), accel.x, accel.y, accel.z); - * - * if(Serial.available() > 0){ - * return (0); - * } - * } - * #endif - * } - */ - -/* - * static int8_t - * test_imu(uint8_t argc, const Menu::arg *argv) - * { - * print_hit_enter(); - * Serial.printf_P(PSTR("ADC\n")); - * adc.Init(&timer_scheduler); - * - * delay(1000); - * Vector3f avg; - * avg.x = adc.Ch(4); - * avg.y = adc.Ch(5); - * avg.z = adc.Ch(6); - * - * //Serial.printf_P(PSTR("init %.2f, %.2f, %.2f\n"), avg.x, avg.y, avg.z); - * Vector3f low = avg; - * Vector3f high = avg; - * - * while(1){ - * delay(100); - * avg.x = avg.x * .95 + adc.Ch(4) * .05; - * avg.y = avg.y * .95 + adc.Ch(5) * .05; - * avg.z = avg.z * .95 + adc.Ch(6) * .05; - * - * if(avg.x > high.x) - * high.x = ceil(high.x *.9 + avg.x * .1); - * - * if(avg.y > high.y) - * high.y = ceil(high.y *.9 + avg.y * .1); - * - * if(avg.z > high.z) - * high.z = ceil(high.z *.9 + avg.z * .1); - * - * // - * if(avg.x < low.x) - * low.x = floor(low.x *.9 + avg.x * .1); - * - * if(avg.y < low.y) - * low.y = floor(low.y *.9 + avg.y * .1); - * - * if(avg.z < low.z) - * low.z = floor(low.z *.9 + avg.z * .1); - * - * Serial.printf_P(PSTR("%.2f, %.2f, %.2f \t| %.2f, %.2f, %.2f \t| %.2f, %.2f, %.2f\n"), avg.x, avg.y, avg.z, low.x, low.y, low.z, high.x, high.y, high.z); - * - * //Serial.printf_P(PSTR("%.2f, %.2f, %.2f \t| %d, %d\n"), avg.x, avg.y, avg.z, _count[0], _sum[0]); - * - * //Serial.println(); - * if(Serial.available() > 0){ - * Serial.printf_P(PSTR("Y to save\n")); - * int16_t c; - * c = Serial.read(); - * - * do { - * c = Serial.read(); - * } while (-1 == c); - * - * if (('y' == c) || ('Y' == c)){ - * ins._x_high = high.x; - * ins._x_low = low.x; - * ins._y_high = high.y; - * ins._y_low = low.y; - * ins._z_high = high.z; - * ins._z_low = low.z; - * ins._x_high.save(); - * ins._x_low.save(); - * ins._y_high.save(); - * ins._y_low.save(); - * ins._z_high.save(); - * ins._z_low.save(); - * Serial.printf_P(PSTR("saved\n")); - * } - * - * return (0); - * } - * } - * } - */ - - - -/* - * test the DCM code, printing Euler angles - */ -/*static int8_t - * //test_dcm_eulers(uint8_t argc, const Menu::arg *argv) - * { - * - * //Serial.printf_P(PSTR("Calibrating.")); - * - * //dcm.kp_yaw(0.02); - * //dcm.ki_yaw(0); - * - * imu.init(IMU::WARM_START, delay, flash_leds, &timer_scheduler); - * - * report_imu(); - * imu.init_gyro(delay, flash_leds); - * report_imu(); - * - * print_hit_enter(); - * delay(1000); - * - * //float cos_roll, sin_roll, cos_pitch, sin_pitch, cos_yaw, sin_yaw; - * fast_loopTimer = millis(); - * - * while(1){ - * //delay(20); - * if (millis() - fast_loopTimer >=20) { - * - * // IMU - * // --- - * read_AHRS(); - * medium_loopCounter++; - * - * if(medium_loopCounter == 4){ - * update_trig(); - * } - * - * if(medium_loopCounter == 1){ - * medium_loopCounter = 0; - * Serial.printf_P(PSTR("dcm: %6.1f, %6.1f, %6.1f omega: %6.1f, %6.1f, %6.1f\n"), - * dcm.roll_sensor/100.0, - * dcm.pitch_sensor/100.0, - * dcm.yaw_sensor/100.0, - * degrees(omega.x), - * degrees(omega.y), - * degrees(omega.z)); - * - * if(g.compass_enabled){ - * compass.read(); // Read magnetometer - * compass.null_offsets(); - * } - * } - * fast_loopTimer = millis(); - * } - * if(Serial.available() > 0){ - * return (0); - * } - * } - * return (0); - * }*/ - static int8_t test_gps(uint8_t argc, const Menu::arg *argv) {