Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 00:13:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Support new battery failsafes

Browse Source
This commit is contained in:
Michael du Breuil 2017-11-09 15:34:12 -07:00 committed by Francisco Ferreira
parent 3f581d0479
commit 7405bb7b85
9 changed files with 63 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.cpp
View File

@ -52,7 +52,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK(gcs_update, 50, 500),
SCHED_TASK(gcs_data_stream_send, 50, 500),
SCHED_TASK(update_events, 50, 150),
SCHED_TASK(read_battery, 10, 300),
SCHED_TASK_CLASS(AP_BattMonitor, &plane.battery, read, 10, 300),
SCHED_TASK(compass_accumulate, 50, 200),
SCHED_TASK(barometer_accumulate, 50, 150),
SCHED_TASK(update_notify, 50, 300),
@ -91,6 +91,8 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
#endif
};
constexpr int8_t Plane::_failsafe_priorities[5];
#if STATS_ENABLED == ENABLED
/*
update AP_Stats

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -8,7 +8,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan)
uint8_t system_status;
uint32_t custom_mode = control_mode;
if (failsafe.state != FAILSAFE_NONE || failsafe.low_battery || failsafe.adsb) {
if (failsafe.state != FAILSAFE_NONE || battery.has_failsafed() || failsafe.adsb) {
system_status = MAV_STATE_CRITICAL;
} else if (plane.crash_state.is_crashed) {
system_status = MAV_STATE_EMERGENCY;

2
ArduPlane/GCS_Mavlink.h
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 | MAV_SYS_STATUS_SENSOR_RC_RECEIVER)
#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_RC_RECEIVER | MAV_SYS_STATUS_SENSOR_BATTERY)
class GCS_MAVLINK_Plane : public GCS_MAVLINK
{

20
ArduPlane/Parameters.cpp
View File

@ -530,22 +530,6 @@ const AP_Param::Info Plane::var_info[] = {
// @User: Standard
GSCALAR(fs_timeout_long, "FS_LONG_TIMEOUT", 5),
// @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 continuously for 10 seconds then the plane will switch to RTL mode.
// @Units: V
// @Increment: 0.1
// @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 switch to RTL mode immediately.
// @Units: mA.h
// @Increment: 50
// @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 FS_LONG_TIMEOUT seconds of no MAVLink heartbeat messages. There are two possible enabled settings. Seeing FS_GCS_ENABL to 1 means that GCS failsafe will be triggered when the aircraft has not received a MAVLink HEARTBEAT message. Setting FS_GCS_ENABL to 2 means that GCS failsafe will be triggered on either a loss of HEARTBEAT messages, or a RADIO_STATUS message from a MAVLink enabled 3DR radio indicating that the ground station is not receiving status updates from the aircraft, which is indicated by the RADIO_STATUS.remrssi field being zero (this may happen if you have a one way link due to asymmetric noise on the ground station and aircraft radios).Setting FS_GCS_ENABL to 3 means that GCS failsafe will be triggered by Heartbeat(like option one), but only in AUTO mode. 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 you should enable ARMING_REQUIRED.
@ -1270,6 +1254,10 @@ const AP_Param::ConversionInfo conversion_table[] = {
{ Parameters::k_param_land_abort_throttle_enable,0,AP_PARAM_INT8, "LAND_ABORT_THR" },
{ Parameters::k_param_land_flap_percent, 0, AP_PARAM_INT8, "LAND_FLAP_PERCENT" },
// battery failsafes
{ 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" },
};
void Plane::load_parameters(void)

6
ArduPlane/Parameters.h
View File

@ -272,8 +272,8 @@ public:
k_param_throttle_suppress_manual,
k_param_throttle_passthru_stabilize,
k_param_rc_12_old,
k_param_fs_batt_voltage,
k_param_fs_batt_mah,
k_param_fs_batt_voltage, // unused - moved to AP_BattMonitor
k_param_fs_batt_mah, // unused - moved to AP_BattMonitor
k_param_fs_timeout_short,
k_param_fs_timeout_long,
k_param_rc_13_old,
@ -430,8 +430,6 @@ public:
AP_Float fs_timeout_short;
AP_Float fs_timeout_long;
AP_Int8 gcs_heartbeat_fs_enabled;
AP_Float fs_batt_voltage;
AP_Float fs_batt_mah;
// Flight modes
//

28
ArduPlane/Plane.h
View File

@ -329,9 +329,6 @@ private:
// has the saved mode for failsafe been set?
bool saved_mode_set:1;
// flag to hold whether battery low voltage threshold has been breached
bool low_battery:1;
// true if an adsb related failsafe has occurred
bool adsb:1;
@ -392,7 +389,9 @@ private:
int32_t altitude_error_cm;
// Battery Sensors
AP_BattMonitor battery{MASK_LOG_CURRENT};
AP_BattMonitor battery{MASK_LOG_CURRENT,
FUNCTOR_BIND_MEMBER(&Plane::handle_battery_failsafe, void, const char*, const int8_t),
_failsafe_priorities};
#if FRSKY_TELEM_ENABLED == ENABLED
// FrSky telemetry support
@ -879,7 +878,7 @@ private:
void failsafe_long_on_event(enum failsafe_state fstype, mode_reason_t reason);
void failsafe_short_off_event(mode_reason_t reason);
void failsafe_long_off_event(mode_reason_t reason);
void low_battery_event(void);
void handle_battery_failsafe(const char* type_str, const int8_t action);
void update_events(void);
uint8_t max_fencepoints(void);
Vector2l get_fence_point_with_index(unsigned i);
@ -932,7 +931,6 @@ private:
void read_rangefinder(void);
void read_airspeed(void);
void zero_airspeed(bool in_startup);
void read_battery(void);
void read_receiver_rssi(void);
void rpm_update(void);
void button_update(void);
@ -1065,6 +1063,24 @@ private:
bool avoid_adsb_init(bool ignore_checks);
void avoid_adsb_run();
enum Failsafe_Action {
Failsafe_Action_None = 0,
Failsafe_Action_RTL = 1,
Failsafe_Action_Land = 2,
Failsafe_Action_Terminate = 3
};
// list of priorities, highest priority first
static constexpr int8_t _failsafe_priorities[] = {
Failsafe_Action_Terminate,
Failsafe_Action_Land,
Failsafe_Action_RTL,
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 failsafe_check(void);

38
ArduPlane/events.cpp
View File

@ -138,19 +138,35 @@ void Plane::failsafe_long_off_event(mode_reason_t reason)
failsafe.state = FAILSAFE_NONE;
}
void Plane::low_battery_event(void)
void Plane::handle_battery_failsafe(const char *type_str, const int8_t action)
{
if (failsafe.low_battery) {
return;
switch ((Failsafe_Action)action) {
case Failsafe_Action_Land:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
// never stop a landing if we were already committed
if (plane.mission.jump_to_landing_sequence()) {
plane.set_mode(AUTO, MODE_REASON_UNKNOWN);
break;
}
}
FALLTHROUGH;
case Failsafe_Action_RTL:
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
// never stop a landing if we were already committed
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();
}
break;
case Failsafe_Action_Terminate:
char battery_type_str[17];
snprintf(battery_type_str, 17, "%s battery", type_str);
afs.gcs_terminate(true, battery_type_str);
break;
case Failsafe_Action_None:
// don't actually do anything, however we should still flag the system as having hit a failsafe
// and ensure all appropriate flags are going off to the user
break;
}
gcs().send_text(MAV_SEVERITY_WARNING, "Low battery %.2fV used %.0f mAh",
(double)battery.voltage(), (double)battery.consumed_mah());
if (flight_stage != AP_Vehicle::FixedWing::FLIGHT_LAND) {
set_mode(RTL, MODE_REASON_BATTERY_FAILSAFE);
aparm.throttle_cruise.load();
}
failsafe.low_battery = true;
AP_Notify::flags.failsafe_battery = true;
}
void Plane::update_events(void)

21
ArduPlane/sensors.cpp
View File

@ -116,19 +116,6 @@ void Plane::zero_airspeed(bool in_startup)
gcs().send_text(MAV_SEVERITY_INFO,"Airspeed calibration started");
}
// read_battery - reads battery voltage and current and invokes failsafe
// should be called at 10hz
void Plane::read_battery(void)
{
battery.read();
compass.set_current(battery.current_amps());
if (hal.util->get_soft_armed() &&
battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah)) {
low_battery_event();
}
}
// read the receiver RSSI as an 8 bit number for MAVLink
// RC_CHANNELS_SCALED message
void Plane::read_receiver_rssi(void)
@ -200,10 +187,6 @@ void Plane::update_sensor_status_flags(void)
control_sensors_present |= MAV_SYS_STATUS_LOGGING;
}
if (plane.battery.healthy()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_BATTERY;
}
// all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control, geofence, motor, and battery output which we will set individually
control_sensors_enabled = control_sensors_present & (~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_GEOFENCE & ~MAV_SYS_STATUS_LOGGING & ~MAV_SYS_STATUS_SENSOR_BATTERY);
@ -219,7 +202,7 @@ void Plane::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;
}
@ -370,7 +353,7 @@ void Plane::update_sensor_status_flags(void)
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
if (plane.failsafe.low_battery) {
if (!plane.battery.healthy() || plane.battery.has_failsafed()) {
control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_BATTERY;
}

3
ArduPlane/system.cpp
View File

@ -117,8 +117,7 @@ void Plane::init_ardupilot()
#if FRSKY_TELEM_ENABLED == ENABLED
// setup frsky, and pass a number of parameters to the library
frsky_telemetry.init(serial_manager, fwver.fw_string,
MAV_TYPE_FIXED_WING,
&g.fs_batt_voltage, &g.fs_batt_mah);
MAV_TYPE_FIXED_WING);
#endif
#if LOGGING_ENABLED == ENABLED