From 222009002e6424ffad17602fcb9bde81827c20c8 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 2 Apr 2012 09:24:05 +1000
Subject: [PATCH] MAVLink: allow for find grained stream rate control
streams can now be requested at any multiple of 20ms. So if you ask
for a stream at 7Hz then you will get it at close to 7Hz.
---
ArduPlane/ArduPlane.pde | 11 +---
ArduPlane/GCS.h | 34 ++++++----
ArduPlane/GCS_Mavlink.pde | 127 +++++++++++++++++++++++---------------
ArduPlane/planner.pde | 5 +-
4 files changed, 101 insertions(+), 76 deletions(-)
diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index 8235efef7..0b0efbf26 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -749,11 +749,8 @@ static void fast_loop()
// ------------------------------
set_servos();
-
- // XXX is it appropriate to be doing the comms below on the fast loop?
-
gcs_update();
- gcs_data_stream_send(45,1000);
+ gcs_data_stream_send();
}
static void medium_loop()
@@ -854,10 +851,6 @@ Serial.println(tempaccel.z, DEC);
if (g.log_bitmask & MASK_LOG_GPS)
Log_Write_GPS(g_gps->time, current_loc.lat, current_loc.lng, g_gps->altitude, current_loc.alt, (long) g_gps->ground_speed, g_gps->ground_course, g_gps->fix, g_gps->num_sats);
-
- // send all requested output streams with rates requested
- // between 5 and 45 Hz
- gcs_data_stream_send(5,45);
break;
// This case controls the slow loop
@@ -919,7 +912,6 @@ static void slow_loop()
update_events();
mavlink_system.sysid = g.sysid_this_mav; // This is just an ugly hack to keep mavlink_system.sysid sync'd with our parameter
- gcs_data_stream_send(3,5);
#if USB_MUX_PIN > 0
check_usb_mux();
@@ -936,7 +928,6 @@ static void one_second_loop()
// send a heartbeat
gcs_send_message(MSG_HEARTBEAT);
- gcs_data_stream_send(1,3);
}
static void update_GPS(void)
diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h
index 6a1fc4182..f3eee5bab 100644
--- a/ArduPlane/GCS.h
+++ b/ArduPlane/GCS.h
@@ -76,16 +76,8 @@ public:
///
void send_text(gcs_severity severity, const prog_char_t *str) {}
- // test if frequency within range requested for loop
- // used by data_stream_send
- static bool freqLoopMatch(uint16_t freq, uint16_t freqMin, uint16_t freqMax)
- {
- if (freq != 0 && freq >= freqMin && freq < freqMax) return true;
- else return false;
- }
-
// send streams which match frequency range
- void data_stream_send(uint16_t freqMin, uint16_t freqMax);
+ void data_stream_send(void);
// set to true if this GCS link is active
bool initialised;
@@ -118,12 +110,28 @@ public:
void send_message(enum ap_message id);
void send_text(gcs_severity severity, const char *str);
void send_text(gcs_severity severity, const prog_char_t *str);
- void data_stream_send(uint16_t freqMin, uint16_t freqMax);
+ void data_stream_send(void);
void queued_param_send();
void queued_waypoint_send();
static const struct AP_Param::GroupInfo var_info[];
+ // NOTE! The streams enum below and the
+ // set of AP_Int16 stream rates _must_ be
+ // kept in the same order
+ enum streams {STREAM_RAW_SENSORS,
+ STREAM_EXTENDED_STATUS,
+ STREAM_RC_CHANNELS,
+ STREAM_RAW_CONTROLLER,
+ STREAM_POSITION,
+ STREAM_EXTRA1,
+ STREAM_EXTRA2,
+ STREAM_EXTRA3,
+ NUM_STREAMS};
+
+ // see if we should send a stream now. Called at 50Hz
+ bool stream_trigger(enum streams stream_num);
+
private:
void handleMessage(mavlink_message_t * msg);
@@ -168,7 +176,8 @@ private:
uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_timeout; // milliseconds
- // data stream rates
+ // data stream rates. The code assumes that
+ // streamRateRawSensors is the first
AP_Int16 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels;
@@ -177,6 +186,9 @@ private:
AP_Int16 streamRateExtra1;
AP_Int16 streamRateExtra2;
AP_Int16 streamRateExtra3;
+
+ // number of 50Hz ticks until we next send this stream
+ uint8_t stream_ticks[NUM_STREAMS];
};
#endif // __GCS_H
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 3d7e2d35b..c8cd783c1 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -886,64 +886,89 @@ GCS_MAVLINK::update(void)
}
}
+// see if we should send a stream now. Called at 50Hz
+bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
+{
+ AP_Int16 *stream_rates = &streamRateRawSensors;
+ uint8_t rate = (uint8_t)stream_rates[stream_num].get();
+
+ 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;
+ return true;
+ }
+
+ // count down at 50Hz
+ stream_ticks[stream_num]--;
+ return false;
+}
+
void
-GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
+GCS_MAVLINK::data_stream_send(void)
{
- if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
+ if (waypoint_sending || waypoint_receiving || _queued_parameter != NULL) {
+ // don't send the streams when transferring bulk data
+ return;
+ }
- if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
- send_message(MSG_RAW_IMU1);
- send_message(MSG_RAW_IMU2);
- send_message(MSG_RAW_IMU3);
- }
+ if (stream_trigger(STREAM_RAW_SENSORS)) {
+ send_message(MSG_RAW_IMU1);
+ send_message(MSG_RAW_IMU2);
+ send_message(MSG_RAW_IMU3);
+ }
- if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
- send_message(MSG_EXTENDED_STATUS1);
- send_message(MSG_EXTENDED_STATUS2);
- send_message(MSG_CURRENT_WAYPOINT);
- send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
- send_message(MSG_NAV_CONTROLLER_OUTPUT);
- send_message(MSG_FENCE_STATUS);
-
- if (last_gps_satellites != g_gps->num_sats) {
- // this message is mostly a huge waste of bandwidth,
- // except it is the only message that gives the number
- // of visible satellites. So only send it when that
- // changes.
- send_message(MSG_GPS_STATUS);
- last_gps_satellites = g_gps->num_sats;
- }
- }
+ if (stream_trigger(STREAM_EXTENDED_STATUS)) {
+ send_message(MSG_EXTENDED_STATUS1);
+ send_message(MSG_EXTENDED_STATUS2);
+ send_message(MSG_CURRENT_WAYPOINT);
+ send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
+ send_message(MSG_NAV_CONTROLLER_OUTPUT);
+ send_message(MSG_FENCE_STATUS);
+
+ if (last_gps_satellites != g_gps->num_sats) {
+ // this message is mostly a huge waste of bandwidth,
+ // except it is the only message that gives the number
+ // of visible satellites. So only send it when that
+ // changes.
+ send_message(MSG_GPS_STATUS);
+ last_gps_satellites = g_gps->num_sats;
+ }
+ }
- if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
- // sent with GPS read
- send_message(MSG_LOCATION);
- }
+ if (stream_trigger(STREAM_POSITION)) {
+ // sent with GPS read
+ send_message(MSG_LOCATION);
+ }
- if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) {
- // This is used for HIL. Do not change without discussing with HIL maintainers
- send_message(MSG_SERVO_OUT);
- }
+ if (stream_trigger(STREAM_RAW_CONTROLLER)) {
+ send_message(MSG_SERVO_OUT);
+ }
- if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
- send_message(MSG_RADIO_OUT);
- send_message(MSG_RADIO_IN);
- }
+ if (stream_trigger(STREAM_RC_CHANNELS)) {
+ send_message(MSG_RADIO_OUT);
+ send_message(MSG_RADIO_IN);
+ }
- if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
- send_message(MSG_ATTITUDE);
- send_message(MSG_SIMSTATE);
- }
+ if (stream_trigger(STREAM_EXTRA1)) {
+ send_message(MSG_ATTITUDE);
+ send_message(MSG_SIMSTATE);
+ }
- if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
- send_message(MSG_VFR_HUD);
- }
+ if (stream_trigger(STREAM_EXTRA2)) {
+ send_message(MSG_VFR_HUD);
+ }
- if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
- send_message(MSG_AHRS);
- send_message(MSG_HWSTATUS);
- }
- }
+ if (stream_trigger(STREAM_EXTRA3)) {
+ send_message(MSG_AHRS);
+ send_message(MSG_HWSTATUS);
+ }
}
@@ -2157,11 +2182,11 @@ static void gcs_send_message(enum ap_message id)
/*
send data streams in the given rate range on both links
*/
-static void gcs_data_stream_send(uint16_t freqMin, uint16_t freqMax)
+static void gcs_data_stream_send(void)
{
- gcs0.data_stream_send(freqMin, freqMax);
+ gcs0.data_stream_send();
if (gcs3.initialised) {
- gcs3.data_stream_send(freqMin, freqMax);
+ gcs3.data_stream_send();
}
}
diff --git a/ArduPlane/planner.pde b/ArduPlane/planner.pde
index 94ef061e4..86147133d 100644
--- a/ArduPlane/planner.pde
+++ b/ArduPlane/planner.pde
@@ -41,11 +41,8 @@ planner_gcs(uint8_t argc, const Menu::arg *argv)
read_radio();
- gcs_data_stream_send(45,1000);
- if ((loopcount % 5) == 0) // 10 hz
- gcs_data_stream_send(5,45);
+ gcs_data_stream_send();
if ((loopcount % 16) == 0) { // 3 hz
- gcs_data_stream_send(1,5);
gcs_send_message(MSG_HEARTBEAT);
}
loopcount++;
--
GitLab