Skip to content

Commit

Permalink
Plane: make voltage and battery capacity failsafe settable at runtime
Browse files Browse the repository at this point in the history 
this also fixes issue ArduPilot#292
  • Loading branch information
@rmackay9
Andrew Tridgell authored and rmackay9 committed Jul 11, 2013
1 parent 7461776 commit 31d6356
Show file tree
Hide file tree
Showing 9 changed files with 65 additions and 49 deletions.
23 changes: 12 additions & 11 deletions ArduPlane/ArduPlane.pde
View file
Expand Up @@ -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
Expand Down
10 changes: 5 additions & 5 deletions ArduPlane/GCS_Mavlink.pde
View file
Expand Up @@ -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) {
Expand All @@ -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 %,
Expand Down
6 changes: 3 additions & 3 deletions ArduPlane/Log.pde
View file
Expand Up @@ -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));
}
Expand Down
4 changes: 4 additions & 0 deletions ArduPlane/Parameters.h
View file
Expand Up @@ -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
Expand Down Expand Up @@ -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
//
Expand Down
14 changes: 14 additions & 0 deletions ArduPlane/Parameters.pde
View file
Expand Up @@ -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.
Expand Down
13 changes: 0 additions & 13 deletions ArduPlane/config.h
View file
Expand Up @@ -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
Expand All @@ -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
Expand Down
9 changes: 6 additions & 3 deletions ArduPlane/events.pde
View file
Expand Up @@ -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
Expand Down
23 changes: 15 additions & 8 deletions ArduPlane/sensors.pde
View file
Expand Up @@ -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();
}
}


Expand Down
12 changes: 6 additions & 6 deletions ArduPlane/test.pde
View file
Expand Up @@ -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
Expand Down

0 comments on commit 31d6356

Please sign in to comment.