Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Sub: Support new battery failsafes

Browse Source
This commit is contained in:
Michael du Breuil 2018-03-01 11:44:02 -07:00 committed by Francisco Ferreira
parent 291531e056
commit 6ebe954d51
9 changed files with 70 additions and 127 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduSub/ArduSub.cpp
View File

@ -80,6 +80,7 @@ const AP_Scheduler::Task Sub::scheduler_tasks[] = {
#endif
};
constexpr int8_t Sub::_failsafe_priorities[5];
void Sub::setup()
{
@ -166,7 +167,7 @@ void Sub::fifty_hz_loop()
void Sub::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

16
ArduSub/GCS_Mavlink.cpp
View File

@ -3,7 +3,7 @@
#include "GCS_Mavlink.h"
// default sensors are present and healthy: gyro, accelerometer, 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_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_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)
void Sub::gcs_send_heartbeat(void)
{
@ -32,7 +32,7 @@ NOINLINE void Sub::send_heartbeat(mavlink_channel_t chan)
uint32_t custom_mode = control_mode;
// set system as critical if any failsafe have triggered
if (failsafe.pilot_input || failsafe.battery || failsafe.gcs || failsafe.ekf || failsafe.terrain) {
if (failsafe.pilot_input || battery.has_failsafed() || failsafe.gcs || failsafe.ekf || failsafe.terrain) {
system_status = MAV_STATE_CRITICAL;
}
@ -130,7 +130,7 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
// all present sensors enabled by default except altitude and position control and motors which we will set individually
control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL &
~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL &
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS);
~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_SENSOR_BATTERY);
switch (control_mode) {
case ALT_HOLD:
@ -151,6 +151,10 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
}
if (battery.num_instances() > 0) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
// default to all healthy except baro, compass, gps and receiver which we set individually
control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE |
MAV_SYS_STATUS_SENSOR_3D_MAG |
@ -184,6 +188,10 @@ NOINLINE void Sub::send_extended_status1(mavlink_channel_t chan)
control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
}
if (!battery.healthy() || battery.has_failsafed()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY;
}
int16_t battery_current = -1;
int8_t battery_remaining = -1;
@ -1653,4 +1661,4 @@ void GCS_MAVLINK_Sub::set_ekf_origin(const Location& loc)
}
// dummy method to avoid linking AFS
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate) { return false; }
bool AP_AdvancedFailsafe::gcs_terminate(bool should_terminate, const char *reason) { return false; }

2
ArduSub/Log.cpp
View File

@ -122,7 +122,7 @@ void Sub::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));

30
ArduSub/Parameters.cpp
View File

@ -95,29 +95,6 @@ const AP_Param::Info Sub::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:Warn only,2:Disarm,3:Enter surface mode
// @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.
// @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.
// @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 what action to take when GCS heartbeat is lost.
@ -675,6 +652,12 @@ ParametersG2::ParametersG2(void)
AP_Param::setup_object_defaults(this, var_info);
}
const AP_Param::ConversionInfo conversion_table[] = {
{ Parameters::k_param_fs_batt_voltage, 0, AP_PARAM_FLOAT, "BATT_FS_LOW_VOLT" },
{ Parameters::k_param_fs_batt_mah, 0, AP_PARAM_FLOAT, "BATT_FS_LOW_MAH" },
{ Parameters::k_param_failsafe_battery_enabled, 0, AP_PARAM_INT8, "BATT_FS_LOW_ACT" },
};
void Sub::load_parameters(void)
{
if (!AP_Param::check_var_info()) {
@ -702,6 +685,7 @@ void Sub::load_parameters(void)
// Load all auto-loaded EEPROM variables
AP_Param::load_all();
hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
AP_Param::convert_old_parameters(&conversion_table[0], ARRAY_SIZE(conversion_table));
AP_Param::set_frame_type_flags(AP_PARAM_FRAME_SUB);

10
ArduSub/Parameters.h
View File

@ -167,9 +167,9 @@ public:
k_param_fs_ekf_thresh,
k_param_fs_ekf_action,
k_param_fs_crash_check,
k_param_failsafe_battery_enabled,
k_param_fs_batt_mah,
k_param_fs_batt_voltage,
k_param_failsafe_battery_enabled, // unused - moved to AP_BattMonitor
k_param_fs_batt_mah, // unused - moved to AP_BattMonitor
k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor
k_param_failsafe_pilot_input,
k_param_failsafe_pilot_input_timeout,
@ -228,10 +228,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_leak; // leak detection failsafe behavior
AP_Int8 failsafe_gcs; // ground station failsafe behavior
AP_Int8 failsafe_pressure;

48
ArduSub/Sub.h
View File

@ -265,26 +265,24 @@ private:
// Failsafe
struct {
uint8_t pilot_input : 1; // true if pilot input failsafe is active, handles things like joystick being disconnected during operation
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 leak : 1; // true if leak recently detected
uint8_t internal_pressure : 1; // true if internal pressure is over threshold
uint8_t internal_temperature : 1; // true if temperature is over threshold
uint8_t crash : 1; // true if we are crashed
uint8_t sensor_health : 1; // true if at least one sensor has triggered a failsafe (currently only used for depth in depth enabled modes)
uint32_t last_leak_warn_ms; // last time a leak warning was sent to gcs
uint32_t last_gcs_warn_ms;
uint32_t last_heartbeat_ms; // the time when the last HEARTBEAT message arrived from a GCS - used for triggering gcs failsafe
uint32_t last_pilot_input_ms; // last time we received pilot input in the form of MANUAL_CONTROL or RC_CHANNELS_OVERRIDE messages
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
uint32_t last_battery_warn_ms; // last time a battery failsafe warning was sent to gcs
uint32_t last_crash_warn_ms; // last time a crash warning was sent to gcs
uint32_t last_ekf_warn_ms; // last time an ekf warning was sent to gcs
uint8_t pilot_input : 1; // true if pilot input failsafe is active, handles things like joystick being disconnected during operation
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 leak : 1; // true if leak recently detected
uint8_t internal_pressure : 1; // true if internal pressure is over threshold
uint8_t internal_temperature : 1; // true if temperature is over threshold
uint8_t crash : 1; // true if we are crashed
uint8_t sensor_health : 1; // true if at least one sensor has triggered a failsafe (currently only used for depth in depth enabled modes)
} failsafe;
// sensor health for logging
@ -327,7 +325,9 @@ private:
uint32_t nav_delay_time_start;
// Battery Sensors
AP_BattMonitor battery{MASK_LOG_CURRENT};
AP_BattMonitor battery{MASK_LOG_CURRENT,
FUNCTOR_BIND_MEMBER(&Sub::handle_battery_failsafe, void, const char*, const int8_t),
_failsafe_priorities};
AP_Arming_Sub arming{ahrs, compass, battery};
@ -581,7 +581,7 @@ private:
void failsafe_sensors_check(void);
void failsafe_crash_check();
void failsafe_ekf_check(void);
void failsafe_battery_check(void);
void handle_battery_failsafe(const char* type_str, const int8_t action);
void failsafe_gcs_check();
void failsafe_pilot_input_check(void);
void set_neutral_controls(void);
@ -628,7 +628,6 @@ private:
void init_optflow();
void update_optical_flow(void);
#endif
void read_battery(void);
void terrain_update();
void terrain_logging();
bool terrain_use();
@ -706,6 +705,25 @@ private:
void convert_old_parameters(void);
enum Failsafe_Action {
Failsafe_Action_None = 0,
Failsafe_Action_Warn = 1,
Failsafe_Action_Disarm = 2,
Failsafe_Action_Surface = 3
};
static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Disarm,
Failsafe_Action_Surface,
Failsafe_Action_Warn,
Failsafe_Action_None,
-1 // the priority list must end with a sentinel of -1
};
static_assert(_failsafe_priorities[ARRAY_SIZE(_failsafe_priorities) - 1] == -1,
"_failsafe_priorities is missing the sentinel");
public:
void mavlink_delay_cb();
void mainloop_failsafe_check();

17
ArduSub/defines.h
View File

@ -216,17 +216,6 @@ enum LoggingParameters {
// Baro specific error codes
#define ERROR_CODE_BAD_DEPTH 0
//////////////////////////////////////////////////////////////////////////////
// Battery monitoring
//
#ifndef FS_BATT_VOLTAGE_DEFAULT
# define FS_BATT_VOLTAGE_DEFAULT 0 // 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
// GCS failsafe
#ifndef FS_GCS
# define FS_GCS DISABLED
@ -256,12 +245,6 @@ enum LoggingParameters {
# define FS_EKF_THRESHOLD_DEFAULT 0.8f // EKF failsafe's default compass and velocity variance threshold above which the EKF failsafe will be triggered
#endif
// Battery failsafe definitions (FS_BATT_ENABLE parameter)
#define FS_BATT_DISABLED 0 // battery failsafe disabled
#define FS_BATT_WARN_ONLY 1 // only warn gcs on battery failsafe
#define FS_BATT_DISARM 2 // disarm on battery failsafe
#define FS_BATT_SURFACE 3 // switch to SURFACE mode on battery failsafe
// GCS failsafe definitions (FS_GCS_ENABLE parameter)
#define FS_GCS_DISABLED 0 // Disabled
#define FS_GCS_WARN_ONLY 1 // Only send warning to gcs (only useful with multiple gcs links)

52
ArduSub/failsafe.cpp
View File

@ -148,49 +148,21 @@ void Sub::failsafe_ekf_check(void)
}
}
// Battery failsafe check
// Check the battery voltage and remaining capacity
void Sub::failsafe_battery_check(void)
// Battery failsafe handler
void Sub::handle_battery_failsafe(const char* type_str, const int8_t action)
{
// Do nothing if the failsafe is disabled, or if we are disarmed
if (g.failsafe_battery_enabled == FS_BATT_DISABLED || !motors.armed()) {
failsafe.battery = false;
AP_Notify::flags.failsafe_battery = false;
return; // Failsafe disabled, nothing to do
}
if (!battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
failsafe.battery = false;
AP_Notify::flags.failsafe_battery = false;
return; // Battery is doing well
}
// Always warn when failsafe condition is met
if (AP_HAL::millis() > failsafe.last_battery_warn_ms + 20000) {
failsafe.last_battery_warn_ms = AP_HAL::millis();
gcs().send_text(MAV_SEVERITY_WARNING, "Low battery");
}
// Don't do anything if failsafe has already been set
if (failsafe.battery) {
return;
}
failsafe.battery = true;
AP_Notify::flags.failsafe_battery = true;
// Log failsafe
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
switch(g.failsafe_battery_enabled) {
case FS_BATT_SURFACE:
set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE);
break;
case FS_BATT_DISARM:
init_disarm_motors();
break;
default:
break;
switch((Failsafe_Action)action) {
case Failsafe_Action_Surface:
set_mode(SURFACE, MODE_REASON_BATTERY_FAILSAFE);
break;
case Failsafe_Action_Disarm:
init_disarm_motors();
break;
case Failsafe_Action_Warn:
case Failsafe_Action_None:
break;
}
}

19
ArduSub/sensors.cpp
View File

@ -162,25 +162,6 @@ void Sub::update_optical_flow(void)
}
#endif // OPTFLOW == ENABLED
// read_battery - check battery voltage and current and invoke failsafe if necessary
// called at 10hz
void Sub::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()) {
motors.set_current(battery.current_amps());
compass.set_current(battery.current_amps());
}
failsafe_battery_check();
}
void Sub::compass_cal_update()
{
if (!hal.util->get_soft_armed()) {