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

GCS_MAVLink: Use a single stream_trigger() implementation 

This has no side effects, but since all implementations were basically
the same, move the implementation to GCS_Common and the only part that
adjusts the rate based on which which stream to each individual
GCS_MAVLINK implementation.
This commit is contained in:
Leandro Pereira 2016-05-05 17:45:37 -03:00 committed by Lucas De Marchi
parent 40ea8225b6
commit f23bd7e09d
6 changed files with 42 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
APMrover2/GCS_Mavlink.cpp
View File

@ -695,38 +695,14 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
AP_GROUPEND
};
// see if we should send a stream now. Called at 50Hz
bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num)
{
if (stream_num >= NUM_STREAMS) {
return false;
}
float rate = (uint8_t)streamRates[stream_num].get();
// send at a much lower rate while handling waypoints and
// parameter sends
if ((stream_num != STREAM_PARAMS) &&
(waypoint_receiving || _queued_parameter != NULL)) {
rate *= 0.25f;
return 0.25f;
}
if (rate <= 0) {
return false;
}
if (stream_ticks[stream_num] == 0) {
// we're triggering now, setup the next trigger point
if (rate > 50) {
rate = 50;
}
stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown;
return true;
}
// count down at 50Hz
stream_ticks[stream_num]--;
return false;
return 1.0f;
}
void

30
AntennaTracker/GCS_Mavlink.cpp
View File

@ -376,35 +376,13 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
AP_GROUPEND
};
// see if we should send a stream now. Called at 50Hz
bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num)
{
if (stream_num >= NUM_STREAMS) {
return false;
}
float rate = (uint8_t)streamRates[stream_num].get();
// send at a much lower rate during parameter sends
if (_queued_parameter != NULL) {
rate *= 0.25f;
if (_queued_parameter != nullptr) {
return 0.25f;
}
if (rate <= 0) {
return false;
}
if (stream_ticks[stream_num] == 0) {
// we're triggering now, setup the next trigger point
if (rate > 50) {
rate = 50;
}
stream_ticks[stream_num] = (50 / rate) -1 + stream_slowdown;
return true;
}
// count down at 50Hz
stream_ticks[stream_num]--;
return false;
return 1.0f;
}
void

31
ArduCopter/GCS_Mavlink.cpp
View File

@ -840,38 +840,13 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
AP_GROUPEND
};
// see if we should send a stream now. Called at 50Hz
bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num)
{
if (stream_num >= NUM_STREAMS) {
return false;
}
float rate = (uint8_t)streamRates[stream_num].get();
// send at a much lower rate while handling waypoints and
// parameter sends
if ((stream_num != STREAM_PARAMS) &&
(waypoint_receiving || _queued_parameter != NULL)) {
rate *= 0.25f;
return 0.25f;
}
if (rate <= 0) {
return false;
}
if (stream_ticks[stream_num] == 0) {
// we're triggering now, setup the next trigger point
if (rate > 50) {
rate = 50;
}
stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown;
return true;
}
// count down at 50Hz
stream_ticks[stream_num]--;
return false;
return 1.0f;
}
void

28
ArduPlane/GCS_Mavlink.cpp
View File

@ -978,38 +978,16 @@ const AP_Param::GroupInfo GCS_MAVLINK::var_info[] = {
AP_GROUPEND
};
// see if we should send a stream now. Called at 50Hz
bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
float GCS_MAVLINK::adjust_rate_for_stream_trigger(enum streams stream_num)
{
if (stream_num >= NUM_STREAMS) {
return false;
}
float rate = (uint8_t)streamRates[stream_num].get();
// send at a much lower rate while handling waypoints and
// parameter sends
if ((stream_num != STREAM_PARAMS) &&
(waypoint_receiving || _queued_parameter != NULL)) {
rate *= 0.25f;
return 0.25f;
}
if (rate <= 0) {
return false;
}
if (stream_ticks[stream_num] == 0) {
// we're triggering now, setup the next trigger point
if (rate > 50) {
rate = 50;
}
stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown;
return true;
}
// count down at 50Hz
stream_ticks[stream_num]--;
return false;
return 1.0f;
}
void

2
libraries/GCS_MAVLink/GCS.h
View File

@ -200,6 +200,8 @@ public:
}
private:
float adjust_rate_for_stream_trigger(enum streams stream_num);
void handleMessage(mavlink_message_t * msg);
/// The stream we are communicating over

27
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -622,6 +622,33 @@ void GCS_MAVLINK::handle_param_set(mavlink_message_t *msg, DataFlash_Class *Data
}
}
// see if we should send a stream now. Called at 50Hz
bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
{
if (stream_num >= NUM_STREAMS) {
return false;
}
float rate = (uint8_t)streamRates[stream_num].get();
rate *= adjust_rate_for_stream_trigger(stream_num);
if (rate <= 0) {
return false;
}
if (stream_ticks[stream_num] == 0) {
// we're triggering now, setup the next trigger point
if (rate > 50) {
rate = 50;
}
stream_ticks[stream_num] = (50 / rate) - 1 + stream_slowdown;
return true;
}
// count down at 50Hz
stream_ticks[stream_num]--;
return false;
}
void
GCS_MAVLINK::send_text(MAV_SEVERITY severity, const char *str)