diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 84d9ef57d6910..c3ae5a12a3a8a 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -417,17 +417,18 @@ static int32_t altitude_error_cm; //////////////////////////////////////////////////////////////////////////////// // Battery Sensors //////////////////////////////////////////////////////////////////////////////// -// Battery pack 1 voltage. Initialized above the low voltage threshold to pre-load the filter and prevent low voltage events at startup. -static float battery_voltage1 = LOW_VOLTAGE * 1.05; -// Battery pack 1 instantaneous currrent draw. Amperes -static float current_amps1; -// Totalized current (Amp-hours) from battery 1 -static float current_total1; - -// To Do - Add support for second battery pack -//static float battery_voltage2 = LOW_VOLTAGE * 1.05; // Battery 2 Voltage, initialized above threshold for filter -//static float current_amps2; // Current (Amperes) draw from battery 2 -//static float current_total2; // Totalized current (Amp-hours) from battery 2 +static struct { + // Battery pack 1 voltage. Initialized above the low voltage + // threshold to pre-load the filter and prevent low voltage events + // at startup. + float voltage; + // Battery pack 1 instantaneous currrent draw. Amperes + float current_amps; + // Totalized current (Amp-hours) from battery 1 + float current_total_mah; + // true when a low battery event has happened + bool low_batttery; +} battery; //////////////////////////////////////////////////////////////////////////////// // Airspeed Sensors diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index d3aa93e210c0d..96c5a7840012a 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -206,11 +206,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack uint16_t battery_current = -1; uint8_t battery_remaining = -1; - if (current_total1 != 0 && g.pack_capacity != 0) { - battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity); + if (battery.current_total_mah != 0 && g.pack_capacity != 0) { + battery_remaining = (100.0 * (g.pack_capacity - battery.current_total_mah) / g.pack_capacity); } - if (current_total1 != 0) { - battery_current = current_amps1 * 100; + if (battery.current_total_mah != 0) { + battery_current = battery.current_amps * 100; } if (g.battery_monitoring == 3) { @@ -226,7 +226,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack control_sensors_enabled, control_sensors_health, (uint16_t)(load * 1000), - battery_voltage1 * 1000, // mV + battery.voltage * 1000, // mV battery_current, // in 10mA units battery_remaining, // in % 0, // comm drops %, diff --git a/ArduPlane/Log.pde b/ArduPlane/Log.pde index 5febb2c350d92..620e9bcbe042d 100644 --- a/ArduPlane/Log.pde +++ b/ArduPlane/Log.pde @@ -375,10 +375,10 @@ static void Log_Write_Current() struct log_Current pkt = { LOG_PACKET_HEADER_INIT(LOG_CURRENT_MSG), throttle_in : g.channel_throttle.control_in, - battery_voltage : (int16_t)(battery_voltage1 * 100.0), - current_amps : (int16_t)(current_amps1 * 100.0), + battery_voltage : (int16_t)(battery.voltage * 100.0), + current_amps : (int16_t)(battery.current_amps * 100.0), board_voltage : board_voltage(), - current_total : current_total1 + current_total : battery.current_total_mah }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 978c4f26e19ba..3b4204407a1d9 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -183,6 +183,8 @@ class Parameters { k_param_throttle_suppress_manual, k_param_throttle_passthru_stabilize, k_param_rc_12, + k_param_fs_batt_voltage, + k_param_fs_batt_mah, // // 200: Feed-forward gains @@ -311,6 +313,8 @@ class Parameters { AP_Int8 short_fs_action; AP_Int8 long_fs_action; AP_Int8 gcs_heartbeat_fs_enabled; + AP_Float fs_batt_voltage; + AP_Float fs_batt_mah; // Flight modes // diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 58bc1579d73d5..e5a361b8b51b2 100644 --- a/ArduPlane/Parameters.pde +++ b/ArduPlane/Parameters.pde @@ -349,6 +349,20 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(long_fs_action, "FS_LONG_ACTN", LONG_FAILSAFE_ACTION), + // @Param: FS_BATT_VOLTAGE + // @DisplayName: Failsafe battery voltage + // @Description: Battery voltage to trigger failsafe. Set to 0 to disable battery voltage failsafe. If the battery voltage drops below this voltage then the plane will RTL + // @Units: Volts + // @User: Standard + GSCALAR(fs_batt_voltage, "FS_BATT_VOLTAGE", 0), + + // @Param: FS_BATT_MAH + // @DisplayName: Failsafe battery milliAmpHours + // @Description: Battery capacity remaining to trigger failsafe. Set to 0 to disable battery remaining failsafe. If the battery remaining drops below this level then the plane will RTL + // @Units: mAh + // @User: Standard + GSCALAR(fs_batt_mah, "FS_BATT_MAH", 0), + // @Param: FS_GCS_ENABL // @DisplayName: GCS failsafe enable // @Description: Enable ground control station telemetry failsafe. Failsafe will trigger after 20 seconds of no MAVLink heartbeat messages. WARNING: Enabling this option opens up the possibility of your plane going into failsafe mode and running the motor on the ground it it loses contact with your ground station. If this option is enabled on an electric plane then either use a separate motor arming switch or remove the propeller in any ground testing. diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 52292075c29e0..766aa5c1cf0cf 100644 --- a/ArduPlane/config.h +++ b/ArduPlane/config.h @@ -231,12 +231,6 @@ ////////////////////////////////////////////////////////////////////////////// // Battery monitoring // -#ifndef BATTERY_EVENT - # define BATTERY_EVENT DISABLED -#endif -#ifndef LOW_VOLTAGE - # define LOW_VOLTAGE 9.6 -#endif #ifndef VOLT_DIV_RATIO # define VOLT_DIV_RATIO 3.56 // This is the proper value for an on-board APM1 voltage divider with a 3.9kOhm resistor //# define VOLT_DIV_RATIO 15.70 // This is the proper value for the AttoPilot 50V/90A sensor @@ -249,13 +243,6 @@ //# define CURR_AMP_PER_VOLT 13.66 // This is the proper value for the AttoPilot 13.6V/45A sensor #endif -////////////////////////////////////////////////////////////////////////////// -// INPUT_VOLTAGE -// -#ifndef INPUT_VOLTAGE - # define INPUT_VOLTAGE 4.68 // 4.68 is the average value for a sample set. This is the value at the processor with 5.02 applied at the servo rail -#endif - ////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION diff --git a/ArduPlane/events.pde b/ArduPlane/events.pde index 1baca4af70010..3c0631d3a08e3 100644 --- a/ArduPlane/events.pde +++ b/ArduPlane/events.pde @@ -80,14 +80,17 @@ static void failsafe_short_off_event() } } -#if BATTERY_EVENT == ENABLED void low_battery_event(void) { - gcs_send_text_P(SEVERITY_HIGH,PSTR("Low Battery!")); + if (battery.low_batttery) { + return; + } + gcs_send_text_fmt(PSTR("Low Battery %.2fV Used %.0f mAh"), + battery.voltage, battery.current_total_mah); set_mode(RTL); g.throttle_cruise = THROTTLE_CRUISE; + battery.low_batttery = true; } -#endif //////////////////////////////////////////////////////////////////////////////// // repeating event control diff --git a/ArduPlane/sensors.pde b/ArduPlane/sensors.pde index 8a128ae38b2ed..6bed6c5d1250e 100644 --- a/ArduPlane/sensors.pde +++ b/ArduPlane/sensors.pde @@ -39,26 +39,33 @@ static void zero_airspeed(void) static void read_battery(void) { if(g.battery_monitoring == 0) { - battery_voltage1 = 0; + battery.voltage = 0; return; } if(g.battery_monitoring == 3 || g.battery_monitoring == 4) { // this copes with changing the pin at runtime batt_volt_pin->set_pin(g.battery_volt_pin); - battery_voltage1 = BATTERY_VOLTAGE(batt_volt_pin); + battery.voltage = BATTERY_VOLTAGE(batt_volt_pin); } if(g.battery_monitoring == 4) { // this copes with changing the pin at runtime batt_curr_pin->set_pin(g.battery_curr_pin); - current_amps1 = CURRENT_AMPS(batt_curr_pin); - current_total1 += current_amps1 * (float)delta_ms_medium_loop * 0.0002778; // .0002778 is 1/3600 (conversion to hours) + battery.current_amps = CURRENT_AMPS(batt_curr_pin); + // .0002778 is 1/3600 (conversion to hours) + battery.current_total_mah += battery.current_amps * (float)delta_ms_medium_loop * 0.0002778; } -#if BATTERY_EVENT == ENABLED - if(battery_voltage1 < LOW_VOLTAGE) low_battery_event(); - if(g.battery_monitoring == 4 && current_total1 > g.pack_capacity) low_battery_event(); -#endif + if (battery.voltage != 0 && + g.fs_batt_voltage > 0 && + battery.voltage < g.fs_batt_voltage) { + low_battery_event(); + } + if (g.battery_monitoring == 4 && + g.fs_batt_mah > 0 && + g.pack_capacity - battery.current_total_mah < g.fs_batt_mah) { + low_battery_event(); + } } diff --git a/ArduPlane/test.pde b/ArduPlane/test.pde index f22e123222428..9212f8ae7afc7 100644 --- a/ArduPlane/test.pde +++ b/ArduPlane/test.pde @@ -262,14 +262,14 @@ test_battery(uint8_t argc, const Menu::arg *argv) read_battery(); if (g.battery_monitoring == 3) { cliSerial->printf_P(PSTR("V: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); + battery.voltage, + battery.current_amps, + battery.current_total_mah); } else { cliSerial->printf_P(PSTR("V: %4.4f, A: %4.4f, mAh: %4.4f\n"), - battery_voltage1, - current_amps1, - current_total1); + battery.voltage, + battery.current_amps, + battery.current_total_mah); } // write out the servo PWM values