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.
This commit is contained in:
Andrew Tridgell 2012-04-02 09:24:05 +10:00
parent 4ddeb82b26
commit 7b50724f60
4 changed files with 101 additions and 76 deletions

View File

@ -749,11 +749,8 @@ static void fast_loop()
// ------------------------------ // ------------------------------
set_servos(); set_servos();
// XXX is it appropriate to be doing the comms below on the fast loop?
gcs_update(); gcs_update();
gcs_data_stream_send(45,1000); gcs_data_stream_send();
} }
static void medium_loop() static void medium_loop()
@ -854,10 +851,6 @@ Serial.println(tempaccel.z, DEC);
if (g.log_bitmask & MASK_LOG_GPS) 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); 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; break;
// This case controls the slow loop // This case controls the slow loop
@ -919,7 +912,6 @@ static void slow_loop()
update_events(); 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 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 #if USB_MUX_PIN > 0
check_usb_mux(); check_usb_mux();
@ -936,7 +928,6 @@ static void one_second_loop()
// send a heartbeat // send a heartbeat
gcs_send_message(MSG_HEARTBEAT); gcs_send_message(MSG_HEARTBEAT);
gcs_data_stream_send(1,3);
} }
static void update_GPS(void) static void update_GPS(void)

View File

@ -76,16 +76,8 @@ public:
/// ///
void send_text(gcs_severity severity, const prog_char_t *str) {} 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 // 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 // set to true if this GCS link is active
bool initialised; bool initialised;
@ -118,12 +110,28 @@ public:
void send_message(enum ap_message id); void send_message(enum ap_message id);
void send_text(gcs_severity severity, const char *str); void send_text(gcs_severity severity, const char *str);
void send_text(gcs_severity severity, const prog_char_t *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_param_send();
void queued_waypoint_send(); void queued_waypoint_send();
static const struct AP_Param::GroupInfo var_info[]; 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: private:
void handleMessage(mavlink_message_t * msg); void handleMessage(mavlink_message_t * msg);
@ -168,7 +176,8 @@ private:
uint16_t waypoint_send_timeout; // milliseconds uint16_t waypoint_send_timeout; // milliseconds
uint16_t waypoint_receive_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 streamRateRawSensors;
AP_Int16 streamRateExtendedStatus; AP_Int16 streamRateExtendedStatus;
AP_Int16 streamRateRCChannels; AP_Int16 streamRateRCChannels;
@ -177,6 +186,9 @@ private:
AP_Int16 streamRateExtra1; AP_Int16 streamRateExtra1;
AP_Int16 streamRateExtra2; AP_Int16 streamRateExtra2;
AP_Int16 streamRateExtra3; AP_Int16 streamRateExtra3;
// number of 50Hz ticks until we next send this stream
uint8_t stream_ticks[NUM_STREAMS];
}; };
#endif // __GCS_H #endif // __GCS_H

View File

@ -886,64 +886,89 @@ GCS_MAVLINK::update(void)
} }
} }
void // see if we should send a stream now. Called at 50Hz
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) bool GCS_MAVLINK::stream_trigger(enum streams stream_num)
{ {
if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) { AP_Int16 *stream_rates = &streamRateRawSensors;
uint8_t rate = (uint8_t)stream_rates[stream_num].get();
if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){ if (rate == 0) {
send_message(MSG_RAW_IMU1); return false;
send_message(MSG_RAW_IMU2); }
send_message(MSG_RAW_IMU3);
}
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) { if (stream_ticks[stream_num] == 0) {
send_message(MSG_EXTENDED_STATUS1); // we're triggering now, setup the next trigger point
send_message(MSG_EXTENDED_STATUS2); if (rate > 50) {
send_message(MSG_CURRENT_WAYPOINT); rate = 50;
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working }
send_message(MSG_NAV_CONTROLLER_OUTPUT); stream_ticks[stream_num] = 50 / rate;
send_message(MSG_FENCE_STATUS); return true;
}
if (last_gps_satellites != g_gps->num_sats) { // count down at 50Hz
// this message is mostly a huge waste of bandwidth, stream_ticks[stream_num]--;
// except it is the only message that gives the number return false;
// 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)) { void
// sent with GPS read GCS_MAVLINK::data_stream_send(void)
send_message(MSG_LOCATION); {
} if (waypoint_sending || waypoint_receiving || _queued_parameter != NULL) {
// don't send the streams when transferring bulk data
return;
}
if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { if (stream_trigger(STREAM_RAW_SENSORS)) {
// This is used for HIL. Do not change without discussing with HIL maintainers send_message(MSG_RAW_IMU1);
send_message(MSG_SERVO_OUT); send_message(MSG_RAW_IMU2);
} send_message(MSG_RAW_IMU3);
}
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { if (stream_trigger(STREAM_EXTENDED_STATUS)) {
send_message(MSG_RADIO_OUT); send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_RADIO_IN); 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 (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info if (last_gps_satellites != g_gps->num_sats) {
send_message(MSG_ATTITUDE); // this message is mostly a huge waste of bandwidth,
send_message(MSG_SIMSTATE); // 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(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info if (stream_trigger(STREAM_POSITION)) {
send_message(MSG_VFR_HUD); // sent with GPS read
} send_message(MSG_LOCATION);
}
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_AHRS); send_message(MSG_SERVO_OUT);
send_message(MSG_HWSTATUS); }
}
} if (stream_trigger(STREAM_RC_CHANNELS)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
}
if (stream_trigger(STREAM_EXTRA1)) {
send_message(MSG_ATTITUDE);
send_message(MSG_SIMSTATE);
}
if (stream_trigger(STREAM_EXTRA2)) {
send_message(MSG_VFR_HUD);
}
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 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) { if (gcs3.initialised) {
gcs3.data_stream_send(freqMin, freqMax); gcs3.data_stream_send();
} }
} }

View File

@ -41,11 +41,8 @@ planner_gcs(uint8_t argc, const Menu::arg *argv)
read_radio(); read_radio();
gcs_data_stream_send(45,1000); gcs_data_stream_send();
if ((loopcount % 5) == 0) // 10 hz
gcs_data_stream_send(5,45);
if ((loopcount % 16) == 0) { // 3 hz if ((loopcount % 16) == 0) { // 3 hz
gcs_data_stream_send(1,5);
gcs_send_message(MSG_HEARTBEAT); gcs_send_message(MSG_HEARTBEAT);
} }
loopcount++; loopcount++;