diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index e42fcf46c6..9734056091 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -567,13 +567,13 @@ uint32_t AP_Frsky_Telem::calc_param(void) uint32_t param = 0; // cycle through paramIDs - if (_paramID >= 4) { + if (_paramID >= 5) { _paramID = 0; } _paramID++; switch(_paramID) { case 1: - param = _params.mav_type; // frame type (see MAV_TYPE in Mavlink definition file common.h) + param = _params.mav_type; // frame type (see MAV_TYPE in Mavlink definition file common.h) break; case 2: if (_params.fs_batt_voltage != nullptr) { @@ -582,15 +582,18 @@ uint32_t AP_Frsky_Telem::calc_param(void) break; case 3: if (_params.fs_batt_mah != nullptr) { - param = (uint32_t)roundf((*_params.fs_batt_mah)); // battery failsafe capacity in mAh + param = (uint32_t)roundf((*_params.fs_batt_mah)); // battery failsafe capacity in mAh } break; case 4: - param = (uint32_t)roundf(_battery.pack_capacity_mah()); // battery pack capacity in mAh as configured by user + param = (uint32_t)roundf(_battery.pack_capacity_mah(0)); // battery pack capacity in mAh + break; + case 5: + param = (uint32_t)roundf(_battery.pack_capacity_mah(1)); // battery pack capacity in mAh break; } //Reserve first 8 bits for param ID, use other 24 bits to store parameter value - param = (_paramID << 24) | (param & 0xFFFFFF); + param = (_paramID << PARAM_ID_OFFSET) | (param & PARAM_VALUE_LIMIT); return param; } diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 257604fe1a..8c51ad074f 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -71,6 +71,9 @@ for FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers) for FrSky SPort Passthrough */ // data bits preparation +// for parameter data +#define PARAM_ID_OFFSET 24 +#define PARAM_VALUE_LIMIT 0xFFFFFF // for gps status data #define GPS_SATS_LIMIT 0xF #define GPS_STATUS_LIMIT 0x3