From 3ecf7b503c59c17c0c98a0641a89a61a8d121992 Mon Sep 17 00:00:00 2001 From: "tridge60@gmail.com" Date: Sun, 4 Sep 2011 23:56:26 +0000 Subject: [PATCH] added MAVLink message queueing 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 --- ArduCopterMega/ArduCopterMega.pde | 7 ++ ArduCopterMega/GCS_Mavlink.pde | 9 +- ArduCopterMega/HIL_Xplane.pde | 4 +- ArduCopterMega/Mavlink_Common.h | 139 +++++++++++++++++++++++++++--- ArduCopterMega/defines.h | 16 ++-- 5 files changed, 154 insertions(+), 21 deletions(-) diff --git a/ArduCopterMega/ArduCopterMega.pde b/ArduCopterMega/ArduCopterMega.pde index 2f3b0d1de8..8eff2f9243 100644 --- a/ArduCopterMega/ArduCopterMega.pde +++ b/ArduCopterMega/ArduCopterMega.pde @@ -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(); diff --git a/ArduCopterMega/GCS_Mavlink.pde b/ArduCopterMega/GCS_Mavlink.pde index 78dab9c3ae..95898197f4 100644 --- a/ArduCopterMega/GCS_Mavlink.pde +++ b/ArduCopterMega/GCS_Mavlink.pde @@ -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 diff --git a/ArduCopterMega/HIL_Xplane.pde b/ArduCopterMega/HIL_Xplane.pde index eeb8d0b4b0..8f9a748e74 100644 --- a/ArduCopterMega/HIL_Xplane.pde +++ b/ArduCopterMega/HIL_Xplane.pde @@ -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; diff --git a/ArduCopterMega/Mavlink_Common.h b/ArduCopterMega/Mavlink_Common.h index 5f744b375f..4c4ceca291 100644 --- a/ArduCopterMega/Mavlink_Common.h +++ b/ArduCopterMega/Mavlink_Common.h @@ -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, diff --git a/ArduCopterMega/defines.h b/ArduCopterMega/defines.h index 6855fda4d3..51a8f27853 100644 --- a/ArduCopterMega/defines.h +++ b/ArduCopterMega/defines.h @@ -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