Copter: Support new battery failsafes
This commit is contained in:
parent
7405bb7b85
commit
291531e056
@ -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;
|
||||
|
@ -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)
|
||||
{
|
||||
|
@ -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
|
||||
|
@ -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);
|
||||
|
@ -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;
|
||||
}
|
||||
|
||||
|
@ -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
|
||||
{
|
||||
|
@ -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));
|
||||
|
@ -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)
|
||||
|
@ -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
|
||||
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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,39 +48,41 @@ 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;
|
||||
}
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_BATT, ERROR_CODE_FAILSAFE_OCCURRED);
|
||||
|
||||
// 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
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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_gcs_check - check for ground station failsafe
|
||||
void Copter::failsafe_gcs_check()
|
||||
{
|
||||
|
@ -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();
|
||||
|
||||
|
@ -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
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
Loading…
Reference in New Issue
Block a user