Browse Source

AP_Frsky_Telem: send ap_status only once initialized

Same as what is found in GCS_MAVLink.cpp for copter
master
floaledm 8 years ago committed by Andrew Tridgell
parent
commit
1aab5eec38
  1. 3
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
  2. 1
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

3
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

@ -50,6 +50,7 @@ void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, const char *fi @@ -50,6 +50,7 @@ void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, const char *fi
_params.fs_batt_voltage = fs_batt_voltage; // failsafe battery voltage in volts
_params.fs_batt_mah = fs_batt_mah; // failsafe reserve capacity in mAh
if (ap_valuep == nullptr) { // ap bit-field
_ap.value = 0x2000; // set "initialised" to 1 for rover and plane
_ap.valuep = &_ap.value;
} else {
_ap.valuep = ap_valuep;
@ -111,8 +112,10 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void) @@ -111,8 +112,10 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
_passthrough.params_timer = AP_HAL::millis();
return;
} else if ((now - _passthrough.ap_status_timer) > 500) {
if ((*_ap.valuep & AP_INITIALIZED_FLAG) > 0) { // send ap status only once vehicle has been initialised
send_uint32(DIY_FIRST_ID+1, calc_ap_status());
_passthrough.ap_status_timer = AP_HAL::millis();
}
return;
} else if ((now - _passthrough.batt_timer) > 1000) {
send_uint32(DIY_FIRST_ID+3, calc_batt());

1
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

@ -91,6 +91,7 @@ for FrSky SPort Passthrough @@ -91,6 +91,7 @@ for FrSky SPort Passthrough
#define AP_SSIMPLE_FLAGS 0x6
#define AP_SSIMPLE_OFFSET 4
#define AP_ISFLYING_FLAG 0x80
#define AP_INITIALIZED_FLAG 0x2000
#define AP_ARMED_OFFSET 8
#define AP_BATT_FS_OFFSET 9
#define AP_EKF_FS_OFFSET 10

Loading…
Cancel
Save