AP_Frsky_Telem: frame 0x5007 refactoring

This commit is contained in:
yaapu 2021-03-05 16:10:02 +01:00 committed by Peter Barker
parent 0536be1c88
commit 592fba7416
2 changed files with 12 additions and 20 deletions

View File

@ -407,35 +407,26 @@ bool AP_Frsky_SPort_Passthrough::get_next_msg_chunk(void)
*/
uint32_t AP_Frsky_SPort_Passthrough::calc_param(void)
{
const AP_BattMonitor &_battery = AP::battery();
uint8_t param_id = _paramID; //cache it because it gets changed inside the switch
uint32_t param_value = 0;
uint32_t param = 0;
uint8_t last_param = AP::battery().num_instances() > 1 ? BATT_CAPACITY_2 : BATT_CAPACITY_1;
// cycle through paramIDs
if (_paramID >= last_param) {
_paramID = 0;
}
_paramID++;
switch (_paramID) {
case NONE:
case FRAME_TYPE:
param = gcs().frame_type(); // see MAV_TYPE in Mavlink definition file common.h
param_value = gcs().frame_type(); // see MAV_TYPE in Mavlink definition file common.h
_paramID = BATT_CAPACITY_1;
break;
case BATT_FS_VOLTAGE: // was used to send the battery failsafe voltage, lend slot to next param
case BATT_FS_CAPACITY: // was used to send the battery failsafe capacity in mAh, lend slot to next param
case BATT_CAPACITY_1:
_paramID = 4;
param = (uint32_t)roundf(_battery.pack_capacity_mah(0)); // battery pack capacity in mAh
param_value = (uint32_t)roundf(AP::battery().pack_capacity_mah(0)); // battery pack capacity in mAh
_paramID = AP::battery().num_instances() > 1 ? BATT_CAPACITY_2 : FRAME_TYPE;
break;
case BATT_CAPACITY_2:
param = (uint32_t)roundf(_battery.pack_capacity_mah(1)); // battery pack capacity in mAh
param_value = (uint32_t)roundf(AP::battery().pack_capacity_mah(1)); // battery pack capacity in mAh
_paramID = FRAME_TYPE;
break;
}
//Reserve first 8 bits for param ID, use other 24 bits to store parameter value
param = (_paramID << PARAM_ID_OFFSET) | (param & PARAM_VALUE_LIMIT);
return param;
return (param_id << PARAM_ID_OFFSET) | (param_value & PARAM_VALUE_LIMIT);
}
/*

View File

@ -85,11 +85,12 @@ private:
AP_Frsky_Parameters *&_frsky_parameters;
enum PassthroughParam : uint8_t {
NONE = 0,
FRAME_TYPE = 1,
BATT_FS_VOLTAGE = 2,
BATT_FS_CAPACITY = 3,
BATT_CAPACITY_1 = 4,
BATT_CAPACITY_2 = 5
BATT_CAPACITY_2 = 5,
};
// methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format