From f7b8fecad9e796c45b572cd20e7ccfce5a3c1f72 Mon Sep 17 00:00:00 2001 From: uncrustify Date: Tue, 21 Aug 2012 19:19:51 -0700 Subject: [PATCH] uncrustify ArduPlane/GCS_Mavlink.pde --- ArduPlane/GCS_Mavlink.pde | 1576 ++++++++++++++++++------------------- 1 file changed, 788 insertions(+), 788 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index b16e04acf9..2fe7b21e32 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -11,19 +11,19 @@ static mavlink_statustext_t pending_status; 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 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) @@ -195,7 +195,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); @@ -206,8 +206,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; } @@ -250,7 +250,7 @@ static void NOINLINE send_location(mavlink_channel_t chan) static void NOINLINE send_nav_controller_output(mavlink_channel_t chan) { - int16_t bearing = (hold_course==-1?nav_bearing_cd:hold_course) / 100; + int16_t bearing = (hold_course==-1 ? nav_bearing_cd : hold_course) / 100; mavlink_msg_nav_controller_output_send( chan, nav_roll_cd * 0.01, @@ -325,18 +325,18 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan) static void NOINLINE send_radio_out(mavlink_channel_t chan) { - mavlink_msg_servo_output_raw_send( - chan, - micros(), - 0, // port - APM_RC.OutputCh_current(0), - APM_RC.OutputCh_current(1), - APM_RC.OutputCh_current(2), - APM_RC.OutputCh_current(3), - APM_RC.OutputCh_current(4), - APM_RC.OutputCh_current(5), - APM_RC.OutputCh_current(6), - APM_RC.OutputCh_current(7)); + mavlink_msg_servo_output_raw_send( + chan, + micros(), + 0, // port + APM_RC.OutputCh_current(0), + APM_RC.OutputCh_current(1), + APM_RC.OutputCh_current(2), + APM_RC.OutputCh_current(3), + APM_RC.OutputCh_current(4), + APM_RC.OutputCh_current(5), + APM_RC.OutputCh_current(6), + APM_RC.OutputCh_current(7)); } static void NOINLINE send_vfr_hud(mavlink_channel_t chan) @@ -623,7 +623,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; } @@ -708,14 +708,14 @@ void mavlink_send_text(mavlink_channel_t chan, gcs_severity severity, const char 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 }; @@ -738,7 +738,7 @@ GCS_MAVLINK::init(FastSerial * port) mavlink_comm_1_port = port; chan = MAVLINK_COMM_1; } - _queued_parameter = NULL; + _queued_parameter = NULL; } void @@ -747,7 +747,7 @@ GCS_MAVLINK::update(void) // receive new packets mavlink_message_t msg; mavlink_status_t status; - status.packet_rx_drop_count = 0; + status.packet_rx_drop_count = 0; // process received bytes while (comm_get_available(chan)) @@ -756,7 +756,7 @@ GCS_MAVLINK::update(void) #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 == 0 && millis() < 20000) { if (c == '\n' || c == '\r') { crlf_count++; @@ -793,12 +793,12 @@ GCS_MAVLINK::update(void) } // stop waypoint sending if timeout - if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout){ + if (waypoint_sending && (millis() - waypoint_timelast_send) > waypoint_send_timeout) { waypoint_sending = false; } // stop waypoint receiving if timeout - if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout){ + if (waypoint_receiving && (millis() - waypoint_timelast_receive) > waypoint_receive_timeout) { waypoint_receiving = false; } } @@ -811,7 +811,7 @@ bool GCS_MAVLINK::stream_trigger(enum streams stream_num) // send at a much lower rate while handling waypoints and // parameter sends - if (waypoint_receiving || waypoint_sending || + if (waypoint_receiving || waypoint_sending || _queued_parameter != NULL) { rate *= 0.25; } @@ -938,433 +938,433 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) switch (msg->msgid) { case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: - { - // decode - mavlink_request_data_stream_t packet; - mavlink_msg_request_data_stream_decode(msg, &packet); + { + // 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.set_and_save_ifchanged(freq); - streamRateExtendedStatus.set_and_save_ifchanged(freq); - streamRateRCChannels.set_and_save_ifchanged(freq); - streamRateRawController.set_and_save_ifchanged(freq); - streamRatePosition.set_and_save_ifchanged(freq); - streamRateExtra1.set_and_save_ifchanged(freq); - streamRateExtra2.set_and_save_ifchanged(freq); - streamRateExtra3.set_and_save_ifchanged(freq); - break; + case MAV_DATA_STREAM_ALL: + streamRateRawSensors.set_and_save_ifchanged(freq); + streamRateExtendedStatus.set_and_save_ifchanged(freq); + streamRateRCChannels.set_and_save_ifchanged(freq); + streamRateRawController.set_and_save_ifchanged(freq); + streamRatePosition.set_and_save_ifchanged(freq); + streamRateExtra1.set_and_save_ifchanged(freq); + streamRateExtra2.set_and_save_ifchanged(freq); + streamRateExtra3.set_and_save_ifchanged(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_ifchanged(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_ifchanged(freq); + break; - case MAV_DATA_STREAM_RC_CHANNELS: - streamRateRCChannels.set_and_save_ifchanged(freq); - break; + case MAV_DATA_STREAM_RC_CHANNELS: + streamRateRCChannels.set_and_save_ifchanged(freq); + break; - case MAV_DATA_STREAM_RAW_CONTROLLER: - streamRateRawController.set_and_save_ifchanged(freq); - break; + case MAV_DATA_STREAM_RAW_CONTROLLER: + streamRateRawController.set_and_save_ifchanged(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.set_and_save_ifchanged(freq); - break; + case MAV_DATA_STREAM_POSITION: + streamRatePosition.set_and_save_ifchanged(freq); + break; - case MAV_DATA_STREAM_EXTRA1: - streamRateExtra1.set_and_save_ifchanged(freq); - break; + case MAV_DATA_STREAM_EXTRA1: + streamRateExtra1.set_and_save_ifchanged(freq); + break; - case MAV_DATA_STREAM_EXTRA2: - streamRateExtra2.set_and_save_ifchanged(freq); - break; + case MAV_DATA_STREAM_EXTRA2: + streamRateExtra2.set_and_save_ifchanged(freq); + break; - case MAV_DATA_STREAM_EXTRA3: - streamRateExtra3.set_and_save_ifchanged(freq); - break; + case MAV_DATA_STREAM_EXTRA3: + streamRateExtra3.set_and_save_ifchanged(freq); + break; - default: - 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_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) { - startup_IMU_ground(true); - } else if (packet.param3 == 1) { - init_barometer(); - if (airspeed.enabled()) { - zero_airspeed(); - } + case MAV_CMD_PREFLIGHT_CALIBRATION: + if (packet.param1 == 1 || + packet.param2 == 1) { + startup_IMU_ground(true); + } else if (packet.param3 == 1) { + init_barometer(); + if (airspeed.enabled()) { + zero_airspeed(); } - if (packet.param4 == 1) { - trim_radio(); - } - result = MAV_RESULT_ACCEPTED; - break; - - - default: - result = MAV_RESULT_UNSUPPORTED; - break; } + 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; } + mavlink_msg_command_ack_send( + chan, + packet.command, + result); + + break; + } + case MAVLINK_MSG_ID_SET_MODE: - { - // 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 MANUAL: - case CIRCLE: - case STABILIZE: - case FLY_BY_WIRE_A: - case FLY_BY_WIRE_B: - case FLY_BY_WIRE_C: - case AUTO: - case RTL: - case LOITER: - set_mode(packet.custom_mode); - break; - } + { + // 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 MANUAL: + case CIRCLE: + case STABILIZE: + case FLY_BY_WIRE_A: + case FLY_BY_WIRE_B: + case FLY_BY_WIRE_C: + case AUTO: + case RTL: + case LOITER: + set_mode(packet.custom_mode); + break; + } + + break; + } case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: - { - // 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; - - // Start sending waypoints - mavlink_msg_mission_count_send( - chan,msg->sysid, - msg->compid, - g.command_total + 1); // + home - - waypoint_timelast_send = millis(); - waypoint_sending = true; - waypoint_receiving = false; - waypoint_dest_sysid = msg->sysid; - waypoint_dest_compid = msg->compid; + { + // 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; - } + + // Start sending waypoints + mavlink_msg_mission_count_send( + chan,msg->sysid, + msg->compid, + g.command_total + 1); // + home + + waypoint_timelast_send = millis(); + waypoint_sending = true; + waypoint_receiving = false; + waypoint_dest_sysid = msg->sysid; + waypoint_dest_compid = msg->compid; + break; + } - // XXX read a WP from EEPROM and send it to the GCS + // XXX read a WP from EEPROM and send it to the GCS case MAVLINK_MSG_ID_MISSION_REQUEST: - { - // 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 + { + // 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 - // decode - mavlink_mission_request_t packet; - mavlink_msg_mission_request_decode(msg, &packet); + // decode + mavlink_mission_request_t packet; + mavlink_msg_mission_request_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; - // send waypoint - tell_command = get_cmd_with_index(packet.seq); + // send waypoint + tell_command = get_cmd_with_index(packet.seq); - // set frame of waypoint - uint8_t frame; + // set frame of waypoint + uint8_t frame; - if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { - frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame + if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + frame = MAV_FRAME_GLOBAL_RELATIVE_ALT; // reference frame + } else { + frame = MAV_FRAME_GLOBAL; // reference frame + } + + float param1 = 0, param2 = 0, param3 = 0, param4 = 0; + + // time that the mav should loiter in milliseconds + uint8_t current = 0; // 1 (true), 0 (false) + + if (packet.seq == (uint16_t)g.command_index) + current = 1; + + uint8_t autocontinue = 1; // 1 (true), 0 (false) + + float x = 0, y = 0, z = 0; + + if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) { + // command needs scaling + x = tell_command.lat/1.0e7; // local (x), global (latitude) + y = tell_command.lng/1.0e7; // local (y), global (longitude) + if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { + z = (tell_command.alt - home.alt) / 1.0e2; // because tell_command.alt already includes a += home.alt } else { - frame = MAV_FRAME_GLOBAL; // reference frame + z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) } + } - float param1 = 0, param2 = 0 , param3 = 0, param4 = 0; + switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - // time that the mav should loiter in milliseconds - uint8_t current = 0; // 1 (true), 0 (false) + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_DO_SET_HOME: + case MAV_CMD_NAV_LOITER_TIME: + param1 = tell_command.p1; + break; - if (packet.seq == (uint16_t)g.command_index) - current = 1; + case MAV_CMD_CONDITION_CHANGE_ALT: + x=0; // Clear fields loaded above that we don't want sent for this command + y=0; + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + param1 = tell_command.lat; + break; - uint8_t autocontinue = 1; // 1 (true), 0 (false) + case MAV_CMD_DO_JUMP: + param2 = tell_command.lat; + param1 = tell_command.p1; + break; - float x = 0, y = 0, z = 0; + 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; - if (tell_command.id < MAV_CMD_NAV_LAST || tell_command.id == MAV_CMD_CONDITION_CHANGE_ALT) { - // command needs scaling - x = tell_command.lat/1.0e7; // local (x), global (latitude) - y = tell_command.lng/1.0e7; // local (y), global (longitude) - if (tell_command.options & MASK_OPTIONS_RELATIVE_ALT) { - z = (tell_command.alt - home.alt) / 1.0e2; // because tell_command.alt already includes a += home.alt - } else { - z = tell_command.alt/1.0e2; // local (z), global/relative (altitude) - } - } - - switch (tell_command.id) { // Switch to map APM command fields inot MAVLink command fields - - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_DO_SET_HOME: - case MAV_CMD_NAV_LOITER_TIME: - param1 = tell_command.p1; - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - x=0; // Clear fields loaded above that we don't want sent for this command - y=0; - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - param1 = tell_command.lat; - break; - - case MAV_CMD_DO_JUMP: - param2 = tell_command.lat; - 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; - } - - 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); - - // update last waypoint comm stamp - waypoint_timelast_send = millis(); + 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; } + 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); + + // update last waypoint comm stamp + waypoint_timelast_send = millis(); + break; + } + case MAVLINK_MSG_ID_MISSION_ACK: - { - // decode - mavlink_mission_ack_t packet; - mavlink_msg_mission_ack_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) 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; - // turn off waypoint send - waypoint_sending = false; - break; - } + // turn off waypoint send + waypoint_sending = false; + break; + } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: - { - // 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; + { + // 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; - // Start sending parameters - next call to ::update will kick the first one out + // 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; - } + _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; } + 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); + break; + } + case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: - { - // 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; + { + // 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; - // clear all commands - g.command_total.set_and_save(0); + // clear all commands + g.command_total.set_and_save(0); - // note that we don't send multiple acks, as otherwise a - // GCS that is doing a clear followed by a set may see - // the additional ACKs as ACKs of the set operations - mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); - break; - } + // note that we don't send multiple acks, as otherwise a + // GCS that is doing a clear followed by a set may see + // the additional ACKs as ACKs of the set operations + mavlink_msg_mission_ack_send(chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); + break; + } case MAVLINK_MSG_ID_MISSION_SET_CURRENT: - { - // 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; + { + // 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; - // set current command - change_command(packet.seq); + // set current command + change_command(packet.seq); - mavlink_msg_mission_current_send(chan, g.command_index); - break; - } + mavlink_msg_mission_current_send(chan, g.command_index); + break; + } case MAVLINK_MSG_ID_MISSION_COUNT: - { - // decode - mavlink_mission_count_t packet; - mavlink_msg_mission_count_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) 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 - 1); - - waypoint_timelast_receive = millis(); - waypoint_timelast_request = 0; - waypoint_receiving = true; - waypoint_sending = false; - waypoint_request_i = 0; - waypoint_request_last= g.command_total; - break; + // start waypoint receiving + if (packet.count > MAX_WAYPOINTS) { + packet.count = MAX_WAYPOINTS; } + g.command_total.set_and_save(packet.count - 1); + + waypoint_timelast_receive = millis(); + waypoint_timelast_request = 0; + waypoint_receiving = true; + waypoint_sending = false; + waypoint_request_i = 0; + waypoint_request_last= g.command_total; + break; + } case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: - { - // decode - mavlink_mission_write_partial_list_t packet; - mavlink_msg_mission_write_partial_list_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; + { + // decode + mavlink_mission_write_partial_list_t packet; + mavlink_msg_mission_write_partial_list_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; - // start waypoint receiving - if (packet.start_index >= g.command_total || - packet.end_index > g.command_total || - packet.end_index < packet.start_index) { - send_text(SEVERITY_LOW,PSTR("flight plan update rejected")); - break; - } - - waypoint_timelast_receive = millis(); - waypoint_timelast_request = 0; - waypoint_receiving = true; - waypoint_sending = false; - waypoint_request_i = packet.start_index; - waypoint_request_last= packet.end_index; + // start waypoint receiving + if (packet.start_index >= g.command_total || + packet.end_index > g.command_total || + packet.end_index < packet.start_index) { + send_text(SEVERITY_LOW,PSTR("flight plan update rejected")); break; } + waypoint_timelast_receive = millis(); + waypoint_timelast_request = 0; + waypoint_receiving = true; + waypoint_sending = false; + waypoint_request_i = packet.start_index; + waypoint_request_last= packet.end_index; + break; + } + #ifdef MAVLINK_MSG_ID_SET_MAG_OFFSETS case MAVLINK_MSG_ID_SET_MAG_OFFSETS: { @@ -1376,191 +1376,191 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) } #endif - // XXX receive a WP from GCS and store in EEPROM + // XXX receive a WP from GCS and store in EEPROM case MAVLINK_MSG_ID_MISSION_ITEM: + { + // decode + mavlink_mission_item_t packet; + uint8_t result = MAV_MISSION_ACCEPTED; + + mavlink_msg_mission_item_decode(msg, &packet); + if (mavlink_check_target(packet.target_system,packet.target_component)) break; + + // defaults + tell_command.id = packet.command; + + switch (packet.frame) { - // decode - mavlink_mission_item_t packet; - uint8_t result = MAV_MISSION_ACCEPTED; - - mavlink_msg_mission_item_decode(msg, &packet); - if (mavlink_check_target(packet.target_system,packet.target_component)) break; - - // defaults - tell_command.id = packet.command; - - 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_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; + } #ifdef MAV_FRAME_LOCAL_NED - case MAV_FRAME_LOCAL_NED: // 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_LOCAL_NED: // 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; + } #endif #ifdef MAV_FRAME_LOCAL - 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_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; + } #endif - case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude - { - 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_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude + { + 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; + } - default: - result = MAV_MISSION_UNSUPPORTED_FRAME; - break; - } + default: + result = MAV_MISSION_UNSUPPORTED_FRAME; + break; + } - - if (result != MAV_MISSION_ACCEPTED) goto mission_failed; - // Switch to map APM command fields into MAVLink command fields - switch (tell_command.id) { - case MAV_CMD_NAV_WAYPOINT: - case MAV_CMD_NAV_LOITER_UNLIM: - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - case MAV_CMD_NAV_LAND: - break; + if (result != MAV_MISSION_ACCEPTED) goto mission_failed; - case MAV_CMD_NAV_LOITER_TURNS: - case MAV_CMD_NAV_TAKEOFF: - case MAV_CMD_DO_SET_HOME: - case MAV_CMD_NAV_LOITER_TIME: - tell_command.p1 = packet.param1; - break; - - case MAV_CMD_CONDITION_CHANGE_ALT: - tell_command.lat = packet.param1; - break; - - case MAV_CMD_CONDITION_DELAY: - case MAV_CMD_CONDITION_DISTANCE: - tell_command.lat = packet.param1; - break; - - case MAV_CMD_DO_JUMP: - tell_command.lat = packet.param2; - 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_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; - - default: - result = MAV_MISSION_UNSUPPORTED; - break; - } - - if (result != MAV_MISSION_ACCEPTED) goto mission_failed; - - 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_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) { - result = MAV_MISSION_ERROR; - goto mission_failed; - } - - // check if this is the requested waypoint - if (packet.seq != waypoint_request_i) { - result = MAV_MISSION_INVALID_SEQUENCE; - goto mission_failed; - } - - 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 > waypoint_request_last) { - mavlink_msg_mission_ack_send( - chan, - msg->sysid, - msg->compid, - result); - - send_text(SEVERITY_LOW,PSTR("flight plan received")); - waypoint_receiving = false; - // XXX ignores waypoint radius for individual waypoints, can - // only set WP_RADIUS parameter - } - } + // Switch to map APM command fields into MAVLink command fields + switch (tell_command.id) { + case MAV_CMD_NAV_WAYPOINT: + case MAV_CMD_NAV_LOITER_UNLIM: + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + case MAV_CMD_NAV_LAND: break; - mission_failed: - // we are rejecting the mission/waypoint + case MAV_CMD_NAV_LOITER_TURNS: + case MAV_CMD_NAV_TAKEOFF: + case MAV_CMD_DO_SET_HOME: + case MAV_CMD_NAV_LOITER_TIME: + tell_command.p1 = packet.param1; + break; + + case MAV_CMD_CONDITION_CHANGE_ALT: + tell_command.lat = packet.param1; + break; + + case MAV_CMD_CONDITION_DELAY: + case MAV_CMD_CONDITION_DISTANCE: + tell_command.lat = packet.param1; + break; + + case MAV_CMD_DO_JUMP: + tell_command.lat = packet.param2; + 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_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; + + default: + result = MAV_MISSION_UNSUPPORTED; + break; + } + + if (result != MAV_MISSION_ACCEPTED) goto mission_failed; + + 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_guided_WP(); + + // verify we recevied the command mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, - result); - break; + 0); + + } else { + // Check if receiving waypoints (mission upload expected) + if (!waypoint_receiving) { + result = MAV_MISSION_ERROR; + goto mission_failed; + } + + // check if this is the requested waypoint + if (packet.seq != waypoint_request_i) { + result = MAV_MISSION_INVALID_SEQUENCE; + goto mission_failed; + } + + 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 > waypoint_request_last) { + mavlink_msg_mission_ack_send( + chan, + msg->sysid, + msg->compid, + result); + + 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; + +mission_failed: + // we are rejecting the mission/waypoint + mavlink_msg_mission_ack_send( + chan, + msg->sysid, + msg->compid, + result); + break; + } #if GEOFENCE_ENABLED == ENABLED - // receive a 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); @@ -1579,7 +1579,7 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } - // send a fence point to GCS + // 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); @@ -1597,295 +1597,295 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) #endif // GEOFENCE_ENABLED case MAVLINK_MSG_ID_PARAM_SET: - { - AP_Param *vp; - enum ap_var_type var_type; + { + AP_Param *vp; + enum ap_var_type var_type; - // decode - mavlink_param_set_t packet; - mavlink_msg_param_set_decode(msg, &packet); + // decode + mavlink_param_set_t packet; + mavlink_msg_param_set_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; - // set parameter + // 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; + 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 + // 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; + // 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 (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 (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 (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... + // 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 (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 (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 (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; } - break; - } // end case + // 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: - { - // 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); + { + // 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; - rc_override_active = APM_RC.setHIL(v); - rc_override_fs_timer = millis(); + 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; + rc_override_active = APM_RC.setHIL(v); + rc_override_fs_timer = millis(); + break; + } 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; - last_heartbeat_ms = rc_override_fs_timer = millis(); - pmTest1++; - break; - } + { + // 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; + last_heartbeat_ms = rc_override_fs_timer = millis(); + pmTest1++; + 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_INT: - { - // decode - mavlink_gps_raw_int_t packet; - mavlink_msg_gps_raw_int_decode(msg, &packet); + // This is used both as a sensor and to pass the location + // in HIL_ATTITUDE mode. + case MAVLINK_MSG_ID_GPS_RAW_INT: + { + // decode + mavlink_gps_raw_int_t packet; + mavlink_msg_gps_raw_int_decode(msg, &packet); - // 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, - packet.vel*1.0e-2, packet.cog*1.0e-2, 0, 10); - break; - } + // 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, + packet.vel*1.0e-2, packet.cog*1.0e-2, 0, 10); + break; + } - // Is this resolved? - MAVLink protocol change..... + // Is this resolved? - MAVLink protocol change..... case MAVLINK_MSG_ID_VFR_HUD: - { - // decode - mavlink_vfr_hud_t packet; - mavlink_msg_vfr_hud_decode(msg, &packet); + { + // decode + mavlink_vfr_hud_t packet; + mavlink_msg_vfr_hud_decode(msg, &packet); - // set airspeed - airspeed.set_HIL(packet.airspeed); - break; - } + // set airspeed + airspeed.set_HIL(packet.airspeed); + break; + } - 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_cd(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); - - #if HIL_MODE == HIL_MODE_SENSORS - - // rad/sec - Vector3f gyros; - gyros.x = packet.rollspeed; - gyros.y = packet.pitchspeed; - gyros.z = packet.yawspeed; + case MAVLINK_MSG_ID_HIL_STATE: + { + mavlink_hil_state_t packet; + mavlink_msg_hil_state_decode(msg, &packet); - // 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; + float vel = sqrt((packet.vx * (float)packet.vx) + (packet.vy * (float)packet.vy)); + float cog = wrap_360_cd(ToDeg(atan2(packet.vx, packet.vy)) * 100); - imu.set_gyro(gyros); + // 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); - imu.set_accel(accels); - - #else + #if HIL_MODE == HIL_MODE_SENSORS - // set AHRS hil sensor - ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, - packet.pitchspeed,packet.yawspeed); + // rad/sec + Vector3f gyros; + gyros.x = packet.rollspeed; + gyros.y = packet.pitchspeed; + gyros.z = packet.yawspeed; - #endif + // 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; - break; - } + imu.set_gyro(gyros); + + imu.set_accel(accels); + + #else + + // set AHRS hil sensor + ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + packet.pitchspeed,packet.yawspeed); + + #endif + + break; + } #endif // HIL_MODE #if HIL_MODE == HIL_MODE_ATTITUDE case MAVLINK_MSG_ID_ATTITUDE: - { - // decode - mavlink_attitude_t packet; - mavlink_msg_attitude_decode(msg, &packet); + { + // decode + mavlink_attitude_t packet; + mavlink_msg_attitude_decode(msg, &packet); - // set AHRS hil sensor - ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, - packet.pitchspeed,packet.yawspeed); - break; - } + // set AHRS hil sensor + ahrs.setHil(packet.roll,packet.pitch,packet.yaw,packet.rollspeed, + packet.pitchspeed,packet.yawspeed); + break; + } #endif #if HIL_MODE == HIL_MODE_SENSORS case MAVLINK_MSG_ID_RAW_IMU: - { - // 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: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc); - //Serial.printf_P(PSTR("gyro: %d %d %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: %d %d %d\n"), packet.xacc, packet.yacc, packet.zacc); + //Serial.printf_P(PSTR("gyro: %d %d %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: - { - // 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 + 101325); - break; - } + // set pressure hil sensor + // TODO: check scaling + float temp = 70; + barometer.setHIL(temp,packet.press_diff1 + 101325); + 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; + } } // end switch } // end handle mavlink @@ -1907,9 +1907,9 @@ GCS_MAVLINK::_count_parameters() } /** -* @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() { @@ -1917,7 +1917,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; @@ -1941,9 +1941,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() { @@ -1958,11 +1958,11 @@ 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; @@ -2004,7 +2004,7 @@ static void mavlink_delay(unsigned long t) } /* - send a message on both GCS links + * send a message on both GCS links */ static void gcs_send_message(enum ap_message id) { @@ -2015,7 +2015,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) { @@ -2026,11 +2026,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 */ void gcs_send_text_fmt(const prog_char_t *fmt, ...) {