diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 0089118281..51dd1799bf 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -13,19 +13,19 @@ static bool mavlink_active; // 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 +#define CHECK_PAYLOAD_SIZE(id) if (payload_space < MAVLINK_MSG_ID_ ## id ## _LEN) return false // prototype this for use inside the GCS class static void gcs_send_text_fmt(const prog_char_t *fmt, ...); /* - !!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 + * !!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) @@ -97,7 +97,7 @@ static NOINLINE void send_attitude(mavlink_channel_t chan) #if AP_LIMITS == ENABLED static NOINLINE void send_limits_status(mavlink_channel_t chan) { - limits_send_mavlink_status(chan); + limits_send_mavlink_status(chan); } #endif @@ -154,7 +154,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack control_sensors_health = control_sensors_present; uint16_t battery_current = -1; - uint8_t battery_remaining = -1; + uint8_t battery_remaining = -1; if (current_total1 != 0 && g.pack_capacity != 0) { battery_remaining = (100.0 * (g.pack_capacity - current_total1) / g.pack_capacity); @@ -165,8 +165,8 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack if (g.battery_monitoring == 3) { /*setting a out-of-range value. - It informs to external devices that - it cannot be calculated properly just by voltage*/ + * It informs to external devices that + * it cannot be calculated properly just by voltage*/ battery_remaining = 150; } @@ -218,7 +218,7 @@ static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) wp_distance / 1.0e2, altitude_error / 1.0e2, 0, - crosstrack_error); // was 0 + crosstrack_error); // was 0 } static void NOINLINE send_ahrs(mavlink_channel_t chan) @@ -282,9 +282,9 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) // normalized values scaled to -10000 to 10000 // This is used for HIL. Do not change without discussing with HIL maintainers - #if FRAME_CONFIG == HELI_FRAME +#if FRAME_CONFIG == HELI_FRAME - mavlink_msg_rc_channels_scaled_send( + mavlink_msg_rc_channels_scaled_send( chan, millis(), 0, // port 0 @@ -297,55 +297,55 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan) 0, 0, rssi); - #else - #if X_PLANE == ENABLED - /* update by JLN for X-Plane HIL */ - if(motors.armed() && motors.auto_armed()){ - mavlink_msg_rc_channels_scaled_send( - chan, - millis(), - 0, // port 0 - g.rc_1.servo_out, - g.rc_2.servo_out, - 10000 * g.rc_3.norm_output(), - g.rc_4.servo_out, - 10000 * g.rc_1.norm_output(), - 10000 * g.rc_2.norm_output(), - 10000 * g.rc_3.norm_output(), - 10000 * g.rc_4.norm_output(), - rssi); - }else{ - mavlink_msg_rc_channels_scaled_send( - chan, - millis(), - 0, // port 0 - 0, - 0, - -10000, - 0, - 10000 * g.rc_1.norm_output(), - 10000 * g.rc_2.norm_output(), - 10000 * g.rc_3.norm_output(), - 10000 * g.rc_4.norm_output(), - rssi); - } +#else + #if X_PLANE == ENABLED + /* update by JLN for X-Plane HIL */ + if(motors.armed() && motors.auto_armed()) { + mavlink_msg_rc_channels_scaled_send( + chan, + millis(), + 0, // port 0 + g.rc_1.servo_out, + g.rc_2.servo_out, + 10000 * g.rc_3.norm_output(), + g.rc_4.servo_out, + 10000 * g.rc_1.norm_output(), + 10000 * g.rc_2.norm_output(), + 10000 * g.rc_3.norm_output(), + 10000 * g.rc_4.norm_output(), + rssi); + }else{ + mavlink_msg_rc_channels_scaled_send( + chan, + millis(), + 0, // port 0 + 0, + 0, + -10000, + 0, + 10000 * g.rc_1.norm_output(), + 10000 * g.rc_2.norm_output(), + 10000 * g.rc_3.norm_output(), + 10000 * g.rc_4.norm_output(), + rssi); + } - #else - mavlink_msg_rc_channels_scaled_send( - chan, - millis(), - 0, // port 0 - g.rc_1.servo_out, - g.rc_2.servo_out, - g.rc_3.radio_out, - g.rc_4.servo_out, - 10000 * g.rc_1.norm_output(), - 10000 * g.rc_2.norm_output(), - 10000 * g.rc_3.norm_output(), - 10000 * g.rc_4.norm_output(), - rssi); - #endif - #endif + #else + mavlink_msg_rc_channels_scaled_send( + chan, + millis(), + 0, // port 0 + g.rc_1.servo_out, + g.rc_2.servo_out, + g.rc_3.radio_out, + g.rc_4.servo_out, + 10000 * g.rc_1.norm_output(), + 10000 * g.rc_2.norm_output(), + 10000 * g.rc_3.norm_output(), + 10000 * g.rc_4.norm_output(), + rssi); + #endif +#endif } static void NOINLINE send_radio_in(mavlink_channel_t chan) @@ -476,7 +476,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, return false; } - switch(id) { + switch(id) { case MSG_HEARTBEAT: CHECK_PAYLOAD_SIZE(HEARTBEAT); send_heartbeat(chan); @@ -583,8 +583,8 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, #if AP_LIMITS == ENABLED case MSG_LIMITS_STATUS: - CHECK_PAYLOAD_SIZE(LIMITS_STATUS); - send_limits_status(chan); + CHECK_PAYLOAD_SIZE(LIMITS_STATUS); + send_limits_status(chan); break; #endif @@ -610,7 +610,7 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, case MSG_RETRY_DEFERRED: break; // just here to prevent a warning - } + } return true; } @@ -689,31 +689,31 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char mavlink_send_message(chan, MSG_STATUSTEXT, 0); } else { // send immediately - mavlink_msg_statustext_send( - chan, - severity, - str); -} + mavlink_msg_statustext_send( + chan, + severity, + str); + } } const AP_Param::GroupInfo GCS_MAVLINK::var_info[] PROGMEM = { AP_GROUPINFO("RAW_SENS", 0, GCS_MAVLINK, streamRateRawSensors, 0), - AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRateExtendedStatus, 0), + AP_GROUPINFO("EXT_STAT", 1, GCS_MAVLINK, streamRateExtendedStatus, 0), AP_GROUPINFO("RC_CHAN", 2, GCS_MAVLINK, streamRateRCChannels, 0), - AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRateRawController, 0), - AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRatePosition, 0), - AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRateExtra1, 0), - AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRateExtra2, 0), - AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRateExtra3, 0), - AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRateParams, 0), + AP_GROUPINFO("RAW_CTRL", 3, GCS_MAVLINK, streamRateRawController, 0), + AP_GROUPINFO("POSITION", 4, GCS_MAVLINK, streamRatePosition, 0), + AP_GROUPINFO("EXTRA1", 5, GCS_MAVLINK, streamRateExtra1, 0), + AP_GROUPINFO("EXTRA2", 6, GCS_MAVLINK, streamRateExtra2, 0), + AP_GROUPINFO("EXTRA3", 7, GCS_MAVLINK, streamRateExtra3, 0), + AP_GROUPINFO("PARAMS", 8, GCS_MAVLINK, streamRateParams, 0), AP_GROUPEND }; GCS_MAVLINK::GCS_MAVLINK() : -packet_drops(0), -waypoint_send_timeout(1000), // 1 second -waypoint_receive_timeout(1000) // 1 second + packet_drops(0), + waypoint_send_timeout(1000), // 1 second + waypoint_receive_timeout(1000) // 1 second { } @@ -721,33 +721,33 @@ waypoint_receive_timeout(1000) // 1 second void GCS_MAVLINK::init(FastSerial * port) { - GCS_Class::init(port); + GCS_Class::init(port); if (port == &Serial) { - mavlink_comm_0_port = port; - chan = MAVLINK_COMM_0; - }else{ - mavlink_comm_1_port = port; - chan = MAVLINK_COMM_1; - } - _queued_parameter = NULL; + mavlink_comm_0_port = port; + chan = MAVLINK_COMM_0; + }else{ + mavlink_comm_1_port = port; + chan = MAVLINK_COMM_1; + } + _queued_parameter = NULL; } void GCS_MAVLINK::update(void) { - // receive new packets - mavlink_message_t msg; - mavlink_status_t status; - status.packet_rx_drop_count = 0; + // receive new packets + mavlink_message_t msg; + mavlink_status_t status; + status.packet_rx_drop_count = 0; - // process received bytes - while(comm_get_available(chan)) - { - uint8_t c = comm_receive_ch(chan); + // process received bytes + while(comm_get_available(chan)) + { + uint8_t c = comm_receive_ch(chan); #if CLI_ENABLED == ENABLED /* allow CLI to be started by hitting enter 3 times, if no - heartbeat packets have been received */ + * heartbeat packets have been received */ if (mavlink_active == false) { if (c == '\n' || c == '\r') { crlf_count++; @@ -760,15 +760,15 @@ GCS_MAVLINK::update(void) } #endif - // Try to get a new message + // Try to get a new message if (mavlink_parse_char(chan, c, &msg, &status)) { mavlink_active = true; handleMessage(&msg); } - } + } - // Update packet drops counter - packet_drops += status.packet_rx_drop_count; + // Update packet drops counter + packet_drops += status.packet_rx_drop_count; if (!waypoint_receiving && !waypoint_sending) { return; @@ -784,12 +784,12 @@ GCS_MAVLINK::update(void) } // stop waypoint sending if timeout - if (waypoint_sending && (tnow - waypoint_timelast_send) > waypoint_send_timeout){ + if (waypoint_sending && (tnow - waypoint_timelast_send) > waypoint_send_timeout) { waypoint_sending = false; } // stop waypoint receiving if timeout - if (waypoint_receiving && (tnow - waypoint_timelast_receive) > waypoint_receive_timeout){ + if (waypoint_receiving && (tnow - waypoint_timelast_receive) > waypoint_receive_timeout) { waypoint_receiving = false; } } @@ -843,63 +843,63 @@ GCS_MAVLINK::data_stream_send(void) } if (stream_trigger(STREAM_RAW_SENSORS)) { - send_message(MSG_RAW_IMU1); - send_message(MSG_RAW_IMU2); - send_message(MSG_RAW_IMU3); - //Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get()); - } + send_message(MSG_RAW_IMU1); + send_message(MSG_RAW_IMU2); + send_message(MSG_RAW_IMU3); + //Serial.printf("mav1 %d\n", (int)streamRateRawSensors.get()); + } 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_LIMITS_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_LIMITS_STATUS); - 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 (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 (stream_trigger(STREAM_POSITION)) { - // sent with GPS read - //Serial.printf("mav3 %d\n", (int)streamRatePosition.get()); - } + // sent with GPS read + //Serial.printf("mav3 %d\n", (int)streamRatePosition.get()); + } if (stream_trigger(STREAM_RAW_CONTROLLER)) { - send_message(MSG_SERVO_OUT); - //Serial.printf("mav4 %d\n", (int)streamRateRawController.get()); - } + send_message(MSG_SERVO_OUT); + //Serial.printf("mav4 %d\n", (int)streamRateRawController.get()); + } if (stream_trigger(STREAM_RC_CHANNELS)) { - send_message(MSG_RADIO_OUT); - send_message(MSG_RADIO_IN); - //Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get()); - } + send_message(MSG_RADIO_OUT); + send_message(MSG_RADIO_IN); + //Serial.printf("mav5 %d\n", (int)streamRateRCChannels.get()); + } if (stream_trigger(STREAM_EXTRA1)) { - send_message(MSG_ATTITUDE); - send_message(MSG_SIMSTATE); - //Serial.printf("mav6 %d\n", (int)streamRateExtra1.get()); - } + send_message(MSG_ATTITUDE); + send_message(MSG_SIMSTATE); + //Serial.printf("mav6 %d\n", (int)streamRateExtra1.get()); + } if (stream_trigger(STREAM_EXTRA2)) { - send_message(MSG_VFR_HUD); - //Serial.printf("mav7 %d\n", (int)streamRateExtra2.get()); - } + send_message(MSG_VFR_HUD); + //Serial.printf("mav7 %d\n", (int)streamRateExtra2.get()); + } if (stream_trigger(STREAM_EXTRA3)) { - send_message(MSG_AHRS); - send_message(MSG_HWSTATUS); - } - } + send_message(MSG_AHRS); + send_message(MSG_HWSTATUS); + } +} @@ -929,455 +929,455 @@ GCS_MAVLINK::send_text(gcs_severity severity, const prog_char_t *str) void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { - struct Location tell_command = {}; // command for telemetry - switch (msg->msgid) { + struct Location tell_command = {}; // command for telemetry + switch (msg->msgid) { - case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: //66 - { - // decode - mavlink_request_data_stream_t packet; - mavlink_msg_request_data_stream_decode(msg, &packet); + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: //66 + { + // decode + mavlink_request_data_stream_t packet; + mavlink_msg_request_data_stream_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; - int16_t freq = 0; // packet frequency + int16_t freq = 0; // packet frequency - if (packet.start_stop == 0) - freq = 0; // stop sending - else if (packet.start_stop == 1) - freq = packet.req_message_rate; // start sending - else - break; + if (packet.start_stop == 0) + freq = 0; // stop sending + else if (packet.start_stop == 1) + freq = packet.req_message_rate; // start sending + else + break; - switch(packet.req_stream_id){ + switch(packet.req_stream_id) { - case MAV_DATA_STREAM_ALL: - streamRateRawSensors = freq; - streamRateExtendedStatus = freq; - streamRateRCChannels = freq; - streamRateRawController = freq; - streamRatePosition = freq; - streamRateExtra1 = freq; - streamRateExtra2 = freq; - //streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. - streamRateExtra3 = freq; // Don't save!! - break; + case MAV_DATA_STREAM_ALL: + streamRateRawSensors = freq; + streamRateExtendedStatus = freq; + streamRateRCChannels = freq; + streamRateRawController = freq; + streamRatePosition = freq; + streamRateExtra1 = freq; + streamRateExtra2 = freq; + //streamRateExtra3.set_and_save(freq); // We just do set and save on the last as it takes care of the whole group. + streamRateExtra3 = freq; // Don't save!! + break; - case MAV_DATA_STREAM_RAW_SENSORS: - streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly - // we will not continue to broadcast raw sensor data at 50Hz. - break; - case MAV_DATA_STREAM_EXTENDED_STATUS: - //streamRateExtendedStatus.set_and_save(freq); - streamRateExtendedStatus = freq; - break; + case MAV_DATA_STREAM_RAW_SENSORS: + streamRateRawSensors = freq; // We do not set and save this one so that if HIL is shut down incorrectly + // we will not continue to broadcast raw sensor data at 50Hz. + break; + case MAV_DATA_STREAM_EXTENDED_STATUS: + //streamRateExtendedStatus.set_and_save(freq); + streamRateExtendedStatus = freq; + break; - case MAV_DATA_STREAM_RC_CHANNELS: - streamRateRCChannels = freq; - break; + case MAV_DATA_STREAM_RC_CHANNELS: + streamRateRCChannels = freq; + break; - case MAV_DATA_STREAM_RAW_CONTROLLER: - streamRateRawController = freq; - break; + case MAV_DATA_STREAM_RAW_CONTROLLER: + streamRateRawController = freq; + break; - //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: - // streamRateRawSensorFusion.set_and_save(freq); - // break; + //case MAV_DATA_STREAM_RAW_SENSOR_FUSION: + // streamRateRawSensorFusion.set_and_save(freq); + // break; - case MAV_DATA_STREAM_POSITION: - streamRatePosition = freq; - break; + case MAV_DATA_STREAM_POSITION: + streamRatePosition = freq; + break; - case MAV_DATA_STREAM_EXTRA1: - streamRateExtra1 = freq; - break; + case MAV_DATA_STREAM_EXTRA1: + streamRateExtra1 = freq; + break; - case MAV_DATA_STREAM_EXTRA2: - streamRateExtra2 = freq; - break; + case MAV_DATA_STREAM_EXTRA2: + streamRateExtra2 = freq; + break; - case MAV_DATA_STREAM_EXTRA3: - streamRateExtra3 = freq; - break; + case MAV_DATA_STREAM_EXTRA3: + streamRateExtra3 = freq; + break; - default: - break; - } - break; - } + default: + break; + } + break; + } case MAVLINK_MSG_ID_COMMAND_LONG: - { - // decode - mavlink_command_long_t packet; - mavlink_msg_command_long_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) break; + { + // decode + mavlink_command_long_t packet; + mavlink_msg_command_long_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) break; - uint8_t result; + uint8_t result; - // do command - send_text(SEVERITY_LOW,PSTR("command received: ")); + // do command + send_text(SEVERITY_LOW,PSTR("command received: ")); - switch(packet.command) { + switch(packet.command) { - case MAV_CMD_NAV_LOITER_UNLIM: - set_mode(LOITER); - result = MAV_RESULT_ACCEPTED; - break; + case MAV_CMD_NAV_LOITER_UNLIM: + set_mode(LOITER); + result = MAV_RESULT_ACCEPTED; + break; - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - set_mode(RTL); - result = MAV_RESULT_ACCEPTED; - break; + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + set_mode(RTL); + result = MAV_RESULT_ACCEPTED; + break; - case MAV_CMD_NAV_LAND: - set_mode(LAND); - result = MAV_RESULT_ACCEPTED; - break; + case MAV_CMD_NAV_LAND: + set_mode(LAND); + result = MAV_RESULT_ACCEPTED; + break; - case MAV_CMD_MISSION_START: - set_mode(AUTO); - result = MAV_RESULT_ACCEPTED; - break; + case MAV_CMD_MISSION_START: + set_mode(AUTO); + result = MAV_RESULT_ACCEPTED; + break; - case MAV_CMD_PREFLIGHT_CALIBRATION: - if (packet.param1 == 1 || - packet.param2 == 1 || - packet.param3 == 1) { - imu.init_accel(mavlink_delay, flash_leds); - } - if (packet.param4 == 1) { - trim_radio(); - } - result = MAV_RESULT_ACCEPTED; - break; - - - default: - result = MAV_RESULT_UNSUPPORTED; - break; + case MAV_CMD_PREFLIGHT_CALIBRATION: + if (packet.param1 == 1 || + packet.param2 == 1 || + packet.param3 == 1) { + imu.init_accel(mavlink_delay, flash_leds); } + if (packet.param4 == 1) { + trim_radio(); + } + result = MAV_RESULT_ACCEPTED; + break; - mavlink_msg_command_ack_send( - chan, - packet.command, - result); + default: + result = MAV_RESULT_UNSUPPORTED; break; } - case MAVLINK_MSG_ID_SET_MODE: //11 - { - // decode - mavlink_set_mode_t packet; - mavlink_msg_set_mode_decode(msg, &packet); + mavlink_msg_command_ack_send( + chan, + packet.command, + result); - if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) { - // we ignore base_mode as there is no sane way to map - // from that bitmap to a APM flight mode. We rely on - // custom_mode instead. - break; - } - switch (packet.custom_mode) { - case STABILIZE: - case ACRO: - case ALT_HOLD: - case AUTO: - case GUIDED: - case LOITER: - case RTL: - case CIRCLE: - case POSITION: - case LAND: - case OF_LOITER: - set_mode(packet.custom_mode); - break; - } + break; + } + case MAVLINK_MSG_ID_SET_MODE: //11 + { + // decode + mavlink_set_mode_t packet; + mavlink_msg_set_mode_decode(msg, &packet); + + if (!(packet.base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED)) { + // we ignore base_mode as there is no sane way to map + // from that bitmap to a APM flight mode. We rely on + // custom_mode instead. break; - } + } + switch (packet.custom_mode) { + case STABILIZE: + case ACRO: + case ALT_HOLD: + case AUTO: + case GUIDED: + case LOITER: + case RTL: + case CIRCLE: + case POSITION: + case LAND: + case OF_LOITER: + set_mode(packet.custom_mode); + break; + } - /*case MAVLINK_MSG_ID_SET_NAV_MODE: - { - // decode - mavlink_set_nav_mode_t packet; - mavlink_msg_set_nav_mode_decode(msg, &packet); - // To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message - mav_nav = packet.nav_mode; - break; - } - */ - case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: //43 - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint request list")); + break; + } - // decode - mavlink_mission_request_list_t packet; - mavlink_msg_mission_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + /*case MAVLINK_MSG_ID_SET_NAV_MODE: + * { + * // decode + * mavlink_set_nav_mode_t packet; + * mavlink_msg_set_nav_mode_decode(msg, &packet); + * // To set some flight modes we must first receive a "set nav mode" message and then a "set mode" message + * mav_nav = packet.nav_mode; + * break; + * } + */ + case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: //43 + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint request list")); - // Start sending waypoints - mavlink_msg_mission_count_send( - chan,msg->sysid, - msg->compid, - g.command_total); // includes home + // decode + mavlink_mission_request_list_t packet; + mavlink_msg_mission_request_list_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; - waypoint_timelast_send = millis(); - waypoint_sending = true; - waypoint_receiving = false; - waypoint_dest_sysid = msg->sysid; - waypoint_dest_compid = msg->compid; - break; - } + // Start sending waypoints + mavlink_msg_mission_count_send( + chan,msg->sysid, + msg->compid, + g.command_total); // includes home - // XXX read a WP from EEPROM and send it to the GCS - case MAVLINK_MSG_ID_MISSION_REQUEST: // 40 - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); + waypoint_timelast_send = millis(); + waypoint_sending = true; + waypoint_receiving = false; + waypoint_dest_sysid = msg->sysid; + waypoint_dest_compid = msg->compid; + break; + } - // Check if sending waypiont - //if (!waypoint_sending) break; - // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW + // XXX read a WP from EEPROM and send it to the GCS + case MAVLINK_MSG_ID_MISSION_REQUEST: // 40 + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint request")); - // decode - mavlink_mission_request_t packet; - mavlink_msg_mission_request_decode(msg, &packet); + // Check if sending waypiont + //if (!waypoint_sending) break; + // 5/10/11 - We are trying out relaxing the requirement that we be in waypoint sending mode to respond to a waypoint request. DEW - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; + // decode + mavlink_mission_request_t packet; + mavlink_msg_mission_request_decode(msg, &packet); - // send waypoint - tell_command = get_cmd_with_index(packet.seq); + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; - // set frame of waypoint - uint8_t frame; + // send waypoint + tell_command = get_cmd_with_index(packet.seq); - if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { - frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame - } else { - frame = MAV_FRAME_GLOBAL; // reference frame - } + // set frame of waypoint + uint8_t frame; - float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; + if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame + } else { + frame = MAV_FRAME_GLOBAL; // reference frame + } - // time that the mav should loiter in milliseconds - uint8_t current = 0; // 1 (true), 0 (false) + float param1 = 0, param2 = 0, param3 = 0, param4 = 0; - if (packet.seq == (uint16_t)g.command_index) - current = 1; + // time that the mav should loiter in milliseconds + uint8_t current = 0; // 1 (true), 0 (false) - uint8_t autocontinue = 1; // 1 (true), 0 (false) + if (packet.seq == (uint16_t)g.command_index) + current = 1; - float x = 0, y = 0, z = 0; + uint8_t autocontinue = 1; // 1 (true), 0 (false) - if (tell_command.id < MAV_CMD_NAV_LAST) { - // command needs scaling - x = tell_command.lat/1.0e7; // local (x), global (latitude) - y = tell_command.lng/1.0e7; // local (y), global (longitude) - // ACM is processing alt inside each command. so we save and load raw values. - this is diffrent to APM - z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) - } + float x = 0, y = 0, z = 0; - // Switch to map APM command fields into MAVLink command fields - switch (tell_command.id) { + if (tell_command.id < MAV_CMD_NAV_LAST) { + // command needs scaling + x = tell_command.lat/1.0e7; // local (x), global (latitude) + y = tell_command.lng/1.0e7; // local (y), global (longitude) + // ACM is processing alt inside each command. so we save and load raw values. - this is diffrent to APM + z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) + } - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_CONDITION_CHANGE_ALT: - case MAV_CMD_DO_SET_HOME: - param1 = tell_command.p1; - break; + // Switch to map APM command fields into MAVLink command fields + switch (tell_command.id) { - case MAV_CMD_NAV_ROI: - param1 = tell_command.p1; // MAV_ROI (aka roi mode) is held in wp's parameter but we actually do nothing with it because we only support pointing at a specific location provided by x,y and z parameters - break; + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_CONDITION_CHANGE_ALT: + case MAV_CMD_DO_SET_HOME: + param1 = tell_command.p1; + break; - case MAV_CMD_CONDITION_YAW: - param3 = tell_command.p1; - param1 = tell_command.alt; - param2 = tell_command.lat; - param4 = tell_command.lng; - break; + case MAV_CMD_NAV_ROI: + param1 = tell_command.p1; // MAV_ROI (aka roi mode) is held in wp's parameter but we actually do nothing with it because we only support pointing at a specific location provided by x,y and z parameters + break; - case MAV_CMD_NAV_TAKEOFF: - param1 = 0; - break; + case MAV_CMD_CONDITION_YAW: + param3 = tell_command.p1; + param1 = tell_command.alt; + param2 = tell_command.lat; + param4 = tell_command.lng; + break; - case MAV_CMD_NAV_LOITER_TIME: - param1 = tell_command.p1; // ACM loiter time is in 1 second increments - break; + case MAV_CMD_NAV_TAKEOFF: + param1 = 0; + break; - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - param1 = tell_command.lat; - break; + case MAV_CMD_NAV_LOITER_TIME: + param1 = tell_command.p1; // ACM loiter time is in 1 second increments + break; - case MAV_CMD_DO_JUMP: - param2 = tell_command.lat; - param1 = tell_command.p1; - break; + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + param1 = tell_command.lat; + break; - case MAV_CMD_DO_REPEAT_SERVO: - param4 = tell_command.lng; - case MAV_CMD_DO_REPEAT_RELAY: - case MAV_CMD_DO_CHANGE_SPEED: - param3 = tell_command.lat; - param2 = tell_command.alt; - param1 = tell_command.p1; - break; + case MAV_CMD_DO_JUMP: + param2 = tell_command.lat; + param1 = tell_command.p1; + break; - case MAV_CMD_NAV_WAYPOINT: - param1 = tell_command.p1; - break; + case MAV_CMD_DO_REPEAT_SERVO: + param4 = tell_command.lng; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + param3 = tell_command.lat; + param2 = tell_command.alt; + param1 = tell_command.p1; + break; - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - param2 = tell_command.alt; - param1 = tell_command.p1; - break; - } + case MAV_CMD_NAV_WAYPOINT: + param1 = tell_command.p1; + break; - mavlink_msg_mission_item_send(chan,msg->sysid, - msg->compid, - packet.seq, - frame, - tell_command.id, - current, - autocontinue, - param1, - param2, - param3, - param4, - x, - y, - z); + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + param2 = tell_command.alt; + param1 = tell_command.p1; + break; + } - // update last waypoint comm stamp - waypoint_timelast_send = millis(); - break; - } + mavlink_msg_mission_item_send(chan,msg->sysid, + msg->compid, + packet.seq, + frame, + tell_command.id, + current, + autocontinue, + param1, + param2, + param3, + param4, + x, + y, + z); - case MAVLINK_MSG_ID_MISSION_ACK: //47 - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint ack")); + // update last waypoint comm stamp + waypoint_timelast_send = millis(); + break; + } - // decode - mavlink_mission_ack_t packet; - mavlink_msg_mission_ack_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + case MAVLINK_MSG_ID_MISSION_ACK: //47 + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint ack")); - // turn off waypoint send - waypoint_sending = false; - break; - } + // decode + mavlink_mission_ack_t packet; + mavlink_msg_mission_ack_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; - case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // 21 - { - // gcs_send_text_P(SEVERITY_LOW,PSTR("param request list")); + // turn off waypoint send + waypoint_sending = false; + break; + } - // decode - mavlink_param_request_list_t packet; - mavlink_msg_param_request_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: // 21 + { + // gcs_send_text_P(SEVERITY_LOW,PSTR("param request list")); - // Start sending parameters - next call to ::update will kick the first one out + // decode + mavlink_param_request_list_t packet; + mavlink_msg_param_request_list_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; - _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type); - _queued_parameter_index = 0; - _queued_parameter_count = _count_parameters(); - break; - } + // Start sending parameters - next call to ::update will kick the first one out + + _queued_parameter = AP_Param::first(&_queued_parameter_token, &_queued_parameter_type); + _queued_parameter_index = 0; + _queued_parameter_count = _count_parameters(); + break; + } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: - { - // decode - mavlink_param_request_read_t packet; - mavlink_msg_param_request_read_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - if (packet.param_index != -1) { - gcs_send_text_P(SEVERITY_LOW, PSTR("Param by index not supported")); - break; - } - - enum ap_var_type p_type; - AP_Param *vp = AP_Param::find(packet.param_id, &p_type); - if (vp == NULL) { - gcs_send_text_fmt(PSTR("Unknown parameter %s"), packet.param_id); - break; - } - char param_name[ONBOARD_PARAM_NAME_LENGTH]; - vp->copy_name(param_name, sizeof(param_name), true); - - float value = vp->cast_to_float(p_type); - mavlink_msg_param_value_send( - chan, - param_name, - value, - mav_var_type(p_type), - -1, -1); + { + // decode + mavlink_param_request_read_t packet; + mavlink_msg_param_request_read_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + if (packet.param_index != -1) { + gcs_send_text_P(SEVERITY_LOW, PSTR("Param by index not supported")); break; } - case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // 45 - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint clear all")); + enum ap_var_type p_type; + AP_Param *vp = AP_Param::find(packet.param_id, &p_type); + if (vp == NULL) { + gcs_send_text_fmt(PSTR("Unknown parameter %s"), packet.param_id); + break; + } + char param_name[ONBOARD_PARAM_NAME_LENGTH]; + vp->copy_name(param_name, sizeof(param_name), true); - // decode - mavlink_mission_clear_all_t packet; - mavlink_msg_mission_clear_all_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) break; + float value = vp->cast_to_float(p_type); + mavlink_msg_param_value_send( + chan, + param_name, + value, + mav_var_type(p_type), + -1, -1); + break; + } - // clear all waypoints - uint8_t type = 0; // ok (0), error(1) - g.command_total.set_and_save(1); + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: // 45 + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint clear all")); - // send acknowledgement 3 times to makes sure it is received - for (int16_t i=0;i<3;i++) - mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, type); + // decode + mavlink_mission_clear_all_t packet; + mavlink_msg_mission_clear_all_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) break; - break; - } + // clear all waypoints + uint8_t type = 0; // ok (0), error(1) + g.command_total.set_and_save(1); - case MAVLINK_MSG_ID_MISSION_SET_CURRENT: // 41 - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint set current")); + // send acknowledgement 3 times to makes sure it is received + for (int16_t i=0; i<3; i++) + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, type); - // decode - mavlink_mission_set_current_t packet; - mavlink_msg_mission_set_current_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + break; + } - // set current command - change_command(packet.seq); + case MAVLINK_MSG_ID_MISSION_SET_CURRENT: // 41 + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint set current")); - mavlink_msg_mission_current_send(chan, g.command_index); - break; - } + // decode + mavlink_mission_set_current_t packet; + mavlink_msg_mission_set_current_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; - case MAVLINK_MSG_ID_MISSION_COUNT: // 44 - { - //send_text_P(SEVERITY_LOW,PSTR("waypoint count")); + // set current command + change_command(packet.seq); - // decode - mavlink_mission_count_t packet; - mavlink_msg_mission_count_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + mavlink_msg_mission_current_send(chan, g.command_index); + break; + } - // start waypoint receiving - if (packet.count > MAX_WAYPOINTS) { - packet.count = MAX_WAYPOINTS; - } - g.command_total.set_and_save(packet.count); + case MAVLINK_MSG_ID_MISSION_COUNT: // 44 + { + //send_text_P(SEVERITY_LOW,PSTR("waypoint count")); - waypoint_timelast_receive = millis(); - waypoint_receiving = true; - waypoint_sending = false; - waypoint_request_i = 0; - waypoint_timelast_request = 0; - break; - } + // decode + mavlink_mission_count_t packet; + mavlink_msg_mission_count_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // start waypoint receiving + if (packet.count > MAX_WAYPOINTS) { + packet.count = MAX_WAYPOINTS; + } + g.command_total.set_and_save(packet.count); + + waypoint_timelast_receive = millis(); + waypoint_receiving = true; + waypoint_sending = false; + waypoint_request_i = 0; + waypoint_timelast_request = 0; + break; + } #ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS case MAVLINK_MSG_ID_SET_MAG_OFFSETS: @@ -1390,518 +1390,518 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } #endif - // XXX receive a WP from GCS and store in EEPROM - case MAVLINK_MSG_ID_MISSION_ITEM: //39 - { - // decode - mavlink_mission_item_t packet; - mavlink_msg_mission_item_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + // XXX receive a WP from GCS and store in EEPROM + case MAVLINK_MSG_ID_MISSION_ITEM: //39 + { + // decode + mavlink_mission_item_t packet; + mavlink_msg_mission_item_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; - // defaults - tell_command.id = packet.command; + // defaults + tell_command.id = packet.command; - /* - switch (packet.frame){ + /* + * switch (packet.frame){ + * + * case MAV_FRAME_MISSION: + * case MAV_FRAME_GLOBAL: + * { + * tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 + * tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 + * tell_command.alt = packet.z*1.0e2; // in as m converted to cm + * tell_command.options = 0; // absolute altitude + * break; + * } + * + * case MAV_FRAME_LOCAL: // local (relative to home position) + * { + * tell_command.lat = 1.0e7*ToDeg(packet.x/ + * (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; + * tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; + * tell_command.alt = packet.z*1.0e2; + * tell_command.options = MASK_OPTIONS_RELATIVE_ALT; + * break; + * } + * //case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude + * default: + * { + * tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 + * tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 + * tell_command.alt = packet.z * 1.0e2; + * tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! + * break; + * } + * } + */ - case MAV_FRAME_MISSION: - case MAV_FRAME_GLOBAL: - { - tell_command.lat = 1.0e7*packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7*packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z*1.0e2; // in as m converted to cm - tell_command.options = 0; // absolute altitude - break; - } + // we only are supporting Abs position, relative Alt + tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 + tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 + tell_command.alt = packet.z * 1.0e2; + tell_command.options = 1; // store altitude relative to home alt!! Always!! - case MAV_FRAME_LOCAL: // local (relative to home position) - { - tell_command.lat = 1.0e7*ToDeg(packet.x/ - (radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lat; - tell_command.lng = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lng; - tell_command.alt = packet.z*1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; - break; - } - //case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude - default: - { - tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2; - tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! - break; - } - } - */ + switch (tell_command.id) { // Switch to map APM command fields into MAVLink command fields + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_DO_SET_HOME: + tell_command.p1 = packet.param1; + break; - // we only are supporting Abs position, relative Alt - tell_command.lat = 1.0e7 * packet.x; // in as DD converted to * t7 - tell_command.lng = 1.0e7 * packet.y; // in as DD converted to * t7 - tell_command.alt = packet.z * 1.0e2; - tell_command.options = 1; // store altitude relative to home alt!! Always!! + case MAV_CMD_NAV_ROI: + tell_command.p1 = packet.param1; // MAV_ROI (aka roi mode) is held in wp's parameter but we actually do nothing with it because we only support pointing at a specific location provided by x,y and z parameters + break; - switch (tell_command.id) { // Switch to map APM command fields into MAVLink command fields - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_DO_SET_HOME: - tell_command.p1 = packet.param1; - break; + case MAV_CMD_CONDITION_YAW: + tell_command.p1 = packet.param3; + tell_command.alt = packet.param1; + tell_command.lat = packet.param2; + tell_command.lng = packet.param4; + break; - case MAV_CMD_NAV_ROI: - tell_command.p1 = packet.param1; // MAV_ROI (aka roi mode) is held in wp's parameter but we actually do nothing with it because we only support pointing at a specific location provided by x,y and z parameters - break; + case MAV_CMD_NAV_TAKEOFF: + tell_command.p1 = 0; + break; - case MAV_CMD_CONDITION_YAW: - tell_command.p1 = packet.param3; - tell_command.alt = packet.param1; - tell_command.lat = packet.param2; - tell_command.lng = packet.param4; - break; + case MAV_CMD_CONDITION_CHANGE_ALT: + tell_command.p1 = packet.param1 * 100; + break; - case MAV_CMD_NAV_TAKEOFF: - tell_command.p1 = 0; - break; + case MAV_CMD_NAV_LOITER_TIME: + tell_command.p1 = packet.param1; // APM loiter time is in ten second increments + break; - case MAV_CMD_CONDITION_CHANGE_ALT: - tell_command.p1 = packet.param1 * 100; - break; + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + tell_command.lat = packet.param1; + break; - case MAV_CMD_NAV_LOITER_TIME: - tell_command.p1 = packet.param1; // APM loiter time is in ten second increments - break; + case MAV_CMD_DO_JUMP: + tell_command.lat = packet.param2; + tell_command.p1 = packet.param1; + break; - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - tell_command.lat = packet.param1; - break; + case MAV_CMD_DO_REPEAT_SERVO: + tell_command.lng = packet.param4; + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CHANGE_SPEED: + tell_command.lat = packet.param3; + tell_command.alt = packet.param2; + tell_command.p1 = packet.param1; + break; - case MAV_CMD_DO_JUMP: - tell_command.lat = packet.param2; - tell_command.p1 = packet.param1; - break; + case MAV_CMD_NAV_WAYPOINT: + tell_command.p1 = packet.param1; + break; - case MAV_CMD_DO_REPEAT_SERVO: - tell_command.lng = packet.param4; - case MAV_CMD_DO_REPEAT_RELAY: - case MAV_CMD_DO_CHANGE_SPEED: - tell_command.lat = packet.param3; - tell_command.alt = packet.param2; - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_NAV_WAYPOINT: - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_DO_SET_PARAMETER: - case MAV_CMD_DO_SET_RELAY: - case MAV_CMD_DO_SET_SERVO: - tell_command.alt = packet.param2; - tell_command.p1 = packet.param1; - break; - } - - if(packet.current == 2){ //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission - guided_WP = tell_command; - - // add home alt if needed - if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT){ - guided_WP.alt += home.alt; - } - - set_mode(GUIDED); - - // make any new wp uploaded instant (in case we are already in Guided mode) - set_next_WP(&guided_WP); - - // verify we recevied the command - mavlink_msg_mission_ack_send( - chan, - msg->sysid, - msg->compid, - 0); - - } else { - // Check if receiving waypoints (mission upload expected) - if (!waypoint_receiving) break; - - - //Serial.printf("req: %d, seq: %d, total: %d\n", waypoint_request_i,packet.seq, g.command_total.get()); - - // check if this is the requested waypoint - if (packet.seq != waypoint_request_i) - break; - - if(packet.seq != 0) - set_cmd_with_index(tell_command, packet.seq); - - // update waypoint receiving state machine - waypoint_timelast_receive = millis(); - waypoint_timelast_request = 0; - waypoint_request_i++; - - if (waypoint_request_i == (uint16_t)g.command_total){ - uint8_t type = 0; // ok (0), error(1) - - mavlink_msg_mission_ack_send( - chan, - msg->sysid, - msg->compid, - type); - - send_text(SEVERITY_LOW,PSTR("flight plan received")); - waypoint_receiving = false; - // XXX ignores waypoint radius for individual waypoints, can - // only set WP_RADIUS parameter - } - } - break; - } - - case MAVLINK_MSG_ID_PARAM_SET: // 23 - { - AP_Param *vp; - enum ap_var_type var_type; - - // decode - mavlink_param_set_t packet; - mavlink_msg_param_set_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; - - // set parameter - - char key[ONBOARD_PARAM_NAME_LENGTH+1]; - strncpy(key, (char *)packet.param_id, ONBOARD_PARAM_NAME_LENGTH); - key[ONBOARD_PARAM_NAME_LENGTH] = 0; - - // find the requested parameter - vp = AP_Param::find(key, &var_type); - if ((NULL != vp) && // exists - !isnan(packet.param_value) && // not nan - !isinf(packet.param_value)) { // not inf - - // add a small amount before casting parameter values - // from float to integer to avoid truncating to the - // next lower integer value. - float rounding_addition = 0.01; - - // handle variables with standard type IDs - if (var_type == AP_PARAM_FLOAT) { - ((AP_Float *)vp)->set_and_save(packet.param_value); - } else if (var_type == AP_PARAM_INT32) { -#if LOGGING_ENABLED == ENABLED - Log_Write_Data(1, ((AP_Int32 *)vp)->get()); -#endif - if (packet.param_value < 0) rounding_addition = -rounding_addition; - float v = packet.param_value+rounding_addition; - v = constrain(v, -2147483648.0, 2147483647.0); - ((AP_Int32 *)vp)->set_and_save(v); - } else if (var_type == AP_PARAM_INT16) { -#if LOGGING_ENABLED == ENABLED - Log_Write_Data(3, (int32_t)((AP_Int16 *)vp)->get()); -#endif - if (packet.param_value < 0) rounding_addition = -rounding_addition; - float v = packet.param_value+rounding_addition; - v = constrain(v, -32768, 32767); - ((AP_Int16 *)vp)->set_and_save(v); - } else if (var_type == AP_PARAM_INT8) { -#if LOGGING_ENABLED == ENABLED - Log_Write_Data(4, (int32_t)((AP_Int8 *)vp)->get()); -#endif - if (packet.param_value < 0) rounding_addition = -rounding_addition; - float v = packet.param_value+rounding_addition; - v = constrain(v, -128, 127); - ((AP_Int8 *)vp)->set_and_save(v); - } else { - // we don't support mavlink set on this parameter - break; - } - - // Report back the new value if we accepted the change - // we send the value we actually set, which could be - // different from the value sent, in case someone sent - // a fractional value to an integer type - mavlink_msg_param_value_send( - chan, - key, - vp->cast_to_float(var_type), - mav_var_type(var_type), - _count_parameters(), - -1); // XXX we don't actually know what its index is... - - } - - break; - } // end case - - case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: //70 - { - // allow override of RC channel values for HIL - // or for complete GCS control of switch position - // and RC PWM values. - if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs - mavlink_rc_channels_override_t packet; - int16_t v[8]; - mavlink_msg_rc_channels_override_decode(msg, &packet); - - if (mavlink_check_target(packet.target_system,packet.target_component)) - break; - - v[0] = packet.chan1_raw; - v[1] = packet.chan2_raw; - v[2] = packet.chan3_raw; - v[3] = packet.chan4_raw; - v[4] = packet.chan5_raw; - v[5] = packet.chan6_raw; - v[6] = packet.chan7_raw; - v[7] = packet.chan8_raw; - APM_RC.setHIL(v); + case MAV_CMD_DO_SET_PARAMETER: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_SET_SERVO: + tell_command.alt = packet.param2; + tell_command.p1 = packet.param1; break; } + if(packet.current == 2) { //current = 2 is a flag to tell us this is a "guided mode" waypoint and not for the mission + guided_WP = tell_command; + + // add home alt if needed + if (guided_WP.options & MASK_OPTIONS_RELATIVE_ALT) { + guided_WP.alt += home.alt; + } + + set_mode(GUIDED); + + // make any new wp uploaded instant (in case we are already in Guided mode) + set_next_WP(&guided_WP); + + // verify we recevied the command + mavlink_msg_mission_ack_send( + chan, + msg->sysid, + msg->compid, + 0); + + } else { + // Check if receiving waypoints (mission upload expected) + if (!waypoint_receiving) break; + + + //Serial.printf("req: %d, seq: %d, total: %d\n", waypoint_request_i,packet.seq, g.command_total.get()); + + // check if this is the requested waypoint + if (packet.seq != waypoint_request_i) + break; + + if(packet.seq != 0) + set_cmd_with_index(tell_command, packet.seq); + + // update waypoint receiving state machine + waypoint_timelast_receive = millis(); + waypoint_timelast_request = 0; + waypoint_request_i++; + + if (waypoint_request_i == (uint16_t)g.command_total) { + uint8_t type = 0; // ok (0), error(1) + + mavlink_msg_mission_ack_send( + chan, + msg->sysid, + msg->compid, + type); + + send_text(SEVERITY_LOW,PSTR("flight plan received")); + waypoint_receiving = false; + // XXX ignores waypoint radius for individual waypoints, can + // only set WP_RADIUS parameter + } + } + break; + } + + case MAVLINK_MSG_ID_PARAM_SET: // 23 + { + AP_Param *vp; + enum ap_var_type var_type; + + // decode + mavlink_param_set_t packet; + mavlink_msg_param_set_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + + // set parameter + + char key[ONBOARD_PARAM_NAME_LENGTH+1]; + strncpy(key, (char *)packet.param_id, ONBOARD_PARAM_NAME_LENGTH); + key[ONBOARD_PARAM_NAME_LENGTH] = 0; + + // find the requested parameter + vp = AP_Param::find(key, &var_type); + if ((NULL != vp) && // exists + !isnan(packet.param_value) && // not nan + !isinf(packet.param_value)) { // not inf + + // add a small amount before casting parameter values + // from float to integer to avoid truncating to the + // next lower integer value. + float rounding_addition = 0.01; + + // handle variables with standard type IDs + if (var_type == AP_PARAM_FLOAT) { + ((AP_Float *)vp)->set_and_save(packet.param_value); + } else if (var_type == AP_PARAM_INT32) { +#if LOGGING_ENABLED == ENABLED + Log_Write_Data(1, ((AP_Int32 *)vp)->get()); +#endif + if (packet.param_value < 0) rounding_addition = -rounding_addition; + float v = packet.param_value+rounding_addition; + v = constrain(v, -2147483648.0, 2147483647.0); + ((AP_Int32 *)vp)->set_and_save(v); + } else if (var_type == AP_PARAM_INT16) { +#if LOGGING_ENABLED == ENABLED + Log_Write_Data(3, (int32_t)((AP_Int16 *)vp)->get()); +#endif + if (packet.param_value < 0) rounding_addition = -rounding_addition; + float v = packet.param_value+rounding_addition; + v = constrain(v, -32768, 32767); + ((AP_Int16 *)vp)->set_and_save(v); + } else if (var_type == AP_PARAM_INT8) { +#if LOGGING_ENABLED == ENABLED + Log_Write_Data(4, (int32_t)((AP_Int8 *)vp)->get()); +#endif + if (packet.param_value < 0) rounding_addition = -rounding_addition; + float v = packet.param_value+rounding_addition; + v = constrain(v, -128, 127); + ((AP_Int8 *)vp)->set_and_save(v); + } else { + // we don't support mavlink set on this parameter + break; + } + + // Report back the new value if we accepted the change + // we send the value we actually set, which could be + // different from the value sent, in case someone sent + // a fractional value to an integer type + mavlink_msg_param_value_send( + chan, + key, + vp->cast_to_float(var_type), + mav_var_type(var_type), + _count_parameters(), + -1); // XXX we don't actually know what its index is... + + } + + break; + } // end case + + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: //70 + { + // allow override of RC channel values for HIL + // or for complete GCS control of switch position + // and RC PWM values. + if(msg->sysid != g.sysid_my_gcs) break; // Only accept control from our gcs + mavlink_rc_channels_override_t packet; + int16_t v[8]; + mavlink_msg_rc_channels_override_decode(msg, &packet); + + if (mavlink_check_target(packet.target_system,packet.target_component)) + break; + + v[0] = packet.chan1_raw; + v[1] = packet.chan2_raw; + v[2] = packet.chan3_raw; + v[3] = packet.chan4_raw; + v[4] = packet.chan5_raw; + v[5] = packet.chan6_raw; + v[6] = packet.chan7_raw; + v[7] = packet.chan8_raw; + APM_RC.setHIL(v); + break; + } + #if HIL_MODE != HIL_MODE_DISABLED - case MAVLINK_MSG_ID_HIL_STATE: - { - mavlink_hil_state_t packet; - mavlink_msg_hil_state_decode(msg, &packet); + case MAVLINK_MSG_ID_HIL_STATE: + { + mavlink_hil_state_t packet; + mavlink_msg_hil_state_decode(msg, &packet); - float vel = sqrt((packet.vx * (float)packet.vx) + (packet.vy * (float)packet.vy)); - float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100); + float vel = sqrt((packet.vx * (float)packet.vx) + (packet.vy * (float)packet.vy)); + float cog = wrap_360(ToDeg(atan2(packet.vx, packet.vy)) * 100); - // set gps hil sensor - g_gps->setHIL(packet.time_usec/1000, - packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, - vel*1.0e-2, cog*1.0e-2, 0, 10); + // set gps hil sensor + g_gps->setHIL(packet.time_usec/1000, + packet.lat*1.0e-7, packet.lon*1.0e-7, packet.alt*1.0e-3, + vel*1.0e-2, cog*1.0e-2, 0, 10); - if (gps_base_alt == 0) { - gps_base_alt = g_gps->altitude; - } - current_loc.lng = g_gps->longitude; - current_loc.lat = g_gps->latitude; - current_loc.alt = g_gps->altitude - gps_base_alt; - if (!home_is_set) { - init_home(); - } + if (gps_base_alt == 0) { + gps_base_alt = g_gps->altitude; + } + current_loc.lng = g_gps->longitude; + current_loc.lat = g_gps->latitude; + current_loc.alt = g_gps->altitude - gps_base_alt; + if (!home_is_set) { + init_home(); + } - // rad/sec - Vector3f gyros; - gyros.x = packet.rollspeed; - gyros.y = packet.pitchspeed; - gyros.z = packet.yawspeed; - // m/s/s - Vector3f accels; - accels.x = (float)packet.xacc / 1000.0; - accels.y = (float)packet.yacc / 1000.0; - accels.z = (float)packet.zacc / 1000.0; + // rad/sec + Vector3f gyros; + gyros.x = packet.rollspeed; + gyros.y = packet.pitchspeed; + gyros.z = packet.yawspeed; + // m/s/s + Vector3f accels; + accels.x = (float)packet.xacc / 1000.0; + accels.y = (float)packet.yacc / 1000.0; + accels.z = (float)packet.zacc / 1000.0; - imu.set_gyro(gyros); + imu.set_gyro(gyros); - imu.set_accel(accels); + imu.set_accel(accels); - // set AHRS hil sensor - ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, - packet.pitchspeed,packet.yawspeed); + // set AHRS hil sensor + ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + packet.pitchspeed,packet.yawspeed); - - break; - } + + break; + } #endif // HIL_MODE != HIL_MODE_DISABLED /* - case MAVLINK_MSG_ID_HEARTBEAT: - { - // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes - if(msg->sysid != g.sysid_my_gcs) break; - rc_override_fs_timer = millis(); - break; - } - - #if HIL_MODE != HIL_MODE_DISABLED - // This is used both as a sensor and to pass the location - // in HIL_ATTITUDE mode. - case MAVLINK_MSG_ID_GPS_RAW: - { - // decode - mavlink_gps_raw_t packet; - mavlink_msg_gps_raw_decode(msg, &packet); - - // set gps hil sensor - g_gps->setHIL(packet.usec/1000,packet.lat,packet.lon,packet.alt, - packet.v,packet.hdg,0,0); - break; - } -#endif -*/ + * case MAVLINK_MSG_ID_HEARTBEAT: + * { + * // We keep track of the last time we received a heartbeat from our GCS for failsafe purposes + * if(msg->sysid != g.sysid_my_gcs) break; + * rc_override_fs_timer = millis(); + * break; + * } + * + * #if HIL_MODE != HIL_MODE_DISABLED + * // This is used both as a sensor and to pass the location + * // in HIL_ATTITUDE mode. + * case MAVLINK_MSG_ID_GPS_RAW: + * { + * // decode + * mavlink_gps_raw_t packet; + * mavlink_msg_gps_raw_decode(msg, &packet); + * + * // set gps hil sensor + * g_gps->setHIL(packet.usec/1000,packet.lat,packet.lon,packet.alt, + * packet.v,packet.hdg,0,0); + * break; + * } + * #endif + */ #if HIL_MODE == HIL_MODE_SENSORS case MAVLINK_MSG_ID_RAW_IMU: // 28 - { - // decode - mavlink_raw_imu_t packet; - mavlink_msg_raw_imu_decode(msg, &packet); + { + // decode + mavlink_raw_imu_t packet; + mavlink_msg_raw_imu_decode(msg, &packet); - // set imu hil sensors - // TODO: check scaling for temp/absPress - float temp = 70; - float absPress = 1; - // Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc); - // Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro); + // set imu hil sensors + // TODO: check scaling for temp/absPress + float temp = 70; + float absPress = 1; + // Serial.printf_P(PSTR("accel:\t%d\t%d\t%d\n"), packet.xacc, packet.yacc, packet.zacc); + // Serial.printf_P(PSTR("gyro:\t%d\t%d\t%d\n"), packet.xgyro, packet.ygyro, packet.zgyro); - // rad/sec - Vector3f gyros; - gyros.x = (float)packet.xgyro / 1000.0; - gyros.y = (float)packet.ygyro / 1000.0; - gyros.z = (float)packet.zgyro / 1000.0; - // m/s/s - Vector3f accels; - accels.x = (float)packet.xacc / 1000.0; - accels.y = (float)packet.yacc / 1000.0; - accels.z = (float)packet.zacc / 1000.0; + // rad/sec + Vector3f gyros; + gyros.x = (float)packet.xgyro / 1000.0; + gyros.y = (float)packet.ygyro / 1000.0; + gyros.z = (float)packet.zgyro / 1000.0; + // m/s/s + Vector3f accels; + accels.x = (float)packet.xacc / 1000.0; + accels.y = (float)packet.yacc / 1000.0; + accels.z = (float)packet.zacc / 1000.0; - imu.set_gyro(gyros); + imu.set_gyro(gyros); - imu.set_accel(accels); + imu.set_accel(accels); - compass.setHIL(packet.xmag,packet.ymag,packet.zmag); - break; - } + compass.setHIL(packet.xmag,packet.ymag,packet.zmag); + break; + } case MAVLINK_MSG_ID_RAW_PRESSURE: //29 - { - // decode - mavlink_raw_pressure_t packet; - mavlink_msg_raw_pressure_decode(msg, &packet); + { + // decode + mavlink_raw_pressure_t packet; + mavlink_msg_raw_pressure_decode(msg, &packet); - // set pressure hil sensor - // TODO: check scaling - float temp = 70; - barometer.setHIL(temp,packet.press_diff1); - break; - } + // set pressure hil sensor + // TODO: check scaling + float temp = 70; + barometer.setHIL(temp,packet.press_diff1); + break; + } #endif // HIL_MODE #if CAMERA == ENABLED case MAVLINK_MSG_ID_DIGICAM_CONFIGURE: - { - g.camera.configure_msg(msg); - break; - } + { + g.camera.configure_msg(msg); + break; + } case MAVLINK_MSG_ID_DIGICAM_CONTROL: - { - g.camera.control_msg(msg); - break; - } + { + g.camera.control_msg(msg); + break; + } #endif // CAMERA == ENABLED #if MOUNT == ENABLED case MAVLINK_MSG_ID_MOUNT_CONFIGURE: - { - camera_mount.configure_msg(msg); - break; - } + { + camera_mount.configure_msg(msg); + break; + } case MAVLINK_MSG_ID_MOUNT_CONTROL: - { - camera_mount.control_msg(msg); - break; - } + { + camera_mount.control_msg(msg); + break; + } case MAVLINK_MSG_ID_MOUNT_STATUS: - { - camera_mount.status_msg(msg); - break; - } + { + camera_mount.status_msg(msg); + break; + } #endif // MOUNT == ENABLED case MAVLINK_MSG_ID_RADIO: - { - mavlink_radio_t packet; - mavlink_msg_radio_decode(msg, &packet); - // use the state of the transmit buffer in the radio to - // control the stream rate, giving us adaptive software - // flow control - if (packet.txbuf < 20 && stream_slowdown < 100) { - // we are very low on space - slow down a lot - stream_slowdown += 3; - } else if (packet.txbuf < 50 && stream_slowdown < 100) { - // we are a bit low on space, slow down slightly - stream_slowdown += 1; - } else if (packet.txbuf > 95 && stream_slowdown > 10) { - // the buffer has plenty of space, speed up a lot - stream_slowdown -= 2; - } else if (packet.txbuf > 90 && stream_slowdown != 0) { - // the buffer has enough space, speed up a bit - stream_slowdown--; - } - break; + { + mavlink_radio_t packet; + mavlink_msg_radio_decode(msg, &packet); + // use the state of the transmit buffer in the radio to + // control the stream rate, giving us adaptive software + // flow control + if (packet.txbuf < 20 && stream_slowdown < 100) { + // we are very low on space - slow down a lot + stream_slowdown += 3; + } else if (packet.txbuf < 50 && stream_slowdown < 100) { + // we are a bit low on space, slow down slightly + stream_slowdown += 1; + } else if (packet.txbuf > 95 && stream_slowdown > 10) { + // the buffer has plenty of space, speed up a lot + stream_slowdown -= 2; + } else if (packet.txbuf > 90 && stream_slowdown != 0) { + // the buffer has enough space, speed up a bit + stream_slowdown--; } + break; + } #ifdef AP_LIMITS - // receive an AP_Limits fence point from GCS and store in EEPROM - // receive a fence point from GCS and store in EEPROM - case MAVLINK_MSG_ID_FENCE_POINT: { - mavlink_fence_point_t packet; - mavlink_msg_fence_point_decode(msg, &packet); - if (packet.count != geofence_limit.fence_total()) { - send_text(SEVERITY_LOW,PSTR("bad fence point")); - } else { - Vector2l point; - point.x = packet.lat*1.0e7; - point.y = packet.lng*1.0e7; - geofence_limit.set_fence_point_with_index(point, packet.idx); - } - break; + // receive an AP_Limits fence point from GCS and store in EEPROM + // receive a fence point from GCS and store in EEPROM + case MAVLINK_MSG_ID_FENCE_POINT: { + mavlink_fence_point_t packet; + mavlink_msg_fence_point_decode(msg, &packet); + if (packet.count != geofence_limit.fence_total()) { + send_text(SEVERITY_LOW,PSTR("bad fence point")); + } else { + Vector2l point; + point.x = packet.lat*1.0e7; + point.y = packet.lng*1.0e7; + geofence_limit.set_fence_point_with_index(point, packet.idx); } - // send a fence point to GCS - case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { - mavlink_fence_fetch_point_t packet; - mavlink_msg_fence_fetch_point_decode(msg, &packet); - if (mavlink_check_target(packet.target_system, packet.target_component)) - break; - if (packet.idx >= geofence_limit.fence_total()) { - send_text(SEVERITY_LOW,PSTR("bad fence point")); - } else { + break; + } + // send a fence point to GCS + case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { + mavlink_fence_fetch_point_t packet; + mavlink_msg_fence_fetch_point_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + if (packet.idx >= geofence_limit.fence_total()) { + send_text(SEVERITY_LOW,PSTR("bad fence point")); + } else { Vector2l point = geofence_limit.get_fence_point_with_index(packet.idx); - mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, geofence_limit.fence_total(), - point.x*1.0e-7, point.y*1.0e-7); - } - break; - } + mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, geofence_limit.fence_total(), + point.x*1.0e-7, point.y*1.0e-7); + } + break; + } #endif // AP_LIMITS ENABLED - } // end switch + } // end switch } // end handle mavlink uint16_t GCS_MAVLINK::_count_parameters() { - // if we haven't cached the parameter count yet... - if (0 == _parameter_count) { + // if we haven't cached the parameter count yet... + if (0 == _parameter_count) { AP_Param *vp; AP_Param::ParamToken token; vp = AP_Param::first(&token, NULL); - do { - _parameter_count++; + do { + _parameter_count++; } while (NULL != (vp = AP_Param::next_scalar(&token, NULL))); - } - return _parameter_count; + } + return _parameter_count; } /** -* @brief Send the next pending parameter, called from deferred message -* handling code -*/ + * @brief Send the next pending parameter, called from deferred message + * handling code + */ void GCS_MAVLINK::queued_param_send() { @@ -1909,7 +1909,7 @@ GCS_MAVLINK::queued_param_send() if (NULL == _queued_parameter) return; AP_Param *vp; - float value; + float value; // copy the current parameter and prepare to move to the next vp = _queued_parameter; @@ -1933,9 +1933,9 @@ GCS_MAVLINK::queued_param_send() } /** -* @brief Send the next pending waypoint, called from deferred message -* handling code -*/ + * @brief Send the next pending waypoint, called from deferred message + * handling code + */ void GCS_MAVLINK::queued_waypoint_send() { @@ -1950,24 +1950,24 @@ GCS_MAVLINK::queued_waypoint_send() } /* - a delay() callback that processes MAVLink packets. We set this as the - callback in long running library initialisation routines to allow - MAVLink to process packets while waiting for the initialisation to - complete -*/ + * a delay() callback that processes MAVLink packets. We set this as the + * callback in long running library initialisation routines to allow + * MAVLink to process packets while waiting for the initialisation to + * complete + */ static void mavlink_delay(unsigned long t) { uint32_t tstart; static uint32_t last_1hz, last_50hz, last_5s; - if (in_mavlink_delay) { + if (in_mavlink_delay) { // this should never happen, but let's not tempt fate by // letting the stack grow too much - delay(t); - return; - } + delay(t); + return; + } - in_mavlink_delay = true; + in_mavlink_delay = true; tstart = millis(); do { @@ -1987,16 +1987,16 @@ static void mavlink_delay(unsigned long t) gcs_send_text_P(SEVERITY_LOW, PSTR("Initialising APM...")); } delay(1); - #if USB_MUX_PIN > 0 +#if USB_MUX_PIN > 0 check_usb_mux(); - #endif +#endif } while (millis() - tstart < t); - in_mavlink_delay = false; + in_mavlink_delay = false; } /* - send a message on both GCS links + * send a message on both GCS links */ static void gcs_send_message(enum ap_message id) { @@ -2007,7 +2007,7 @@ 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(void) { @@ -2018,11 +2018,11 @@ static void gcs_data_stream_send(void) } /* - look for incoming commands on the GCS links + * look for incoming commands on the GCS links */ static void gcs_update(void) { - gcs0.update(); + gcs0.update(); if (gcs3.initialised) { gcs3.update(); } @@ -2045,9 +2045,9 @@ static void gcs_send_text_P(gcs_severity severity, const prog_char_t *str) } /* - send a low priority formatted message to the GCS - only one fits in the queue, so if you send more than one before the - last one gets into the serial buffer then the old one will be lost + * send a low priority formatted message to the GCS + * only one fits in the queue, so if you send more than one before the + * last one gets into the serial buffer then the old one will be lost */ static void gcs_send_text_fmt(const prog_char_t *fmt, ...) {