diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index d0d60a70cf..33771a5314 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1079,6 +1079,11 @@ static void medium_loop() case 0: medium_loopCounter++; + // read battery before compass because it may be used for motor interference compensation + if (g.battery_monitoring != 0) { + read_battery(); + } + #if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode if(g.compass_enabled) { if (compass.read()) { @@ -1154,10 +1159,6 @@ static void medium_loop() case 4: medium_loopCounter = 0; - if (g.battery_monitoring != 0) { - read_battery(); - } - // Accel trims = hold > 2 seconds // Throttle cruise = switch less than 1 second // -------------------------------------------- @@ -1327,6 +1328,7 @@ static void super_slow_loop() Log_Write_Data(DATA_AP_STATE, ap.value); } + // log battery info to the dataflash if (g.log_bitmask & MASK_LOG_CURRENT && motors.armed()) Log_Write_Current(); diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 73b9f627d6..25222e119b 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -208,7 +208,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack battery_current = current_amps1 * 100; } - if (g.battery_monitoring == 3) { + if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) { /*setting a out-of-range value. * It informs to external devices that * it cannot be calculated properly just by voltage*/ diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 180730fd4d..75ab7e1951 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -82,7 +82,7 @@ const AP_Param::Info var_info[] PROGMEM = { // @Description: Controls enabling monitoring of the battery's voltage and current // @Values: 0:Disabled,3:Voltage Only,4:Voltage and Current // @User: Standard - GSCALAR(battery_monitoring, "BATT_MONITOR", DISABLED), + GSCALAR(battery_monitoring, "BATT_MONITOR", BATT_MONITOR_DISABLED), // @Param: FS_BATT_ENABLE // @DisplayName: Battery Failsafe Enable diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 6556df029c..d7d3c16a9e 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -350,6 +350,10 @@ enum gcs_severity { #define BATTERY_VOLTAGE(x) (x->voltage_average()*g.volt_div_ratio) #define CURRENT_AMPS(x) (x->voltage_average()-CURR_AMPS_OFFSET)*g.curr_amp_per_volt +#define BATT_MONITOR_DISABLED 0 +#define BATT_MONITOR_VOLTAGE_ONLY 3 +#define BATT_MONITOR_VOLTAGE_AND_CURRENT 4 + /* ************************************************************** */ /* Expansion PIN's that people can use for various things. */ diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 4be13dc756..21e0072f2b 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -111,23 +111,26 @@ static void read_battery(void) { static uint8_t low_battery_counter = 0; - if(g.battery_monitoring == 0) { + if(g.battery_monitoring == BATT_MONITOR_DISABLED) { battery_voltage1 = 0; return; } - if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { + if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY || g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) { batt_volt_analog_source->set_pin(g.battery_volt_pin); battery_voltage1 = BATTERY_VOLTAGE(batt_volt_analog_source); } - if(g.battery_monitoring == 4) { + if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) { batt_curr_analog_source->set_pin(g.battery_curr_pin); current_amps1 = CURRENT_AMPS(batt_curr_analog_source); current_total1 += current_amps1 * 0.02778f; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours) + + // update compass with current value + compass.set_current(current_amps1); } // check for low voltage or current if the low voltage check hasn't already been triggered - if (!ap.low_battery && ( battery_voltage1 < g.low_voltage || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity))) { + if (!ap.low_battery && ( battery_voltage1 < g.low_voltage || (g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT && current_total1 > g.pack_capacity))) { low_battery_counter++; if( low_battery_counter >= BATTERY_FS_COUNTER ) { low_battery_counter = BATTERY_FS_COUNTER; // ensure counter does not overflow diff --git a/ArduCopter/setup.pde b/ArduCopter/setup.pde index cfcf05e124..d890154d3a 100644 --- a/ArduCopter/setup.pde +++ b/ArduCopter/setup.pde @@ -463,13 +463,32 @@ setup_compass(uint8_t argc, const Menu::arg *argv) static int8_t setup_compassmot(uint8_t argc, const Menu::arg *argv) { + int8_t comp_type; // throttle or current based compensation Vector3f compass_base; // compass vector when throttle is zero Vector3f motor_impact; // impact of motors on compass vector Vector3f motor_impact_scaled; // impact of motors on compass vector scaled with throttle Vector3f motor_compensation; // final compensation to be stored to eeprom - float throttle_pct; // throttle as a percentage 0.0 ~ 1.0 + float throttle_pct; // throttle as a percentage 0.0 ~ 1.0 uint32_t last_run_time; - uint8_t print_counter = 0; + uint8_t print_counter = 49; + bool updated = false; // have we updated the compensation vector at least once + + // default compensation type to use current if possible + if( g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT ) { + comp_type = AP_COMPASS_MOT_COMP_CURRENT; + }else{ + comp_type = AP_COMPASS_MOT_COMP_THROTTLE; + } + + // check if user wants throttle compensation + if( !strcmp_P(argv[1].str, PSTR("t")) || !strcmp_P(argv[1].str, PSTR("T")) ) { + comp_type = AP_COMPASS_MOT_COMP_THROTTLE; + } + + // check if user wants current compensation + if( !strcmp_P(argv[1].str, PSTR("c")) || !strcmp_P(argv[1].str, PSTR("C")) ) { + comp_type = AP_COMPASS_MOT_COMP_CURRENT; + } // check compass is enabled if( !g.compass_enabled ) { @@ -477,30 +496,39 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv) return 0; } + // check if we have a current monitor + if( comp_type == AP_COMPASS_MOT_COMP_CURRENT && g.battery_monitoring != BATT_MONITOR_VOLTAGE_AND_CURRENT ) { + cliSerial->print_P(PSTR("current monitor disabled, exiting")); + return 0; + } + // initialise compass init_compass(); - // set motor compensation to zero + // disable motor compensation + compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); compass.set_motor_compensation(Vector3f(0,0,0)); // print warning that motors will spin - cliSerial->print_P(PSTR("This setup records the impact on the compass of spinning up the motors. The motors will spin!\n")); - // ask user to raise throttle - cliSerial->print_P(PSTR("Hold throttle low, then raise as high as possible (without taking off) for 10 sec.\n")); - // inform how to stop test - cliSerial->print_P(PSTR("At any time you may press any key to exit.\n")); + cliSerial->print_P(PSTR("This setup records the impact on the compass of spinning up the motors. The motors will spin!\nHold throttle low, then raise as high as safely possible for 10 sec.\nAt any time you may press any key to exit.\nmeasuring compass vs ")); - // wait 1 second for user to read message - delay(2000); + // inform what type of compensation we are attempting + if( comp_type == AP_COMPASS_MOT_COMP_CURRENT ) { + cliSerial->print_P(PSTR("CURRENT\n")); + }else{ + cliSerial->print_P(PSTR("THROTTLE\n")); + } + // clear out user input while( cliSerial->available() ) { cliSerial->read(); } - // disable throttle failsafe + // disable throttle and battery failsafe g.failsafe_throttle = FS_THR_DISABLED; + g.failsafe_battery_enabled = false; // read radio read_radio(); @@ -561,6 +589,9 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv) // read some compass values compass.read(); + // read current + read_battery(); + // calculate scaling for throttle throttle_pct = (float)g.rc_3.control_in / 1000.0f; throttle_pct = constrain(throttle_pct,0.0f,1.0f); @@ -574,22 +605,36 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv) // causing printing to happen as soon as throttle is lifted print_counter = 49; }else{ + // calculate diff from compass base and scale with throttle motor_impact.x = compass.mag_x - compass_base.x; motor_impact.y = compass.mag_y - compass_base.y; motor_impact.z = compass.mag_z - compass_base.z; - // scale by throttle - motor_impact_scaled = motor_impact / throttle_pct; + // throttle based compensation + if( comp_type == AP_COMPASS_MOT_COMP_THROTTLE ) { + // scale by throttle + motor_impact_scaled = motor_impact / throttle_pct; - // adjust the motor compensation to negate the impact - motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f; + // adjust the motor compensation to negate the impact + motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f; + updated = true; + }else{ + // current based compensation if more than 3amps being drawn + motor_impact_scaled = motor_impact / current_amps1; + + // adjust the motor compensation to negate the impact if drawing over 3amps + if( current_amps1 >= 3.0f ) { + motor_compensation = motor_compensation * 0.99f - motor_impact_scaled * 0.01f; + updated = true; + } + } // display output at 1hz if throttle is above zero print_counter++; if(print_counter >= 50) { print_counter = 0; - cliSerial->printf_P(PSTR("thr:%d mot x:%4.1f\ty:%4.1f\tz:%4.1f\tcomp x:%4.1f\ty:%4.1f\tz:%4.1f\n"),(int)g.rc_3.control_in, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z); + cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)current_amps1, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z); } } }else{ @@ -607,9 +652,19 @@ setup_compassmot(uint8_t argc, const Menu::arg *argv) cliSerial->read(); } + // print one more time so the last thing printed matches what appears in the report_compass + cliSerial->printf_P(PSTR("thr:%d cur:%4.2f mot x:%4.1f y:%4.1f z:%4.1f comp x:%4.2f y:%4.2f z:%4.2f\n"),(int)g.rc_3.control_in, (float)current_amps1, (float)motor_impact.x, (float)motor_impact.y, (float)motor_impact.z, (float)motor_compensation.x, (float)motor_compensation.y, (float)motor_compensation.z); + // set and save motor compensation - compass.set_motor_compensation(motor_compensation); - compass.save_motor_compensation(); + if( updated ) { + compass.motor_compensation_type(comp_type); + compass.set_motor_compensation(motor_compensation); + compass.save_motor_compensation(); + }else{ + // compensation vector never updated, report failure + cliSerial->printf_P(PSTR("Failed! Compensation disabled. Did you forget to raise the throttle high enough?")); + compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED); + } // display new motor offsets and save report_compass(); @@ -946,9 +1001,9 @@ static void report_batt_monitor() { cliSerial->printf_P(PSTR("\nBatt Mon:\n")); print_divider(); - if(g.battery_monitoring == 0) print_enabled(false); - if(g.battery_monitoring == 3) cliSerial->printf_P(PSTR("volts")); - if(g.battery_monitoring == 4) cliSerial->printf_P(PSTR("volts and cur")); + if(g.battery_monitoring == BATT_MONITOR_DISABLED) print_enabled(false); + if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) cliSerial->printf_P(PSTR("volts")); + if(g.battery_monitoring == BATT_MONITOR_VOLTAGE_AND_CURRENT) cliSerial->printf_P(PSTR("volts and cur")); print_blanks(2); } @@ -1044,11 +1099,22 @@ static void report_compass() offsets.z); // motor compensation - Vector3f motor_compensation = compass.get_motor_compensation(); - cliSerial->printf_P(PSTR("Mot comp: %4.0f, %4.0f, %4.0f\n"), - motor_compensation.x, - motor_compensation.y, - motor_compensation.z); + cliSerial->print_P(PSTR("Motor Comp: ")); + if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_DISABLED ) { + cliSerial->print_P(PSTR("Off\n")); + }else{ + if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_THROTTLE ) { + cliSerial->print_P(PSTR("Throttle")); + } + if( compass.motor_compensation_type() == AP_COMPASS_MOT_COMP_CURRENT ) { + cliSerial->print_P(PSTR("Current")); + } + Vector3f motor_compensation = compass.get_motor_compensation(); + cliSerial->printf_P(PSTR("\nComp Vec: %4.2f, %4.2f, %4.2f\n"), + motor_compensation.x, + motor_compensation.y, + motor_compensation.z); + } print_blanks(1); } diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 46240906ca..1e44d20218 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -680,7 +680,7 @@ test_battery(uint8_t argc, const Menu::arg *argv) delay(100); read_radio(); read_battery(); - if (g.battery_monitoring == 3) { + if (g.battery_monitoring == BATT_MONITOR_VOLTAGE_ONLY) { cliSerial->printf_P(PSTR("V: %4.4f\n"), battery_voltage1, current_amps1,