diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 8a2d6fd9d7..4b5c26f470 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -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 diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 45e501d37e..f3ee9154c4 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -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; diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 706ed7c77b..d319e9873d 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -3,7 +3,7 @@ #include // 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 { diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 8dff3ff811..903f4621e3 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -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) diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 031afc2f46..52f67ffc66 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -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 // diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 46c1b9c0fd..9869ded917 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -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); diff --git a/ArduPlane/events.cpp b/ArduPlane/events.cpp index c0e0be63ba..f5ee41f45e 100644 --- a/ArduPlane/events.cpp +++ b/ArduPlane/events.cpp @@ -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) diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 8d6cd3f4d8..8f70356fc8 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -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; } diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp index aa6e29be9f..545fac7a80 100644 --- a/ArduPlane/system.cpp +++ b/ArduPlane/system.cpp @@ -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