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: fix *_ap.value issue for Plane

Browse Source 
set_is_flying does operations on a nullptr, plus we loose the ability to
update ap_status accordingly for Plane
This commit is contained in:
floaledm 2016-09-26 12:35:47 -05:00
parent 620cd06347
commit 5e022bf631
2 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -35,7 +35,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, AP_Float *fs_batt_voltage, AP_Float *fs_batt_mah, uint32_t *ap_value)
void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, const char *firmware_str, const uint8_t mav_type, AP_Float *fs_batt_voltage, AP_Float *fs_batt_mah, 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))) {
@ -49,7 +49,11 @@ void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, const char *fi
_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
_ap.value = ap_value; // ap bit-field
if (ap_valuep == nullptr) { // ap bit-field
_ap.valuep = &_ap.value;
} else {
_ap.valuep = ap_valuep;
}
}
if (_port != NULL) {
@ -634,12 +638,10 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void)
// control/flight mode number (limit to 31 (0x1F) since the value is stored on 5 bits)
ap_status = (uint8_t)((_ap.control_mode+1) & AP_CONTROL_MODE_LIMIT);
if (_ap.value != nullptr) {
// simple/super simple modes flags
ap_status |= (uint8_t)(*_ap.value & AP_SSIMPLE_FLAGS)<<AP_SSIMPLE_OFFSET;
// is_flying flag
ap_status |= (uint8_t)((*_ap.value & AP_ISFLYING_FLAG) ^ AP_ISFLYING_FLAG);
}
// simple/super simple modes flags
ap_status |= (uint8_t)(*_ap.valuep & AP_SSIMPLE_FLAGS)<<AP_SSIMPLE_OFFSET;
// is_flying flag
ap_status |= (uint8_t)((*_ap.valuep & AP_ISFLYING_FLAG) ^ AP_ISFLYING_FLAG);
// armed flag
ap_status |= (uint8_t)(AP_Notify::flags.armed)<<AP_ARMED_OFFSET;
// battery failsafe flag

7
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -118,7 +118,7 @@ public:
AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng);
// init - perform required initialisation
void init(const AP_SerialManager &serial_manager, const char *firmware_str, const uint8_t mav_type, AP_Float *fs_batt_voltage = nullptr, AP_Float *fs_batt_mah = nullptr, uint32_t *ap_value = nullptr);
void init(const AP_SerialManager &serial_manager, const char *firmware_str, const uint8_t mav_type, AP_Float *fs_batt_voltage = nullptr, AP_Float *fs_batt_mah = nullptr, uint32_t *ap_valuep = nullptr);
// add statustext message to FrSky lib message queue
void queue_message(MAV_SEVERITY severity, const char *text);
@ -127,7 +127,7 @@ public:
void update_control_mode(uint8_t mode) { _ap.control_mode = mode; }
// update whether we're flying (used for Plane)
void set_is_flying(bool is_flying) { *_ap.value == is_flying ? (*_ap.value | AP_ISFLYING_FLAG) : (*_ap.value & (~AP_ISFLYING_FLAG)); }
void set_is_flying(bool is_flying) { _ap.value == is_flying ? (_ap.value | AP_ISFLYING_FLAG) : (_ap.value & (~AP_ISFLYING_FLAG)); }
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
@ -157,7 +157,8 @@ private:
struct
{
uint8_t control_mode;
uint32_t *value;
uint32_t value;
uint32_t *valuep;
uint32_t sensor_status_flags;
} _ap;