Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: reworked _statustext_queue.empty() check

This commit is contained in:
floaledm 2016-09-26 14:06:11 -05:00 committed by Tom Pittenger
parent 8001cc403b
commit fcfaa98bb6
2 changed files with 33 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
View File

@ -101,8 +101,8 @@ void AP_Frsky_Telem::send_SPort_Passthrough(void)
// build mavlink message queue for sensor_status_flags // build mavlink message queue for sensor_status_flags
check_sensor_status_flags(); check_sensor_status_flags();
// if there's any message in the queue, start sending them chunk by chunk; three times each chunk // if there's any message in the queue, start sending them chunk by chunk; three times each chunk
if (!_statustext_queue.empty()) { if (get_next_msg_chunk()) {
send_uint32(DIY_FIRST_ID, get_next_msg_chunk()); send_uint32(DIY_FIRST_ID, _msg_chunk.chunk);
return; return;
} }
// send other sensor data if it's time for them, and reset the corresponding timer if sent // send other sensor data if it's time for them, and reset the corresponding timer if sent
@ -416,11 +416,14 @@ void AP_Frsky_Telem::send_uint16(uint16_t id, uint16_t data)
} }
/* /*
* grabs one "chunk" (4 bytes) of the mavlink statustext message to be transmitted * grabs one "chunk" (4 bytes) of the queued message to be transmitted
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers) * for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/ */
uint32_t AP_Frsky_Telem::get_next_msg_chunk(void) bool AP_Frsky_Telem::get_next_msg_chunk(void)
{ {
if (_statustext_queue.empty()) {
return false;
} else {
if (_msg_chunk.repeats == 0) { if (_msg_chunk.repeats == 0) {
_msg_chunk.chunk = 0; _msg_chunk.chunk = 0;
uint8_t character = _statustext_queue[0]->text[_msg_chunk.char_index++]; uint8_t character = _statustext_queue[0]->text[_msg_chunk.char_index++];
@ -454,7 +457,8 @@ uint32_t AP_Frsky_Telem::get_next_msg_chunk(void)
_statustext_queue.remove(0); _statustext_queue.remove(0);
} }
} }
return _msg_chunk.chunk; }
return true;
} }
/* /*

4
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
View File

@ -215,7 +215,7 @@ private:
struct struct
{ {
uint32_t chunk; // a "chunk" (four characters/bytes) at a time of the mavlink message to be sent uint32_t chunk; // a "chunk" (four characters/bytes) at a time of the queued message to be sent
uint8_t repeats; // send each message "chunk" 3 times to make sure the entire messsage gets through without getting cut uint8_t repeats; // send each message "chunk" 3 times to make sure the entire messsage gets through without getting cut
uint8_t char_index; // index of which character to get in the message uint8_t char_index; // index of which character to get in the message
} _msg_chunk; } _msg_chunk;
@ -237,7 +237,7 @@ private:
void send_uint16(uint16_t id, uint16_t data); void send_uint16(uint16_t id, uint16_t data);
// methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
uint32_t get_next_msg_chunk(void); bool get_next_msg_chunk(void);
void check_sensor_status_flags(void); void check_sensor_status_flags(void);
uint32_t calc_param(void); uint32_t calc_param(void);
uint32_t calc_gps_latlng(bool *send_latitude); uint32_t calc_gps_latlng(bool *send_latitude);