From 291531e05687e647999dde9c6dc857dd51b90037 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Wed, 28 Feb 2018 16:32:16 -0700 Subject: [PATCH] Copter: Support new battery failsafes --- ArduCopter/AP_Arming.cpp | 17 +--------- ArduCopter/AP_State.cpp | 7 ----- ArduCopter/ArduCopter.cpp | 3 +- ArduCopter/Copter.h | 54 +++++++++++++++++++++++--------- ArduCopter/GCS_Mavlink.cpp | 2 +- ArduCopter/GCS_Mavlink.h | 2 +- ArduCopter/Log.cpp | 2 +- ArduCopter/Parameters.cpp | 27 +++------------- ArduCopter/Parameters.h | 10 ++---- ArduCopter/compassmot.cpp | 6 ++-- ArduCopter/config.h | 8 ----- ArduCopter/defines.h | 7 ----- ArduCopter/events.cpp | 64 ++++++++++++++++++++------------------ ArduCopter/motor_test.cpp | 4 +-- ArduCopter/motors.cpp | 3 -- ArduCopter/sensors.cpp | 36 +++------------------ ArduCopter/system.cpp | 2 +- 17 files changed, 95 insertions(+), 159 deletions(-) diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index ecd5421be7..977b48a5f9 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -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; diff --git a/ArduCopter/AP_State.cpp b/ArduCopter/AP_State.cpp index a48b30667e..843071cf90 100644 --- a/ArduCopter/AP_State.cpp +++ b/ArduCopter/AP_State.cpp @@ -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) { diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 29261e67d2..f9b001949a 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -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 diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 516d833025..59498bcadd 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 1143be939a..acc0e38d96 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -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; } diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index 4ddbb84c53..e237a70983 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/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) +#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 { diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 9bd625814c..b13001c9ad 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -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)); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 3ff5285abb..79edcd9856 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -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) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 9b92825e28..60846c85e6 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -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 diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 6860477fce..dffdb84a26 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -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; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 3681ea26ff..7f6235a825 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index fcedb83065..f9fa22e0f9 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 39dac52b12..3a525266e6 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -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 diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 4d70656c31..a83df915e5 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -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(); diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 2330bdbcb1..789f7c473e 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -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 diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 1399aab5b0..b1b85fe5cf 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -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; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 0fee061582..3b19e918f8 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -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