Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 18:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

added MAVLink message queueing

Browse Source 
this ensures we never block while writing a MAVLink message to a
serial port, by checking the number of available bytes in the serial
transmit buffer and deferring any message that would cause a blocking
write.

This should prevent the main loop from clagging up due to excessive
telemetry data being sent

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3251 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-09-04 23:56:26 +00:00
parent 9169fff986
commit 5b30a58491
5 changed files with 154 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
ArduCopterMega/ArduCopterMega.pde
View File

@ -601,6 +601,13 @@ static void fast_loop()
// ----------
read_radio();
// try to send any deferred messages if the serial port now has
// some space available
gcs.send_message(MSG_RETRY_DEFERRED);
#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK && (HIL_MODE != HIL_MODE_DISABLED || HIL_PORT == 0)
hil.send_message(MSG_RETRY_DEFERRED);
#endif
// custom code/exceptions for flight modes
// ---------------------------------------
update_current_flight_mode();

9
ArduCopterMega/GCS_Mavlink.pde
View File

@ -88,12 +88,15 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
if (waypoint_sending == false && waypoint_receiving == false && _queued_parameter == NULL) {
if (freqLoopMatch(streamRateRawSensors, freqMin, freqMax)){
send_message(MSG_RAW_IMU);
send_message(MSG_RAW_IMU1);
send_message(MSG_RAW_IMU2);
send_message(MSG_RAW_IMU3);
//Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get());
}
if (freqLoopMatch(streamRateExtendedStatus, freqMin, freqMax)) {
send_message(MSG_EXTENDED_STATUS);
send_message(MSG_EXTENDED_STATUS1);
send_message(MSG_EXTENDED_STATUS2);
send_message(MSG_GPS_STATUS);
send_message(MSG_CURRENT_WAYPOINT);
send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working
@ -141,7 +144,7 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax)
void
GCS_MAVLINK::send_message(uint8_t id, uint32_t param)
{
mavlink_send_message(chan,id,param,packet_drops);
mavlink_send_message(chan,id,packet_drops);
}
void

4
ArduCopterMega/HIL_Xplane.pde
View File

@ -102,7 +102,9 @@ HIL_XPLANE::send_message(uint8_t id, uint32_t param)
break;
case MSG_RADIO_OUT:
break;
case MSG_RAW_IMU:
case MSG_RAW_IMU1:
case MSG_RAW_IMU2:
case MSG_RAW_IMU3:
break;
case MSG_GPS_STATUS:
break;

139
ArduCopterMega/Mavlink_Common.h
View File

@ -25,21 +25,36 @@ static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid)
}
}
void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, uint16_t packet_drops)
// try to send a message, return false if it won't fit in the serial tx buffer
static bool mavlink_try_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
{
uint64_t timeStamp = micros();
int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES;
#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false
if (chan == MAVLINK_COMM_1 && millis() < 1000) {
// defer any messages on the telemetry port for 1 second after
// bootup, to try to prevent bricking of Xbees
return false;
}
switch(id) {
case MSG_HEARTBEAT:
{
CHECK_PAYLOAD_SIZE(HEARTBEAT);
mavlink_msg_heartbeat_send(
chan,
mavlink_system.type,
MAV_AUTOPILOT_ARDUPILOTMEGA);
break;
}
case MSG_EXTENDED_STATUS:
case MSG_EXTENDED_STATUS1:
{
CHECK_PAYLOAD_SIZE(SYS_STATUS);
uint8_t mode = MAV_MODE_UNINIT;
uint8_t nav_mode = MAV_NAV_VECTOR;
@ -76,17 +91,21 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
battery_voltage * 1000,
battery_remaining,
packet_drops);
break;
}
#ifdef MAVLINK_MSG_ID_MEMINFO
case MSG_EXTENDED_STATUS2:
{
CHECK_PAYLOAD_SIZE(MEMINFO);
extern unsigned __brkval;
mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory());
#endif
break;
}
case MSG_ATTITUDE:
{
//Vector3f omega = dcm.get_gyro();
CHECK_PAYLOAD_SIZE(ATTITUDE);
mavlink_msg_attitude_send(
chan,
timeStamp,
@ -101,6 +120,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_LOCATION:
{
CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_global_position_int_send(
chan,
@ -117,6 +137,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_NAV_CONTROLLER_OUTPUT:
{
//if (control_mode != MANUAL) {
CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT);
mavlink_msg_nav_controller_output_send(
chan,
nav_roll / 1.0e2,
@ -133,6 +154,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_LOCAL_LOCATION:
{
CHECK_PAYLOAD_SIZE(LOCAL_POSITION);
Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now
mavlink_msg_local_position_send(
chan,
@ -148,6 +170,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_GPS_RAW:
{
CHECK_PAYLOAD_SIZE(GPS_RAW);
mavlink_msg_gps_raw_send(
chan,
timeStamp,
@ -164,6 +187,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_SERVO_OUT:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED);
uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with HIL maintainers
@ -183,6 +207,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_RADIO_IN:
{
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
uint8_t rssi = 1;
mavlink_msg_rc_channels_raw_send(
chan,
@ -200,6 +225,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_RADIO_OUT:
{
CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW);
mavlink_msg_servo_output_raw_send(
chan,
motor_out[0],
@ -215,6 +241,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_VFR_HUD:
{
CHECK_PAYLOAD_SIZE(VFR_HUD);
mavlink_msg_vfr_hud_send(
chan,
(float)airspeed / 100.0,
@ -228,8 +255,9 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
}
#if HIL_MODE != HIL_MODE_ATTITUDE
case MSG_RAW_IMU:
case MSG_RAW_IMU1:
{
CHECK_PAYLOAD_SIZE(RAW_IMU);
Vector3f accel = imu.get_accel();
Vector3f gyro = imu.get_gyro();
//Serial.printf_P(PSTR("sending accel: %f %f %f\n"), accel.x, accel.y, accel.z);
@ -246,21 +274,42 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
compass.mag_x,
compass.mag_y,
compass.mag_z);
break;
}
/* This message is not working. Why?
case MSG_RAW_IMU2:
{
CHECK_PAYLOAD_SIZE(SCALED_PRESSURE);
mavlink_msg_scaled_pressure_send(
chan,
timeStamp,
(float)barometer.Press/100.,
(float)adc.Ch(AIRSPEED_CH), // We don't calculate the differential pressure value anywhere, so use raw
(int)(barometer.Temp*100));
*/
(float)barometer.Press/100.0,
(float)(barometer.Press-ground_pressure)/100.0,
(int)(barometer.Temp*10));
break;
}
case MSG_RAW_IMU3:
{
CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS);
Vector3f mag_offsets = compass.get_offsets();
mavlink_msg_sensor_offsets_send(chan,
mag_offsets.x,
mag_offsets.y,
mag_offsets.z,
compass.get_declination(),
barometer.RawPress,
barometer.RawTemp,
imu.gx(), imu.gy(), imu.gz(),
imu.ax(), imu.ay(), imu.az());
break;
}
#endif // HIL_PROTOCOL != HIL_PROTOCOL_ATTITUDE
case MSG_GPS_STATUS:
{
CHECK_PAYLOAD_SIZE(GPS_STATUS);
mavlink_msg_gps_status_send(
chan,
g_gps->num_sats,
@ -274,6 +323,7 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
case MSG_CURRENT_WAYPOINT:
{
CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT);
mavlink_msg_waypoint_current_send(
chan,
g.waypoint_index);
@ -283,10 +333,77 @@ void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint32_t param, ui
default:
break;
}
return true;
}
#define MAX_DEFERRED_MESSAGES 17 // should be at least equal to number of
// switch types in mavlink_try_send_message()
static struct mavlink_queue {
uint8_t deferred_messages[MAX_DEFERRED_MESSAGES];
uint8_t next_deferred_message;
uint8_t num_deferred_messages;
} mavlink_queue[2];
// send a message using mavlink
static void mavlink_send_message(mavlink_channel_t chan, uint8_t id, uint16_t packet_drops)
{
uint8_t i, nextid;
struct mavlink_queue *q = &mavlink_queue[(uint8_t)chan];
// see if we can send the deferred messages, if any
while (q->num_deferred_messages != 0) {
if (!mavlink_try_send_message(chan,
q->deferred_messages[q->next_deferred_message],
packet_drops)) {
break;
}
q->next_deferred_message++;
if (q->next_deferred_message == MAX_DEFERRED_MESSAGES) {
q->next_deferred_message = 0;
}
q->num_deferred_messages--;
}
if (id == MSG_RETRY_DEFERRED) {
return;
}
// this message id might already be deferred
for (i=0, nextid = q->next_deferred_message; i < q->num_deferred_messages; i++) {
if (q->deferred_messages[nextid] == id) {
// its already deferred, discard
return;
}
nextid++;
if (nextid == MAX_DEFERRED_MESSAGES) {
nextid = 0;
}
}
if (q->num_deferred_messages != 0 ||
!mavlink_try_send_message(chan, id, packet_drops)) {
// can't send it now, so defer it
if (q->num_deferred_messages == MAX_DEFERRED_MESSAGES) {
// the defer buffer is full, discard
return;
}
nextid = q->next_deferred_message + q->num_deferred_messages;
if (nextid >= MAX_DEFERRED_MESSAGES) {
nextid -= MAX_DEFERRED_MESSAGES;
}
q->deferred_messages[nextid] = id;
q->num_deferred_messages++;
}
}
void mavlink_send_text(mavlink_channel_t chan, uint8_t severity, const char *str)
{
if (chan == MAVLINK_COMM_0 && millis() < 1000) {
// don't send status MAVLink messages for 1 second after
// bootup, to try to prevent Xbee bricking
return;
}
mavlink_msg_statustext_send(
chan,
severity,

16
ArduCopterMega/defines.h
View File

@ -196,9 +196,10 @@
#define MSG_MODE_CHANGE 0x07 //This is different than HEARTBEAT because it occurs only when the mode is actually changed
#define MSG_VERSION_REQUEST 0x08
#define MSG_VERSION 0x09
#define MSG_EXTENDED_STATUS 0x0a
#define MSG_CPU_LOAD 0x0b
#define MSG_NAV_CONTROLLER_OUTPUT 0x0c
#define MSG_EXTENDED_STATUS1 0x0a
#define MSG_EXTENDED_STATUS2 0x0b
#define MSG_CPU_LOAD 0x0c
#define MSG_NAV_CONTROLLER_OUTPUT 0x0d
#define MSG_COMMAND_REQUEST 0x20
#define MSG_COMMAND_UPLOAD 0x21
@ -221,9 +222,11 @@
#define MSG_RADIO_OUT 0x53
#define MSG_RADIO_IN 0x54
#define MSG_RAW_IMU 0x60
#define MSG_GPS_STATUS 0x61
#define MSG_GPS_RAW 0x62
#define MSG_RAW_IMU1 0x60
#define MSG_RAW_IMU2 0x61
#define MSG_RAW_IMU3 0x62
#define MSG_GPS_STATUS 0x63
#define MSG_GPS_RAW 0x64
#define MSG_SERVO_OUT 0x70
@ -241,6 +244,7 @@
#define MSG_POSITION_SET 0xb2
#define MSG_ATTITUDE_SET 0xb3
#define MSG_LOCAL_LOCATION 0xb4
#define MSG_RETRY_DEFERRED 0xff
#define SEVERITY_LOW 1
#define SEVERITY_MEDIUM 2