diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index 9c68f749da..cca3526b17 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -36,7 +36,7 @@ AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, con /* * init - perform required initialisation */ -void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, const char *firmware_str, const uint8_t mav_type, const AP_Float *fs_batt_voltage, const AP_Float *fs_batt_mah, const uint32_t *ap_valuep) +void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, const char *firmware_str, const uint8_t mav_type, const uint32_t *ap_valuep) { // check for protocol configured for a serial port - only the first serial port with one of these protocols will then run (cannot have FrSky on multiple serial ports) if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_D, 0))) { @@ -51,8 +51,6 @@ void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, const char *fi queue_message(MAV_SEVERITY_INFO, firmware_str); // save main parameters locally _params.mav_type = mav_type; // frame type (see MAV_TYPE in Mavlink definition file common.h) - _params.fs_batt_voltage = fs_batt_voltage; // failsafe battery voltage in volts - _params.fs_batt_mah = fs_batt_mah; // failsafe reserve capacity in mAh if (ap_valuep == nullptr) { // ap bit-field _ap.value = 0x2000; // set "initialised" to 1 for rover and plane _ap.valuep = &_ap.value; @@ -582,15 +580,8 @@ uint32_t AP_Frsky_Telem::calc_param(void) case 1: param = _params.mav_type; // frame type (see MAV_TYPE in Mavlink definition file common.h) break; - case 2: - if (_params.fs_batt_voltage != nullptr) { - param = (uint32_t)roundf((*_params.fs_batt_voltage) * 100.0f); // battery failsafe voltage in centivolts - } - break; - case 3: - if (_params.fs_batt_mah != nullptr) { - param = (uint32_t)roundf((*_params.fs_batt_mah)); // battery failsafe capacity in mAh - } + case 2: // was used to send the battery failsafe voltage + case 3: // was used to send the battery failsafe capacity in mAh break; case 4: param = (uint32_t)roundf(_battery.pack_capacity_mah(0)); // battery pack capacity in mAh diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 5de86760a3..fb9ad67ff9 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -120,7 +120,7 @@ public: AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete; // init - perform required initialisation - void init(const AP_SerialManager &serial_manager, const char *firmware_str, const uint8_t mav_type, const AP_Float *fs_batt_voltage = nullptr, const AP_Float *fs_batt_mah = nullptr, const uint32_t *ap_valuep = nullptr); + void init(const AP_SerialManager &serial_manager, const char *firmware_str, const uint8_t mav_type, const uint32_t *ap_valuep = nullptr); // add statustext message to FrSky lib message queue void queue_message(MAV_SEVERITY severity, const char *text); @@ -153,8 +153,6 @@ private: struct { uint8_t mav_type; // frame type (see MAV_TYPE in Mavlink definition file common.h) - const AP_Float *fs_batt_voltage; // failsafe battery voltage in volts - const AP_Float *fs_batt_mah; // failsafe reserve capacity in mAh } _params; struct