Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: frame 0x5007 refactoring

Browse Source
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
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) 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) { switch (_paramID) {
case NONE:
case FRAME_TYPE: 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; 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: case BATT_CAPACITY_1:
_paramID = 4; param_value = (uint32_t)roundf(AP::battery().pack_capacity_mah(0)); // battery pack capacity in mAh
param = (uint32_t)roundf(_battery.pack_capacity_mah(0)); // battery pack capacity in mAh _paramID = AP::battery().num_instances() > 1 ? BATT_CAPACITY_2 : FRAME_TYPE;
break; break;
case BATT_CAPACITY_2: 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; break;
} }
//Reserve first 8 bits for param ID, use other 24 bits to store parameter value //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_id << PARAM_ID_OFFSET) | (param_value & PARAM_VALUE_LIMIT);
return param;
} }
/* /*

3
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h
View File

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