Copter: Support new battery failsafes

This commit is contained in:
Michael du Breuil 2018-02-28 16:32:16 -07:00 committed by Francisco Ferreira
parent 7405bb7b85
commit 291531e056
17 changed files with 95 additions and 159 deletions

View File

@ -146,30 +146,15 @@ bool AP_Arming_Copter::board_voltage_checks(bool display_failure)
return false;
}
Parameters &g = copter.g;
// check battery voltage
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_VOLTAGE)) {
if (copter.failsafe.battery) {
if (copter.battery.has_failsafed()) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Battery failsafe");
}
return false;
}
// all following checks are skipped if USB is connected
if (copter.ap.usb_connected) {
return true;
}
// check if battery is exhausted
if (copter.battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery");
}
return false;
}
// call parent battery checks
if (!AP_Arming::battery_checks(display_failure)) {
return false;

View File

@ -70,13 +70,6 @@ void Copter::set_failsafe_radio(bool b)
}
// ---------------------------------------------
void Copter::set_failsafe_battery(bool b)
{
failsafe.battery = b;
AP_Notify::flags.failsafe_battery = b;
}
// ---------------------------------------------
void Copter::set_failsafe_gcs(bool b)
{

View File

@ -191,6 +191,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
#endif
};
constexpr int8_t Copter::_failsafe_priorities[7];
void Copter::setup()
{
@ -297,7 +298,7 @@ void Copter::throttle_loop()
void Copter::update_batt_compass(void)
{
// read battery before compass because it may be used for motor interference compensation
read_battery();
battery.read();
if(g.compass_enabled) {
// update compass with throttle value - used for compassmot

View File

@ -373,19 +373,18 @@ private:
// Failsafe
struct {
uint8_t rc_override_active : 1; // 0 // true if rc control are overwritten by ground station
uint8_t radio : 1; // 1 // A status flag for the radio failsafe
uint8_t battery : 1; // 2 // A status flag for the battery failsafe
uint8_t gcs : 1; // 4 // A status flag for the ground station failsafe
uint8_t ekf : 1; // 5 // true if ekf failsafe has occurred
uint8_t terrain : 1; // 6 // true if the missing terrain data failsafe has occurred
uint8_t adsb : 1; // 7 // true if an adsb related failsafe has occurred
int8_t radio_counter; // number of iterations with throttle below throttle_fs_value
uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
uint32_t terrain_first_failure_ms; // the first time terrain data access failed - used to calculate the duration of the failure
uint32_t terrain_last_failure_ms; // the most recent time terrain data access failed
int8_t radio_counter; // number of iterations with throttle below throttle_fs_value
uint8_t rc_override_active : 1; // true if rc control are overwritten by ground station
uint8_t radio : 1; // A status flag for the radio failsafe
uint8_t gcs : 1; // A status flag for the ground station failsafe
uint8_t ekf : 1; // true if ekf failsafe has occurred
uint8_t terrain : 1; // true if the missing terrain data failsafe has occurred
uint8_t adsb : 1; // true if an adsb related failsafe has occurred
} failsafe;
// sensor health for logging
@ -425,7 +424,9 @@ private:
int32_t initial_armed_bearing;
// Battery Sensors
AP_BattMonitor battery{MASK_LOG_CURRENT};
AP_BattMonitor battery{MASK_LOG_CURRENT,
FUNCTOR_BIND_MEMBER(&Copter::handle_battery_failsafe, void, const char*, const int8_t),
_failsafe_priorities};
#if FRSKY_TELEM_ENABLED == ENABLED
// FrSky telemetry support
@ -628,11 +629,37 @@ private:
static const AP_Param::Info var_info[];
static const struct LogStructure log_structure[];
enum Failsafe_Action {
Failsafe_Action_None = 0,
Failsafe_Action_Land = 1,
Failsafe_Action_RTL = 2,
Failsafe_Action_SmartRTL = 3,
Failsafe_Action_SmartRTL_Land = 4,
Failsafe_Action_Terminate = 5
};
static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Terminate,
Failsafe_Action_Land,
Failsafe_Action_RTL,
Failsafe_Action_SmartRTL_Land,
Failsafe_Action_SmartRTL,
Failsafe_Action_None,
-1 // the priority list must end with a sentinel of -1
};
#define FAILSAFE_LAND_PRIORITY 1
static_assert(_failsafe_priorities[FAILSAFE_LAND_PRIORITY] == Failsafe_Action_Land,
"FAILSAFE_LAND_PRIORITY must match the entry in _failsafe_priorities");
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
"_failsafe_priorities is missing the sentinel");
// AP_State.cpp
void set_auto_armed(bool b);
void set_simple_mode(uint8_t b);
void set_failsafe_radio(bool b);
void set_failsafe_battery(bool b);
void set_failsafe_gcs(bool b);
void update_using_interlock();
void set_motor_emergency_stop(bool b);
@ -718,7 +745,7 @@ private:
// events.cpp
void failsafe_radio_on_event();
void failsafe_radio_off_event();
void failsafe_battery_event(void);
void handle_battery_failsafe(const char* type_str, const int8_t action);
void failsafe_gcs_check();
void failsafe_gcs_off_event(void);
void failsafe_terrain_check();
@ -880,7 +907,6 @@ private:
void compass_accumulate(void);
void init_optflow();
void update_optical_flow(void);
void read_battery(void);
void read_receiver_rssi(void);
void compass_cal_update(void);
void accel_cal_update(void);

View File

@ -29,7 +29,7 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
uint32_t custom_mode = control_mode;
// set system as critical if any failsafe have triggered
if (failsafe.radio || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb) {
if (failsafe.radio || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain || failsafe.adsb) {
system_status = MAV_STATE_CRITICAL;
}

View File

@ -3,7 +3,7 @@
#include <GCS_MAVLink/GCS.h>
// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS)
#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS | MAV_SYS_STATUS_SENSOR_BATTERY)
class GCS_MAVLINK_Copter : public GCS_MAVLINK
{

View File

@ -183,7 +183,7 @@ void Copter::Log_Write_MotBatt()
time_us : AP_HAL::micros64(),
lift_max : (float)(motors->get_lift_max()),
bat_volt : (float)(motors->get_batt_voltage_filt()),
bat_res : (float)(motors->get_batt_resistance()),
bat_res : (float)(battery.get_resistance()),
th_limit : (float)(motors->get_throttle_limit())
};
DataFlash.WriteBlock(&pkt_mot, sizeof(pkt_mot));

View File

@ -179,29 +179,6 @@ const AP_Param::Info Copter::var_info[] = {
GSCALAR(rangefinder_gain, "RNGFND_GAIN", RANGEFINDER_GAIN_DEFAULT),
#endif
// @Param: FS_BATT_ENABLE
// @DisplayName: Battery Failsafe Enable
// @Description: Controls whether failsafe will be invoked when battery voltage or current runs low
// @Values: 0:Disabled,1:Land,2:RTL,3:SmartRTL or RTL,4:SmartRTL or Land
// @User: Standard
GSCALAR(failsafe_battery_enabled, "FS_BATT_ENABLE", FS_BATT_DISABLED),
// @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 copter will RTL
// @Units: V
// @Increment: 0.1
// @User: Standard
GSCALAR(fs_batt_voltage, "FS_BATT_VOLTAGE", FS_BATT_VOLTAGE_DEFAULT),
// @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 copter will RTL
// @Units: mA.h
// @Increment: 50
// @User: Standard
GSCALAR(fs_batt_mah, "FS_BATT_MAH", FS_BATT_MAH_DEFAULT),
// @Param: FS_GCS_ENABLE
// @DisplayName: Ground Station Failsafe Enable
// @Description: Controls whether failsafe will be invoked (and what action to take) when connection with Ground station is lost for at least 5 seconds. NB. The GCS Failsafe is only active when RC_OVERRIDE is being used to control the vehicle.
@ -1061,6 +1038,10 @@ const AP_Param::ConversionInfo conversion_table[] = {
{ Parameters::k_param_serial1_baud, 0, AP_PARAM_INT16, "SERIAL1_BAUD" },
{ Parameters::k_param_serial2_baud, 0, AP_PARAM_INT16, "SERIAL2_BAUD" },
{ Parameters::k_param_arming_check_old, 0, AP_PARAM_INT8, "ARMING_CHECK" },
// battery
{ Parameters::k_param_fs_batt_voltage, 0, AP_PARAM_INT8, "BATT_FS_LOW_VOLT" },
{ Parameters::k_param_fs_batt_mah, 0, AP_PARAM_INT8, "BATT_FS_LOW_MAH" },
{ Parameters::k_param_failsafe_battery_enabled, 0, AP_PARAM_INT8, "BATT_FS_LOW_ACT" },
};
void Copter::load_parameters(void)

View File

@ -108,7 +108,7 @@ public:
k_param_angle_max,
k_param_gps_hdop_good,
k_param_battery,
k_param_fs_batt_mah,
k_param_fs_batt_mah, // unused - moved to AP_BattMonitor
k_param_angle_rate_max, // remove
k_param_rssi_range, // unused, replaced by rssi_ library parameters
k_param_rc_feel_rp, // deprecated
@ -239,7 +239,7 @@ public:
k_param_rangefinder_enabled_old, // deprecated
k_param_frame_type,
k_param_optflow_enabled, // deprecated
k_param_fs_batt_voltage,
k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor
k_param_ch7_option,
k_param_auto_slew_rate, // deprecated - can be deleted
k_param_rangefinder_type_old, // deprecated
@ -295,7 +295,7 @@ public:
k_param_radio_tuning_high,
k_param_radio_tuning_low,
k_param_rc_speed = 192,
k_param_failsafe_battery_enabled,
k_param_failsafe_battery_enabled, // unused - moved to AP_BattMonitor
k_param_throttle_mid, // remove
k_param_failsafe_gps_enabled, // remove
k_param_rc_9_old,
@ -394,10 +394,6 @@ public:
AP_Float rangefinder_gain;
#endif
AP_Int8 failsafe_battery_enabled; // battery failsafe enabled
AP_Float fs_batt_voltage; // battery voltage below which failsafe will be triggered
AP_Float fs_batt_mah; // battery capacity (in mah) below which failsafe will be triggered
AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int16 gps_hdop_good; // GPS Hdop value at or below this value represent a good position

View File

@ -108,9 +108,8 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
gcs_chan.send_text(MAV_SEVERITY_INFO, "Throttle");
}
// disable throttle and battery failsafe
// disable throttle failsafe
g.failsafe_throttle = FS_THR_DISABLED;
g.failsafe_battery_enabled = FS_BATT_DISABLED;
// disable motor compensation
compass.motor_compensation_type(AP_COMPASS_MOT_COMP_DISABLED);
@ -164,7 +163,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
compass.read();
// read current
read_battery();
battery.read();
// calculate scaling for throttle
throttle_pct = (float)channel_throttle->get_control_in() / 1000.0f;
@ -269,7 +268,6 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
// re-enable failsafes
g.failsafe_throttle.load();
g.failsafe_battery_enabled.load();
// flag we have completed
ap.compass_mot = false;

View File

@ -131,14 +131,6 @@
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef FS_BATT_VOLTAGE_DEFAULT
# define FS_BATT_VOLTAGE_DEFAULT 10.5f // default battery voltage below which failsafe will be triggered
#endif
#ifndef FS_BATT_MAH_DEFAULT
# define FS_BATT_MAH_DEFAULT 0 // default battery capacity (in mah) below which failsafe will be triggered
#endif
#ifndef BOARD_VOLTAGE_MIN
# define BOARD_VOLTAGE_MIN 4.3f // min board voltage in volts for pre-arm checks
#endif

View File

@ -475,13 +475,6 @@ enum LoggingParameters {
#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_RTL 4
#define FS_THR_ENABLED_ALWAYS_SMARTRTL_OR_LAND 5
// Battery failsafe definitions (FS_BATT_ENABLE parameter)
#define FS_BATT_DISABLED 0 // battery failsafe disabled
#define FS_BATT_LAND 1 // switch to LAND mode on battery failsafe
#define FS_BATT_RTL 2 // switch to RTL mode on battery failsafe
#define FS_BATT_SMARTRTL_OR_RTL 3 // switch to SmartRTL, if can't, switch to RTL
#define FS_BATT_SMARTRTL_OR_LAND 4 // switch to SmartRTL, if can't, swtich to LAND
// GCS failsafe definitions (FS_GCS_ENABLE parameter)
#define FS_GCS_DISABLED 0
#define FS_GCS_ENABLED_ALWAYS_RTL 1

View File

@ -16,8 +16,10 @@ void Copter::failsafe_radio_on_event()
} else {
if (control_mode == AUTO && g.failsafe_throttle == FS_THR_ENABLED_CONTINUE_MISSION) {
// continue mission
} else if (control_mode == LAND && g.failsafe_battery_enabled == FS_BATT_LAND && failsafe.battery) {
// continue landing
} else if (control_mode == LAND &&
battery.has_failsafed() &&
battery.get_highest_failsafe_priority() <= FAILSAFE_LAND_PRIORITY) {
// continue landing or other high priority failsafes
} else {
if (g.failsafe_throttle == FS_THR_ENABLED_ALWAYS_RTL) {
set_mode_RTL_or_land_with_pause(MODE_REASON_RADIO_FAILSAFE);
@ -46,37 +48,39 @@ void Copter::failsafe_radio_off_event()
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_RADIO, ERROR_CODE_FAILSAFE_RESOLVED);
}
void Copter::failsafe_battery_event(void)
void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
{
// return immediately if low battery event has already been triggered
if (failsafe.battery) {
return;
}
// failsafe check
if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors->armed()) {
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
if (g.failsafe_battery_enabled == FS_BATT_SMARTRTL_OR_RTL) {
set_mode_SmartRTL_or_RTL(MODE_REASON_BATTERY_FAILSAFE);
} else if (g.failsafe_battery_enabled == FS_BATT_SMARTRTL_OR_LAND) {
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
} else if (g.failsafe_battery_enabled == FS_BATT_RTL) {
set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
} else { // g.failsafe_battery_enabled == FS_BATT_LAND
set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
}
}
}
// set the low battery flag
set_failsafe_battery(true);
// warn the ground station and log to dataflash
gcs().send_text(MAV_SEVERITY_WARNING,"Low battery");
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
// failsafe check
if (should_disarm_on_failsafe()) {
init_disarm_motors();
} else {
switch ((Failsafe_Action)action) {
case Failsafe_Action_None:
return;
case Failsafe_Action_Land:
set_mode_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
break;
case Failsafe_Action_RTL:
set_mode_RTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
break;
case Failsafe_Action_SmartRTL:
set_mode_SmartRTL_or_RTL(MODE_REASON_BATTERY_FAILSAFE);
break;
case Failsafe_Action_SmartRTL_Land:
set_mode_SmartRTL_or_land_with_pause(MODE_REASON_BATTERY_FAILSAFE);
break;
case Failsafe_Action_Terminate:
#if ADVANCED_FAILSAFE == ENABLED
char battery_type_str[17];
snprintf(battery_type_str, 17, "%s battery", type_str);
afs.gcs_terminate(true, battery_type_str);
#else
init_disarm_motors();
#endif
}
}
}
// failsafe_gcs_check - check for ground station failsafe

View File

@ -153,9 +153,8 @@ MAV_RESULT Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t moto
motors->armed(true);
}
// disable throttle, battery and gps failsafe
// disable throttle and gps failsafe
g.failsafe_throttle = FS_THR_DISABLED;
g.failsafe_battery_enabled = FS_BATT_DISABLED;
g.failsafe_gcs = FS_GCS_DISABLED;
g.fs_ekf_action = 0;
@ -204,7 +203,6 @@ void Copter::motor_test_stop()
// re-enable failsafes
g.failsafe_throttle.load();
g.failsafe_battery_enabled.load();
g.failsafe_gcs.load();
g.fs_ekf_action.load();

View File

@ -156,9 +156,6 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
// disable cpu failsafe because initialising everything takes a while
failsafe_disable();
// reset battery failsafe
set_failsafe_battery(false);
// notify that arming will occur (we do this early to give plenty of warning)
AP_Notify::flags.armed = true;
// call notify update a few times to ensure the message gets out

View File

@ -173,32 +173,6 @@ void Copter::update_optical_flow(void)
}
#endif // OPTFLOW == ENABLED
// read_battery - check battery voltage and current and invoke failsafe if necessary
// called at 10hz
void Copter::read_battery(void)
{
battery.read();
// update motors with voltage and current
if (battery.get_type() != AP_BattMonitor_Params::BattMonitor_TYPE_NONE) {
motors->set_voltage(battery.voltage());
}
if (battery.has_current()) {
compass.set_current(battery.current_amps());
motors->set_current(battery.current_amps());
motors->set_resistance(battery.get_resistance());
motors->set_voltage_resting_estimate(battery.voltage_resting_estimate());
}
// 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)) {
failsafe_battery_event();
}
}
// read the receiver RSSI as an 8 bit number for MAVLink
// RC_CHANNELS_SCALED message
void Copter::read_receiver_rssi(void)
@ -305,9 +279,6 @@ void Copter::update_sensor_status_flags(void)
control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
}
#endif
if (copter.battery.healthy()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
#if AC_FENCE == ENABLED
if (copter.fence.sys_status_present()) {
control_sensors_present |= MAV_SYS_STATUS_GEOFENCE;
@ -358,7 +329,7 @@ void Copter::update_sensor_status_flags(void)
control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
}
if (g.fs_batt_voltage > 0 || g.fs_batt_mah > 0) {
if (battery.num_instances() > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
#if AC_FENCE == ENABLED
@ -453,8 +424,9 @@ void Copter::update_sensor_status_flags(void)
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
if (copter.failsafe.battery) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY; }
if (!copter.battery.healthy() || copter.battery.has_failsafed()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY;
}
#if AC_FENCE == ENABLED
if (copter.fence.sys_status_failed()) {
control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE;

View File

@ -103,7 +103,7 @@ void Copter::init_ardupilot()
snprintf(firmware_buf, sizeof(firmware_buf), "%s %s", fwver.fw_string, get_frame_string());
frsky_telemetry.init(serial_manager, firmware_buf,
get_frame_mav_type(),
&g.fs_batt_voltage, &g.fs_batt_mah, &ap.value);
&ap.value);
#endif
#if LOGGING_ENABLED == ENABLED