From 3a4d122e32f55bc72511bfed53593e577a4920f0 Mon Sep 17 00:00:00 2001 From: rmackay9 Date: Tue, 13 Nov 2012 23:43:54 +0900 Subject: [PATCH] ArduCopter: low baterry failsafe --- ArduCopter/Parameters.h | 6 +++-- ArduCopter/Parameters.pde | 8 +++++++ ArduCopter/config.h | 8 +++---- ArduCopter/events.pde | 47 ++++++++++++++++++++++++++++++--------- ArduCopter/sensors.pde | 35 +++++++++++------------------ 5 files changed, 66 insertions(+), 38 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index ff89bc819b..a000c9b5e7 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -174,6 +174,7 @@ public: k_param_radio_tuning_high, k_param_radio_tuning_low, k_param_rc_speed = 192, + k_param_battery_fs_enabled, // 193 // // 200: flight modes @@ -246,8 +247,9 @@ public: AP_Float volt_div_ratio; AP_Float curr_amp_per_volt; AP_Float input_voltage; - AP_Int16 pack_capacity; // Battery pack capacity less - // reserve + AP_Int16 pack_capacity; // Battery pack capacity less reserve + AP_Int8 battery_fs_enabled; // battery failsafe enabled + AP_Int8 compass_enabled; AP_Int8 optflow_enabled; AP_Float low_voltage; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 030e31f9f0..1574884b5a 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -53,8 +53,16 @@ const AP_Param::Info var_info[] PROGMEM = { GSCALAR(sonar_enabled, "SONAR_ENABLE", DISABLED), GSCALAR(sonar_type, "SONAR_TYPE", AP_RANGEFINDER_MAXSONARXL), + GSCALAR(battery_monitoring, "BATT_MONITOR", DISABLED), + // @Param: BATT_FAILSAFE + // @DisplayName: Battery Failsafe Enable + // @Description: Controls whether failsafe will be invoked when battery voltage or current runs low + // @Values: 0:Disabled,1:Enabled + // @User: Standard + GSCALAR(battery_fs_enabled, "BATT_FAILSAFE", BATTERY_FAILSAFE), + // @Param: VOLT_DIVIDER // @DisplayName: Voltage Divider // @Description: TODO diff --git a/ArduCopter/config.h b/ArduCopter/config.h index f8175e4afd..95c98780c5 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -303,9 +303,6 @@ ////////////////////////////////////////////////////////////////////////////// // Battery monitoring // -#ifndef BATTERY_EVENT - # define BATTERY_EVENT DISABLED -#endif #ifndef LOW_VOLTAGE # define LOW_VOLTAGE 9.6 #endif @@ -323,7 +320,10 @@ # define HIGH_DISCHARGE 1760 #endif - +// Battery failsafe +#ifndef BATTERY_FAILSAFE + # define BATTERY_FAILSAFE DISABLED +#endif diff --git a/ArduCopter/events.pde b/ArduCopter/events.pde index a121c8eb24..5e362e6b7f 100644 --- a/ArduCopter/events.pde +++ b/ArduCopter/events.pde @@ -6,6 +6,11 @@ */ static void failsafe_on_event() { + // if motors are not armed there is nothing to do + if( !motors.armed() ) { + return; + } + // This is how to handle a failsafe. switch(control_mode) { case STABILIZE: @@ -27,6 +32,7 @@ static void failsafe_on_event() case AUTO: // throttle_fs_action is 1 do RTL, everything else means continue with the mission if (g.throttle_fs_action == THROTTLE_FAILSAFE_ACTION_ALWAYS_RTL) { + // To-Do: check distance from home before initiating RTL set_mode(RTL); } // if throttle_fs_action is 2 (i.e. THROTTLE_FAILSAFE_ACTION_CONTINUE_MISSION) no need to do anything @@ -35,6 +41,7 @@ static void failsafe_on_event() if(ap.home_is_set == true) { // same as above ^ // do_rtl sets the altitude to the current altitude by default + // To-Do: check distance from home before initiating RTL set_mode(RTL); }else{ // We have no GPS so we will crash land in alt hold mode @@ -58,19 +65,39 @@ static void failsafe_off_event() static void low_battery_event(void) { - static uint32_t last_low_battery_message; - uint32_t tnow = millis(); - if (((uint32_t)(tnow - last_low_battery_message)) > 5000) { - // only send this message at 5s intervals at most or we may - // flood the link - gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!")); - last_low_battery_message = tnow; + // warn the ground station + gcs_send_text_P(SEVERITY_LOW,PSTR("Low Battery!")); + + // failsafe check + if (g.battery_fs_enabled && !ap.low_battery && motors.armed()) { + switch(control_mode) { + case STABILIZE: + case ACRO: + // if throttle is zero disarm motors + if (g.rc_3.control_in == 0) { + init_disarm_motors(); + }else{ + set_mode(LAND); + } + break; + case AUTO: + // To-Do: check distance to home before initiating RTL? + set_mode(RTL); + break; + default: + set_mode(LAND); + break; + } } + // set the low battery flag set_low_battery(true); - // if we are in Auto mode, come home - if(control_mode >= AUTO) - set_mode(RTL); + +#if COPTER_LEDS == ENABLED + if ( bitRead(g.copter_leds_mode, 3) ) { // Only Activate if a battery is connected to avoid alarm on USB only + piezo_on(); + } +#endif // COPTER_LEDS } diff --git a/ArduCopter/sensors.pde b/ArduCopter/sensors.pde index 7576354904..ec4cc0d1c3 100644 --- a/ArduCopter/sensors.pde +++ b/ArduCopter/sensors.pde @@ -68,8 +68,12 @@ static void init_optflow() #endif } +// read_battery - check battery voltage and current and invoke failsafe if necessary +// called at 10hz +#define BATTERY_FS_COUNTER 100 // 100 iterations at 10hz is 10 seconds static void read_battery(void) { + static uint8_t low_battery_counter = 0; if(g.battery_monitoring == 0) { battery_voltage1 = 0; @@ -88,28 +92,15 @@ static void read_battery(void) current_total1 += current_amps1 * 0.02778; // called at 100ms on average, .0002778 is 1/3600 (conversion to hours) } -#if BATTERY_EVENT == ENABLED - //if(battery_voltage < g.low_voltage) - // low_battery_event(); - - if((battery_voltage1 < g.low_voltage) || (g.battery_monitoring == 4 && current_total1 > g.pack_capacity)) { - low_battery_event(); - - #if COPTER_LEDS == ENABLED - if ( bitRead(g.copter_leds_mode, 3) ) { // Only Activate if a battery is connected to avoid alarm on USB only - if (battery_voltage1 > 1) { - piezo_on(); - }else{ - piezo_off(); - } + // 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))) { + low_battery_counter++; + if( low_battery_counter >= BATTERY_FS_COUNTER ) { + low_battery_counter = BATTERY_FS_COUNTER; // ensure counter does not overflow + low_battery_event(); } - - - }else if ( bitRead(g.copter_leds_mode, 3) ) { - piezo_off(); - #endif // COPTER_LEDS + }else{ + // reset low_battery_counter in case it was a temporary voltage dip + low_battery_counter = 0; } -#endif //BATTERY_EVENT } - -//v: 10.9453, a: 17.4023, mah: 8.2