mirror of https://github.com/ArduPilot/ardupilot
AP_Frsky_Telem: frame 0x5007 refactoring
This commit is contained in:
parent
0536be1c88
commit
592fba7416
|
@ -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);
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
|
@ -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
|
||||
|
|
Loading…
Reference in New Issue