diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h index a82d854a7f..2fade6f73f 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Friday, August 5 2011, 07:37 UTC + * Generated on Saturday, August 13 2011, 04:23 UTC */ #ifndef ARDUPILOTMEGA_H #define ARDUPILOTMEGA_H @@ -33,12 +33,13 @@ extern "C" { // MESSAGE DEFINITIONS +#include "./mavlink_msg_sensor_offsets.h" // MESSAGE LENGTHS #undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink.h b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink.h index 486083900b..587422f06c 100644 --- a/libraries/GCS_MAVLink/include/ardupilotmega/mavlink.h +++ b/libraries/GCS_MAVLink/include/ardupilotmega/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Friday, August 5 2011, 07:37 UTC + * Generated on Saturday, August 13 2011, 04:23 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/libraries/GCS_MAVLink/include/common/common.h b/libraries/GCS_MAVLink/include/common/common.h index d6168006e8..79d3caa427 100644 --- a/libraries/GCS_MAVLink/include/common/common.h +++ b/libraries/GCS_MAVLink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Friday, August 5 2011, 08:13 UTC + * Generated on Saturday, August 13 2011, 04:23 UTC */ #ifndef COMMON_H #define COMMON_H @@ -28,7 +28,7 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ +/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */ enum MAV_CMD { MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint. | Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) | Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) | 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. | Desired yaw angle at waypoint (rotary wing) | Latitude | Longitude | Altitude | */ @@ -63,7 +63,7 @@ enum MAV_CMD MAV_CMD_ENUM_END }; -/** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a recommendation to the autopilot software. Individual autopilots may or may not obey the recommended messages. */ +/** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a recommendation to the autopilot software. Individual autopilots may or may not obey the recommended messages. */ enum MAV_DATA_STREAM { MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */ @@ -138,16 +138,17 @@ enum MAV_ROI #include "./mavlink_msg_control_status.h" #include "./mavlink_msg_safety_set_allowed_area.h" #include "./mavlink_msg_safety_allowed_area.h" -#include "./mavlink_msg_set_roll_pitch_yaw.h" -#include "./mavlink_msg_set_roll_pitch_yaw_speed.h" -#include "./mavlink_msg_attitude_controller_output.h" -#include "./mavlink_msg_position_controller_output.h" +#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h" +#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h" +#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h" +#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h" #include "./mavlink_msg_nav_controller_output.h" #include "./mavlink_msg_position_target.h" #include "./mavlink_msg_state_correction.h" #include "./mavlink_msg_set_altitude.h" #include "./mavlink_msg_request_data_stream.h" -#include "./mavlink_msg_full_state.h" +#include "./mavlink_msg_hil_state.h" +#include "./mavlink_msg_hil_controls.h" #include "./mavlink_msg_manual_control.h" #include "./mavlink_msg_rc_channels_override.h" #include "./mavlink_msg_global_position_int.h" @@ -164,7 +165,7 @@ enum MAV_ROI // MESSAGE LENGTHS #undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/common/mavlink.h b/libraries/GCS_MAVLink/include/common/mavlink.h index 2d1f4c6f65..b144e178dc 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink.h +++ b/libraries/GCS_MAVLink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Friday, August 5 2011, 08:13 UTC + * Generated on Saturday, August 13 2011, 04:23 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/libraries/GCS_MAVLink/include/common/mavlink_msg_request_data_stream.h b/libraries/GCS_MAVLink/include/common/mavlink_msg_request_data_stream.h index f390e4e370..15ee1a6a5f 100644 --- a/libraries/GCS_MAVLink/include/common/mavlink_msg_request_data_stream.h +++ b/libraries/GCS_MAVLink/include/common/mavlink_msg_request_data_stream.h @@ -7,7 +7,7 @@ typedef struct __mavlink_request_data_stream_t uint8_t target_system; ///< The target requested to send the message stream. uint8_t target_component; ///< The target requested to send the message stream. uint8_t req_stream_id; ///< The ID of the requested message type - uint16_t req_message_rate; ///< The requested interval between two messages of this type + uint16_t req_message_rate; ///< Update rate in Hertz uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. } mavlink_request_data_stream_t; @@ -23,7 +23,7 @@ typedef struct __mavlink_request_data_stream_t * @param target_system The target requested to send the message stream. * @param target_component The target requested to send the message stream. * @param req_stream_id The ID of the requested message type - * @param req_message_rate The requested interval between two messages of this type + * @param req_message_rate Update rate in Hertz * @param start_stop 1 to start sending, 0 to stop sending. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -35,7 +35,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream. i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream. i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type - i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // The requested interval between two messages of this type + i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // Update rate in Hertz i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. return mavlink_finalize_message(msg, system_id, component_id, i); @@ -50,7 +50,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u * @param target_system The target requested to send the message stream. * @param target_component The target requested to send the message stream. * @param req_stream_id The ID of the requested message type - * @param req_message_rate The requested interval between two messages of this type + * @param req_message_rate Update rate in Hertz * @param start_stop 1 to start sending, 0 to stop sending. * @return length of the message in bytes (excluding serial stream start sign) */ @@ -62,7 +62,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_ i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream. i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream. i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type - i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // The requested interval between two messages of this type + i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // Update rate in Hertz i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending. return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); @@ -88,7 +88,7 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, * @param target_system The target requested to send the message stream. * @param target_component The target requested to send the message stream. * @param req_stream_id The ID of the requested message type - * @param req_message_rate The requested interval between two messages of this type + * @param req_message_rate Update rate in Hertz * @param start_stop 1 to start sending, 0 to stop sending. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS @@ -136,7 +136,7 @@ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const ma /** * @brief Get field req_message_rate from request_data_stream message * - * @return The requested interval between two messages of this type + * @return Update rate in Hertz */ static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg) { diff --git a/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml b/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml index 72bf88a7cf..98303cbb01 100644 --- a/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml +++ b/libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml @@ -1,6 +1,29 @@ - + - common.xml - - + common.xml + + + + Offsets and calibrations values for hardware + sensors. This makes it easier to debug the calibration process. + magnetometer X offset + magnetometer Y offset + magnetometer Z offset + magnetic declination (radians) + raw pressure from barometer + raw temperature from barometer + gyro X calibration + gyro Y calibration + gyro Z calibration + accel X calibration + accel Y calibration + accel Z calibration + + diff --git a/libraries/GCS_MAVLink/message_definitions/common.xml b/libraries/GCS_MAVLink/message_definitions/common.xml index 4a468ec26c..62de770bc7 100644 --- a/libraries/GCS_MAVLink/message_definitions/common.xml +++ b/libraries/GCS_MAVLink/message_definitions/common.xml @@ -1,268 +1,266 @@ - 2 - - - Commands to be executed by the MAV. They can be executed on user request, - or as part of a mission script. If the action is used in a mission, the parameter mapping - to the waypoint/mission message is as follows: - Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what - ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. - - - Navigate to waypoint. - Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) - Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) - 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. - Desired yaw angle at waypoint (rotary wing) - Latitude - Longitude - Altitude - - - Loiter around this waypoint an unlimited amount of time - Empty - Empty - Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise - Desired yaw angle. - Latitude - Longitude - Altitude - - - Loiter around this waypoint for X turns - Turns - Empty - Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise - Desired yaw angle. - Latitude - Longitude - Altitude - - - Loiter around this waypoint for X seconds - Seconds (decimal) - Empty - Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise - Desired yaw angle. - Latitude - Longitude - Altitude - - - Return to launch location - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Land at location - Empty - Empty - Empty - Desired yaw angle. - Latitude - Longitude - Altitude - - - Takeoff from ground / hand - Minimum pitch (if airspeed sensor present), desired pitch without sensor - Empty - Empty - Yaw angle (if magnetometer present), ignored without magnetometer - Latitude - Longitude - Altitude - - - Sets the region of interest (ROI) for a sensor set or the - vehicle itself. This can then be used by the vehicles control - system to control the vehicle attitude and the attitude of various - sensors such as cameras. - - Region of intereset mode. (see MAV_ROI enum) - Waypoint index/ target ID. (see MAV_ROI enum) - ROI index (allows a vehicle to manage multiple ROI's) - Empty - x the location of the fixed ROI (see MAV_FRAME) - y - z - - - Control autonomous path planning on the MAV. - 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning - 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid - Empty - Yaw angle at goal, in compass degrees, [0..360] - Latitude/X of goal - Longitude/Y of goal - Altitude/Z of goal - - - NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Delay mission state machine. - Delay in seconds (decimal) - Empty - Empty - Empty - Empty - Empty - Empty - - - Ascend/descend at rate. Delay mission state machine until desired altitude reached. - Descent / Ascend rate (m/s) - Empty - Empty - Empty - Empty - Empty - Finish Altitude - - - Delay mission state machine until within desired distance of next NAV point. - Distance (meters) - Empty - Empty - Empty - Empty - Empty - Empty - - - Reach a certain target angle. - target angle: [0-360], 0 is north - speed during yaw change:[deg per second] - direction: negative: counter clockwise, positive: clockwise [-1,1] - relative offset or absolute angle: [ 1,0] - Empty - Empty - Empty - - - NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Set system mode. - Mode, as defined by ENUM MAV_MODE - Empty - Empty - Empty - Empty - Empty - Empty - - - Jump to the desired command in the mission list. Repeat this action only the specified number of times - Sequence number - Repeat count - Empty - Empty - Empty - Empty - Empty - - - Change speed and/or throttle set points. - Speed type (0=Airspeed, 1=Ground Speed) - Speed (m/s, -1 indicates no change) - Throttle ( Percent, -1 indicates no change) - Empty - Empty - Empty - Empty - - - Changes the home location either to the current location or a specified location. - Use current (1=use current location, 0=use specified location) - Empty - Empty - Empty - Latitude - Longitude - Altitude - - - Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. - Parameter number - Parameter value - Empty - Empty - Empty - Empty - Empty - - - Set a relay to a condition. - Relay number - Setting (1=on, 0=off, others possible depending on system hardware) - Empty - Empty - Empty - Empty - Empty - - - Cycle a relay on and off for a desired number of cyles with a desired period. - Relay number - Cycle count - Cycle time (seconds, decimal) - Empty - Empty - Empty - Empty - - - Set a servo to a desired PWM value. - Servo number - PWM (microseconds, 1000 to 2000 typical) - Empty - Empty - Empty - Empty - Empty - - - Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. - Servo number - PWM (microseconds, 1000 to 2000 typical) - Cycle count - Cycle time (seconds) - Empty - Empty - Empty - - - Control onboard camera capturing. - Camera ID (-1 for all) - Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw - Transmission mode: 0: video stream, >0: single images every n seconds (decimal) - Recording: 0: disabled, 1: enabled compressed, 2: enabled raw - Empty - Empty - Empty - + 2 + + + Commands to be executed by the MAV. They can be executed on user request, + or as part of a mission script. If the action is used in a mission, the parameter mapping + to the waypoint/mission message is as follows: + Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what + ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. + + Navigate to waypoint. + Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) + Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) + 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. + Desired yaw angle at waypoint (rotary wing) + Latitude + Longitude + Altitude + + + Loiter around this waypoint an unlimited amount of time + Empty + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X turns + Turns + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Loiter around this waypoint for X seconds + Seconds (decimal) + Empty + Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise + Desired yaw angle. + Latitude + Longitude + Altitude + + + Return to launch location + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Land at location + Empty + Empty + Empty + Desired yaw angle. + Latitude + Longitude + Altitude + + + Takeoff from ground / hand + Minimum pitch (if airspeed sensor present), desired pitch without sensor + Empty + Empty + Yaw angle (if magnetometer present), ignored without magnetometer + Latitude + Longitude + Altitude + + + Sets the region of interest (ROI) for a sensor set or the + vehicle itself. This can then be used by the vehicles control + system to control the vehicle attitude and the attitude of various + sensors such as cameras. + Region of intereset mode. (see MAV_ROI enum) + Waypoint index/ target ID. (see MAV_ROI enum) + ROI index (allows a vehicle to manage multiple ROI's) + Empty + x the location of the fixed ROI (see MAV_FRAME) + y + z + + + Control autonomous path planning on the MAV. + 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning + 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid + Empty + Yaw angle at goal, in compass degrees, [0..360] + Latitude/X of goal + Longitude/Y of goal + Altitude/Z of goal + + + NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Delay mission state machine. + Delay in seconds (decimal) + Empty + Empty + Empty + Empty + Empty + Empty + + + Ascend/descend at rate. Delay mission state machine until desired altitude reached. + Descent / Ascend rate (m/s) + Empty + Empty + Empty + Empty + Empty + Finish Altitude + + + Delay mission state machine until within desired distance of next NAV point. + Distance (meters) + Empty + Empty + Empty + Empty + Empty + Empty + + + Reach a certain target angle. + target angle: [0-360], 0 is north + speed during yaw change:[deg per second] + direction: negative: counter clockwise, positive: clockwise [-1,1] + relative offset or absolute angle: [ 1,0] + Empty + Empty + Empty + + + NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Set system mode. + Mode, as defined by ENUM MAV_MODE + Empty + Empty + Empty + Empty + Empty + Empty + + + Jump to the desired command in the mission list. Repeat this action only the specified number of times + Sequence number + Repeat count + Empty + Empty + Empty + Empty + Empty + + + Change speed and/or throttle set points. + Speed type (0=Airspeed, 1=Ground Speed) + Speed (m/s, -1 indicates no change) + Throttle ( Percent, -1 indicates no change) + Empty + Empty + Empty + Empty + + + Changes the home location either to the current location or a specified location. + Use current (1=use current location, 0=use specified location) + Empty + Empty + Empty + Latitude + Longitude + Altitude + + + Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. + Parameter number + Parameter value + Empty + Empty + Empty + Empty + Empty + + + Set a relay to a condition. + Relay number + Setting (1=on, 0=off, others possible depending on system hardware) + Empty + Empty + Empty + Empty + Empty + + + Cycle a relay on and off for a desired number of cyles with a desired period. + Relay number + Cycle count + Cycle time (seconds, decimal) + Empty + Empty + Empty + Empty + + + Set a servo to a desired PWM value. + Servo number + PWM (microseconds, 1000 to 2000 typical) + Empty + Empty + Empty + Empty + Empty + + + Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. + Servo number + PWM (microseconds, 1000 to 2000 typical) + Cycle count + Cycle time (seconds) + Empty + Empty + Empty + + + Control onboard camera capturing. + Camera ID (-1 for all) + Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw + Transmission mode: 0: video stream, >0: single images every n seconds (decimal) + Recording: 0: disabled, 1: enabled compressed, 2: enabled raw + Empty + Empty + Empty + Sets the region of interest (ROI) for a sensor set or the @@ -278,71 +276,70 @@ y z - - - NOP - This command is only used to mark the upper limit of the DO commands in the enumeration - Empty - Empty - Empty - Empty - Empty - Empty - Empty - - - Trigger calibration. This command will be only accepted if in pre-flight mode. - Gyro calibration: 0: no, 1: yes - Magnetometer calibration: 0: no, 1: yes - Ground pressure: 0: no, 1: yes - Radio calibration: 0: no, 1: yes - Empty - Empty - Empty - - - Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. - Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM - Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM - Reserved - Reserved - Empty - Empty - Empty - - - - Data stream IDs. A data stream is not a fixed set of messages, but rather a - recommendation to the autopilot software. Individual autopilots may or may not obey - the recommended messages. - - - Enable all data streams - - - Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. - - - Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS - - - Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW - - - Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. - - - Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. - - - Dependent on the autopilot - - - Dependent on the autopilot - - - Dependent on the autopilot - - + + NOP - This command is only used to mark the upper limit of the DO commands in the enumeration + Empty + Empty + Empty + Empty + Empty + Empty + Empty + + + Trigger calibration. This command will be only accepted if in pre-flight mode. + Gyro calibration: 0: no, 1: yes + Magnetometer calibration: 0: no, 1: yes + Ground pressure: 0: no, 1: yes + Radio calibration: 0: no, 1: yes + Empty + Empty + Empty + + + Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. + Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM + Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM + Reserved + Reserved + Empty + Empty + Empty + + + + Data stream IDs. A data stream is not a fixed set of messages, but rather a + recommendation to the autopilot software. Individual autopilots may or may not obey + the recommended messages. + + + Enable all data streams + + + Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. + + + Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS + + + Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW + + + Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. + + + Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. + + + Dependent on the autopilot + + + Dependent on the autopilot + + + Dependent on the autopilot + + The ROI (region of interest) for the vehicle. This can be be used by the vehicle for camera/vehicle attitude alignment (see @@ -365,452 +362,449 @@ - - - The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). - Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - MAVLink version - - - The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions. - The onboard software version - - - The system time is the time of the master clock, typically the computer clock of the main onboard computer. - Timestamp of the master clock in microseconds since UNIX epoch. - - - A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. - PING sequence - 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - Unix timestamp in microseconds - - - UTC date and time from GPS module - GPS UTC date ddmmyy - GPS UTC time hhmmss - - - Request to control this MAV - System the GCS requests control for - 0: request control of this MAV, 1: Release control of this MAV - 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - - - Accept / deny control of this MAV - ID of the GCS this message - 0: request control of this MAV, 1: Release control of this MAV - 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - - - Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. - key - - - This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - The action id - 0: Action DENIED, 1: Action executed - - - An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h - The system executing the action - The component executing the action - The action id - - - Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. - The system setting the mode - The new mode - - - Set the system navigation mode, as defined by enum MAV_NAV_MODE in mavlink/include/mavlink_types.h. The navigation mode applies to the whole aircraft and thus all components. - The system setting the mode - The new navigation mode - - - Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. - System ID - Component ID - Onboard parameter id - Parameter index. Send -1 to use the param ID field as identifier - - - Request all parameters of this component. After his request, all parameters are emitted. - System ID - Component ID - - - Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. - Onboard parameter id - Onboard parameter value - Total number of onboard parameters - Index of this onboard parameter - - - Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. - System ID - Component ID - Onboard parameter id - Onboard parameter value - - - The global position, as returned by the Global Positioning System (GPS). This is - NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) - - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - Latitude in 1E7 degrees - Longitude in 1E7 degrees - Altitude in 1E3 meters (millimeters) - GPS HDOP - GPS VDOP - GPS ground speed (m/s) - Compass heading in degrees, 0..360 degrees - - - The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X acceleration (mg) - Y acceleration (mg) - Z acceleration (mg) - Angular speed around X axis (millirad /sec) - Angular speed around Y axis (millirad /sec) - Angular speed around Z axis (millirad /sec) - X Magnetic field (milli tesla) - Y Magnetic field (milli tesla) - Z Magnetic field (milli tesla) - - - The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. - Number of satellites visible - Global satellite ID - 0: Satellite not used, 1: used for localization - Elevation (0: right on top of receiver, 90: on the horizon) of satellite - Direction of satellite, 0: 0 deg, 255: 360 deg. - Signal to noise ratio of satellite - - - The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X acceleration (raw) - Y acceleration (raw) - Z acceleration (raw) - Angular speed around X axis (raw) - Angular speed around Y axis (raw) - Angular speed around Z axis (raw) - X Magnetic field (raw) - Y Magnetic field (raw) - Z Magnetic field (raw) - - - The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Absolute pressure (raw) - Differential pressure 1 (raw) - Differential pressure 2 (raw) - Raw Temperature measurement (raw) - - - The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Absolute pressure (hectopascal) - Differential pressure 1 (hectopascal) - Temperature measurement (0.01 degrees celsius) - - - The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - Roll angle (rad) - Pitch angle (rad) - Yaw angle (rad) - Roll angular speed (rad/s) - Pitch angular speed (rad/s) - Yaw angular speed (rad/s) - - - The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame) - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - X Position - Y Position - Z Position - X Speed - Y Speed - Z Speed - - - The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame) - Timestamp (microseconds since unix epoch) - Latitude, in degrees - Longitude, in degrees - Absolute altitude, in meters - X Speed (in Latitude direction, positive: going north) - Y Speed (in Longitude direction, positive: going east) - Z Speed (in Altitude direction, positive: going up) - - - The global position, as returned by the Global Positioning System (GPS). This is - NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) - - Timestamp (microseconds since UNIX epoch or microseconds since system boot) - 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - Latitude in degrees - Longitude in degrees - Altitude in meters - GPS HDOP - GPS VDOP - GPS ground speed - Compass heading in degrees, 0..360 degrees - - - The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. - System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - Navigation mode, see MAV_NAV_MODE ENUM - System status flag, see MAV_STATUS ENUM - Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - Battery voltage, in millivolts (1 = 1 millivolt) - Remaining battery energy: (0%: 0, 100%: 1000) - Dropped packets (packets that were corrupted on reception on the MAV) - - - The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. - RC channel 1 value, in microseconds - RC channel 2 value, in microseconds - RC channel 3 value, in microseconds - RC channel 4 value, in microseconds - RC channel 5 value, in microseconds - RC channel 6 value, in microseconds - RC channel 7 value, in microseconds - RC channel 8 value, in microseconds - Receive signal strength indicator, 0: 0%, 255: 100% - - - The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - Receive signal strength indicator, 0: 0%, 255: 100% - - - The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. - Servo output 1 value, in microseconds - Servo output 2 value, in microseconds - Servo output 3 value, in microseconds - Servo output 4 value, in microseconds - Servo output 5 value, in microseconds - Servo output 6 value, in microseconds - Servo output 7 value, in microseconds - Servo output 8 value, in microseconds - - - Message encoding a waypoint. This message is emitted to announce - the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed - - System ID - Component ID - Sequence - The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - false:0, true:1 - autocontinue to next wp - PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - PARAM5 / local: x position, global: latitude - PARAM6 / y position: global: longitude - PARAM7 / z position: global: altitude - - - Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message. - System ID - Component ID - Sequence - - - Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between). - System ID - Component ID - Sequence - - - Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint. - Sequence - - - Request the overall list of waypoints from the system/component. - System ID - Component ID - - - This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints. - System ID - Component ID - Number of Waypoints in the Sequence - - - Delete all waypoints at once. - System ID - Component ID - - - A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. - Sequence - - - Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). - System ID - Component ID - 0: OK, 1: Error - - - As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor. - System ID - Component ID - global position * 1E7 - global position * 1E7 - global position * 1000 - - - Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position - Latitude (WGS84), expressed as * 1E7 - Longitude (WGS84), expressed as * 1E7 - Altitude(WGS84), expressed as * 1000 - - - Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message. - System ID - Component ID - x position - y position - z position - Desired yaw angle - - - Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS. - x position - y position - z position - Desired yaw angle - - - Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - Attitude estimation health: 0: poor, 255: excellent - 0: Attitude control disabled, 1: enabled - 0: X, Y position control disabled, 1: enabled - 0: Z position control disabled, 1: enabled - 0: Yaw angle control disabled, 1: enabled - - - Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. - System ID - Component ID - Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - x position 1 / Latitude 1 - y position 1 / Longitude 1 - z position 1 / Altitude 1 - x position 2 / Latitude 2 - y position 2 / Longitude 2 - z position 2 / Altitude 2 - - - Read out the safety zone the MAV currently assumes. - Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - x position 1 / Latitude 1 - y position 1 / Longitude 1 - z position 1 / Altitude 1 - x position 2 / Latitude 2 - y position 2 / Longitude 2 - z position 2 / Altitude 2 - - - Set roll, pitch and yaw. - System ID - Component ID - Desired roll angle in radians - Desired pitch angle in radians - Desired yaw angle in radians - - - Set roll, pitch and yaw. - System ID - Component ID - Desired roll angular speed in rad/s - Desired pitch angular speed in rad/s - Desired yaw angular speed in rad/s - - - The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Attitude roll: -128: -100%, 127: +100% - Attitude pitch: -128: -100%, 127: +100% - Attitude yaw: -128: -100%, 127: +100% - Attitude thrust: -128: -100%, 127: +100% - - - The output of the position controller. The primary use of this message is to check the response and signs of the controller before the actual flight. - 1: enabled, 0: disabled - Position x: -128: -100%, 127: +100% - Position y: -128: -100%, 127: +100% - Position z: -128: -100%, 127: +100% - Position yaw: -128: -100%, 127: +100% - - - Outputs of the APM navigation controller. The primary use of this message is to check the response and signs - of the controller before actual flight and to assist with tuning controller parameters - - Current desired roll in degrees - Current desired pitch in degrees - Current desired heading in degrees - Bearing to current waypoint/target in degrees - Distance to active waypoint in meters - Current altitude error in meters - Current airspeed error in meters/second - Current crosstrack error on x-y plane in meters - - - The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint. - x position - y position - z position - yaw orientation in radians, 0 = NORTH - - - Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle. - x position error - y position error - z position error - roll error (radians) - pitch error (radians) - yaw error (radians) - x velocity - y velocity - z velocity - - - The system setting the altitude - The new altitude in meters - - - The target requested to send the message stream. - The target requested to send the message stream. - The ID of the requested message type - The requested interval between two messages of this type - 1 to start sending, 0 to stop sending. - - - + + + The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + MAVLink version + + + The boot message indicates that a system is starting. The onboard software version allows to keep track of onboard soft/firmware revisions. + The onboard software version + + + The system time is the time of the master clock, typically the computer clock of the main onboard computer. + Timestamp of the master clock in microseconds since UNIX epoch. + + + A ping message either requesting or responding to a ping. This allows to measure the system latencies, including serial port, radio modem and UDP connections. + PING sequence + 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system + 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system + Unix timestamp in microseconds + + + UTC date and time from GPS module + GPS UTC date ddmmyy + GPS UTC time hhmmss + + + Request to control this MAV + System the GCS requests control for + 0: request control of this MAV, 1: Release control of this MAV + 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. + Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" + + + Accept / deny control of this MAV + ID of the GCS this message + 0: request control of this MAV, 1: Release control of this MAV + 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control + + + Emit an encrypted signature / key identifying this system. PLEASE NOTE: This protocol has been kept simple, so transmitting the key requires an encrypted channel for true safety. + key + + + This message acknowledges an action. IMPORTANT: The acknowledgement can be also negative, e.g. the MAV rejects a reset message because it is in-flight. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h + The action id + 0: Action DENIED, 1: Action executed + + + An action message allows to execute a certain onboard action. These include liftoff, land, storing parameters too EEPROM, shutddown, etc. The action ids are defined in ENUM MAV_ACTION in mavlink/include/mavlink_types.h + The system executing the action + The component executing the action + The action id + + + Set the system mode, as defined by enum MAV_MODE in mavlink/include/mavlink_types.h. There is no target component id as the mode is by definition for the overall aircraft, not only for one component. + The system setting the mode + The new mode + + + Set the system navigation mode, as defined by enum MAV_NAV_MODE in mavlink/include/mavlink_types.h. The navigation mode applies to the whole aircraft and thus all components. + The system setting the mode + The new navigation mode + + + Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. + System ID + Component ID + Onboard parameter id + Parameter index. Send -1 to use the param ID field as identifier + + + Request all parameters of this component. After his request, all parameters are emitted. + System ID + Component ID + + + Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. + Onboard parameter id + Onboard parameter value + Total number of onboard parameters + Index of this onboard parameter + + + Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. + System ID + Component ID + Onboard parameter id + Onboard parameter value + + + The global position, as returned by the Global Positioning System (GPS). This is +NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + Latitude in 1E7 degrees + Longitude in 1E7 degrees + Altitude in 1E3 meters (millimeters) + GPS HDOP + GPS VDOP + GPS ground speed (m/s) + Compass heading in degrees, 0..360 degrees + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should contain the scaled values to the described units + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X acceleration (mg) + Y acceleration (mg) + Z acceleration (mg) + Angular speed around X axis (millirad /sec) + Angular speed around Y axis (millirad /sec) + Angular speed around Z axis (millirad /sec) + X Magnetic field (milli tesla) + Y Magnetic field (milli tesla) + Z Magnetic field (milli tesla) + + + The positioning status, as reported by GPS. This message is intended to display status information about each satellite visible to the receiver. See message GLOBAL_POSITION for the global position estimate. This message can contain information for up to 20 satellites. + Number of satellites visible + Global satellite ID + 0: Satellite not used, 1: used for localization + Elevation (0: right on top of receiver, 90: on the horizon) of satellite + Direction of satellite, 0: 0 deg, 255: 360 deg. + Signal to noise ratio of satellite + + + The RAW IMU readings for the usual 9DOF sensor setup. This message should always contain the true raw values without any scaling to allow data capture and system debugging. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X acceleration (raw) + Y acceleration (raw) + Z acceleration (raw) + Angular speed around X axis (raw) + Angular speed around Y axis (raw) + Angular speed around Z axis (raw) + X Magnetic field (raw) + Y Magnetic field (raw) + Z Magnetic field (raw) + + + The RAW pressure readings for the typical setup of one absolute pressure and one differential pressure sensor. The sensor values should be the raw, UNSCALED ADC values. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Absolute pressure (raw) + Differential pressure 1 (raw) + Differential pressure 2 (raw) + Raw Temperature measurement (raw) + + + The pressure readings for the typical setup of one absolute and differential pressure sensor. The units are as specified in each field. + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Absolute pressure (hectopascal) + Differential pressure 1 (hectopascal) + Temperature measurement (0.01 degrees celsius) + + + The attitude in the aeronautical frame (right-handed, Z-down, X-front, Y-right). + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Roll angle (rad) + Pitch angle (rad) + Yaw angle (rad) + Roll angular speed (rad/s) + Pitch angular speed (rad/s) + Yaw angular speed (rad/s) + + + The filtered local position (e.g. fused computer vision and accelerometers). Coordinate frame is right-handed, Z-axis down (aeronautical frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + X Position + Y Position + Z Position + X Speed + Y Speed + Z Speed + + + The filtered global position (e.g. fused GPS and accelerometers). Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since unix epoch) + Latitude, in degrees + Longitude, in degrees + Absolute altitude, in meters + X Speed (in Latitude direction, positive: going north) + Y Speed (in Longitude direction, positive: going east) + Z Speed (in Altitude direction, positive: going up) + + + The global position, as returned by the Global Positioning System (GPS). This is +NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + Latitude in degrees + Longitude in degrees + Altitude in meters + GPS HDOP + GPS VDOP + GPS ground speed + Compass heading in degrees, 0..360 degrees + + + The general system state. If the system is following the MAVLink standard, the system state is mainly defined by three orthogonal states/modes: The system mode, which is either LOCKED (motors shut down and locked), MANUAL (system under RC control), GUIDED (system with autonomous position control, position setpoint controlled manually) or AUTO (system guided by path/waypoint planner). The NAV_MODE defined the current flight state: LIFTOFF (often an open-loop maneuver), LANDING, WAYPOINTS or VECTOR. This represents the internal navigation state machine. The system status shows wether the system is currently active or not and if an emergency occured. During the CRITICAL and EMERGENCY states the MAV is still considered to be active, but should start emergency procedures autonomously. After a failure occured it should first move from active to critical to allow manual intervention and then move to emergency after a certain timeout. + System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + Navigation mode, see MAV_NAV_MODE ENUM + System status flag, see MAV_STATUS ENUM + Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + Battery voltage, in millivolts (1 = 1 millivolt) + Remaining battery energy: (0%: 0, 100%: 1000) + Dropped packets (packets that were corrupted on reception on the MAV) + + + The RAW values of the RC channels received. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + Receive signal strength indicator, 0: 0%, 255: 100% + + + The scaled values of the RC channels received. (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 + Receive signal strength indicator, 0: 0%, 255: 100% + + + The RAW values of the servo outputs (for RC input from the remote, use the RC_CHANNELS messages). The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. + Servo output 1 value, in microseconds + Servo output 2 value, in microseconds + Servo output 3 value, in microseconds + Servo output 4 value, in microseconds + Servo output 5 value, in microseconds + Servo output 6 value, in microseconds + Servo output 7 value, in microseconds + Servo output 8 value, in microseconds + + + Message encoding a waypoint. This message is emitted to announce + the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed + System ID + Component ID + Sequence + The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + false:0, true:1 + autocontinue to next wp + PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters + PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds + PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. + PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH + PARAM5 / local: x position, global: latitude + PARAM6 / y position: global: longitude + PARAM7 / z position: global: altitude + + + Request the information of the waypoint with the sequence number seq. The response of the system to this message should be a WAYPOINT message. + System ID + Component ID + Sequence + + + Set the waypoint with sequence number seq as current waypoint. This means that the MAV will continue to this waypoint on the shortest path (not following the waypoints in-between). + System ID + Component ID + Sequence + + + Message that announces the sequence number of the current active waypoint. The MAV will fly towards this waypoint. + Sequence + + + Request the overall list of waypoints from the system/component. + System ID + Component ID + + + This message is emitted as response to WAYPOINT_REQUEST_LIST by the MAV. The GCS can then request the individual waypoints based on the knowledge of the total number of waypoints. + System ID + Component ID + Number of Waypoints in the Sequence + + + Delete all waypoints at once. + System ID + Component ID + + + A certain waypoint has been reached. The system will either hold this position (or circle on the orbit) or (if the autocontinue on the WP was set) continue to the next waypoint. + Sequence + + + Ack message during waypoint handling. The type field states if this message is a positive ack (type=0) or if an error happened (type=non-zero). + System ID + Component ID + 0: OK, 1: Error + + + As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor. + System ID + Component ID + global position * 1E7 + global position * 1E7 + global position * 1000 + + + Once the MAV sets a new GPS-Local correspondence, this message announces the origin (0,0,0) position + Latitude (WGS84), expressed as * 1E7 + Longitude (WGS84), expressed as * 1E7 + Altitude(WGS84), expressed as * 1000 + + + Set the setpoint for a local position controller. This is the position in local coordinates the MAV should fly to. This message is sent by the path/waypoint planner to the onboard position controller. As some MAVs have a degree of freedom in yaw (e.g. all helicopters/quadrotors), the desired yaw angle is part of the message. + System ID + Component ID + x position + y position + z position + Desired yaw angle + + + Transmit the current local setpoint of the controller to other MAVs (collision avoidance) and to the GCS. + x position + y position + z position + Desired yaw angle + + + Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix + Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix + GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix + Attitude estimation health: 0: poor, 255: excellent + 0: Attitude control disabled, 1: enabled + 0: X, Y position control disabled, 1: enabled + 0: Z position control disabled, 1: enabled + 0: Yaw angle control disabled, 1: enabled + + + Set a safety zone (volume), which is defined by two corners of a cube. This message can be used to tell the MAV which setpoints/waypoints to accept and which to reject. Safety areas are often enforced by national or competition regulations. + System ID + Component ID + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + Read out the safety zone the MAV currently assumes. + Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. + x position 1 / Latitude 1 + y position 1 / Longitude 1 + z position 1 / Altitude 1 + x position 2 / Latitude 2 + y position 2 / Longitude 2 + z position 2 / Altitude 2 + + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + Collective thrust, normalized to 0 .. 1 + + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + Collective thrust, normalized to 0 .. 1 + + + Setpoint in roll, pitch, yaw currently active on the system. + Timestamp in milliseconds since system boot + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + Collective thrust, normalized to 0 .. 1 + + + Setpoint in rollspeed, pitchspeed, yawspeed currently active on the system. + Timestamp in milliseconds since system boot + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + Collective thrust, normalized to 0 .. 1 + + + Outputs of the APM navigation controller. The primary use of this message is to check the response and signs +of the controller before actual flight and to assist with tuning controller parameters + Current desired roll in degrees + Current desired pitch in degrees + Current desired heading in degrees + Bearing to current waypoint/target in degrees + Distance to active waypoint in meters + Current altitude error in meters + Current airspeed error in meters/second + Current crosstrack error on x-y plane in meters + + + The goal position of the system. This position is the input to any navigation or path planning algorithm and does NOT represent the current controller setpoint. + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + Corrects the systems state by adding an error correction term to the position and velocity, and by rotating the attitude by a correction angle. + x position error + y position error + z position error + roll error (radians) + pitch error (radians) + yaw error (radians) + x velocity + y velocity + z velocity + + + The system setting the altitude + The new altitude in meters + + + The target requested to send the message stream. + The target requested to send the message stream. + The ID of the requested message type + Update rate in Hertz + 1 to start sending, 0 to stop sending. + + This packet is useful for high throughput - applications such as hardware in the loop. + applications such as hardware in the loop simulations. Timestamp (microseconds since UNIX epoch or microseconds since system boot) Roll angle (rad) @@ -829,92 +823,101 @@ Y acceleration (mg) Z acceleration (mg) - + + Hardware in the loop control outputs + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Control output -3 .. 1 + Control output -1 .. 1 + Control output -1 .. 1 + Throttle 0 .. 1 + System mode (MAV_MODE) + Navigation mode (MAV_NAV_MODE) + - The system to be controlled - roll - pitch - yaw - thrust - roll control enabled auto:0, manual:1 - pitch auto:0, manual:1 - yaw auto:0, manual:1 - thrust auto:0, manual:1 - - - The RAW values of the RC channels sent to the MAV to override info received from the RC radio. A value of -1 means no change to that channel. A value of 0 means control of that channel should be released back to the RC radio. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. - System ID - Component ID - RC channel 1 value, in microseconds - RC channel 2 value, in microseconds - RC channel 3 value, in microseconds - RC channel 4 value, in microseconds - RC channel 5 value, in microseconds - RC channel 6 value, in microseconds - RC channel 7 value, in microseconds - RC channel 8 value, in microseconds - - - The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) - Latitude, expressed as * 1E7 - Longitude, expressed as * 1E7 - Altitude in meters, expressed as * 1000 (millimeters) - Ground X Speed (Latitude), expressed as m/s * 100 - Ground Y Speed (Longitude), expressed as m/s * 100 - Ground Z Speed (Altitude), expressed as m/s * 100 - - - Metrics typically displayed on a HUD for fixed wing aircraft - Current airspeed in m/s - Current ground speed in m/s - Current heading in degrees, in compass units (0..360, 0=north) - Current throttle setting in integer percent, 0 to 100 - Current altitude (MSL), in meters - Current climb rate in meters/second - - - Send a command with up to four parameters to the MAV - System which should execute the command - Component which should execute the command, 0 for all components - Command ID, as defined by MAV_CMD enum. - 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - Parameter 1, as defined by MAV_CMD enum. - Parameter 2, as defined by MAV_CMD enum. - Parameter 3, as defined by MAV_CMD enum. - Parameter 4, as defined by MAV_CMD enum. - - - Report status of a command. Includes feedback wether the command was executed - Current airspeed in m/s - 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - - - - Name - Timestamp - x - y - z - - - Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. - Name of the debug variable - Floating point value - - - Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. - Name of the debug variable - Signed integer value - - - Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). - Severity of status, 0 = info message, 255 = critical fault - Status text message, without null termination character - - - Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. - index of debug variable - DEBUG value - - + The system to be controlled + roll + pitch + yaw + thrust + roll control enabled auto:0, manual:1 + pitch auto:0, manual:1 + yaw auto:0, manual:1 + thrust auto:0, manual:1 + + + The RAW values of the RC channels sent to the MAV to override info received from the RC radio. A value of -1 means no change to that channel. A value of 0 means control of that channel should be released back to the RC radio. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + System ID + Component ID + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + + + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) + Latitude, expressed as * 1E7 + Longitude, expressed as * 1E7 + Altitude in meters, expressed as * 1000 (millimeters) + Ground X Speed (Latitude), expressed as m/s * 100 + Ground Y Speed (Longitude), expressed as m/s * 100 + Ground Z Speed (Altitude), expressed as m/s * 100 + + + Metrics typically displayed on a HUD for fixed wing aircraft + Current airspeed in m/s + Current ground speed in m/s + Current heading in degrees, in compass units (0..360, 0=north) + Current throttle setting in integer percent, 0 to 100 + Current altitude (MSL), in meters + Current climb rate in meters/second + + + Send a command with up to four parameters to the MAV + System which should execute the command + Component which should execute the command, 0 for all components + Command ID, as defined by MAV_CMD enum. + 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) + Parameter 1, as defined by MAV_CMD enum. + Parameter 2, as defined by MAV_CMD enum. + Parameter 3, as defined by MAV_CMD enum. + Parameter 4, as defined by MAV_CMD enum. + + + Report status of a command. Includes feedback wether the command was executed + Current airspeed in m/s + 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION + + + + Name + Timestamp + x + y + z + + + Send a key-value pair as float. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Name of the debug variable + Floating point value + + + Send a key-value pair as integer. The use of this message is discouraged for normal packets, but a quite efficient way for testing new messages and getting experimental debug output. + Name of the debug variable + Signed integer value + + + Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). + Severity of status, 0 = info message, 255 = critical fault + Status text message, without null termination character + + + Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. + index of debug variable + DEBUG value + + diff --git a/libraries/GCS_MAVLink/missionlib/testing/main.c b/libraries/GCS_MAVLink/missionlib/testing/main.c index 9e2dc55a26..0bec1155ad 100644 --- a/libraries/GCS_MAVLink/missionlib/testing/main.c +++ b/libraries/GCS_MAVLink/missionlib/testing/main.c @@ -52,32 +52,23 @@ #include #endif -#include - +/* 0: Include MAVLink types */ #include <../mavlink_types.h> +/* 1: Define mavlink system storage */ mavlink_system_t mavlink_system; -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ +/* 2: Include actual protocol, REQUIRES mavlink_system */ #include +/* 3: Define waypoint helper functions */ +void mavlink_wpm_send_message(mavlink_message_t* msg); +void mavlink_wpm_send_gcs_string(const char* string); +uint64_t mavlink_wpm_get_system_timestamp(); -/* Provide the interface functions for the waypoint manager */ - -/* - * @brief Sends a MAVLink message over UDP - */ -void mavlink_wpm_send_message(mavlink_message_t* msg) -{ - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); - - printf("SENT %d bytes", bytes_sent); -} - - -#include +/* 4: Include waypoint protocol */ +#include "waypoints.h" +mavlink_wpm_storage wpm; #define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) @@ -103,6 +94,36 @@ unsigned int temp = 0; uint64_t microsSinceEpoch(); + + +/* Provide the interface functions for the waypoint manager */ + +/* + * @brief Sends a MAVLink message over UDP + */ +void mavlink_wpm_send_message(mavlink_message_t* msg) +{ + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + + printf("SENT %d bytes", bytes_sent); +} + +void mavlink_wpm_send_gcs_string(const char* string) +{ + printf("%s",string); +} + +uint64_t mavlink_wpm_get_system_timestamp() +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; +} + + + int main(int argc, char* argv[]) { // Initialize MAVLink @@ -171,7 +192,7 @@ int main(int argc, char* argv[]) { /*Send Heartbeat */ - mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_CLASS_GENERIC); + mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_HELICOPTER, MAV_CLASS_GENERIC); len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); @@ -220,7 +241,7 @@ int main(int argc, char* argv[]) printf("\n"); } memset(buf, 0, BUFFER_LENGTH); - sleep(1); // Sleep one second + usleep(50000); // Sleep one second } } diff --git a/libraries/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/project.pbxproj b/libraries/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/project.pbxproj index 1dc88f3c65..ba27d2f32e 100644 --- a/libraries/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/project.pbxproj +++ b/libraries/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/project.pbxproj @@ -7,6 +7,7 @@ objects = { /* Begin PBXBuildFile section */ + 34E8AFDB13F5064C001100AA /* waypoints.c in Sources */ = {isa = PBXBuildFile; fileRef = 34E8AFDA13F5064C001100AA /* waypoints.c */; }; 8DD76FAC0486AB0100D96B5E /* main.c in Sources */ = {isa = PBXBuildFile; fileRef = 08FB7796FE84155DC02AAC07 /* main.c */; settings = {ATTRIBUTES = (); }; }; 8DD76FB00486AB0100D96B5E /* udptest.1 in CopyFiles */ = {isa = PBXBuildFile; fileRef = C6A0FF2C0290799A04C91782 /* udptest.1 */; }; /* End PBXBuildFile section */ @@ -26,6 +27,8 @@ /* Begin PBXFileReference section */ 08FB7796FE84155DC02AAC07 /* main.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = main.c; sourceTree = ""; }; + 34E8AFDA13F5064C001100AA /* waypoints.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = waypoints.c; path = ../waypoints.c; sourceTree = SOURCE_ROOT; }; + 34E8AFDC13F50659001100AA /* waypoints.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = waypoints.h; path = ../waypoints.h; sourceTree = SOURCE_ROOT; }; 8DD76FB20486AB0100D96B5E /* udptest */ = {isa = PBXFileReference; explicitFileType = "compiled.mach-o.executable"; includeInIndex = 0; path = udptest; sourceTree = BUILT_PRODUCTS_DIR; }; C6A0FF2C0290799A04C91782 /* udptest.1 */ = {isa = PBXFileReference; lastKnownFileType = text.man; path = udptest.1; sourceTree = ""; }; /* End PBXFileReference section */ @@ -44,6 +47,7 @@ 08FB7794FE84155DC02AAC07 /* udptest */ = { isa = PBXGroup; children = ( + 34E8AFDC13F50659001100AA /* waypoints.h */, 08FB7795FE84155DC02AAC07 /* Source */, C6A0FF2B0290797F04C91782 /* Documentation */, 1AB674ADFE9D54B511CA2CBB /* Products */, @@ -54,6 +58,7 @@ 08FB7795FE84155DC02AAC07 /* Source */ = { isa = PBXGroup; children = ( + 34E8AFDA13F5064C001100AA /* waypoints.c */, 08FB7796FE84155DC02AAC07 /* main.c */, ); name = Source; @@ -126,6 +131,7 @@ buildActionMask = 2147483647; files = ( 8DD76FAC0486AB0100D96B5E /* main.c in Sources */, + 34E8AFDB13F5064C001100AA /* waypoints.c in Sources */, ); runOnlyForDeploymentPostprocessing = 0; }; diff --git a/libraries/GCS_MAVLink/missionlib/waypoints.c b/libraries/GCS_MAVLink/missionlib/waypoints.c index 335f593a9e..a7b8ee5f5c 100644 --- a/libraries/GCS_MAVLink/missionlib/waypoints.c +++ b/libraries/GCS_MAVLink/missionlib/waypoints.c @@ -17,13 +17,21 @@ ****************************************************************************/ -#include - -static mavlink_wpm_storage wpm; +#include "waypoints.h" +#include bool debug = true; bool verbose = true; +extern mavlink_system_t mavlink_system; +extern mavlink_wpm_storage wpm; + +extern void mavlink_wpm_send_message(mavlink_message_t* msg); +extern void mavlink_wpm_send_gcs_string(const char* string); +extern uint64_t mavlink_wpm_get_system_timestamp(); + + +#define MAVLINK_WPM_NO_PRINTF void mavlink_wpm_init(mavlink_wpm_storage* state) { @@ -48,18 +56,6 @@ void mavlink_wpm_init(mavlink_wpm_storage* state) } -void mavlink_wpm_send_gcs_string(const char* string) -{ - printf("%s",string); -} - -uint64_t mavlink_wpm_get_system_timestamp() -{ - struct timeval tv; - gettimeofday(&tv, NULL); - return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; -} - /* * @brief Sends an waypoint ack message */ @@ -79,7 +75,11 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) if (MAVLINK_WPM_TEXT_FEEDBACK) { - //printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("Sent waypoint ACK"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); +#endif mavlink_wpm_send_gcs_string("Sent waypoint ACK"); } } @@ -109,7 +109,7 @@ void mavlink_wpm_send_waypoint_current(uint16_t seq) // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //printf("Broadcasted new current waypoint %u\n", wpc.seq); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq); } else { @@ -151,14 +151,14 @@ void mavlink_wpm_send_setpoint(uint16_t seq) } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//// if (verbose) // printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); } wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp(); } else { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //if (verbose) printf("ERROR: index out of bounds\n"); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n"); } } @@ -174,7 +174,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } @@ -189,7 +189,7 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) wp->target_component = wpm.current_partner_compid; mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } @@ -210,7 +210,7 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s wpr.seq = seq; mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } @@ -237,7 +237,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); } @@ -278,43 +278,45 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) // } // else // { -// if (verbose) printf("ERROR: index out of bounds\n"); +// // if (verbose) // printf("ERROR: index out of bounds\n"); // } // return -1.f; //} float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) { -// if (seq < wpm.size) -// { -// mavlink_waypoint_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// return (C-A).length(); -// } -// else -// { -// if (verbose) printf("ERROR: index out of bounds\n"); -// } + // if (seq < wpm.size) + // { + // mavlink_waypoint_t *cur = waypoints->at(seq); + // + // const PxVector3 A(cur->x, cur->y, cur->z); + // const PxVector3 C(x, y, z); + // + // return (C-A).length(); + // } + // else + // { + // // if (verbose) // printf("ERROR: index out of bounds\n"); + // } return -1.f; } -static void mavlink_wpm_message_handler(const mavlink_message_t* msg) +void mavlink_wpm_message_handler(const mavlink_message_t* msg) { // Handle param messages //paramClient->handleMAVLinkPacket(msg); //check for timed-out operations - struct timeval tv; - gettimeofday(&tv, NULL); - uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + uint64_t now = mavlink_wpm_get_system_timestamp(); if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { - if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); - wpm.current_state = MAVLINK_WPM_STATE_IDLE; +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("Operation timeout switching -> IDLE"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); +#endif + wpm.current_state = MAVLINK_WPM_STATE_IDLE; wpm.current_count = 0; wpm.current_partner_sysid = 0; wpm.current_partner_compid = 0; @@ -376,7 +378,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_local_position_t pos; mavlink_msg_local_position_decode(msg, &pos); - //if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); + //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); wpm.pos_reached = false; @@ -403,52 +405,52 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) break; } -// case MAVLINK_MSG_ID_CMD: // special action from ground station -// { -// mavlink_cmd_t action; -// mavlink_msg_cmd_decode(msg, &action); -// if(action.target == mavlink_system.sysid) -// { -// if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; -// switch (action.action) -// { -// // case MAV_ACTION_LAUNCH: -// // if (verbose) std::cerr << "Launch received" << std::endl; -// // current_active_wp_id = 0; -// // if (wpm.size>0) -// // { -// // setActive(waypoints[current_active_wp_id]); -// // } -// // else -// // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// // break; -// -// // case MAV_ACTION_CONTINUE: -// // if (verbose) std::c -// // err << "Continue received" << std::endl; -// // idle = false; -// // setActive(waypoints[current_active_wp_id]); -// // break; -// -// // case MAV_ACTION_HALT: -// // if (verbose) std::cerr << "Halt received" << std::endl; -// // idle = true; -// // break; -// -// // default: -// // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// // break; -// } -// } -// break; -// } + // case MAVLINK_MSG_ID_CMD: // special action from ground station + // { + // mavlink_cmd_t action; + // mavlink_msg_cmd_decode(msg, &action); + // if(action.target == mavlink_system.sysid) + // { + // // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; + // switch (action.action) + // { + // // case MAV_ACTION_LAUNCH: + // // // if (verbose) std::cerr << "Launch received" << std::endl; + // // current_active_wp_id = 0; + // // if (wpm.size>0) + // // { + // // setActive(waypoints[current_active_wp_id]); + // // } + // // else + // // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; + // // break; + // + // // case MAV_ACTION_CONTINUE: + // // // if (verbose) std::c + // // err << "Continue received" << std::endl; + // // idle = false; + // // setActive(waypoints[current_active_wp_id]); + // // break; + // + // // case MAV_ACTION_HALT: + // // // if (verbose) std::cerr << "Halt received" << std::endl; + // // idle = true; + // // break; + // + // // default: + // // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; + // // break; + // } + // } + // break; + // } case MAVLINK_MSG_ID_WAYPOINT_ACK: { mavlink_waypoint_ack_t wpa; mavlink_msg_waypoint_ack_decode(msg, &wpa); - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid && wpa.target_component == mavlink_system.compid)) + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_system.compid*/)) { wpm.timestamp_lastaction = now; @@ -456,15 +458,23 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { if (wpm.current_wp_id == wpm.size-1) { - if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); - wpm.current_state = MAVLINK_WPM_STATE_IDLE; +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("Got last WP ACK state -> IDLE"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); +#endif + wpm.current_state = MAVLINK_WPM_STATE_IDLE; wpm.current_wp_id = 0; } } } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif } break; } @@ -474,7 +484,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_set_current_t wpc; mavlink_msg_waypoint_set_current_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_system.compid*/) { wpm.timestamp_lastaction = now; @@ -482,7 +492,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { if (wpc.seq < wpm.size) { - if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + // if (verbose) // printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); wpm.current_active_wp_id = wpc.seq; uint32_t i; for(i = 0; i < wpm.size; i++) @@ -496,7 +506,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) wpm.waypoints[i].current = false; } } - if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("NEW WP SET"); +#else + if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id); +#endif wpm.yaw_reached = false; wpm.pos_reached = false; mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); @@ -505,17 +519,29 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) } else { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Not in list"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); +#endif } } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); +#endif } } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif } break; } @@ -524,7 +550,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_request_list_t wprl; mavlink_msg_waypoint_request_list_decode(msg, &wprl); - if(wprl.target_system == mavlink_system.sysid && wprl.target_component == mavlink_system.compid) + if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_system.compid*/) { wpm.timestamp_lastaction = now; @@ -532,30 +558,30 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { if (wpm.size > 0) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; wpm.current_wp_id = 0; wpm.current_partner_sysid = msg->sysid; wpm.current_partner_compid = msg->compid; } else { - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + // if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); } wpm.current_count = wpm.size; mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); } else { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); } } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); } - + break; } @@ -563,16 +589,37 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_request_t wpr; mavlink_msg_waypoint_request_decode(msg, &wpr); - if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) + if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) { wpm.timestamp_lastaction = now; //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("GOT WP REQ, state -> SEND"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); +#endif + } + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("GOT 2nd WP REQ"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); +#endif + } + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("GOT 2nd WP REQ"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); +#endif + } wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; wpm.current_wp_id = wpr.seq; @@ -580,34 +627,78 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) } else { - if (verbose) + // if (verbose) { - if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; } + if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); +#endif + break; + } else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) { - if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + if (wpr.seq != 0) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: First id != 0"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); +#endif + } } else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { - if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); - else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); +#endif + } + else if (wpr.seq >= wpm.size) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP not in list"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); +#endif + } } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: ?"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); +#endif + } } } } else { //we we're target but already communicating with someone else - if((wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) + if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); +#endif } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif } - + } break; } @@ -616,7 +707,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_count_t wpc; mavlink_msg_waypoint_count_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_system.compid*/) { wpm.timestamp_lastaction = now; @@ -624,8 +715,22 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { if (wpc.count > 0) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("WP CMD OK: state -> GETLIST"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); +#endif + } + if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("WP CMD OK AGAIN"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); +#endif + } wpm.current_state = MAVLINK_WPM_STATE_GETLIST; wpm.current_wp_id = 0; @@ -633,25 +738,33 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) wpm.current_partner_compid = msg->compid; wpm.current_count = wpc.count; - printf("clearing receive buffer and readying for receiving waypoints\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("CLR RCV BUF: READY"); +#else + if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); +#endif wpm.rcv_size = 0; //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints_receive_buffer->back(); -// waypoints_receive_buffer->pop_back(); -// } + // { + // delete waypoints_receive_buffer->back(); + // waypoints_receive_buffer->pop_back(); + // } mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); } else if (wpc.count == 0) { - printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("COUNT 0"); +#else + if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); +#endif wpm.rcv_size = 0; //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints->back(); -// waypoints->pop_back(); -// } + // { + // delete waypoints->back(); + // waypoints->pop_back(); + // } wpm.current_active_wp_id = -1; wpm.yaw_reached = false; wpm.pos_reached = false; @@ -660,19 +773,48 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) } else { - if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("IGN WP CMD"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); +#endif } } else { - if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); - else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); +#endif + } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); +#endif + } + else + { +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: ?"); +#else + if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); +#endif + } } } else { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#ifdef MAVLINK_WPM_NO_PRINTF + mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch"); +#else + if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); +#endif } } @@ -683,30 +825,30 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_t wp; mavlink_msg_waypoint_decode(msg, &wp); - if (verbose) printf("GOT WAYPOINT!"); + // if (verbose) // printf("GOT WAYPOINT!"); - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid)) + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/)) { wpm.timestamp_lastaction = now; //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); - - wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); - wpm.current_wp_id = wp.seq + 1; + wpm.current_wp_id = wp.seq + 1; - if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (verbose) printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); + // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); @@ -730,7 +872,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) if (wpm.waypoints[i].current == 1) { wpm.current_active_wp_id = i; - //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); wpm.yaw_reached = false; wpm.pos_reached = false; mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); @@ -761,36 +903,58 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) { //we're done receiving waypoints, answer with ack. mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + // printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); } - if (verbose) + // if (verbose) { - if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; } + if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); + break; + } else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) { - if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + if(!(wp.seq == 0)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + } + else + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } } else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { - if (!(wp.seq == wpm.current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); - else if (!(wp.seq < wpm.current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + if (!(wp.seq == wpm.current_wp_id)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); + } + else if (!(wp.seq < wpm.current_count)) + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + } + else + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + else + { + // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } } } } else { //we we're target but already communicating with someone else - if((wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); } - else if(wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) + else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_system.compid*/) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); } } break; @@ -801,27 +965,27 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_clear_all_t wpca; mavlink_msg_waypoint_clear_all_decode(msg, &wpca); - if(wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state == MAVLINK_WPM_STATE_IDLE) + if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE) { wpm.timestamp_lastaction = now; - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + // if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); // Delete all waypoints wpm.size = 0; wpm.current_active_wp_id = -1; wpm.yaw_reached = false; wpm.pos_reached = false; } - else if (wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); + // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); } break; } default: { - if (debug) printf("Waypoint: received message of unknown type"); + // if (debug) // printf("Waypoint: received message of unknown type"); break; } } @@ -836,7 +1000,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) if (wpm.timestamp_firstinside_orbit == 0) { // Announce that last waypoint was reached - if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + // if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq); mavlink_wpm_send_waypoint_reached(cur_wp->seq); wpm.timestamp_firstinside_orbit = now; } @@ -867,7 +1031,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg) wpm.waypoints[wpm.current_active_wp_id].current = true; wpm.pos_reached = false; wpm.yaw_reached = false; - if (verbose) printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); + // if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); } } }