From 225e6d760f7cd1fa9d8df1e772c2153cc61633b4 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 18 Sep 2011 16:39:09 +1000 Subject: [PATCH] remove Mavlink_Common.h and used deferred logic for params/waypoints this moves the mavlink send logic into GCS_Mavlink.pde, and also ensures we only ever send parameters and waypoints when there is sufficient space in the serial send buffer --- ArduPlane/GCS.h | 5 +- ArduPlane/GCS_Mavlink.pde | 569 +++++++++++++++++++++++++++++++++---- ArduPlane/Mavlink_Common.h | 470 ------------------------------ ArduPlane/defines.h | 5 + 4 files changed, 518 insertions(+), 531 deletions(-) delete mode 100644 ArduPlane/Mavlink_Common.h diff --git a/ArduPlane/GCS.h b/ArduPlane/GCS.h index 7b64f84dd5..986fa67c6a 100644 --- a/ArduPlane/GCS.h +++ b/ArduPlane/GCS.h @@ -131,12 +131,14 @@ public: void send_text(uint8_t severity, const char *str); void send_text(uint8_t severity, const prog_char_t *str); void data_stream_send(uint16_t freqMin, uint16_t freqMax); + void queued_param_send(); + void queued_waypoint_send(); + private: void handleMessage(mavlink_message_t * msg); /// Perform queued sending operations /// - void _queued_send(); AP_Var *_queued_parameter; ///< next parameter to be sent in queue uint16_t _queued_parameter_index; ///< next queued parameter's index @@ -160,7 +162,6 @@ private: uint16_t packet_drops; // waypoints - uint16_t requested_interface; // request port to use uint16_t waypoint_request_i; // request index uint16_t waypoint_dest_sysid; // where to send requests uint16_t waypoint_dest_compid; // " diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index c59a0c2910..b904023fd2 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -1,12 +1,475 @@ // -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- -#if GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK - -#include "Mavlink_Common.h" - // use this to prevent recursion during sensor init static bool in_mavlink_delay; +static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) +{ + if (sysid != mavlink_system.sysid) + return 1; + // Currently we are not checking for correct compid since APM is not passing mavlink info to any subsystem + // If it is addressed to our system ID we assume it is for us + return 0; // no error +} + + +// check if a message will fit in the payload space available +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false + +/* + !!NOTE!! + + the use of NOINLINE separate functions for each message type avoids + a compiler bug in gcc that would cause it to use far more stack + space than is needed. Without the NOINLINE we use the sum of the + stack needed for each message type. Please be careful to follow the + pattern below when adding any new messages + */ + +static NOINLINE void send_heartbeat(mavlink_channel_t chan) +{ + mavlink_msg_heartbeat_send( + chan, + mavlink_system.type, + MAV_AUTOPILOT_ARDUPILOTMEGA); +} + +static NOINLINE void send_attitude(mavlink_channel_t chan) +{ + Vector3f omega = dcm.get_gyro(); + mavlink_msg_attitude_send( + chan, + micros(), + dcm.roll, + dcm.pitch, + dcm.yaw, + omega.x, + omega.y, + omega.z); +} + +static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) +{ + uint8_t mode = MAV_MODE_UNINIT; + uint8_t nav_mode = MAV_NAV_VECTOR; + + switch(control_mode) { + case MANUAL: + mode = MAV_MODE_MANUAL; + break; + case STABILIZE: + mode = MAV_MODE_TEST1; + break; + case FLY_BY_WIRE_A: + mode = MAV_MODE_TEST2; + nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case FLY_BY_WIRE_B: + mode = MAV_MODE_TEST2; + nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. + break; + case GUIDED: + mode = MAV_MODE_GUIDED; + break; + case AUTO: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_WAYPOINT; + break; + case RTL: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_RETURNING; + break; + case LOITER: + mode = MAV_MODE_AUTO; + nav_mode = MAV_NAV_LOITER; + break; + case INITIALISING: + mode = MAV_MODE_UNINIT; + nav_mode = MAV_NAV_GROUNDED; + break; + } + + uint8_t status = MAV_STATE_ACTIVE; + uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 + + mavlink_msg_sys_status_send( + chan, + mode, + nav_mode, + status, + load * 1000, + battery_voltage * 1000, + battery_remaining, + packet_drops); +} + +static void NOINLINE send_meminfo(mavlink_channel_t chan) +{ + extern unsigned __brkval; + mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); +} + +static void NOINLINE send_location(mavlink_channel_t chan) +{ + Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now + mavlink_msg_global_position_int_send( + chan, + current_loc.lat, + current_loc.lng, + current_loc.alt * 10, + g_gps->ground_speed * rot.a.x, + g_gps->ground_speed * rot.b.x, + g_gps->ground_speed * rot.c.x); +} + +static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) +{ + mavlink_msg_nav_controller_output_send( + chan, + nav_roll / 1.0e2, + nav_pitch / 1.0e2, + nav_bearing / 1.0e2, + target_bearing / 1.0e2, + wp_distance, + altitude_error / 1.0e2, + airspeed_error, + crosstrack_error); +} + +static void NOINLINE send_gps_raw(mavlink_channel_t chan) +{ + mavlink_msg_gps_raw_send( + chan, + micros(), + g_gps->status(), + g_gps->latitude / 1.0e7, + g_gps->longitude / 1.0e7, + g_gps->altitude / 100.0, + g_gps->hdop, + 0.0, + g_gps->ground_speed / 100.0, + g_gps->ground_course / 100.0); +} + +static void NOINLINE send_servo_out(mavlink_channel_t chan) +{ + const uint8_t rssi = 1; + // normalized values scaled to -10000 to 10000 + // This is used for HIL. Do not change without discussing with HIL maintainers + mavlink_msg_rc_channels_scaled_send( + chan, + 10000 * g.channel_roll.norm_output(), + 10000 * g.channel_pitch.norm_output(), + 10000 * g.channel_throttle.norm_output(), + 10000 * g.channel_rudder.norm_output(), + 0, + 0, + 0, + 0, + rssi); +} + +static void NOINLINE send_radio_in(mavlink_channel_t chan) +{ + uint8_t rssi = 1; + mavlink_msg_rc_channels_raw_send( + chan, + g.channel_roll.radio_in, + g.channel_pitch.radio_in, + g.channel_throttle.radio_in, + g.channel_rudder.radio_in, + g.rc_5.radio_in, // XXX currently only 4 RC channels defined + g.rc_6.radio_in, + g.rc_7.radio_in, + g.rc_8.radio_in, + rssi); +} + +static void NOINLINE send_radio_out(mavlink_channel_t chan) +{ + mavlink_msg_servo_output_raw_send( + chan, + g.channel_roll.radio_out, + g.channel_pitch.radio_out, + g.channel_throttle.radio_out, + g.channel_rudder.radio_out, + g.rc_5.radio_out, // XXX currently only 4 RC channels defined + g.rc_6.radio_out, + g.rc_7.radio_out, + g.rc_8.radio_out); +} + +static void NOINLINE send_vfr_hud(mavlink_channel_t chan) +{ + mavlink_msg_vfr_hud_send( + chan, + (float)airspeed / 100.0, + (float)g_gps->ground_speed / 100.0, + (dcm.yaw_sensor / 100) % 360, + (int)g.channel_throttle.servo_out, + current_loc.alt / 100.0, + climb_rate); +} + +#if HIL_MODE != HIL_MODE_ATTITUDE +static void NOINLINE send_raw_imu1(mavlink_channel_t chan) +{ + Vector3f accel = imu.get_accel(); + Vector3f gyro = imu.get_gyro(); + + mavlink_msg_raw_imu_send( + chan, + micros(), + accel.x * 1000.0 / gravity, + accel.y * 1000.0 / gravity, + accel.z * 1000.0 / gravity, + gyro.x * 1000.0, + gyro.y * 1000.0, + gyro.z * 1000.0, + compass.mag_x, + compass.mag_y, + compass.mag_z); +} + +static void NOINLINE send_raw_imu2(mavlink_channel_t chan) +{ + mavlink_msg_scaled_pressure_send( + chan, + micros(), + (float)barometer.Press/100.0, + (float)(barometer.Press-g.ground_pressure)/100.0, + (int)(barometer.Temp*10)); +} + +static void NOINLINE send_raw_imu3(mavlink_channel_t chan) +{ + 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()); +} +#endif // HIL_MODE != HIL_MODE_ATTITUDE + +static void NOINLINE send_gps_status(mavlink_channel_t chan) +{ + mavlink_msg_gps_status_send( + chan, + g_gps->num_sats, + NULL, + NULL, + NULL, + NULL, + NULL); +} + +static void NOINLINE send_current_waypoint(mavlink_channel_t chan) +{ + mavlink_msg_waypoint_current_send( + chan, + g.waypoint_index); +} + + +// 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, enum ap_message id, uint16_t packet_drops) +{ + int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; + + if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { + // 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); + send_heartbeat(chan); + return true; + + case MSG_EXTENDED_STATUS1: + CHECK_PAYLOAD_SIZE(SYS_STATUS); + send_extended_status1(chan, packet_drops); + break; + + case MSG_EXTENDED_STATUS2: + CHECK_PAYLOAD_SIZE(MEMINFO); + send_meminfo(chan); + break; + + case MSG_ATTITUDE: + CHECK_PAYLOAD_SIZE(ATTITUDE); + send_attitude(chan); + break; + + case MSG_LOCATION: + CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); + send_location(chan); + break; + + case MSG_NAV_CONTROLLER_OUTPUT: + if (control_mode != MANUAL) { + CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); + send_nav_controller_output(chan); + } + break; + + case MSG_GPS_RAW: + CHECK_PAYLOAD_SIZE(GPS_RAW); + send_gps_raw(chan); + break; + + case MSG_SERVO_OUT: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); + send_servo_out(chan); + break; + + case MSG_RADIO_IN: + CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); + send_radio_in(chan); + break; + + case MSG_RADIO_OUT: + CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); + send_radio_out(chan); + break; + + case MSG_VFR_HUD: + CHECK_PAYLOAD_SIZE(VFR_HUD); + send_vfr_hud(chan); + break; + +#if HIL_MODE != HIL_MODE_ATTITUDE + case MSG_RAW_IMU1: + CHECK_PAYLOAD_SIZE(RAW_IMU); + send_raw_imu1(chan); + break; + + case MSG_RAW_IMU2: + CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); + send_raw_imu2(chan); + break; + + case MSG_RAW_IMU3: + CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); + send_raw_imu3(chan); + break; +#endif // HIL_MODE != HIL_MODE_ATTITUDE + + case MSG_GPS_STATUS: + CHECK_PAYLOAD_SIZE(GPS_STATUS); + send_gps_status(chan); + break; + + case MSG_CURRENT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); + send_current_waypoint(chan); + break; + + case MSG_NEXT_PARAM: + CHECK_PAYLOAD_SIZE(PARAM_VALUE); + if (chan == MAVLINK_COMM_0) { + gcs0.queued_param_send(); + } else { + gcs3.queued_param_send(); + } + break; + + case MSG_NEXT_WAYPOINT: + CHECK_PAYLOAD_SIZE(WAYPOINT_REQUEST); + if (chan == MAVLINK_COMM_0) { + gcs0.queued_waypoint_send(); + } else { + gcs3.queued_waypoint_send(); + } + break; + + case MSG_RETRY_DEFERRED: + break; // just here to prevent a warning + } + return true; +} + + +#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED +static struct mavlink_queue { + enum ap_message 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, enum ap_message 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_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { + // don't send status MAVLink messages for 1 second after + // bootup, to try to prevent Xbee bricking + return; + } + mavlink_msg_statustext_send( + chan, + severity, + (const int8_t*) str); +} + GCS_MAVLINK::GCS_MAVLINK(AP_Var::Key key) : packet_drops(0), @@ -35,7 +498,7 @@ void GCS_MAVLINK::init(FastSerial * port) { GCS_Class::init(port); - if (port == &Serial) { // to split hil vs gcs + if (port == &Serial) { mavlink_comm_0_port = port; chan = MAVLINK_COMM_0; }else{ @@ -54,29 +517,34 @@ GCS_MAVLINK::update(void) status.packet_rx_drop_count = 0; // process received bytes - while(comm_get_available(chan)) + while (comm_get_available(chan)) { uint8_t c = comm_receive_ch(chan); // Try to get a new message - if(mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); + if (mavlink_parse_char(chan, c, &msg, &status)) handleMessage(&msg); } // Update packet drops counter packet_drops += status.packet_rx_drop_count; // send out queued params/ waypoints - _queued_send(); + if (NULL != _queued_parameter) { + send_message(MSG_NEXT_PARAM); + } + + if (waypoint_receiving && + waypoint_request_i <= (unsigned)g.waypoint_total) { + send_message(MSG_NEXT_WAYPOINT); + } // stop waypoint sending if timeout if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ - send_text_P(SEVERITY_LOW,PSTR("waypoint send timeout")); waypoint_sending = false; } // stop waypoint receiving if timeout if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ - send_text_P(SEVERITY_LOW,PSTR("waypoint receive timeout")); waypoint_receiving = false; } } @@ -90,7 +558,6 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) 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)) { @@ -100,44 +567,39 @@ GCS_MAVLINK::data_stream_send(uint16_t freqMin, uint16_t freqMax) 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); - //Serial.printf("mav2 %d\n", (int)streamRateExtendedStatus.get()); } if (freqLoopMatch(streamRatePosition, freqMin, freqMax)) { // sent with GPS read send_message(MSG_LOCATION); - //Serial.printf("mav3 %d\n", (int)streamRatePosition.get()); } if (freqLoopMatch(streamRateRawController, freqMin, freqMax)) { // This is used for HIL. Do not change without discussing with HIL maintainers send_message(MSG_SERVO_OUT); - //Serial.printf("mav4 %d\n", (int)streamRateRawController.get()); } if (freqLoopMatch(streamRateRCChannels, freqMin, freqMax)) { send_message(MSG_RADIO_OUT); send_message(MSG_RADIO_IN); - //Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get()); } if (freqLoopMatch(streamRateExtra1, freqMin, freqMax)){ // Use Extra 1 for AHRS info send_message(MSG_ATTITUDE); - //Serial.printf("mav6 %d\n", (int)streamRateExtra1.get()); } if (freqLoopMatch(streamRateExtra2, freqMin, freqMax)){ // Use Extra 2 for additional HIL info send_message(MSG_VFR_HUD); - //Serial.printf("mav7 %d\n", (int)streamRateExtra2.get()); } if (freqLoopMatch(streamRateExtra3, freqMin, freqMax)){ // Available datastream - //Serial.printf("mav8 %d\n", (int)streamRateExtra3.get()); } } } + + void GCS_MAVLINK::send_message(enum ap_message id) { @@ -430,7 +892,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) waypoint_receiving = false; waypoint_dest_sysid = msg->sysid; waypoint_dest_compid = msg->compid; - requested_interface = chan; break; } @@ -575,7 +1036,6 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) _queued_parameter = AP_Var::first(); _queued_parameter_index = 0; _queued_parameter_count = _count_parameters(); - requested_interface = chan; break; } @@ -1020,65 +1480,56 @@ GCS_MAVLINK::_find_parameter(uint16_t index) } /** -* @brief Send low-priority messages at a maximum rate of xx Hertz -* -* This function sends messages at a lower rate to not exceed the wireless -* bandwidth. It sends one message each time it is called until the buffer is empty. -* Call this function with xx Hertz to increase/decrease the bandwidth. +* @brief Send the next pending parameter, called from deferred message +* handling code */ void -GCS_MAVLINK::_queued_send() +GCS_MAVLINK::queued_param_send() { // Check to see if we are sending parameters - if (NULL != _queued_parameter && (requested_interface == (unsigned)chan) && mavdelay > 1) { - AP_Var *vp; - float value; + if (NULL == _queued_parameter) return; - // copy the current parameter and prepare to move to the next - vp = _queued_parameter; - _queued_parameter = _queued_parameter->next(); + AP_Var *vp; + float value; - // if the parameter can be cast to float, report it here and break out of the loop - value = vp->cast_to_float(); - if (!isnan(value)) { + // copy the current parameter and prepare to move to the next + vp = _queued_parameter; + _queued_parameter = _queued_parameter->next(); - char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK - vp->copy_name(param_name, sizeof(param_name)); + // if the parameter can be cast to float, report it here and break out of the loop + value = vp->cast_to_float(); + if (!isnan(value)) { + char param_name[ONBOARD_PARAM_NAME_LENGTH]; /// XXX HACK + vp->copy_name(param_name, sizeof(param_name)); - mavlink_msg_param_value_send( - chan, - (int8_t*)param_name, - value, - _queued_parameter_count, - _queued_parameter_index); + mavlink_msg_param_value_send( + chan, + (int8_t*)param_name, + value, + _queued_parameter_count, + _queued_parameter_index); - _queued_parameter_index++; - } - mavdelay = 0; + _queued_parameter_index++; } +} - // this is called at 50hz, count runs to prevent flooding serialport and delayed to allow eeprom write - mavdelay++; - - // request waypoints one by one - // XXX note that this is pan-interface +/** +* @brief Send the next pending waypoint, called from deferred message +* handling code +*/ +void +GCS_MAVLINK::queued_waypoint_send() +{ if (waypoint_receiving && - (requested_interface == (unsigned)chan) && - waypoint_request_i <= (unsigned)g.waypoint_total && - mavdelay > 15) { // limits to 3.33 hz - + waypoint_request_i <= (unsigned)g.waypoint_total) { mavlink_msg_waypoint_request_send( chan, waypoint_dest_sysid, waypoint_dest_compid, waypoint_request_i); - - mavdelay = 0; } } -#endif // GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK || HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK - /* a delay() callback that processes MAVLink packets. We set this as the callback in long running library initialisation routines to allow diff --git a/ArduPlane/Mavlink_Common.h b/ArduPlane/Mavlink_Common.h deleted file mode 100644 index 2cb7e1d1df..0000000000 --- a/ArduPlane/Mavlink_Common.h +++ /dev/null @@ -1,470 +0,0 @@ -/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - -#ifndef Mavlink_Common_H -#define Mavlink_Common_H - -#if HIL_PROTOCOL == HIL_PROTOCOL_MAVLINK || GCS_PROTOCOL == GCS_PROTOCOL_MAVLINK - -byte mavdelay = 0; - - -static uint8_t mavlink_check_target(uint8_t sysid, uint8_t compid) -{ -//Serial.print("target = "); Serial.print(sysid, DEC); Serial.print("\tcomp = "); Serial.println(compid, DEC); //This line for debug only - if (sysid != mavlink_system.sysid){ - return 1; - - // Currently we are not checking for correct compid since APM is not passing mavlink info to any subsystem - // If it is addressed to our system ID we assume it is for us - //}else if(compid != mavlink_system.compid){ - // gcs.send_text_P(SEVERITY_LOW,PSTR("component id mismatch")); - // return 1; // XXX currently not receiving correct compid from gcs - - }else{ - return 0; // no error - } -} - -#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_## id ##_LEN) return false - -/* - !!NOTE!! - - the use of NOINLINE separate functions for each message type avoids - a compiler bug in gcc that would cause it to use far more stack - space than is needed. Without the NOINLINE we use the sum of the - stack needed for each message type. Please be careful to follow the - pattern below when adding any new messages - */ - -#define NOINLINE __attribute__((noinline)) - -static NOINLINE void send_heartbeat(mavlink_channel_t chan) -{ - mavlink_msg_heartbeat_send( - chan, - mavlink_system.type, - MAV_AUTOPILOT_ARDUPILOTMEGA); -} - -static NOINLINE void send_attitude(mavlink_channel_t chan) -{ - Vector3f omega = dcm.get_gyro(); - mavlink_msg_attitude_send( - chan, - micros(), - dcm.roll, - dcm.pitch, - dcm.yaw, - omega.x, - omega.y, - omega.z); -} - -static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) -{ - uint8_t mode = MAV_MODE_UNINIT; - uint8_t nav_mode = MAV_NAV_VECTOR; - - switch(control_mode) { - case MANUAL: - mode = MAV_MODE_MANUAL; - break; - case STABILIZE: - mode = MAV_MODE_TEST1; - break; - case FLY_BY_WIRE_A: - mode = MAV_MODE_TEST2; - nav_mode = 1; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. - break; - case FLY_BY_WIRE_B: - mode = MAV_MODE_TEST2; - nav_mode = 2; //FBW nav_mode mapping; 1=A, 2=B, 3=C, etc. - break; - case GUIDED: - mode = MAV_MODE_GUIDED; - break; - case AUTO: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_WAYPOINT; - break; - case RTL: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_RETURNING; - break; - case LOITER: - mode = MAV_MODE_AUTO; - nav_mode = MAV_NAV_LOITER; - break; - case INITIALISING: - mode = MAV_MODE_UNINIT; - nav_mode = MAV_NAV_GROUNDED; - break; - } - - uint8_t status = MAV_STATE_ACTIVE; - uint16_t battery_remaining = 1000.0 * (float)(g.pack_capacity - current_total)/(float)g.pack_capacity; //Mavlink scaling 100% = 1000 - - mavlink_msg_sys_status_send( - chan, - mode, - nav_mode, - status, - load * 1000, - battery_voltage * 1000, - battery_remaining, - packet_drops); -} - -static void NOINLINE send_meminfo(mavlink_channel_t chan) -{ - extern unsigned __brkval; - mavlink_msg_meminfo_send(chan, __brkval, memcheck_available_memory()); -} - -static void NOINLINE send_location(mavlink_channel_t chan) -{ - Matrix3f rot = dcm.get_dcm_matrix(); // neglecting angle of attack for now - mavlink_msg_global_position_int_send( - chan, - current_loc.lat, - current_loc.lng, - current_loc.alt * 10, - g_gps->ground_speed * rot.a.x, - g_gps->ground_speed * rot.b.x, - g_gps->ground_speed * rot.c.x); -} - -static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) -{ - mavlink_msg_nav_controller_output_send( - chan, - nav_roll / 1.0e2, - nav_pitch / 1.0e2, - nav_bearing / 1.0e2, - target_bearing / 1.0e2, - wp_distance, - altitude_error / 1.0e2, - airspeed_error, - crosstrack_error); -} - -static void NOINLINE send_gps_raw(mavlink_channel_t chan) -{ - mavlink_msg_gps_raw_send( - chan, - micros(), - g_gps->status(), - g_gps->latitude / 1.0e7, - g_gps->longitude / 1.0e7, - g_gps->altitude / 100.0, - g_gps->hdop, - 0.0, - g_gps->ground_speed / 100.0, - g_gps->ground_course / 100.0); -} - -static void NOINLINE send_servo_out(mavlink_channel_t chan) -{ - const uint8_t rssi = 1; - // normalized values scaled to -10000 to 10000 - // This is used for HIL. Do not change without discussing with HIL maintainers - mavlink_msg_rc_channels_scaled_send( - chan, - 10000 * g.channel_roll.norm_output(), - 10000 * g.channel_pitch.norm_output(), - 10000 * g.channel_throttle.norm_output(), - 10000 * g.channel_rudder.norm_output(), - 0, - 0, - 0, - 0, - rssi); -} - -static void NOINLINE send_radio_in(mavlink_channel_t chan) -{ - uint8_t rssi = 1; - mavlink_msg_rc_channels_raw_send( - chan, - g.channel_roll.radio_in, - g.channel_pitch.radio_in, - g.channel_throttle.radio_in, - g.channel_rudder.radio_in, - g.rc_5.radio_in, // XXX currently only 4 RC channels defined - g.rc_6.radio_in, - g.rc_7.radio_in, - g.rc_8.radio_in, - rssi); -} - -static void NOINLINE send_radio_out(mavlink_channel_t chan) -{ - mavlink_msg_servo_output_raw_send( - chan, - g.channel_roll.radio_out, - g.channel_pitch.radio_out, - g.channel_throttle.radio_out, - g.channel_rudder.radio_out, - g.rc_5.radio_out, // XXX currently only 4 RC channels defined - g.rc_6.radio_out, - g.rc_7.radio_out, - g.rc_8.radio_out); -} - -static void NOINLINE send_vfr_hud(mavlink_channel_t chan) -{ - mavlink_msg_vfr_hud_send( - chan, - (float)airspeed / 100.0, - (float)g_gps->ground_speed / 100.0, - (dcm.yaw_sensor / 100) % 360, - (int)g.channel_throttle.servo_out, - current_loc.alt / 100.0, - climb_rate); -} - -#if HIL_MODE != HIL_MODE_ATTITUDE -static void NOINLINE send_raw_imu1(mavlink_channel_t chan) -{ - Vector3f accel = imu.get_accel(); - Vector3f gyro = imu.get_gyro(); - - mavlink_msg_raw_imu_send( - chan, - micros(), - accel.x * 1000.0 / gravity, - accel.y * 1000.0 / gravity, - accel.z * 1000.0 / gravity, - gyro.x * 1000.0, - gyro.y * 1000.0, - gyro.z * 1000.0, - compass.mag_x, - compass.mag_y, - compass.mag_z); -} - -static void NOINLINE send_raw_imu2(mavlink_channel_t chan) -{ - mavlink_msg_scaled_pressure_send( - chan, - micros(), - (float)barometer.Press/100.0, - (float)(barometer.Press-g.ground_pressure)/100.0, - (int)(barometer.Temp*10)); -} - -static void NOINLINE send_raw_imu3(mavlink_channel_t chan) -{ - 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()); -} -#endif // HIL_MODE != HIL_MODE_ATTITUDE - -static void NOINLINE send_gps_status(mavlink_channel_t chan) -{ - mavlink_msg_gps_status_send( - chan, - g_gps->num_sats, - NULL, - NULL, - NULL, - NULL, - NULL); -} - -static void NOINLINE send_current_waypoint(mavlink_channel_t chan) -{ - mavlink_msg_waypoint_current_send( - chan, - g.waypoint_index); -} - - -// 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, enum ap_message id, uint16_t packet_drops) -{ - int payload_space = comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES; - - if (chan == MAVLINK_COMM_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { - // 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); - send_heartbeat(chan); - return true; - - case MSG_EXTENDED_STATUS1: - CHECK_PAYLOAD_SIZE(SYS_STATUS); - send_extended_status1(chan, packet_drops); - break; - - case MSG_EXTENDED_STATUS2: - CHECK_PAYLOAD_SIZE(MEMINFO); - send_meminfo(chan); - break; - - case MSG_ATTITUDE: - CHECK_PAYLOAD_SIZE(ATTITUDE); - send_attitude(chan); - break; - - case MSG_LOCATION: - CHECK_PAYLOAD_SIZE(GLOBAL_POSITION_INT); - send_location(chan); - break; - - case MSG_NAV_CONTROLLER_OUTPUT: - if (control_mode != MANUAL) { - CHECK_PAYLOAD_SIZE(NAV_CONTROLLER_OUTPUT); - send_nav_controller_output(chan); - } - break; - - case MSG_GPS_RAW: - CHECK_PAYLOAD_SIZE(GPS_RAW); - send_gps_raw(chan); - break; - - case MSG_SERVO_OUT: - CHECK_PAYLOAD_SIZE(RC_CHANNELS_SCALED); - send_servo_out(chan); - break; - - case MSG_RADIO_IN: - CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW); - send_radio_in(chan); - break; - - case MSG_RADIO_OUT: - CHECK_PAYLOAD_SIZE(SERVO_OUTPUT_RAW); - send_radio_out(chan); - break; - - case MSG_VFR_HUD: - CHECK_PAYLOAD_SIZE(VFR_HUD); - send_vfr_hud(chan); - break; - -#if HIL_MODE != HIL_MODE_ATTITUDE - case MSG_RAW_IMU1: - CHECK_PAYLOAD_SIZE(RAW_IMU); - send_raw_imu1(chan); - break; - - case MSG_RAW_IMU2: - CHECK_PAYLOAD_SIZE(SCALED_PRESSURE); - send_raw_imu2(chan); - break; - - case MSG_RAW_IMU3: - CHECK_PAYLOAD_SIZE(SENSOR_OFFSETS); - send_raw_imu3(chan); - break; -#endif // HIL_MODE != HIL_MODE_ATTITUDE - - case MSG_GPS_STATUS: - CHECK_PAYLOAD_SIZE(GPS_STATUS); - send_gps_status(chan); - break; - - case MSG_CURRENT_WAYPOINT: - CHECK_PAYLOAD_SIZE(WAYPOINT_CURRENT); - send_current_waypoint(chan); - break; - - default: - break; - } - return true; -} - - -#define MAX_DEFERRED_MESSAGES MSG_RETRY_DEFERRED -static struct mavlink_queue { - enum ap_message 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, enum ap_message 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_1 && millis() < MAVLINK_TELEMETRY_PORT_DELAY) { - // don't send status MAVLink messages for 1 second after - // bootup, to try to prevent Xbee bricking - return; - } - mavlink_msg_statustext_send( - chan, - severity, - (const int8_t*) str); -} - -#endif // mavlink in use - -#endif // inclusion guard diff --git a/ArduPlane/defines.h b/ArduPlane/defines.h index eeb41f8110..d43f4dc8bf 100644 --- a/ArduPlane/defines.h +++ b/ArduPlane/defines.h @@ -126,6 +126,8 @@ enum ap_message { MSG_GPS_STATUS, MSG_GPS_RAW, MSG_SERVO_OUT, + MSG_NEXT_WAYPOINT, + MSG_NEXT_PARAM, MSG_RETRY_DEFERRED // this must be last }; @@ -229,4 +231,7 @@ enum ap_message { // convert a boolean (0 or 1) to a sign for multiplying (0 maps to 1, 1 maps to -1) #define BOOL_TO_SIGN(bvalue) ((bvalue)?-1:1) +// mark a function as not to be inlined +#define NOINLINE __attribute__((noinline)) + #endif // _DEFINES_H