Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Plane: On Critical Battery, Land #5000

Closed
wants to merge 3 commits into from
Closed

Plane: On Critical Battery, Land #5000

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7e0c68a
BattMonitor, report battery state including critical
Oct 16, 2016
eed5621
Plane, Critical battery failsafe
Oct 16, 2016
18f1fb6
Copter, update for BattMonitor changes.
Oct 16, 2016
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
6 changes: 4 additions & 2 deletions ArduCopter/arming_checks.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -317,7 +317,8 @@ bool Copter::board_voltage_checks(bool display_failure)

// check battery voltage
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) {
if (failsafe.battery || (!ap.usb_connected &&
(battery.status(g.fs_batt_voltage, 0.0, g.fs_batt_mah, 0.0) != AP_BattMonitor::BattMonitor_STATUS_NORMAL))) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery");
}
Expand Down Expand Up @@ -752,7 +753,8 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)

// check battery voltage
if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) {
if (failsafe.battery || (!ap.usb_connected &&
(battery.status(g.fs_batt_voltage, 0.0, g.fs_batt_mah, 0.0) != AP_BattMonitor::BattMonitor_STATUS_NORMAL))) {
if (display_failure) {
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Check Battery");
}
Expand Down
3 changes: 2 additions & 1 deletion ArduCopter/sensors.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -169,7 +169,8 @@ void Copter::read_battery(void)

// check for low voltage or current if the low voltage check hasn't already been triggered
// we only check when we're not powered by USB to avoid false alarms during bench tests
if (!ap.usb_connected && !failsafe.battery && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
if (!ap.usb_connected && !failsafe.battery &&
(battery.status(g.fs_batt_voltage, 0.0, g.fs_batt_mah, 0.0) != AP_BattMonitor::BattMonitor_STATUS_NORMAL)) {
failsafe_battery_event();
}

Expand Down
16 changes: 16 additions & 0 deletions ArduPlane/Parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1362,6 +1362,22 @@ const AP_Param::Info Plane::var_info[] = {
// @Group:
// @Path: Parameters.cpp
GOBJECT(g2, "", ParametersG2),

// @Param: FS_BATT_CRT_VOLT
// @DisplayName: Failsafe battery critical voltage
// @Description: Battery voltage to trigger critcal failsafe. Set to 0 to disable critical battery failsafe. If the battery voltage drops below this level then the plane will switch to a preplanned landing if present, otherwise RTL.
// @Units: Volts
// @Increment: 0.1
// @User: Standard
GSCALAR(fs_batt_critical_voltage, "FS_BATT_CRT_VOLT", 0),

// @Param: FS_BATT_CRT_MAH
// @DisplayName: Failsafe battery critical mah
// @Description: Battery mAh to trigger critcal failsafe. Set to 0 to disable critical battery failsafe. If the remaining battery drops below this level then the plane will switch to a preplanned landing if present, otherwise RTL.
// @Units: mAh
// @Increment: 50
// @User: Standard
GSCALAR(fs_batt_critical_mah, "FS_BATT_CRT_MAH", 0),

AP_VAREND
};
Expand Down
4 changes: 4 additions & 0 deletions ArduPlane/Parameters.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -346,6 +346,8 @@ class Parameters {
k_param_mixing_offset,
k_param_dspoiler_rud_rate,

k_param_fs_batt_critical_voltage = 251,
k_param_fs_batt_critical_mah = 252,
k_param_DataFlash = 253, // Logging Group

// 254,255: reserved
Expand Down Expand Up @@ -436,7 +438,9 @@ class Parameters {
AP_Float long_fs_timeout;
AP_Int8 gcs_heartbeat_fs_enabled;
AP_Float fs_batt_voltage;
AP_Float fs_batt_critical_voltage;
AP_Float fs_batt_mah;
AP_Float fs_batt_critical_mah;

// Flight modes
//
Expand Down
4 changes: 4 additions & 0 deletions ArduPlane/Plane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -355,6 +355,9 @@ class Plane : public AP_HAL::HAL::Callbacks {
// flag to hold whether battery low voltage threshold has been breached
uint8_t low_battery:1;

// flag to hold whether battery critical voltage threshold has been breached
uint8_t critical_battery:1;

// true if an adsb related failsafe has occurred
uint8_t adsb:1;

Expand Down Expand Up @@ -920,6 +923,7 @@ class Plane : public AP_HAL::HAL::Callbacks {
void failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason);
void failsafe_short_off_event(mode_reason_t reason);
void low_battery_event(void);
void critical_battery_event(void);
void update_events(void);
uint8_t max_fencepoints(void);
Vector2l get_fence_point_with_index(unsigned i);
Expand Down
29 changes: 27 additions & 2 deletions ArduPlane/events.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -146,13 +146,38 @@ void Plane::low_battery_event(void)
if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();
}
failsafe.low_battery = true;
AP_Notify::flags.failsafe_battery = true;
}

void Plane::critical_battery_event(void)
{
if (failsafe.critical_battery) {
return;
}
gcs_send_text_fmt(MAV_SEVERITY_WARNING, "Critical battery %.2fV used %.0f mAh",
(double)battery.voltage(), (double)battery.current_total_mah());
if (flight_stage != AP_SpdHgtControl::FLIGHT_LAND_FINAL &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_PREFLARE &&
flight_stage != AP_SpdHgtControl::FLIGHT_LAND_APPROACH) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

also needs to check for ::FLIGHT_LAND_ABORT. If you're ever in that state then you're already at home trying to land

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@magicrub Should that thought be applied to the low_battery_event() as well? (Line 146 of this same file) I can see the arguments either way on that one...

set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();

// Go directly to the landing sequence
jump_to_landing_sequence();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

remove jump_to_landing_seq and auto_state.checked_for_auto. They are handled when RTL is set and RTL_AUTOLAND >0.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@magicrub this is meant to be independent of RTL_AUTOLAND

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we don't want it to autoland if RTL_AUTOLAND == 0, it should come back home and loiter like a simple RTL.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

RTL_AUTOLAND 3 being this new behaviour would be fine. The objection is that none of the current RTL_AUTOLAND options allow RTL without also doing a landing. I want RTL on low battery, but I want to have time before just going into the landing sequence, and on normal RTL there should be a option that says loiter indefinitely at a rallypoint/home until battery is low, then land. That is primarily what this is meant to address. If you think RTL_AUTOLAND 3 meaning perform a normal RTL until critical battery and only then perform a autoland I'm fine with that.


// prevent running the expensive jump_to_landing_sequence
// on every loop
auto_state.checked_for_autoland = true;
}
failsafe.critical_battery = true;
AP_Notify::flags.failsafe_battery = true;
}


void Plane::update_events(void)
{
ServoRelayEvents.update_events();
Expand Down
16 changes: 12 additions & 4 deletions ArduPlane/sensors.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,10 +126,18 @@ void Plane::read_battery(void)
battery.read();
compass.set_current(battery.current_amps());

if (!usb_connected &&
hal.util->get_soft_armed() &&
battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
low_battery_event();
if (!usb_connected && hal.util->get_soft_armed()) {

switch(battery.status(g.fs_batt_voltage, g.fs_batt_critical_voltage, g.fs_batt_mah, g.fs_batt_critical_mah)){
case AP_BattMonitor::BattMonitor_STATUS_NORMAL:
break;
case AP_BattMonitor::BattMonitor_STATUS_LOW:
low_battery_event();
break;
case AP_BattMonitor::BattMonitor_STATUS_CRITICAL:
critical_battery_event();
break;
}
}

if (should_log(MASK_LOG_CURRENT)) {
Expand Down
33 changes: 26 additions & 7 deletions libraries/AP_BattMonitor/AP_BattMonitor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -279,12 +279,31 @@ uint8_t AP_BattMonitor::capacity_remaining_pct(uint8_t instance) const
}
}

/// exhausted - returns true if the voltage remains below the low_voltage for 10 seconds or remaining capacity falls below min_capacity_mah
bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_capacity_mah)
/// status - returns BattMonitor_STATUS_CRITICAL if the voltage remains below the critical_voltage
/// for 10 seconds or remaining capacity falls below critical_capacity_mah.
/// returns BattMonitor_STATUS_LOW if the voltage remains below the low_voltage for 10
/// seconds or remaining capacity falls below min_capacity_mah.
/// returns BattMonitor_STATUS_NORMAL otherwise.
AP_BattMonitor::BattMonitor_Status AP_BattMonitor::status(uint8_t instance, float low_voltage,
float critical_voltage, float min_capacity_mah, float critical_capacity_mah)
{
// exit immediately if no monitors setup
if (_num_instances == 0 || instance >= _num_instances) {
return false;
return BattMonitor_STATUS_NORMAL;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

there should probably be a BattMonitor_STATUS_UNKNOWN to handle this case but then you can't use != BattMonitor_STATUS_NORMAL in the code

}

if((state[instance].voltage > 0) && (critical_voltage > 0) && (state[instance].voltage < critical_voltage)) {
// this is the first time our voltage has dropped below minimum so start timer
if (state[instance].critical_voltage_start_ms == 0) {
state[instance].critical_voltage_start_ms = AP_HAL::millis();
} else if (AP_HAL::millis() - state[instance].critical_voltage_start_ms > AP_BATT_LOW_VOLT_TIMEOUT_MS) {
return BattMonitor_STATUS_CRITICAL;
}
}

// check capacity if current monitoring is enabled
if (has_current(instance) && (critical_capacity_mah > 0) && (_pack_capacity[instance] - state[instance].current_total_mah < critical_capacity_mah)) {
return BattMonitor_STATUS_CRITICAL;
}

// check voltage
Expand All @@ -293,20 +312,20 @@ bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_ca
if (state[instance].low_voltage_start_ms == 0) {
state[instance].low_voltage_start_ms = AP_HAL::millis();
} else if (AP_HAL::millis() - state[instance].low_voltage_start_ms > AP_BATT_LOW_VOLT_TIMEOUT_MS) {
return true;
return BattMonitor_STATUS_LOW;
}
} else {
// acceptable voltage so reset timer
state[instance].low_voltage_start_ms = 0;
state[instance].critical_voltage_start_ms = 0;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This also needs to be reset on it's own for proper debounce

}

// check capacity if current monitoring is enabled
if (has_current(instance) && (min_capacity_mah > 0) && (_pack_capacity[instance] - state[instance].current_total_mah < min_capacity_mah)) {
return true;
return BattMonitor_STATUS_LOW;
}

// if we've gotten this far then battery is ok
return false;
return BattMonitor_STATUS_NORMAL;
}

// return true if any battery is pushing too much power
Expand Down
19 changes: 16 additions & 3 deletions libraries/AP_BattMonitor/AP_BattMonitor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,13 @@ class AP_BattMonitor
BattMonitor_TYPE_BEBOP = 6
};

// Battery monitor driver types
enum BattMonitor_Status {
BattMonitor_STATUS_NORMAL,
BattMonitor_STATUS_LOW,
BattMonitor_STATUS_CRITICAL,
};

// The BattMonitor_State structure is filled in by the backend driver
struct BattMonitor_State {
uint8_t instance; // the instance number of this monitor
Expand All @@ -54,6 +61,7 @@ class AP_BattMonitor
float current_total_mah; // total current draw since start-up
uint32_t last_time_micros; // time when voltage and current was last read
uint32_t low_voltage_start_ms; // time when voltage dropped below the minimum
uint32_t critical_voltage_start_ms; // time when voltage dropped below the minimum
};

// Return the number of battery monitor instances
Expand Down Expand Up @@ -98,9 +106,14 @@ class AP_BattMonitor
int32_t pack_capacity_mah(uint8_t instance) const;
int32_t pack_capacity_mah() const { return pack_capacity_mah(AP_BATT_PRIMARY_INSTANCE); }

/// exhausted - returns true if the battery's voltage remains below the low_voltage for 10 seconds or remaining capacity falls below min_capacity
bool exhausted(uint8_t instance, float low_voltage, float min_capacity_mah);
bool exhausted(float low_voltage, float min_capacity_mah) { return exhausted(AP_BATT_PRIMARY_INSTANCE, low_voltage, min_capacity_mah); }
/// status - returns BattMonitor_STATUS_CRITICAL if the voltage remains below the critical_voltage
/// for 10 seconds or remaining capacity falls below critical_capacity_mah.
/// returns BattMonitor_STATUS_LOW if the voltage remains below the low_voltage for 10
/// seconds or remaining capacity falls below min_capacity_mah.
/// returns BattMonitor_STATUS_NORMAL otherwise.
enum BattMonitor_Status status(uint8_t instance, float low_voltage, float critical_voltage, float min_capacity_mah, float critical_capacity_mah);
enum BattMonitor_Status status(float low_voltage, float critical_voltage, float min_capacity_mah, float critical_capacity_mah) {
return status(AP_BATT_PRIMARY_INSTANCE, low_voltage, critical_voltage, min_capacity_mah, critical_capacity_mah); }
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

when doing multi-line functions in header, move the last } below it.


/// get_type - returns battery monitor type
enum BattMonitor_Type get_type() { return get_type(AP_BATT_PRIMARY_INSTANCE); }
Expand Down