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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduPlane/ArduPlane.pde
View File

@ -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)

34
ArduPlane/GCS.h
View File

@ -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

127
ArduPlane/GCS_Mavlink.pde
View File

@ -886,64 +886,89 @@ GCS_MAVLINK::update(void)
}
}
void
GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
// see if we should send a stream now. Called at 50Hz
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)){
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
}
if (rate == 0) {
return false;
}
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 (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;
}
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;
}
}
// count down at 50Hz
stream_ticks[stream_num]--;
return false;
}
if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) {
// sent with GPS read
send_message(MSG_LOCATION);
}
void
GCS_MAVLINK::data_stream_send(void)
{
if (waypoint_sending || waypoint_receiving || _queued_parameter != NULL) {
// don't send the streams when transferring bulk data
return;
}
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_SENSORS)) {
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
}
if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) {
send_message(MSG_RADIO_OUT);
send_message(MSG_RADIO_IN);
}
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 (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info
send_message(MSG_ATTITUDE);
send_message(MSG_SIMSTATE);
}
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(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info
send_message(MSG_VFR_HUD);
}
if (stream_trigger(STREAM_POSITION)) {
// sent with GPS read
send_message(MSG_LOCATION);
}
if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){
send_message(MSG_AHRS);
send_message(MSG_HWSTATUS);
}
}
if (stream_trigger(STREAM_RAW_CONTROLLER)) {
send_message(MSG_SERVO_OUT);
}
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
*/
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();
}
}

5
ArduPlane/planner.pde
View File

@ -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++;