Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Updated GCS_MAVLink to match mavlink master. 

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3017 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
james.goppert 2011-08-05 08:18:08 +00:00
parent 43fd56ec46
commit 1e4f463298
40 changed files with 7258 additions and 1442 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/GCS_MAVLink/include/ardupilotmega/ardupilotmega.h
View File

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Saturday, April 16 2011, 04:01 UTC
* @see http://qgroundcontrol.org/mavlink/
* Generated on Friday, August 5 2011, 07:37 UTC
*/
#ifndef ARDUPILOTMEGA_H
#define ARDUPILOTMEGA_H
@ -38,7 +38,7 @@ extern "C" {
// 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, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 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, 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 }
#ifdef __cplusplus
}

2
libraries/GCS_MAVLink/include/ardupilotmega/mavlink.h
View File

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Saturday, April 16 2011, 04:01 UTC
* Generated on Friday, August 5 2011, 07:37 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H

96
libraries/GCS_MAVLink/include/common/common.h
View File

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Wednesday, May 18 2011, 03:15 UTC
* @see http://qgroundcontrol.org/mavlink/
* Generated on Friday, August 5 2011, 08:13 UTC
*/
#ifndef COMMON_H
#define COMMON_H
@ -31,60 +31,61 @@ extern "C" {
/** @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)LatitudeLongitudeAltitude*/
MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of timeEmptyEmptyRadius around waypoint, in meters. If positive loiter clockwise, else counter-clockwiseDesired yaw angle.LatitudeLongitudeAltitude*/
MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turnsTurnsEmptyRadius around waypoint, in meters. If positive loiter clockwise, else counter-clockwiseDesired yaw angle.LatitudeLongitudeAltitude*/
MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this waypoint for X secondsSeconds (decimal)EmptyRadius around waypoint, in meters. If positive loiter clockwise, else counter-clockwiseDesired yaw angle.LatitudeLongitudeAltitude*/
MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch locationEmptyEmptyEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_NAV_LAND=21, /* Land at locationEmptyEmptyEmptyDesired yaw angle.LatitudeLongitudeAltitude*/
MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / handMinimum pitch (if airspeed sensor present), desired pitch without sensorEmptyEmptyYaw angle (if magnetometer present), ignored without magnetometerLatitudeLongitudeAltitude*/
MAV_CMD_NAV_PATHPLANNING=81, /* 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 planning0: 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 gridEmptyYaw angle at goal, in compass degrees, [0..360]Latitude/X of goalLongitude/Y of goalAltitude/Z of goal*/
MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumerationEmptyEmptyEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine.Delay in seconds (decimal)EmptyEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached.Descent / Ascend rate (m/s)EmptyEmptyEmptyEmptyEmptyFinish Altitude*/
MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point.Distance (meters)EmptyEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle.target angle: [0-360], 0 is northspeed during yaw change:[deg per second]direction: negative: counter clockwise, positive: clockwise [-1,1]relative offset or absolute angle: [ 1,0]EmptyEmptyEmpty*/
MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumerationEmptyEmptyEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_DO_SET_MODE=176, /* Set system mode.Mode, as defined by ENUM MAV_MODEEmptyEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of timesSequence numberRepeat countEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_DO_CHANGE_SPEED=178, /* 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)EmptyEmptyEmptyEmpty*/
MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location.Use current (1=use current location, 0=use specified location)EmptyEmptyEmptyLatitudeLongitudeAltitude*/
MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter.Parameter numberParameter valueEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition.Relay numberSetting (1=on, 0=off, others possible depending on system hardware)EmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period.Relay numberCycle countCycle time (seconds, decimal)EmptyEmptyEmptyEmpty*/
MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value.Servo numberPWM (microseconds, 1000 to 2000 typical)EmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period.Servo numberPWM (microseconds, 1000 to 2000 typical)Cycle countCycle time (seconds)EmptyEmptyEmpty*/
MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system.Camera ID (-1 for all)Transmission: 0: disabled, 1: enabled compressed, 2: enabled rawTransmission mode: 0: video stream, >0: single images every n seconds (decimal)Recording: 0: disabled, 1: enabled compressed, 2: enabled rawEmptyEmptyEmpty*/
MAV_CMD_DO_SET_ROI=201, /* 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 devices such as cameras.Region of interest mode. (see MAV_ROI enum)Waypoint index/ target ID. (see MAV_ROI enum)ROI index (allows a vehicle to manage multiple cameras etc.)Emptyx the location of the fixed ROI (see MAV_FRAME)yz*/
MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumerationEmptyEmptyEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode.Gyro calibration: 0: no, 1: yesMagnetometer calibration: 0: no, 1: yesGround pressure: 0: no, 1: yesRadio calibration: 0: no, 1: yesEmptyEmptyEmpty*/
MAV_CMD_PREFLIGHT_STORAGE=245, /* 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/EEPROMMission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROMReservedReservedEmptyEmptyEmpty*/
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 | */
MAV_CMD_NAV_LOITER_UNLIM=17, /* 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 | */
MAV_CMD_NAV_LOITER_TURNS=18, /* 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 | */
MAV_CMD_NAV_LOITER_TIME=19, /* 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 | */
MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location | Empty | Empty | Empty | Empty | Empty | Empty | Empty | */
MAV_CMD_NAV_LAND=21, /* Land at location | Empty | Empty | Empty | Desired yaw angle. | Latitude | Longitude | Altitude | */
MAV_CMD_NAV_TAKEOFF=22, /* 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 | */
MAV_CMD_NAV_ROI=80, /* 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 | */
MAV_CMD_NAV_PATHPLANNING=81, /* 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 | */
MAV_CMD_NAV_LAST=95, /* 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 | */
MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. | Delay in seconds (decimal) | Empty | Empty | Empty | Empty | Empty | Empty | */
MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. | Descent / Ascend rate (m/s) | Empty | Empty | Empty | Empty | Empty | Finish Altitude | */
MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. | Distance (meters) | Empty | Empty | Empty | Empty | Empty | Empty | */
MAV_CMD_CONDITION_YAW=115, /* 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 | */
MAV_CMD_CONDITION_LAST=159, /* 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 | */
MAV_CMD_DO_SET_MODE=176, /* Set system mode. | Mode, as defined by ENUM MAV_MODE | Empty | Empty | Empty | Empty | Empty | Empty | */
MAV_CMD_DO_JUMP=177, /* 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 | */
MAV_CMD_DO_CHANGE_SPEED=178, /* 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 | */
MAV_CMD_DO_SET_HOME=179, /* 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 | */
MAV_CMD_DO_SET_PARAMETER=180, /* 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 | */
MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. | Relay number | Setting (1=on, 0=off, others possible depending on system hardware) | Empty | Empty | Empty | Empty | Empty | */
MAV_CMD_DO_REPEAT_RELAY=182, /* 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 | */
MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. | Servo number | PWM (microseconds, 1000 to 2000 typical) | Empty | Empty | Empty | Empty | Empty | */
MAV_CMD_DO_REPEAT_SERVO=184, /* 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 | */
MAV_CMD_DO_CONTROL_VIDEO=200, /* 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 | */
MAV_CMD_DO_SET_ROI=201, /* 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 devices such as cameras. | Region of interest mode. (see MAV_ROI enum) | Waypoint index/ target ID. (see MAV_ROI enum) | ROI index (allows a vehicle to manage multiple cameras etc.) | Empty | x the location of the fixed ROI (see MAV_FRAME) | y | z | */
MAV_CMD_DO_LAST=240, /* 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 | */
MAV_CMD_PREFLIGHT_CALIBRATION=241, /* 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 | */
MAV_CMD_PREFLIGHT_STORAGE=245, /* 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 | */
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. */
enum MAV_DATA_STREAM
{
MAV_DATA_STREAM_ALL=0, /* Enable all data streams*/
MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets.*/
MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS*/
MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW*/
MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT.*/
MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages.*/
MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot*/
MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot*/
MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot*/
MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */
MAV_DATA_STREAM_RAW_SENSORS=1, /* Enable IMU_RAW, GPS_RAW, GPS_STATUS packets. | */
MAV_DATA_STREAM_EXTENDED_STATUS=2, /* Enable GPS_STATUS, CONTROL_STATUS, AUX_STATUS | */
MAV_DATA_STREAM_RC_CHANNELS=3, /* Enable RC_CHANNELS_SCALED, RC_CHANNELS_RAW, SERVO_OUTPUT_RAW | */
MAV_DATA_STREAM_RAW_CONTROLLER=4, /* Enable ATTITUDE_CONTROLLER_OUTPUT, POSITION_CONTROLLER_OUTPUT, NAV_CONTROLLER_OUTPUT. | */
MAV_DATA_STREAM_POSITION=6, /* Enable LOCAL_POSITION, GLOBAL_POSITION/GLOBAL_POSITION_INT messages. | */
MAV_DATA_STREAM_EXTRA1=10, /* Dependent on the autopilot | */
MAV_DATA_STREAM_EXTRA2=11, /* Dependent on the autopilot | */
MAV_DATA_STREAM_EXTRA3=12, /* Dependent on the autopilot | */
MAV_DATA_STREAM_ENUM_END
};
/** @brief The ROI (region of interest) for the vehicle. This can be be used by the vehicle for camera/vehicle attitude alignment (see MAV_CMD_DO_SET_ROI). */
/** @brief The ROI (region of interest) for the vehicle. This can be be used by the vehicle for camera/vehicle attitude alignment (see MAV_CMD_NAV_ROI). */
enum MAV_ROI
{
MAV_ROI_NONE=0, /* No region of interest.*/
MAV_ROI_WPNEXT=1, /* Point toward next waypoint.*/
MAV_ROI_WPINDEX=2, /* Point toward given waypoint.*/
MAV_ROI_LOCATION=3, /* Point toward fixed location.*/
MAV_ROI_TARGET=4, /* Point toward target of given id.*/
MAV_ROI_NONE=0, /* No region of interest. | */
MAV_ROI_WPNEXT=1, /* Point toward next waypoint. | */
MAV_ROI_WPINDEX=2, /* Point toward given waypoint. | */
MAV_ROI_LOCATION=3, /* Point toward fixed location. | */
MAV_ROI_TARGET=4, /* Point toward of given id. | */
MAV_ROI_ENUM_END
};
@ -137,6 +138,8 @@ 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_nav_controller_output.h"
@ -144,6 +147,7 @@ enum MAV_ROI
#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_manual_control.h"
#include "./mavlink_msg_rc_channels_override.h"
#include "./mavlink_msg_global_position_int.h"
@ -160,7 +164,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, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 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, 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 }
#ifdef __cplusplus
}

2
libraries/GCS_MAVLink/include/common/mavlink.h
View File

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Wednesday, May 18 2011, 03:15 UTC
* Generated on Friday, August 5 2011, 08:13 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H

428
libraries/GCS_MAVLink/include/common/mavlink_msg_full_state.h Normal file
View File

@ -0,0 +1,428 @@
// MESSAGE FULL_STATE PACKING
#define MAVLINK_MSG_ID_FULL_STATE 67
typedef struct __mavlink_full_state_t
{
uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
float roll; ///< Roll angle (rad)
float pitch; ///< Pitch angle (rad)
float yaw; ///< Yaw angle (rad)
float rollspeed; ///< Roll angular speed (rad/s)
float pitchspeed; ///< Pitch angular speed (rad/s)
float yawspeed; ///< Yaw angular speed (rad/s)
int32_t lat; ///< Latitude, expressed as * 1E7
int32_t lon; ///< Longitude, expressed as * 1E7
int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters)
int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100
int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100
int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100
int16_t xacc; ///< X acceleration (mg)
int16_t yacc; ///< Y acceleration (mg)
int16_t zacc; ///< Z acceleration (mg)
} mavlink_full_state_t;
/**
* @brief Pack a full_state message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param rollspeed Roll angular speed (rad/s)
* @param pitchspeed Pitch angular speed (rad/s)
* @param yawspeed Yaw angular speed (rad/s)
* @param lat Latitude, expressed as * 1E7
* @param lon Longitude, expressed as * 1E7
* @param alt Altitude in meters, expressed as * 1000 (millimeters)
* @param vx Ground X Speed (Latitude), expressed as m/s * 100
* @param vy Ground Y Speed (Longitude), expressed as m/s * 100
* @param vz Ground Z Speed (Altitude), expressed as m/s * 100
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_full_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_FULL_STATE;
i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad)
i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad)
i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad)
i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s)
i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s)
i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s)
i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7
i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7
i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters)
i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100
i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100
i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100
i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg)
i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg)
i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg)
return mavlink_finalize_message(msg, system_id, component_id, i);
}
/**
* @brief Pack a full_state message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param rollspeed Roll angular speed (rad/s)
* @param pitchspeed Pitch angular speed (rad/s)
* @param yawspeed Yaw angular speed (rad/s)
* @param lat Latitude, expressed as * 1E7
* @param lon Longitude, expressed as * 1E7
* @param alt Altitude in meters, expressed as * 1000 (millimeters)
* @param vx Ground X Speed (Latitude), expressed as m/s * 100
* @param vy Ground Y Speed (Longitude), expressed as m/s * 100
* @param vz Ground Z Speed (Altitude), expressed as m/s * 100
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_full_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_FULL_STATE;
i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (microseconds since UNIX epoch or microseconds since system boot)
i += put_float_by_index(roll, i, msg->payload); // Roll angle (rad)
i += put_float_by_index(pitch, i, msg->payload); // Pitch angle (rad)
i += put_float_by_index(yaw, i, msg->payload); // Yaw angle (rad)
i += put_float_by_index(rollspeed, i, msg->payload); // Roll angular speed (rad/s)
i += put_float_by_index(pitchspeed, i, msg->payload); // Pitch angular speed (rad/s)
i += put_float_by_index(yawspeed, i, msg->payload); // Yaw angular speed (rad/s)
i += put_int32_t_by_index(lat, i, msg->payload); // Latitude, expressed as * 1E7
i += put_int32_t_by_index(lon, i, msg->payload); // Longitude, expressed as * 1E7
i += put_int32_t_by_index(alt, i, msg->payload); // Altitude in meters, expressed as * 1000 (millimeters)
i += put_int16_t_by_index(vx, i, msg->payload); // Ground X Speed (Latitude), expressed as m/s * 100
i += put_int16_t_by_index(vy, i, msg->payload); // Ground Y Speed (Longitude), expressed as m/s * 100
i += put_int16_t_by_index(vz, i, msg->payload); // Ground Z Speed (Altitude), expressed as m/s * 100
i += put_int16_t_by_index(xacc, i, msg->payload); // X acceleration (mg)
i += put_int16_t_by_index(yacc, i, msg->payload); // Y acceleration (mg)
i += put_int16_t_by_index(zacc, i, msg->payload); // Z acceleration (mg)
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
/**
* @brief Encode a full_state struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param full_state C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_full_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_full_state_t* full_state)
{
return mavlink_msg_full_state_pack(system_id, component_id, msg, full_state->usec, full_state->roll, full_state->pitch, full_state->yaw, full_state->rollspeed, full_state->pitchspeed, full_state->yawspeed, full_state->lat, full_state->lon, full_state->alt, full_state->vx, full_state->vy, full_state->vz, full_state->xacc, full_state->yacc, full_state->zacc);
}
/**
* @brief Send a full_state message
* @param chan MAVLink channel to send the message
*
* @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot)
* @param roll Roll angle (rad)
* @param pitch Pitch angle (rad)
* @param yaw Yaw angle (rad)
* @param rollspeed Roll angular speed (rad/s)
* @param pitchspeed Pitch angular speed (rad/s)
* @param yawspeed Yaw angular speed (rad/s)
* @param lat Latitude, expressed as * 1E7
* @param lon Longitude, expressed as * 1E7
* @param alt Altitude in meters, expressed as * 1000 (millimeters)
* @param vx Ground X Speed (Latitude), expressed as m/s * 100
* @param vy Ground Y Speed (Longitude), expressed as m/s * 100
* @param vz Ground Z Speed (Altitude), expressed as m/s * 100
* @param xacc X acceleration (mg)
* @param yacc Y acceleration (mg)
* @param zacc Z acceleration (mg)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_full_state_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc)
{
mavlink_message_t msg;
mavlink_msg_full_state_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, roll, pitch, yaw, rollspeed, pitchspeed, yawspeed, lat, lon, alt, vx, vy, vz, xacc, yacc, zacc);
mavlink_send_uart(chan, &msg);
}
#endif
// MESSAGE FULL_STATE UNPACKING
/**
* @brief Get field usec from full_state message
*
* @return Timestamp (microseconds since UNIX epoch or microseconds since system boot)
*/
static inline uint64_t mavlink_msg_full_state_get_usec(const mavlink_message_t* msg)
{
generic_64bit r;
r.b[7] = (msg->payload)[0];
r.b[6] = (msg->payload)[1];
r.b[5] = (msg->payload)[2];
r.b[4] = (msg->payload)[3];
r.b[3] = (msg->payload)[4];
r.b[2] = (msg->payload)[5];
r.b[1] = (msg->payload)[6];
r.b[0] = (msg->payload)[7];
return (uint64_t)r.ll;
}
/**
* @brief Get field roll from full_state message
*
* @return Roll angle (rad)
*/
static inline float mavlink_msg_full_state_get_roll(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t))[0];
r.b[2] = (msg->payload+sizeof(uint64_t))[1];
r.b[1] = (msg->payload+sizeof(uint64_t))[2];
r.b[0] = (msg->payload+sizeof(uint64_t))[3];
return (float)r.f;
}
/**
* @brief Get field pitch from full_state message
*
* @return Pitch angle (rad)
*/
static inline float mavlink_msg_full_state_get_pitch(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field yaw from full_state message
*
* @return Yaw angle (rad)
*/
static inline float mavlink_msg_full_state_get_yaw(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field rollspeed from full_state message
*
* @return Roll angular speed (rad/s)
*/
static inline float mavlink_msg_full_state_get_rollspeed(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field pitchspeed from full_state message
*
* @return Pitch angular speed (rad/s)
*/
static inline float mavlink_msg_full_state_get_pitchspeed(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field yawspeed from full_state message
*
* @return Yaw angular speed (rad/s)
*/
static inline float mavlink_msg_full_state_get_yawspeed(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field lat from full_state message
*
* @return Latitude, expressed as * 1E7
*/
static inline int32_t mavlink_msg_full_state_get_lat(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (int32_t)r.i;
}
/**
* @brief Get field lon from full_state message
*
* @return Longitude, expressed as * 1E7
*/
static inline int32_t mavlink_msg_full_state_get_lon(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t))[3];
return (int32_t)r.i;
}
/**
* @brief Get field alt from full_state message
*
* @return Altitude in meters, expressed as * 1000 (millimeters)
*/
static inline int32_t mavlink_msg_full_state_get_alt(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t))[3];
return (int32_t)r.i;
}
/**
* @brief Get field vx from full_state message
*
* @return Ground X Speed (Latitude), expressed as m/s * 100
*/
static inline int16_t mavlink_msg_full_state_get_vx(const mavlink_message_t* msg)
{
generic_16bit r;
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[0];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t))[1];
return (int16_t)r.s;
}
/**
* @brief Get field vy from full_state message
*
* @return Ground Y Speed (Longitude), expressed as m/s * 100
*/
static inline int16_t mavlink_msg_full_state_get_vy(const mavlink_message_t* msg)
{
generic_16bit r;
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[0];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t))[1];
return (int16_t)r.s;
}
/**
* @brief Get field vz from full_state message
*
* @return Ground Z Speed (Altitude), expressed as m/s * 100
*/
static inline int16_t mavlink_msg_full_state_get_vz(const mavlink_message_t* msg)
{
generic_16bit r;
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[0];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t))[1];
return (int16_t)r.s;
}
/**
* @brief Get field xacc from full_state message
*
* @return X acceleration (mg)
*/
static inline int16_t mavlink_msg_full_state_get_xacc(const mavlink_message_t* msg)
{
generic_16bit r;
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1];
return (int16_t)r.s;
}
/**
* @brief Get field yacc from full_state message
*
* @return Y acceleration (mg)
*/
static inline int16_t mavlink_msg_full_state_get_yacc(const mavlink_message_t* msg)
{
generic_16bit r;
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1];
return (int16_t)r.s;
}
/**
* @brief Get field zacc from full_state message
*
* @return Z acceleration (mg)
*/
static inline int16_t mavlink_msg_full_state_get_zacc(const mavlink_message_t* msg)
{
generic_16bit r;
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[0];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int32_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t)+sizeof(int16_t))[1];
return (int16_t)r.s;
}
/**
* @brief Decode a full_state message into a struct
*
* @param msg The message to decode
* @param full_state C-struct to decode the message contents into
*/
static inline void mavlink_msg_full_state_decode(const mavlink_message_t* msg, mavlink_full_state_t* full_state)
{
full_state->usec = mavlink_msg_full_state_get_usec(msg);
full_state->roll = mavlink_msg_full_state_get_roll(msg);
full_state->pitch = mavlink_msg_full_state_get_pitch(msg);
full_state->yaw = mavlink_msg_full_state_get_yaw(msg);
full_state->rollspeed = mavlink_msg_full_state_get_rollspeed(msg);
full_state->pitchspeed = mavlink_msg_full_state_get_pitchspeed(msg);
full_state->yawspeed = mavlink_msg_full_state_get_yawspeed(msg);
full_state->lat = mavlink_msg_full_state_get_lat(msg);
full_state->lon = mavlink_msg_full_state_get_lon(msg);
full_state->alt = mavlink_msg_full_state_get_alt(msg);
full_state->vx = mavlink_msg_full_state_get_vx(msg);
full_state->vy = mavlink_msg_full_state_get_vy(msg);
full_state->vz = mavlink_msg_full_state_get_vz(msg);
full_state->xacc = mavlink_msg_full_state_get_xacc(msg);
full_state->yacc = mavlink_msg_full_state_get_yacc(msg);
full_state->zacc = mavlink_msg_full_state_get_zacc(msg);
}

184
libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw.h Normal file
View File

@ -0,0 +1,184 @@
// MESSAGE SET_ROLL_PITCH_YAW PACKING
#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW 55
typedef struct __mavlink_set_roll_pitch_yaw_t
{
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
float roll; ///< Desired roll angle in radians
float pitch; ///< Desired pitch angle in radians
float yaw; ///< Desired yaw angle in radians
} mavlink_set_roll_pitch_yaw_t;
/**
* @brief Pack a set_roll_pitch_yaw message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param target_system System ID
* @param target_component Component ID
* @param roll Desired roll angle in radians
* @param pitch Desired pitch angle in radians
* @param yaw Desired yaw angle in radians
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW;
i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians
i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians
i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians
return mavlink_finalize_message(msg, system_id, component_id, i);
}
/**
* @brief Pack a set_roll_pitch_yaw message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @param roll Desired roll angle in radians
* @param pitch Desired pitch angle in radians
* @param yaw Desired yaw angle in radians
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW;
i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
i += put_float_by_index(roll, i, msg->payload); // Desired roll angle in radians
i += put_float_by_index(pitch, i, msg->payload); // Desired pitch angle in radians
i += put_float_by_index(yaw, i, msg->payload); // Desired yaw angle in radians
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
/**
* @brief Encode a set_roll_pitch_yaw struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param set_roll_pitch_yaw C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw)
{
return mavlink_msg_set_roll_pitch_yaw_pack(system_id, component_id, msg, set_roll_pitch_yaw->target_system, set_roll_pitch_yaw->target_component, set_roll_pitch_yaw->roll, set_roll_pitch_yaw->pitch, set_roll_pitch_yaw->yaw);
}
/**
* @brief Send a set_roll_pitch_yaw message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
* @param roll Desired roll angle in radians
* @param pitch Desired pitch angle in radians
* @param yaw Desired yaw angle in radians
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_set_roll_pitch_yaw_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw)
{
mavlink_message_t msg;
mavlink_msg_set_roll_pitch_yaw_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll, pitch, yaw);
mavlink_send_uart(chan, &msg);
}
#endif
// MESSAGE SET_ROLL_PITCH_YAW UNPACKING
/**
* @brief Get field target_system from set_roll_pitch_yaw message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_system(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload)[0];
}
/**
* @brief Get field target_component from set_roll_pitch_yaw message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_component(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
}
/**
* @brief Get field roll from set_roll_pitch_yaw message
*
* @return Desired roll angle in radians
*/
static inline float mavlink_msg_set_roll_pitch_yaw_get_roll(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3];
return (float)r.f;
}
/**
* @brief Get field pitch from set_roll_pitch_yaw message
*
* @return Desired pitch angle in radians
*/
static inline float mavlink_msg_set_roll_pitch_yaw_get_pitch(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field yaw from set_roll_pitch_yaw message
*
* @return Desired yaw angle in radians
*/
static inline float mavlink_msg_set_roll_pitch_yaw_get_yaw(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Decode a set_roll_pitch_yaw message into a struct
*
* @param msg The message to decode
* @param set_roll_pitch_yaw C-struct to decode the message contents into
*/
static inline void mavlink_msg_set_roll_pitch_yaw_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw)
{
set_roll_pitch_yaw->target_system = mavlink_msg_set_roll_pitch_yaw_get_target_system(msg);
set_roll_pitch_yaw->target_component = mavlink_msg_set_roll_pitch_yaw_get_target_component(msg);
set_roll_pitch_yaw->roll = mavlink_msg_set_roll_pitch_yaw_get_roll(msg);
set_roll_pitch_yaw->pitch = mavlink_msg_set_roll_pitch_yaw_get_pitch(msg);
set_roll_pitch_yaw->yaw = mavlink_msg_set_roll_pitch_yaw_get_yaw(msg);
}

184
libraries/GCS_MAVLink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h Normal file
View File

@ -0,0 +1,184 @@
// MESSAGE SET_ROLL_PITCH_YAW_SPEED PACKING
#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED 56
typedef struct __mavlink_set_roll_pitch_yaw_speed_t
{
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
float roll_speed; ///< Desired roll angular speed in rad/s
float pitch_speed; ///< Desired pitch angular speed in rad/s
float yaw_speed; ///< Desired yaw angular speed in rad/s
} mavlink_set_roll_pitch_yaw_speed_t;
/**
* @brief Pack a set_roll_pitch_yaw_speed message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param target_system System ID
* @param target_component Component ID
* @param roll_speed Desired roll angular speed in rad/s
* @param pitch_speed Desired pitch angular speed in rad/s
* @param yaw_speed Desired yaw angular speed in rad/s
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED;
i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s
i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s
i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s
return mavlink_finalize_message(msg, system_id, component_id, i);
}
/**
* @brief Pack a set_roll_pitch_yaw_speed message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @param roll_speed Desired roll angular speed in rad/s
* @param pitch_speed Desired pitch angular speed in rad/s
* @param yaw_speed Desired yaw angular speed in rad/s
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED;
i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID
i += put_uint8_t_by_index(target_component, i, msg->payload); // Component ID
i += put_float_by_index(roll_speed, i, msg->payload); // Desired roll angular speed in rad/s
i += put_float_by_index(pitch_speed, i, msg->payload); // Desired pitch angular speed in rad/s
i += put_float_by_index(yaw_speed, i, msg->payload); // Desired yaw angular speed in rad/s
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
/**
* @brief Encode a set_roll_pitch_yaw_speed struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param set_roll_pitch_yaw_speed C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed)
{
return mavlink_msg_set_roll_pitch_yaw_speed_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed->target_system, set_roll_pitch_yaw_speed->target_component, set_roll_pitch_yaw_speed->roll_speed, set_roll_pitch_yaw_speed->pitch_speed, set_roll_pitch_yaw_speed->yaw_speed);
}
/**
* @brief Send a set_roll_pitch_yaw_speed message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
* @param roll_speed Desired roll angular speed in rad/s
* @param pitch_speed Desired pitch angular speed in rad/s
* @param yaw_speed Desired yaw angular speed in rad/s
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_set_roll_pitch_yaw_speed_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
{
mavlink_message_t msg;
mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, roll_speed, pitch_speed, yaw_speed);
mavlink_send_uart(chan, &msg);
}
#endif
// MESSAGE SET_ROLL_PITCH_YAW_SPEED UNPACKING
/**
* @brief Get field target_system from set_roll_pitch_yaw_speed message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload)[0];
}
/**
* @brief Get field target_component from set_roll_pitch_yaw_speed message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
}
/**
* @brief Get field roll_speed from set_roll_pitch_yaw_speed message
*
* @return Desired roll angular speed in rad/s
*/
static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[3];
return (float)r.f;
}
/**
* @brief Get field pitch_speed from set_roll_pitch_yaw_speed message
*
* @return Desired pitch angular speed in rad/s
*/
static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field yaw_speed from set_roll_pitch_yaw_speed message
*
* @return Desired yaw angular speed in rad/s
*/
static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Decode a set_roll_pitch_yaw_speed message into a struct
*
* @param msg The message to decode
* @param set_roll_pitch_yaw_speed C-struct to decode the message contents into
*/
static inline void mavlink_msg_set_roll_pitch_yaw_speed_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed)
{
set_roll_pitch_yaw_speed->target_system = mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(msg);
set_roll_pitch_yaw_speed->target_component = mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(msg);
set_roll_pitch_yaw_speed->roll_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(msg);
set_roll_pitch_yaw_speed->pitch_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(msg);
set_roll_pitch_yaw_speed->yaw_speed = mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(msg);
}

31
libraries/GCS_MAVLink/include/mavlink_types.h
View File

@ -13,6 +13,7 @@ enum MAV_CLASS
MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints
MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands
MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set
MAV_CLASS_NONE = 8, ///< No valid autopilot
MAV_CLASS_NB ///< Number of autopilot classes
};
@ -101,7 +102,8 @@ enum MAV_NAV
MAV_NAV_RETURNING,
MAV_NAV_LANDING,
MAV_NAV_LOST,
MAV_NAV_LOITER
MAV_NAV_LOITER,
MAV_NAV_FREE_DRIFT
};
enum MAV_TYPE
@ -112,7 +114,12 @@ enum MAV_TYPE
MAV_COAXIAL = 3,
MAV_HELICOPTER = 4,
MAV_GROUND = 5,
OCU = 6
OCU = 6,
MAV_AIRSHIP = 7,
MAV_FREE_BALLOON = 8,
MAV_ROCKET = 9,
UGV_GROUND_ROVER = 10,
UGV_SURFACE_SHIP = 11
};
enum MAV_AUTOPILOT_TYPE
@ -120,7 +127,8 @@ enum MAV_AUTOPILOT_TYPE
MAV_AUTOPILOT_GENERIC = 0,
MAV_AUTOPILOT_PIXHAWK = 1,
MAV_AUTOPILOT_SLUGS = 2,
MAV_AUTOPILOT_ARDUPILOTMEGA = 3
MAV_AUTOPILOT_ARDUPILOTMEGA = 3,
MAV_AUTOPILOT_NONE = 4
};
enum MAV_COMPONENT
@ -133,6 +141,8 @@ enum MAV_COMPONENT
MAV_COMP_ID_MAPPER,
MAV_COMP_ID_CAMERA,
MAV_COMP_ID_IMU = 200,
MAV_COMP_ID_IMU_2 = 201,
MAV_COMP_ID_IMU_3 = 202,
MAV_COMP_ID_UDP_BRIDGE = 240,
MAV_COMP_ID_UART_BRIDGE = 241,
MAV_COMP_ID_SYSTEM_CONTROL = 250
@ -143,7 +153,19 @@ enum MAV_FRAME
MAV_FRAME_GLOBAL = 0,
MAV_FRAME_LOCAL = 1,
MAV_FRAME_MISSION = 2,
MAV_FRAME_GLOBAL_RELATIVE_ALT = 3
MAV_FRAME_GLOBAL_RELATIVE_ALT = 3,
MAV_FRAME_LOCAL_ENU = 4
};
enum MAVLINK_DATA_STREAM_TYPE
{
MAVLINK_DATA_STREAM_IMG_JPEG,
MAVLINK_DATA_STREAM_IMG_BMP,
MAVLINK_DATA_STREAM_IMG_RAW8U,
MAVLINK_DATA_STREAM_IMG_RAW32U,
MAVLINK_DATA_STREAM_IMG_PGM,
MAVLINK_DATA_STREAM_IMG_PNG
};
#define MAVLINK_STX 0x55 ///< Packet start sign
@ -165,6 +187,7 @@ typedef struct __mavlink_system {
uint8_t type; ///< Unused, can be used by user to store the system's type
uint8_t state; ///< Unused, can be used by user to store the system's state
uint8_t mode; ///< Unused, can be used by user to store the system's mode
uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode
} mavlink_system_t;
typedef struct __mavlink_message {

11
libraries/GCS_MAVLink/include/minimal/mavlink.h Normal file
View File

@ -0,0 +1,11 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Friday, August 5 2011, 07:37 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H
#include "minimal.h"
#endif

132
libraries/GCS_MAVLink/include/minimal/mavlink_msg_heartbeat.h Normal file
View File

@ -0,0 +1,132 @@
// MESSAGE HEARTBEAT PACKING
#define MAVLINK_MSG_ID_HEARTBEAT 0
typedef struct __mavlink_heartbeat_t
{
uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
uint8_t autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
uint8_t mavlink_version; ///< MAVLink version
} mavlink_heartbeat_t;
/**
* @brief Pack a heartbeat message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
* @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t autopilot)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version
return mavlink_finalize_message(msg, system_id, component_id, i);
}
/**
* @brief Pack a heartbeat message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
* @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t autopilot)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_HEARTBEAT;
i += put_uint8_t_by_index(type, i, msg->payload); // Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
i += put_uint8_t_by_index(autopilot, i, msg->payload); // Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
i += put_uint8_t_by_index(2, i, msg->payload); // MAVLink version
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
/**
* @brief Encode a heartbeat struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param heartbeat C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_heartbeat_t* heartbeat)
{
return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot);
}
/**
* @brief Send a heartbeat message
* @param chan MAVLink channel to send the message
*
* @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
* @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot)
{
mavlink_message_t msg;
mavlink_msg_heartbeat_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, type, autopilot);
mavlink_send_uart(chan, &msg);
}
#endif
// MESSAGE HEARTBEAT UNPACKING
/**
* @brief Get field type from heartbeat message
*
* @return Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
*/
static inline uint8_t mavlink_msg_heartbeat_get_type(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload)[0];
}
/**
* @brief Get field autopilot from heartbeat message
*
* @return Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
*/
static inline uint8_t mavlink_msg_heartbeat_get_autopilot(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
}
/**
* @brief Get field mavlink_version from heartbeat message
*
* @return MAVLink version
*/
static inline uint8_t mavlink_msg_heartbeat_get_mavlink_version(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
}
/**
* @brief Decode a heartbeat message into a struct
*
* @param msg The message to decode
* @param heartbeat C-struct to decode the message contents into
*/
static inline void mavlink_msg_heartbeat_decode(const mavlink_message_t* msg, mavlink_heartbeat_t* heartbeat)
{
heartbeat->type = mavlink_msg_heartbeat_get_type(msg);
heartbeat->autopilot = mavlink_msg_heartbeat_get_autopilot(msg);
heartbeat->mavlink_version = mavlink_msg_heartbeat_get_mavlink_version(msg);
}

45
libraries/GCS_MAVLink/include/minimal/minimal.h Normal file
View File

@ -0,0 +1,45 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://qgroundcontrol.org/mavlink/
* Generated on Friday, August 5 2011, 07:37 UTC
*/
#ifndef MINIMAL_H
#define MINIMAL_H
#ifdef __cplusplus
extern "C" {
#endif
#include "../protocol.h"
#define MAVLINK_ENABLED_MINIMAL
// MAVLINK VERSION
#ifndef MAVLINK_VERSION
#define MAVLINK_VERSION 2
#endif
#if (MAVLINK_VERSION == 0)
#undef MAVLINK_VERSION
#define MAVLINK_VERSION 2
#endif
// ENUM DEFINITIONS
// MESSAGE DEFINITIONS
#include "./mavlink_msg_heartbeat.h"
// MESSAGE LENGTHS
#undef MAVLINK_MESSAGE_LENGTHS
#define MAVLINK_MESSAGE_LENGTHS { }
#ifdef __cplusplus
}
#endif
#endif

2
libraries/GCS_MAVLink/include/pixhawk/mavlink.h
View File

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Saturday, April 16 2011, 04:02 UTC
* Generated on Friday, August 5 2011, 07:37 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H

76
libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_available.h
View File

@ -24,6 +24,9 @@ typedef struct __mavlink_image_available_t
float lat; ///< GPS X coordinate
float lon; ///< GPS Y coordinate
float alt; ///< Global frame altitude
float ground_x; ///< Ground truth X
float ground_y; ///< Ground truth Y
float ground_z; ///< Ground truth Z
} mavlink_image_available_t;
@ -55,9 +58,12 @@ typedef struct __mavlink_image_available_t
* @param lat GPS X coordinate
* @param lon GPS Y coordinate
* @param alt Global frame altitude
* @param ground_x Ground truth X
* @param ground_y Ground truth Y
* @param ground_z Ground truth Z
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt)
static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE;
@ -82,6 +88,9 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8
i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate
i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate
i += put_float_by_index(alt, i, msg->payload); // Global frame altitude
i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X
i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y
i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z
return mavlink_finalize_message(msg, system_id, component_id, i);
}
@ -112,9 +121,12 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8
* @param lat GPS X coordinate
* @param lon GPS Y coordinate
* @param alt Global frame altitude
* @param ground_x Ground truth X
* @param ground_y Ground truth Y
* @param ground_z Ground truth Z
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt)
static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE;
@ -139,6 +151,9 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id,
i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate
i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate
i += put_float_by_index(alt, i, msg->payload); // Global frame altitude
i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X
i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y
i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
@ -153,7 +168,7 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id,
*/
static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_available_t* image_available)
{
return mavlink_msg_image_available_pack(system_id, component_id, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt);
return mavlink_msg_image_available_pack(system_id, component_id, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z);
}
/**
@ -180,13 +195,16 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin
* @param lat GPS X coordinate
* @param lon GPS Y coordinate
* @param alt Global frame altitude
* @param ground_x Ground truth X
* @param ground_y Ground truth Y
* @param ground_z Ground truth Z
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt)
static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z)
{
mavlink_message_t msg;
mavlink_msg_image_available_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, cam_id, cam_no, timestamp, valid_until, img_seq, img_buf_index, width, height, depth, channels, key, exposure, gain, roll, pitch, yaw, local_z, lat, lon, alt);
mavlink_msg_image_available_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, cam_id, cam_no, timestamp, valid_until, img_seq, img_buf_index, width, height, depth, channels, key, exposure, gain, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z);
mavlink_send_uart(chan, &msg);
}
@ -489,6 +507,51 @@ static inline float mavlink_msg_image_available_get_alt(const mavlink_message_t*
return (float)r.f;
}
/**
* @brief Get field ground_x from image_available message
*
* @return Ground truth X
*/
static inline float mavlink_msg_image_available_get_ground_x(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field ground_y from image_available message
*
* @return Ground truth Y
*/
static inline float mavlink_msg_image_available_get_ground_y(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field ground_z from image_available message
*
* @return Ground truth Z
*/
static inline float mavlink_msg_image_available_get_ground_z(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint8_t)+sizeof(uint64_t)+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint16_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Decode a image_available message into a struct
*
@ -517,4 +580,7 @@ static inline void mavlink_msg_image_available_decode(const mavlink_message_t* m
image_available->lat = mavlink_msg_image_available_get_lat(msg);
image_available->lon = mavlink_msg_image_available_get_lon(msg);
image_available->alt = mavlink_msg_image_available_get_alt(msg);
image_available->ground_x = mavlink_msg_image_available_get_ground_x(msg);
image_available->ground_y = mavlink_msg_image_available_get_ground_y(msg);
image_available->ground_z = mavlink_msg_image_available_get_ground_z(msg);
}

76
libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_image_triggered.h
View File

@ -13,6 +13,9 @@ typedef struct __mavlink_image_triggered_t
float lat; ///< GPS X coordinate
float lon; ///< GPS Y coordinate
float alt; ///< Global frame altitude
float ground_x; ///< Ground truth X
float ground_y; ///< Ground truth Y
float ground_z; ///< Ground truth Z
} mavlink_image_triggered_t;
@ -33,9 +36,12 @@ typedef struct __mavlink_image_triggered_t
* @param lat GPS X coordinate
* @param lon GPS Y coordinate
* @param alt Global frame altitude
* @param ground_x Ground truth X
* @param ground_y Ground truth Y
* @param ground_z Ground truth Z
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt)
static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED;
@ -49,6 +55,9 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8
i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate
i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate
i += put_float_by_index(alt, i, msg->payload); // Global frame altitude
i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X
i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y
i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z
return mavlink_finalize_message(msg, system_id, component_id, i);
}
@ -68,9 +77,12 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8
* @param lat GPS X coordinate
* @param lon GPS Y coordinate
* @param alt Global frame altitude
* @param ground_x Ground truth X
* @param ground_y Ground truth Y
* @param ground_z Ground truth Z
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt)
static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED;
@ -84,6 +96,9 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id,
i += put_float_by_index(lat, i, msg->payload); // GPS X coordinate
i += put_float_by_index(lon, i, msg->payload); // GPS Y coordinate
i += put_float_by_index(alt, i, msg->payload); // Global frame altitude
i += put_float_by_index(ground_x, i, msg->payload); // Ground truth X
i += put_float_by_index(ground_y, i, msg->payload); // Ground truth Y
i += put_float_by_index(ground_z, i, msg->payload); // Ground truth Z
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
@ -98,7 +113,7 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id,
*/
static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_image_triggered_t* image_triggered)
{
return mavlink_msg_image_triggered_pack(system_id, component_id, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt);
return mavlink_msg_image_triggered_pack(system_id, component_id, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z);
}
/**
@ -114,13 +129,16 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin
* @param lat GPS X coordinate
* @param lon GPS Y coordinate
* @param alt Global frame altitude
* @param ground_x Ground truth X
* @param ground_y Ground truth Y
* @param ground_z Ground truth Z
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt)
static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z)
{
mavlink_message_t msg;
mavlink_msg_image_triggered_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, timestamp, seq, roll, pitch, yaw, local_z, lat, lon, alt);
mavlink_msg_image_triggered_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, timestamp, seq, roll, pitch, yaw, local_z, lat, lon, alt, ground_x, ground_y, ground_z);
mavlink_send_uart(chan, &msg);
}
@ -266,6 +284,51 @@ static inline float mavlink_msg_image_triggered_get_alt(const mavlink_message_t*
return (float)r.f;
}
/**
* @brief Get field ground_x from image_triggered message
*
* @return Ground truth X
*/
static inline float mavlink_msg_image_triggered_get_ground_x(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field ground_y from image_triggered message
*
* @return Ground truth Y
*/
static inline float mavlink_msg_image_triggered_get_ground_y(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field ground_z from image_triggered message
*
* @return Ground truth Z
*/
static inline float mavlink_msg_image_triggered_get_ground_z(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Decode a image_triggered message into a struct
*
@ -283,4 +346,7 @@ static inline void mavlink_msg_image_triggered_decode(const mavlink_message_t* m
image_triggered->lat = mavlink_msg_image_triggered_get_lat(msg);
image_triggered->lon = mavlink_msg_image_triggered_get_lon(msg);
image_triggered->alt = mavlink_msg_image_triggered_get_alt(msg);
image_triggered->ground_x = mavlink_msg_image_triggered_get_ground_x(msg);
image_triggered->ground_y = mavlink_msg_image_triggered_get_ground_y(msg);
image_triggered->ground_z = mavlink_msg_image_triggered_get_ground_z(msg);
}

176
libraries/GCS_MAVLink/include/pixhawk/mavlink_msg_vision_speed_estimate.h Normal file
View File

@ -0,0 +1,176 @@
// MESSAGE VISION_SPEED_ESTIMATE PACKING
#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 113
typedef struct __mavlink_vision_speed_estimate_t
{
uint64_t usec; ///< Timestamp (milliseconds)
float x; ///< Global X speed
float y; ///< Global Y speed
float z; ///< Global Z speed
} mavlink_vision_speed_estimate_t;
/**
* @brief Pack a vision_speed_estimate message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param usec Timestamp (milliseconds)
* @param x Global X speed
* @param y Global Y speed
* @param z Global Z speed
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float x, float y, float z)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE;
i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds)
i += put_float_by_index(x, i, msg->payload); // Global X speed
i += put_float_by_index(y, i, msg->payload); // Global Y speed
i += put_float_by_index(z, i, msg->payload); // Global Z speed
return mavlink_finalize_message(msg, system_id, component_id, i);
}
/**
* @brief Pack a vision_speed_estimate message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param usec Timestamp (milliseconds)
* @param x Global X speed
* @param y Global Y speed
* @param z Global Z speed
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float x, float y, float z)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE;
i += put_uint64_t_by_index(usec, i, msg->payload); // Timestamp (milliseconds)
i += put_float_by_index(x, i, msg->payload); // Global X speed
i += put_float_by_index(y, i, msg->payload); // Global Y speed
i += put_float_by_index(z, i, msg->payload); // Global Z speed
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
/**
* @brief Encode a vision_speed_estimate struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param vision_speed_estimate C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate)
{
return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z);
}
/**
* @brief Send a vision_speed_estimate message
* @param chan MAVLink channel to send the message
*
* @param usec Timestamp (milliseconds)
* @param x Global X speed
* @param y Global Y speed
* @param z Global Z speed
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z)
{
mavlink_message_t msg;
mavlink_msg_vision_speed_estimate_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, usec, x, y, z);
mavlink_send_uart(chan, &msg);
}
#endif
// MESSAGE VISION_SPEED_ESTIMATE UNPACKING
/**
* @brief Get field usec from vision_speed_estimate message
*
* @return Timestamp (milliseconds)
*/
static inline uint64_t mavlink_msg_vision_speed_estimate_get_usec(const mavlink_message_t* msg)
{
generic_64bit r;
r.b[7] = (msg->payload)[0];
r.b[6] = (msg->payload)[1];
r.b[5] = (msg->payload)[2];
r.b[4] = (msg->payload)[3];
r.b[3] = (msg->payload)[4];
r.b[2] = (msg->payload)[5];
r.b[1] = (msg->payload)[6];
r.b[0] = (msg->payload)[7];
return (uint64_t)r.ll;
}
/**
* @brief Get field x from vision_speed_estimate message
*
* @return Global X speed
*/
static inline float mavlink_msg_vision_speed_estimate_get_x(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t))[0];
r.b[2] = (msg->payload+sizeof(uint64_t))[1];
r.b[1] = (msg->payload+sizeof(uint64_t))[2];
r.b[0] = (msg->payload+sizeof(uint64_t))[3];
return (float)r.f;
}
/**
* @brief Get field y from vision_speed_estimate message
*
* @return Global Y speed
*/
static inline float mavlink_msg_vision_speed_estimate_get_y(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field z from vision_speed_estimate message
*
* @return Global Z speed
*/
static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint64_t)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Decode a vision_speed_estimate message into a struct
*
* @param msg The message to decode
* @param vision_speed_estimate C-struct to decode the message contents into
*/
static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_message_t* msg, mavlink_vision_speed_estimate_t* vision_speed_estimate)
{
vision_speed_estimate->usec = mavlink_msg_vision_speed_estimate_get_usec(msg);
vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg);
vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg);
vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg);
}

16
libraries/GCS_MAVLink/include/pixhawk/pixhawk.h
View File

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Saturday, April 16 2011, 04:02 UTC
* @see http://qgroundcontrol.org/mavlink/
* Generated on Friday, August 5 2011, 07:37 UTC
*/
#ifndef PIXHAWK_H
#define PIXHAWK_H
@ -30,15 +30,6 @@ extern "C" {
// ENUM DEFINITIONS
/** @brief Slugs parameter interface subsets */
enum SLUGS_PID_INDX_IDS
{
PID_YAW_DAMPER=2,
PID_PITCH=3, /* With comment: PID Pitch parameter*/
PID_ALT_HOLD=50,
SLUGS_PID_INDX_IDS_ENUM_END
};
/** @brief Content Types for data transmission handshake */
enum DATA_TYPES
{
@ -58,6 +49,7 @@ enum DATA_TYPES
#include "./mavlink_msg_image_available.h"
#include "./mavlink_msg_vision_position_estimate.h"
#include "./mavlink_msg_vicon_position_estimate.h"
#include "./mavlink_msg_vision_speed_estimate.h"
#include "./mavlink_msg_position_control_setpoint_set.h"
#include "./mavlink_msg_position_control_offset_set.h"
#include "./mavlink_msg_position_control_setpoint.h"
@ -79,7 +71,7 @@ enum DATA_TYPES
// 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, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 40, 1, 80, 0, 0, 0, 0, 0, 0, 0, 32, 32, 0, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 42, 54, 0, 0, 0, 0, 0, 0, 0, 8, 255, 53, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 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, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 0, 0, 32, 32, 20, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 42, 54, 0, 0, 0, 0, 0, 0, 0, 8, 255, 53, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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
}

2
libraries/GCS_MAVLink/include/protocol.h
View File

@ -805,7 +805,7 @@ static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t p
// First pack everything we can into the current 'open' byte
//curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8
//FIXME
if (bits_remain <= (unsigned)(8 - i_bit_index))
if (bits_remain <= (8 - i_bit_index))
{
// Enough space
curr_bits_n = bits_remain;

2
libraries/GCS_MAVLink/include/slugs/mavlink.h
View File

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Saturday, April 16 2011, 04:02 UTC
* Generated on Friday, August 5 2011, 07:37 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H

6
libraries/GCS_MAVLink/include/slugs/slugs.h
View File

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Saturday, April 16 2011, 04:02 UTC
* @see http://qgroundcontrol.org/mavlink/
* Generated on Friday, August 5 2011, 07:37 UTC
*/
#ifndef SLUGS_H
#define SLUGS_H
@ -48,7 +48,7 @@ extern "C" {
// 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, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 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, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 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, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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
}

2
libraries/GCS_MAVLink/include/ualberta/mavlink.h
View File

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Saturday, April 16 2011, 04:02 UTC
* Generated on Friday, August 5 2011, 07:37 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H

104
libraries/GCS_MAVLink/include/ualberta/mavlink_msg_radio_calibration.h
View File

@ -1,15 +1,15 @@
// MESSAGE RADIO_CALIBRATION PACKING
#define MAVLINK_MSG_ID_RADIO_CALIBRATION 222
#define MAVLINK_MSG_ID_RADIO_CALIBRATION 221
typedef struct __mavlink_radio_calibration_t
{
float aileron[3]; ///< Aileron setpoints: high, center, low
float elevator[3]; ///< Elevator setpoints: high, center, low
float rudder[3]; ///< Rudder setpoints: high, center, low
float gyro[2]; ///< Tail gyro mode/gain setpoints: heading hold, rate mode
float pitch[5]; ///< Pitch curve setpoints (every 25%)
float throttle[5]; ///< Throttle curve setpoints (every 25%)
uint16_t aileron[3]; ///< Aileron setpoints: left, center, right
uint16_t elevator[3]; ///< Elevator setpoints: nose down, center, nose up
uint16_t rudder[3]; ///< Rudder setpoints: nose left, center, nose right
uint16_t gyro[2]; ///< Tail gyro mode/gain setpoints: heading hold, rate mode
uint16_t pitch[5]; ///< Pitch curve setpoints (every 25%)
uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%)
} mavlink_radio_calibration_t;
@ -27,25 +27,25 @@ typedef struct __mavlink_radio_calibration_t
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param aileron Aileron setpoints: high, center, low
* @param elevator Elevator setpoints: high, center, low
* @param rudder Rudder setpoints: high, center, low
* @param aileron Aileron setpoints: left, center, right
* @param elevator Elevator setpoints: nose down, center, nose up
* @param rudder Rudder setpoints: nose left, center, nose right
* @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode
* @param pitch Pitch curve setpoints (every 25%)
* @param throttle Throttle curve setpoints (every 25%)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const float* aileron, const float* elevator, const float* rudder, const float* gyro, const float* pitch, const float* throttle)
static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION;
i += put_array_by_index((const int8_t*)aileron, sizeof(float)*3, i, msg->payload); // Aileron setpoints: high, center, low
i += put_array_by_index((const int8_t*)elevator, sizeof(float)*3, i, msg->payload); // Elevator setpoints: high, center, low
i += put_array_by_index((const int8_t*)rudder, sizeof(float)*3, i, msg->payload); // Rudder setpoints: high, center, low
i += put_array_by_index((const int8_t*)gyro, sizeof(float)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode
i += put_array_by_index((const int8_t*)pitch, sizeof(float)*5, i, msg->payload); // Pitch curve setpoints (every 25%)
i += put_array_by_index((const int8_t*)throttle, sizeof(float)*5, i, msg->payload); // Throttle curve setpoints (every 25%)
i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right
i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up
i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right
i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode
i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%)
i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%)
return mavlink_finalize_message(msg, system_id, component_id, i);
}
@ -56,25 +56,25 @@ static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uin
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param aileron Aileron setpoints: high, center, low
* @param elevator Elevator setpoints: high, center, low
* @param rudder Rudder setpoints: high, center, low
* @param aileron Aileron setpoints: left, center, right
* @param elevator Elevator setpoints: nose down, center, nose up
* @param rudder Rudder setpoints: nose left, center, nose right
* @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode
* @param pitch Pitch curve setpoints (every 25%)
* @param throttle Throttle curve setpoints (every 25%)
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const float* aileron, const float* elevator, const float* rudder, const float* gyro, const float* pitch, const float* throttle)
static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION;
i += put_array_by_index((const int8_t*)aileron, sizeof(float)*3, i, msg->payload); // Aileron setpoints: high, center, low
i += put_array_by_index((const int8_t*)elevator, sizeof(float)*3, i, msg->payload); // Elevator setpoints: high, center, low
i += put_array_by_index((const int8_t*)rudder, sizeof(float)*3, i, msg->payload); // Rudder setpoints: high, center, low
i += put_array_by_index((const int8_t*)gyro, sizeof(float)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode
i += put_array_by_index((const int8_t*)pitch, sizeof(float)*5, i, msg->payload); // Pitch curve setpoints (every 25%)
i += put_array_by_index((const int8_t*)throttle, sizeof(float)*5, i, msg->payload); // Throttle curve setpoints (every 25%)
i += put_array_by_index((const int8_t*)aileron, sizeof(uint16_t)*3, i, msg->payload); // Aileron setpoints: left, center, right
i += put_array_by_index((const int8_t*)elevator, sizeof(uint16_t)*3, i, msg->payload); // Elevator setpoints: nose down, center, nose up
i += put_array_by_index((const int8_t*)rudder, sizeof(uint16_t)*3, i, msg->payload); // Rudder setpoints: nose left, center, nose right
i += put_array_by_index((const int8_t*)gyro, sizeof(uint16_t)*2, i, msg->payload); // Tail gyro mode/gain setpoints: heading hold, rate mode
i += put_array_by_index((const int8_t*)pitch, sizeof(uint16_t)*5, i, msg->payload); // Pitch curve setpoints (every 25%)
i += put_array_by_index((const int8_t*)throttle, sizeof(uint16_t)*5, i, msg->payload); // Throttle curve setpoints (every 25%)
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
@ -96,16 +96,16 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u
* @brief Send a radio_calibration message
* @param chan MAVLink channel to send the message
*
* @param aileron Aileron setpoints: high, center, low
* @param elevator Elevator setpoints: high, center, low
* @param rudder Rudder setpoints: high, center, low
* @param aileron Aileron setpoints: left, center, right
* @param elevator Elevator setpoints: nose down, center, nose up
* @param rudder Rudder setpoints: nose left, center, nose right
* @param gyro Tail gyro mode/gain setpoints: heading hold, rate mode
* @param pitch Pitch curve setpoints (every 25%)
* @param throttle Throttle curve setpoints (every 25%)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const float* aileron, const float* elevator, const float* rudder, const float* gyro, const float* pitch, const float* throttle)
static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle)
{
mavlink_message_t msg;
mavlink_msg_radio_calibration_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, aileron, elevator, rudder, gyro, pitch, throttle);
@ -118,37 +118,37 @@ static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, co
/**
* @brief Get field aileron from radio_calibration message
*
* @return Aileron setpoints: high, center, low
* @return Aileron setpoints: left, center, right
*/
static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, float* r_data)
static inline uint16_t mavlink_msg_radio_calibration_get_aileron(const mavlink_message_t* msg, uint16_t* r_data)
{
memcpy(r_data, msg->payload, sizeof(float)*3);
return sizeof(float)*3;
memcpy(r_data, msg->payload, sizeof(uint16_t)*3);
return sizeof(uint16_t)*3;
}
/**
* @brief Get field elevator from radio_calibration message
*
* @return Elevator setpoints: high, center, low
* @return Elevator setpoints: nose down, center, nose up
*/
static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, float* r_data)
static inline uint16_t mavlink_msg_radio_calibration_get_elevator(const mavlink_message_t* msg, uint16_t* r_data)
{
memcpy(r_data, msg->payload+sizeof(float)*3, sizeof(float)*3);
return sizeof(float)*3;
memcpy(r_data, msg->payload+sizeof(uint16_t)*3, sizeof(uint16_t)*3);
return sizeof(uint16_t)*3;
}
/**
* @brief Get field rudder from radio_calibration message
*
* @return Rudder setpoints: high, center, low
* @return Rudder setpoints: nose left, center, nose right
*/
static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, float* r_data)
static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_message_t* msg, uint16_t* r_data)
{
memcpy(r_data, msg->payload+sizeof(float)*3+sizeof(float)*3, sizeof(float)*3);
return sizeof(float)*3;
memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*3);
return sizeof(uint16_t)*3;
}
/**
@ -156,11 +156,11 @@ static inline uint16_t mavlink_msg_radio_calibration_get_rudder(const mavlink_me
*
* @return Tail gyro mode/gain setpoints: heading hold, rate mode
*/
static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, float* r_data)
static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_message_t* msg, uint16_t* r_data)
{
memcpy(r_data, msg->payload+sizeof(float)*3+sizeof(float)*3+sizeof(float)*3, sizeof(float)*2);
return sizeof(float)*2;
memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3, sizeof(uint16_t)*2);
return sizeof(uint16_t)*2;
}
/**
@ -168,11 +168,11 @@ static inline uint16_t mavlink_msg_radio_calibration_get_gyro(const mavlink_mess
*
* @return Pitch curve setpoints (every 25%)
*/
static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, float* r_data)
static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_message_t* msg, uint16_t* r_data)
{
memcpy(r_data, msg->payload+sizeof(float)*3+sizeof(float)*3+sizeof(float)*3+sizeof(float)*2, sizeof(float)*5);
return sizeof(float)*5;
memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2, sizeof(uint16_t)*5);
return sizeof(uint16_t)*5;
}
/**
@ -180,11 +180,11 @@ static inline uint16_t mavlink_msg_radio_calibration_get_pitch(const mavlink_mes
*
* @return Throttle curve setpoints (every 25%)
*/
static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, float* r_data)
static inline uint16_t mavlink_msg_radio_calibration_get_throttle(const mavlink_message_t* msg, uint16_t* r_data)
{
memcpy(r_data, msg->payload+sizeof(float)*3+sizeof(float)*3+sizeof(float)*3+sizeof(float)*2+sizeof(float)*5, sizeof(float)*5);
return sizeof(float)*5;
memcpy(r_data, msg->payload+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*3+sizeof(uint16_t)*2+sizeof(uint16_t)*5, sizeof(uint16_t)*5);
return sizeof(uint16_t)*5;
}
/**

135
libraries/GCS_MAVLink/include/ualberta/mavlink_msg_ualberta_sys_status.h Normal file
View File

@ -0,0 +1,135 @@
// MESSAGE UALBERTA_SYS_STATUS PACKING
#define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS 222
typedef struct __mavlink_ualberta_sys_status_t
{
uint8_t mode; ///< System mode, see UALBERTA_AUTOPILOT_MODE ENUM
uint8_t nav_mode; ///< Navigation mode, see UALBERTA_NAV_MODE ENUM
uint8_t pilot; ///< Pilot mode, see UALBERTA_PILOT_MODE
} mavlink_ualberta_sys_status_t;
/**
* @brief Pack a ualberta_sys_status message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM
* @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM
* @param pilot Pilot mode, see UALBERTA_PILOT_MODE
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t pilot)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS;
i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM
i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see UALBERTA_NAV_MODE ENUM
i += put_uint8_t_by_index(pilot, i, msg->payload); // Pilot mode, see UALBERTA_PILOT_MODE
return mavlink_finalize_message(msg, system_id, component_id, i);
}
/**
* @brief Pack a ualberta_sys_status message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM
* @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM
* @param pilot Pilot mode, see UALBERTA_PILOT_MODE
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_ualberta_sys_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t mode, uint8_t nav_mode, uint8_t pilot)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS;
i += put_uint8_t_by_index(mode, i, msg->payload); // System mode, see UALBERTA_AUTOPILOT_MODE ENUM
i += put_uint8_t_by_index(nav_mode, i, msg->payload); // Navigation mode, see UALBERTA_NAV_MODE ENUM
i += put_uint8_t_by_index(pilot, i, msg->payload); // Pilot mode, see UALBERTA_PILOT_MODE
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
/**
* @brief Encode a ualberta_sys_status struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param ualberta_sys_status C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ualberta_sys_status_t* ualberta_sys_status)
{
return mavlink_msg_ualberta_sys_status_pack(system_id, component_id, msg, ualberta_sys_status->mode, ualberta_sys_status->nav_mode, ualberta_sys_status->pilot);
}
/**
* @brief Send a ualberta_sys_status message
* @param chan MAVLink channel to send the message
*
* @param mode System mode, see UALBERTA_AUTOPILOT_MODE ENUM
* @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM
* @param pilot Pilot mode, see UALBERTA_PILOT_MODE
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot)
{
mavlink_message_t msg;
mavlink_msg_ualberta_sys_status_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, mode, nav_mode, pilot);
mavlink_send_uart(chan, &msg);
}
#endif
// MESSAGE UALBERTA_SYS_STATUS UNPACKING
/**
* @brief Get field mode from ualberta_sys_status message
*
* @return System mode, see UALBERTA_AUTOPILOT_MODE ENUM
*/
static inline uint8_t mavlink_msg_ualberta_sys_status_get_mode(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload)[0];
}
/**
* @brief Get field nav_mode from ualberta_sys_status message
*
* @return Navigation mode, see UALBERTA_NAV_MODE ENUM
*/
static inline uint8_t mavlink_msg_ualberta_sys_status_get_nav_mode(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
}
/**
* @brief Get field pilot from ualberta_sys_status message
*
* @return Pilot mode, see UALBERTA_PILOT_MODE
*/
static inline uint8_t mavlink_msg_ualberta_sys_status_get_pilot(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
}
/**
* @brief Decode a ualberta_sys_status message into a struct
*
* @param msg The message to decode
* @param ualberta_sys_status C-struct to decode the message contents into
*/
static inline void mavlink_msg_ualberta_sys_status_decode(const mavlink_message_t* msg, mavlink_ualberta_sys_status_t* ualberta_sys_status)
{
ualberta_sys_status->mode = mavlink_msg_ualberta_sys_status_get_mode(msg);
ualberta_sys_status->nav_mode = mavlink_msg_ualberta_sys_status_get_nav_mode(msg);
ualberta_sys_status->pilot = mavlink_msg_ualberta_sys_status_get_pilot(msg);
}

38
libraries/GCS_MAVLink/include/ualberta/ualberta.h
View File

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Saturday, April 16 2011, 04:02 UTC
* @see http://qgroundcontrol.org/mavlink/
* Generated on Friday, August 5 2011, 07:37 UTC
*/
#ifndef UALBERTA_H
#define UALBERTA_H
@ -30,18 +30,48 @@ extern "C" {
// ENUM DEFINITIONS
/** @brief Available autopilot modes for ualberta uav */
enum UALBERTA_AUTOPILOT_MODE
{
MODE_MANUAL_DIRECT=0, /* */
MODE_MANUAL_SCALED=1, /* */
MODE_AUTO_PID_ATT=2, /* */
MODE_AUTO_PID_VEL=3, /* */
MODE_AUTO_PID_POS=4, /* */
UALBERTA_AUTOPILOT_MODE_ENUM_END
};
/** @brief Navigation filter mode */
enum UALBERTA_NAV_MODE
{
NAV_AHRS_INIT=0,
NAV_AHRS=1, /* */
NAV_INS_GPS_INIT=2, /* */
NAV_INS_GPS=3, /* */
UALBERTA_NAV_MODE_ENUM_END
};
/** @brief Mode currently commanded by pilot */
enum UALBERTA_PILOT_MODE
{
PILOT_MANUAL=0, /* */
PILOT_AUTO=1, /* */
PILOT_ROTO=2, /* */
UALBERTA_PILOT_MODE_ENUM_END
};
// MESSAGE DEFINITIONS
#include "./mavlink_msg_nav_filter_bias.h"
#include "./mavlink_msg_request_rc_channels.h"
#include "./mavlink_msg_radio_calibration.h"
#include "./mavlink_msg_ualberta_sys_status.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, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 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, 32, 1, 84, 0, 0, 0, 0, 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, 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, 32, 42, 3, 0, 0, 0, 0, 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
}

1020
libraries/GCS_MAVLink/message_definitions/common.xml
View File

File diff suppressed because it is too large Load Diff

13
libraries/GCS_MAVLink/message_definitions/minimal.xml Normal file
View File

@ -0,0 +1,13 @@
<?xml version='1.0'?>
<mavlink>
<version>2</version>
<enums/>
<messages>
<message id="0" name="HEARTBEAT">
<description>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).</description>
<field type="uint8_t" name="type">Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)</field>
<field type="uint8_t" name="autopilot">Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM</field>
<field type="uint8_t_mavlink_version" name="mavlink_version">MAVLink version</field>
</message>
</messages>
</mavlink>

35
libraries/GCS_MAVLink/message_definitions/pixhawk.xml
View File

@ -3,18 +3,14 @@
<include>common.xml</include>
<enums>
<enum name="SLUGS_PID_INDX_IDS" >
<description>Slugs parameter interface subsets</description>
<entry name = "PID_YAW_DAMPER" value="2" />
<entry name = "PID_PITCH">With comment: PID Pitch parameter</entry>
<entry name = "PID_ALT_HOLD" value="50" />
</enum>
<enum name="DATA_TYPES">
<description>Content Types for data transmission handshake</description>
<entry name = "DATA_TYPE_JPEG_IMAGE" value="1" />
<entry name = "DATA_TYPE_RAW_IMAGE" value="2" />
<entry name = "DATA_TYPE_KINECT" />
<entry name = "DATA_TYPE_JPEG_IMAGE" value="1">
</entry>
<entry name = "DATA_TYPE_RAW_IMAGE" value="2">
</entry>
<entry name = "DATA_TYPE_KINECT" value="3">
</entry>
</enum>
</enums>
@ -50,6 +46,9 @@
<field name="lat" type="float">GPS X coordinate</field>
<field name="lon" type="float">GPS Y coordinate</field>
<field name="alt" type="float">Global frame altitude</field>
<field name="ground_x" type="float">Ground truth X</field>
<field name="ground_y" type="float">Ground truth Y</field>
<field name="ground_z" type="float">Ground truth Z</field>
</message>
<message name="IMAGE_TRIGGER_CONTROL" id="102">
@ -77,6 +76,9 @@
<field name="lat" type="float">GPS X coordinate</field>
<field name="lon" type="float">GPS Y coordinate</field>
<field name="alt" type="float">Global frame altitude</field>
<field name="ground_x" type="float">Ground truth X</field>
<field name="ground_y" type="float">Ground truth Y</field>
<field name="ground_z" type="float">Ground truth Z</field>
</message>
<message name="VISION_POSITION_ESTIMATE" id="111">
@ -99,6 +101,13 @@
<field name="yaw" type="float">Yaw angle in rad</field>
</message>
<message name="VISION_SPEED_ESTIMATE" id="113">
<field name="usec" type="uint64_t">Timestamp (milliseconds)</field>
<field name="x" type="float">Global X speed</field>
<field name="y" type="float">Global Y speed</field>
<field name="z" type="float">Global Z speed</field>
</message>
<message name="POSITION_CONTROL_SETPOINT_SET" id="120">
<description>Message sent to the MAV to set a new position as reference for the controller</description>
<field name="target_system" type="uint8_t">System ID</field>
@ -197,7 +206,8 @@
<message name="POINT_OF_INTEREST" id="161">
<description>Notifies the operator about a point of interest (POI). This can be anything detected by the
system. This generic message is intented to help interfacing to generic visualizations and to display
the POI on a map.</description>
the POI on a map.
</description>
<field name="type" type="uint8_t">0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug</field>
<field name="color" type="uint8_t">0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta</field>
<field name="coordinate_system" type="uint8_t">0: global, 1:local</field>
@ -211,7 +221,8 @@
<message name="POINT_OF_INTEREST_CONNECTION" id="162">
<description>Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the
system. This generic message is intented to help interfacing to generic visualizations and to display
the POI on a map.</description>
the POI on a map.
</description>
<field name="type" type="uint8_t">0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug</field>
<field name="color" type="uint8_t">0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta</field>
<field name="coordinate_system" type="uint8_t">0: global, 1:local</field>

2
libraries/GCS_MAVLink/message_definitions/slugs.xml
View File

@ -112,6 +112,7 @@
<field name="rCommand" type="float">Log value 3 </field>
</message>
<message name="CTRL_SRFC_PT" id="181">
This message configures the Selective Passthrough mode. it allows to select which control surfaces the Pilot can control from his console. It is implemented as a bitfield as follows:
Position Bit Code
@ -131,6 +132,7 @@ This message configures the Selective Passthrough mode. it allows to select whic
</message>
<message name="SLUGS_ACTION" id="183">
Action messages focused on the SLUGS AP.
<field name="target" type="uint8_t">The system reporting the action</field>

65
libraries/GCS_MAVLink/message_definitions/ualberta.xml
View File

@ -1,29 +1,54 @@
<?xml version="1.0"?>
<?xml version='1.0'?>
<mavlink>
<include>common.xml</include>
<enums>
<enum name="UALBERTA_AUTOPILOT_MODE">
<description>Available autopilot modes for ualberta uav</description>
<entry name="MODE_MANUAL_DIRECT">Raw input pulse widts sent to output</entry>
<entry name="MODE_MANUAL_SCALED">Inputs are normalized using calibration, the converted back to raw pulse widths for output</entry>
<entry name="MODE_AUTO_PID_ATT"> dfsdfs</entry>
<entry name="MODE_AUTO_PID_VEL"> dfsfds</entry>
<entry name="MODE_AUTO_PID_POS"> dfsdfsdfs</entry>
</enum>
<enum name="UALBERTA_NAV_MODE">
<description>Navigation filter mode</description>
<entry name="NAV_AHRS_INIT" />
<entry name="NAV_AHRS">AHRS mode</entry>
<entry name="NAV_INS_GPS_INIT">INS/GPS initialization mode</entry>
<entry name="NAV_INS_GPS">INS/GPS mode</entry>
</enum>
<enum name="UALBERTA_PILOT_MODE">
<description>Mode currently commanded by pilot</description>
<entry name="PILOT_MANUAL"> sdf</entry>
<entry name="PILOT_AUTO"> dfs</entry>
<entry name="PILOT_ROTO"> Rotomotion mode </entry>
</enum>
</enums>
<messages>
<message name="NAV_FILTER_BIAS" id="220">
<message id="220" name="NAV_FILTER_BIAS">
<description>Accelerometer and Gyro biases from the navigation filter</description>
<field name="usec" type="uint64_t">Timestamp (microseconds)</field>
<field name="accel_0" type="float">b_f[0]</field>
<field name="accel_1" type="float">b_f[1]</field>
<field name="accel_2" type="float">b_f[2]</field>
<field name="gyro_0" type="float">b_f[0]</field>
<field name="gyro_1" type="float">b_f[1]</field>
<field name="gyro_2" type="float">b_f[2]</field>
<field type="uint64_t" name="usec">Timestamp (microseconds)</field>
<field type="float" name="accel_0">b_f[0]</field>
<field type="float" name="accel_1">b_f[1]</field>
<field type="float" name="accel_2">b_f[2]</field>
<field type="float" name="gyro_0">b_f[0]</field>
<field type="float" name="gyro_1">b_f[1]</field>
<field type="float" name="gyro_2">b_f[2]</field>
</message>
<message name="REQUEST_RC_CHANNELS" id="221">
<description>Request raw and normalized rc data from the UAV</description>
<field name="enabled" type="uint8_t">True: start sending data; False: stop sending data</field>
</message>
<message name="RADIO_CALIBRATION" id="222">
<message id="221" name="RADIO_CALIBRATION">
<description>Complete set of calibration parameters for the radio</description>
<field name="aileron" type="float[3]">Aileron setpoints: high, center, low</field>
<field name="elevator" type="float[3]">Elevator setpoints: high, center, low</field>
<field name="rudder" type="float[3]">Rudder setpoints: high, center, low</field>
<field name="gyro" type="float[2]">Tail gyro mode/gain setpoints: heading hold, rate mode</field>
<field name="pitch" type="float[5]">Pitch curve setpoints (every 25%)</field>
<field name="throttle" type="float[5]">Throttle curve setpoints (every 25%)</field>
<field type="uint16_t[3]" name="aileron">Aileron setpoints: left, center, right</field>
<field type="uint16_t[3]" name="elevator">Elevator setpoints: nose down, center, nose up</field>
<field type="uint16_t[3]" name="rudder">Rudder setpoints: nose left, center, nose right</field>
<field type="uint16_t[2]" name="gyro">Tail gyro mode/gain setpoints: heading hold, rate mode</field>
<field type="uint16_t[5]" name="pitch">Pitch curve setpoints (every 25%)</field>
<field type="uint16_t[5]" name="throttle">Throttle curve setpoints (every 25%)</field>
</message>
<message id="222" name="UALBERTA_SYS_STATUS">
<description>System status specific to ualberta uav</description>
<field type="uint8_t" name="mode">System mode, see UALBERTA_AUTOPILOT_MODE ENUM</field>
<field type="uint8_t" name="nav_mode">Navigation mode, see UALBERTA_NAV_MODE ENUM</field>
<field type="uint8_t" name="pilot">Pilot mode, see UALBERTA_PILOT_MODE</field>
</message>
</messages>
</mavlink>

3
libraries/GCS_MAVLink/missionlib/testing/.gitignore vendored Normal file
View File

@ -0,0 +1,3 @@
*.o
udptest
build

12
libraries/GCS_MAVLink/missionlib/testing/Makefile Normal file
View File

@ -0,0 +1,12 @@
VERSION = 1.00
CC = gcc
CFLAGS = -I ../../include/common -Wall -Werror -DVERSION=\"$(MAVLINK_VERSION)\" -std=c99
LDFLAGS = ""
OBJ = udp.o
udptest: $(OBJ)
$(CC) $(CFLAGS) -o udptest $(OBJ) $(LDFLAGS)
%.o: %.c
$(CC) $(CFLAGS) -c $<

255
libraries/GCS_MAVLink/missionlib/testing/main.c Normal file
View File

@ -0,0 +1,255 @@
/*******************************************************************************
Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
and Bryan Godbolt godbolt ( a t ) ualberta.ca
adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
****************************************************************************/
/*
This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets
cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from
qgroundcontrol are printed by this program along with the heartbeats.
I compiled this program sucessfully on Ubuntu 10.04 with the following command
gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c
the rt library is needed for the clock_gettime on linux
*/
/* These headers are for QNX, but should all be standard on unix/linux */
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <unistd.h>
#include <stdlib.h>
#include <fcntl.h>
#include <time.h>
#if (defined __QNX__) | (defined __QNXNTO__)
/* QNX specific headers */
#include <unix.h>
#else
/* Linux / MacOS POSIX timer headers */
#include <sys/time.h>
#include <time.h>
#include <arpa/inet.h>
#endif
#include <math.h>
#include <../mavlink_types.h>
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 */
#include <mavlink.h>
/* 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 <waypoints.h>
#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)
char help[] = "--help";
char target_ip[100];
float position[6] = {};
int sock;
struct sockaddr_in gcAddr;
struct sockaddr_in locAddr;
uint8_t buf[BUFFER_LENGTH];
ssize_t recsize;
socklen_t fromlen;
int bytes_sent;
mavlink_message_t msg;
uint16_t len;
int i = 0;
unsigned int temp = 0;
uint64_t microsSinceEpoch();
int main(int argc, char* argv[])
{
// Initialize MAVLink
mavlink_wpm_init(&wpm);
mavlink_system.sysid = 1;
mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER;
// Create socket
sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
// Check if --help flag was used
if ((argc == 2) && (strcmp(argv[1], help) == 0))
{
printf("\n");
printf("\tUsage:\n\n");
printf("\t");
printf("%s", argv[0]);
printf(" <ip address of QGroundControl>\n");
printf("\tDefault for localhost: udp-server 127.0.0.1\n\n");
exit(EXIT_FAILURE);
}
// Change the target ip if parameter was given
strcpy(target_ip, "127.0.0.1");
if (argc == 2)
{
strcpy(target_ip, argv[1]);
}
memset(&locAddr, 0, sizeof(locAddr));
locAddr.sin_family = AF_INET;
locAddr.sin_addr.s_addr = INADDR_ANY;
locAddr.sin_port = htons(14551);
/* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */
if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr)))
{
perror("error bind failed");
close(sock);
exit(EXIT_FAILURE);
}
/* Attempt to make it non blocking */
if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno));
close(sock);
exit(EXIT_FAILURE);
}
memset(&gcAddr, 0, sizeof(gcAddr));
gcAddr.sin_family = AF_INET;
gcAddr.sin_addr.s_addr = inet_addr(target_ip);
gcAddr.sin_port = htons(14550);
printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n");
for (;;)
{
/*Send Heartbeat */
mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_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));
/* Send Status */
mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
/* Send Local Position */
mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(),
position[0], position[1], position[2],
position[3], position[4], position[5]);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
/* Send attitude */
mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
mavlink_message_t msg;
mavlink_status_t status;
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
for (i = 0; i < recsize; ++i)
{
temp = buf[i];
printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
{
// Packet received
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
// Handle packet with waypoint component
mavlink_wpm_message_handler(&msg);
// Handle packet with parameter component
}
}
printf("\n");
}
memset(buf, 0, BUFFER_LENGTH);
sleep(1); // Sleep one second
}
}
/* QNX timer version */
#if (defined __QNX__) | (defined __QNXNTO__)
uint64_t microsSinceEpoch()
{
struct timespec time;
uint64_t micros = 0;
clock_gettime(CLOCK_REALTIME, &time);
micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000;
return micros;
}
#else
uint64_t microsSinceEpoch()
{
struct timeval tv;
uint64_t micros = 0;
gettimeofday(&tv, NULL);
micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
return micros;
}
#endif

1073
libraries/GCS_MAVLink/missionlib/testing/udp.c Normal file
View File

File diff suppressed because it is too large Load Diff

79
libraries/GCS_MAVLink/missionlib/testing/udptest.1 Normal file
View File

@ -0,0 +1,79 @@
.\"Modified from man(1) of FreeBSD, the NetBSD mdoc.template, and mdoc.samples.
.\"See Also:
.\"man mdoc.samples for a complete listing of options
.\"man mdoc for the short list of editing options
.\"/usr/share/misc/mdoc.template
.Dd 01.08.11 \" DATE
.Dt udptest 1 \" Program name and manual section number
.Os Darwin
.Sh NAME \" Section Header - required - don't modify
.Nm udptest,
.\" The following lines are read in generating the apropos(man -k) database. Use only key
.\" words here as the database is built based on the words here and in the .ND line.
.Nm Other_name_for_same_program(),
.Nm Yet another name for the same program.
.\" Use .Nm macro to designate other names for the documented program.
.Nd This line parsed for whatis database.
.Sh SYNOPSIS \" Section Header - required - don't modify
.Nm
.Op Fl abcd \" [-abcd]
.Op Fl a Ar path \" [-a path]
.Op Ar file \" [file]
.Op Ar \" [file ...]
.Ar arg0 \" Underlined argument - use .Ar anywhere to underline
arg2 ... \" Arguments
.Sh DESCRIPTION \" Section Header - required - don't modify
Use the .Nm macro to refer to your program throughout the man page like such:
.Nm
Underlining is accomplished with the .Ar macro like this:
.Ar underlined text .
.Pp \" Inserts a space
A list of items with descriptions:
.Bl -tag -width -indent \" Begins a tagged list
.It item a \" Each item preceded by .It macro
Description of item a
.It item b
Description of item b
.El \" Ends the list
.Pp
A list of flags and their descriptions:
.Bl -tag -width -indent \" Differs from above in tag removed
.It Fl a \"-a flag as a list item
Description of -a flag
.It Fl b
Description of -b flag
.El \" Ends the list
.Pp
.\" .Sh ENVIRONMENT \" May not be needed
.\" .Bl -tag -width "ENV_VAR_1" -indent \" ENV_VAR_1 is width of the string ENV_VAR_1
.\" .It Ev ENV_VAR_1
.\" Description of ENV_VAR_1
.\" .It Ev ENV_VAR_2
.\" Description of ENV_VAR_2
.\" .El
.Sh FILES \" File used or created by the topic of the man page
.Bl -tag -width "/Users/joeuser/Library/really_long_file_name" -compact
.It Pa /usr/share/file_name
FILE_1 description
.It Pa /Users/joeuser/Library/really_long_file_name
FILE_2 description
.El \" Ends the list
.\" .Sh DIAGNOSTICS \" May not be needed
.\" .Bl -diag
.\" .It Diagnostic Tag
.\" Diagnostic informtion here.
.\" .It Diagnostic Tag
.\" Diagnostic informtion here.
.\" .El
.Sh SEE ALSO
.\" List links in ascending order by section, alphabetically within a section.
.\" Please do not reference files that do not exist without filing a bug report
.Xr a 1 ,
.Xr b 1 ,
.Xr c 1 ,
.Xr a 2 ,
.Xr b 2 ,
.Xr a 3 ,
.Xr b 3
.\" .Sh BUGS \" Document known, unremedied bugs
.\" .Sh HISTORY \" Document history if command behaves in a unique manner

211
libraries/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/project.pbxproj Normal file
View File

@ -0,0 +1,211 @@
// !$*UTF8*$!
{
archiveVersion = 1;
classes = {
};
objectVersion = 45;
objects = {
/* Begin PBXBuildFile section */
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 */
/* Begin PBXCopyFilesBuildPhase section */
8DD76FAF0486AB0100D96B5E /* CopyFiles */ = {
isa = PBXCopyFilesBuildPhase;
buildActionMask = 8;
dstPath = /usr/share/man/man1/;
dstSubfolderSpec = 0;
files = (
8DD76FB00486AB0100D96B5E /* udptest.1 in CopyFiles */,
);
runOnlyForDeploymentPostprocessing = 1;
};
/* End PBXCopyFilesBuildPhase section */
/* Begin PBXFileReference section */
08FB7796FE84155DC02AAC07 /* main.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = main.c; sourceTree = "<group>"; };
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 = "<group>"; };
/* End PBXFileReference section */
/* Begin PBXFrameworksBuildPhase section */
8DD76FAD0486AB0100D96B5E /* Frameworks */ = {
isa = PBXFrameworksBuildPhase;
buildActionMask = 2147483647;
files = (
);
runOnlyForDeploymentPostprocessing = 0;
};
/* End PBXFrameworksBuildPhase section */
/* Begin PBXGroup section */
08FB7794FE84155DC02AAC07 /* udptest */ = {
isa = PBXGroup;
children = (
08FB7795FE84155DC02AAC07 /* Source */,
C6A0FF2B0290797F04C91782 /* Documentation */,
1AB674ADFE9D54B511CA2CBB /* Products */,
);
name = udptest;
sourceTree = "<group>";
};
08FB7795FE84155DC02AAC07 /* Source */ = {
isa = PBXGroup;
children = (
08FB7796FE84155DC02AAC07 /* main.c */,
);
name = Source;
sourceTree = "<group>";
};
1AB674ADFE9D54B511CA2CBB /* Products */ = {
isa = PBXGroup;
children = (
8DD76FB20486AB0100D96B5E /* udptest */,
);
name = Products;
sourceTree = "<group>";
};
C6A0FF2B0290797F04C91782 /* Documentation */ = {
isa = PBXGroup;
children = (
C6A0FF2C0290799A04C91782 /* udptest.1 */,
);
name = Documentation;
sourceTree = "<group>";
};
/* End PBXGroup section */
/* Begin PBXNativeTarget section */
8DD76FA90486AB0100D96B5E /* udptest */ = {
isa = PBXNativeTarget;
buildConfigurationList = 1DEB928508733DD80010E9CD /* Build configuration list for PBXNativeTarget "udptest" */;
buildPhases = (
8DD76FAB0486AB0100D96B5E /* Sources */,
8DD76FAD0486AB0100D96B5E /* Frameworks */,
8DD76FAF0486AB0100D96B5E /* CopyFiles */,
);
buildRules = (
);
dependencies = (
);
name = udptest;
productInstallPath = "$(HOME)/bin";
productName = udptest;
productReference = 8DD76FB20486AB0100D96B5E /* udptest */;
productType = "com.apple.product-type.tool";
};
/* End PBXNativeTarget section */
/* Begin PBXProject section */
08FB7793FE84155DC02AAC07 /* Project object */ = {
isa = PBXProject;
buildConfigurationList = 1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "udptest" */;
compatibilityVersion = "Xcode 3.1";
developmentRegion = English;
hasScannedForEncodings = 1;
knownRegions = (
English,
Japanese,
French,
German,
);
mainGroup = 08FB7794FE84155DC02AAC07 /* udptest */;
projectDirPath = "";
projectRoot = "";
targets = (
8DD76FA90486AB0100D96B5E /* udptest */,
);
};
/* End PBXProject section */
/* Begin PBXSourcesBuildPhase section */
8DD76FAB0486AB0100D96B5E /* Sources */ = {
isa = PBXSourcesBuildPhase;
buildActionMask = 2147483647;
files = (
8DD76FAC0486AB0100D96B5E /* main.c in Sources */,
);
runOnlyForDeploymentPostprocessing = 0;
};
/* End PBXSourcesBuildPhase section */
/* Begin XCBuildConfiguration section */
1DEB928608733DD80010E9CD /* Debug */ = {
isa = XCBuildConfiguration;
buildSettings = {
ALWAYS_SEARCH_USER_PATHS = NO;
COPY_PHASE_STRIP = NO;
GCC_DYNAMIC_NO_PIC = NO;
GCC_ENABLE_FIX_AND_CONTINUE = YES;
GCC_MODEL_TUNING = G5;
GCC_OPTIMIZATION_LEVEL = 0;
INSTALL_PATH = /usr/local/bin;
PRODUCT_NAME = udptest;
};
name = Debug;
};
1DEB928708733DD80010E9CD /* Release */ = {
isa = XCBuildConfiguration;
buildSettings = {
ALWAYS_SEARCH_USER_PATHS = NO;
DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym";
GCC_MODEL_TUNING = G5;
INSTALL_PATH = /usr/local/bin;
PRODUCT_NAME = udptest;
};
name = Release;
};
1DEB928A08733DD80010E9CD /* Debug */ = {
isa = XCBuildConfiguration;
buildSettings = {
ARCHS = "$(ARCHS_STANDARD_32_64_BIT)";
GCC_C_LANGUAGE_STANDARD = gnu99;
GCC_OPTIMIZATION_LEVEL = 0;
GCC_WARN_ABOUT_RETURN_TYPE = YES;
GCC_WARN_UNUSED_VARIABLE = YES;
HEADER_SEARCH_PATHS = ../../include/common/;
ONLY_ACTIVE_ARCH = YES;
PREBINDING = NO;
SDKROOT = macosx10.6;
};
name = Debug;
};
1DEB928B08733DD80010E9CD /* Release */ = {
isa = XCBuildConfiguration;
buildSettings = {
ARCHS = "$(ARCHS_STANDARD_32_64_BIT)";
GCC_C_LANGUAGE_STANDARD = gnu99;
GCC_WARN_ABOUT_RETURN_TYPE = YES;
GCC_WARN_UNUSED_VARIABLE = YES;
PREBINDING = NO;
SDKROOT = macosx10.6;
};
name = Release;
};
/* End XCBuildConfiguration section */
/* Begin XCConfigurationList section */
1DEB928508733DD80010E9CD /* Build configuration list for PBXNativeTarget "udptest" */ = {
isa = XCConfigurationList;
buildConfigurations = (
1DEB928608733DD80010E9CD /* Debug */,
1DEB928708733DD80010E9CD /* Release */,
);
defaultConfigurationIsVisible = 0;
defaultConfigurationName = Release;
};
1DEB928908733DD80010E9CD /* Build configuration list for PBXProject "udptest" */ = {
isa = XCConfigurationList;
buildConfigurations = (
1DEB928A08733DD80010E9CD /* Debug */,
1DEB928B08733DD80010E9CD /* Release */,
);
defaultConfigurationIsVisible = 0;
defaultConfigurationName = Release;
};
/* End XCConfigurationList section */
};
rootObject = 08FB7793FE84155DC02AAC07 /* Project object */;
}

1440
libraries/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/user.mode1v3 Normal file
View File

File diff suppressed because it is too large Load Diff

236
libraries/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/user.pbxuser Normal file
View File

@ -0,0 +1,236 @@
// !$*UTF8*$!
{
08FB7793FE84155DC02AAC07 /* Project object */ = {
activeBuildConfigurationName = Debug;
activeExecutable = CEA4FF2913E6BD6D002506EF /* udptest */;
activeTarget = 8DD76FA90486AB0100D96B5E /* udptest */;
codeSenseManager = CEA4FF4013E6BD8E002506EF /* Code sense */;
executables = (
CEA4FF2913E6BD6D002506EF /* udptest */,
);
perUserDictionary = {
PBXConfiguration.PBXFileTableDataSource3.PBXFileTableDataSource = {
PBXFileTableDataSourceColumnSortingDirectionKey = "-1";
PBXFileTableDataSourceColumnSortingKey = PBXFileDataSource_Filename_ColumnID;
PBXFileTableDataSourceColumnWidthsKey = (
20,
833,
20,
48,
43,
43,
20,
);
PBXFileTableDataSourceColumnsKey = (
PBXFileDataSource_FiletypeID,
PBXFileDataSource_Filename_ColumnID,
PBXFileDataSource_Built_ColumnID,
PBXFileDataSource_ObjectSize_ColumnID,
PBXFileDataSource_Errors_ColumnID,
PBXFileDataSource_Warnings_ColumnID,
PBXFileDataSource_Target_ColumnID,
);
};
PBXConfiguration.PBXTargetDataSource.PBXTargetDataSource = {
PBXFileTableDataSourceColumnSortingDirectionKey = "-1";
PBXFileTableDataSourceColumnSortingKey = PBXFileDataSource_Filename_ColumnID;
PBXFileTableDataSourceColumnWidthsKey = (
20,
301,
60,
20,
48,
43,
43,
);
PBXFileTableDataSourceColumnsKey = (
PBXFileDataSource_FiletypeID,
PBXFileDataSource_Filename_ColumnID,
PBXTargetDataSource_PrimaryAttribute,
PBXFileDataSource_Built_ColumnID,
PBXFileDataSource_ObjectSize_ColumnID,
PBXFileDataSource_Errors_ColumnID,
PBXFileDataSource_Warnings_ColumnID,
);
};
PBXPerProjectTemplateStateSaveDate = 333907732;
PBXWorkspaceStateSaveDate = 333907732;
};
perUserProjectItems = {
CED081EC13E6F90000BAE9DD = CED081EC13E6F90000BAE9DD /* PBXTextBookmark */;
CED081ED13E6F90000BAE9DD = CED081ED13E6F90000BAE9DD /* PBXTextBookmark */;
CED081EF13E6F90000BAE9DD = CED081EF13E6F90000BAE9DD /* PBXTextBookmark */;
CED081F113E6F90000BAE9DD = CED081F113E6F90000BAE9DD /* PBXTextBookmark */;
CED081F313E6F90000BAE9DD = CED081F313E6F90000BAE9DD /* PBXTextBookmark */;
CED081F913E6F90000BAE9DD = CED081F913E6F90000BAE9DD /* PBXTextBookmark */;
CED081FB13E6F90000BAE9DD = CED081FB13E6F90000BAE9DD /* PBXTextBookmark */;
};
sourceControlManager = CEA4FF3F13E6BD8E002506EF /* Source Control */;
userBuildSettings = {
};
};
08FB7796FE84155DC02AAC07 /* main.c */ = {
uiCtxt = {
sepNavIntBoundsRect = "{{0, 0}, {1797, 14781}}";
sepNavSelRange = "{32946, 0}";
sepNavVisRange = "{32608, 2708}";
sepNavWindowFrame = "{{374, 47}, {906, 731}}";
};
};
8DD76FA90486AB0100D96B5E /* udptest */ = {
activeExec = 0;
executables = (
CEA4FF2913E6BD6D002506EF /* udptest */,
);
};
CEA4FF2913E6BD6D002506EF /* udptest */ = {
isa = PBXExecutable;
activeArgIndices = (
);
argumentStrings = (
);
autoAttachOnCrash = 1;
breakpointsEnabled = 0;
configStateDict = {
};
customDataFormattersEnabled = 1;
dataTipCustomDataFormattersEnabled = 1;
dataTipShowTypeColumn = 1;
dataTipSortType = 0;
debuggerPlugin = GDBDebugging;
disassemblyDisplayState = 0;
dylibVariantSuffix = "";
enableDebugStr = 1;
environmentEntries = (
);
executableSystemSymbolLevel = 0;
executableUserSymbolLevel = 0;
libgmallocEnabled = 0;
name = udptest;
savedGlobals = {
};
showTypeColumn = 0;
sourceDirectories = (
);
};
CEA4FF3F13E6BD8E002506EF /* Source Control */ = {
isa = PBXSourceControlManager;
fallbackIsa = XCSourceControlManager;
isSCMEnabled = 0;
scmConfiguration = {
repositoryNamesForRoots = {
"" = "";
};
};
};
CEA4FF4013E6BD8E002506EF /* Code sense */ = {
isa = PBXCodeSenseManager;
indexTemplatePath = "";
};
CED081EC13E6F90000BAE9DD /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = 08FB7796FE84155DC02AAC07 /* main.c */;
name = "main.c: 678";
rLen = 31;
rLoc = 25201;
rType = 0;
vrLen = 2936;
vrLoc = 23777;
};
CED081ED13E6F90000BAE9DD /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = CED081EE13E6F90000BAE9DD /* mavlink_msg_waypoint_request.h */;
name = "mavlink_msg_waypoint_request.h: 1";
rLen = 0;
rLoc = 0;
rType = 0;
vrLen = 1362;
vrLoc = 0;
};
CED081EE13E6F90000BAE9DD /* mavlink_msg_waypoint_request.h */ = {
isa = PBXFileReference;
lastKnownFileType = sourcecode.c.h;
name = mavlink_msg_waypoint_request.h;
path = /Users/user/src/mavlink10/include/common/mavlink_msg_waypoint_request.h;
sourceTree = "<absolute>";
};
CED081EF13E6F90000BAE9DD /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = CED081F013E6F90000BAE9DD /* mavlink_msg_waypoint_clear_all.h */;
name = "mavlink_msg_waypoint_clear_all.h: 1";
rLen = 0;
rLoc = 0;
rType = 0;
vrLen = 1447;
vrLoc = 0;
};
CED081F013E6F90000BAE9DD /* mavlink_msg_waypoint_clear_all.h */ = {
isa = PBXFileReference;
lastKnownFileType = sourcecode.c.h;
name = mavlink_msg_waypoint_clear_all.h;
path = /Users/user/src/mavlink10/include/common/mavlink_msg_waypoint_clear_all.h;
sourceTree = "<absolute>";
};
CED081F113E6F90000BAE9DD /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = CED081F213E6F90000BAE9DD /* mavlink_msg_waypoint_count.h */;
rLen = 0;
rLoc = 9223372036854775808;
rType = 0;
};
CED081F213E6F90000BAE9DD /* mavlink_msg_waypoint_count.h */ = {
isa = PBXFileReference;
lastKnownFileType = sourcecode.c.h;
name = mavlink_msg_waypoint_count.h;
path = /Users/user/src/mavlink10/include/common/mavlink_msg_waypoint_count.h;
sourceTree = "<absolute>";
};
CED081F313E6F90000BAE9DD /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = CED081F413E6F90000BAE9DD /* mavlink_msg_waypoint_count.h */;
name = "mavlink_msg_waypoint_count.h: 1";
rLen = 0;
rLoc = 0;
rType = 0;
vrLen = 1530;
vrLoc = 0;
};
CED081F413E6F90000BAE9DD /* mavlink_msg_waypoint_count.h */ = {
isa = PBXFileReference;
lastKnownFileType = sourcecode.c.h;
name = mavlink_msg_waypoint_count.h;
path = /Users/user/src/mavlink10/include/common/mavlink_msg_waypoint_count.h;
sourceTree = "<absolute>";
};
CED081F913E6F90000BAE9DD /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = CED081FA13E6F90000BAE9DD /* px_waypointplanner.cc */;
rLen = 0;
rLoc = 41642;
rType = 0;
};
CED081FA13E6F90000BAE9DD /* px_waypointplanner.cc */ = {
isa = PBXFileReference;
lastKnownFileType = sourcecode.cpp.cpp;
name = px_waypointplanner.cc;
path = /Users/user/Documents/pixhawk/mavconn/src/planning/px_waypointplanner.cc;
sourceTree = "<absolute>";
};
CED081FB13E6F90000BAE9DD /* PBXTextBookmark */ = {
isa = PBXTextBookmark;
fRef = CED081FC13E6F90000BAE9DD /* px_waypointplanner.cc */;
name = "px_waypointplanner.cc: 30";
rLen = 561;
rLoc = 785;
rType = 0;
vrLen = 1834;
vrLoc = 33761;
};
CED081FC13E6F90000BAE9DD /* px_waypointplanner.cc */ = {
isa = PBXFileReference;
lastKnownFileType = sourcecode.cpp.cpp;
name = px_waypointplanner.cc;
path = /Users/user/Documents/pixhawk/mavconn/src/planning/px_waypointplanner.cc;
sourceTree = "<absolute>";
};
}

880
libraries/GCS_MAVLink/missionlib/waypoints.c Normal file
View File

@ -0,0 +1,880 @@
/*******************************************************************************
Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
****************************************************************************/
#include <waypoints.h>
static mavlink_wpm_storage wpm;
bool debug = true;
bool verbose = true;
void mavlink_wpm_init(mavlink_wpm_storage* state)
{
// Set all waypoints to zero
// Set count to zero
state->size = 0;
state->max_size = MAVLINK_WPM_MAX_WP_COUNT;
state->current_state = MAVLINK_WPM_STATE_IDLE;
state->current_partner_sysid = 0;
state->current_partner_compid = 0;
state->timestamp_lastaction = 0;
state->timestamp_last_send_setpoint = 0;
state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT;
state->idle = false; ///< indicates if the system is following the waypoints or is waiting
state->current_active_wp_id = -1; ///< id of current waypoint
state->yaw_reached = false; ///< boolean for yaw attitude reached
state->pos_reached = false; ///< boolean for position reached
state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value
state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value
}
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
*/
void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
{
mavlink_message_t msg;
mavlink_waypoint_ack_t wpa;
wpa.target_system = wpm.current_partner_sysid;
wpa.target_component = wpm.current_partner_compid;
wpa.type = type;
mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa);
mavlink_wpm_send_message(&msg);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
if (MAVLINK_WPM_TEXT_FEEDBACK)
{
//printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
mavlink_wpm_send_gcs_string("Sent waypoint ACK");
}
}
/*
* @brief Broadcasts the new target waypoint and directs the MAV to fly there
*
* This function broadcasts its new active waypoint sequence number and
* sends a message to the controller, advising it to fly to the coordinates
* of the waypoint with a given orientation
*
* @param seq The waypoint sequence number the MAV should fly to.
*/
void mavlink_wpm_send_waypoint_current(uint16_t seq)
{
if(seq < wpm.size)
{
mavlink_waypoint_t *cur = &(wpm.waypoints[seq]);
mavlink_message_t msg;
mavlink_waypoint_current_t wpc;
wpc.seq = cur->seq;
mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc);
mavlink_wpm_send_message(&msg);
// 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);
}
else
{
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: index out of bounds\n");
}
}
/*
* @brief Directs the MAV to fly to a position
*
* Sends a message to the controller, advising it to fly to the coordinates
* of the waypoint with a given orientation
*
* @param seq The waypoint sequence number the MAV should fly to.
*/
void mavlink_wpm_send_setpoint(uint16_t seq)
{
if(seq < wpm.size)
{
mavlink_waypoint_t *cur = &(wpm.waypoints[seq]);
mavlink_message_t msg;
mavlink_local_position_setpoint_set_t position_control_set_point;
// Send new NED or ENU setpoint to onbaord autopilot
if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU)
{
position_control_set_point.target_system = mavlink_system.sysid;
position_control_set_point.target_component = MAV_COMP_ID_IMU;
position_control_set_point.x = cur->x;
position_control_set_point.y = cur->y;
position_control_set_point.z = cur->z;
position_control_set_point.yaw = cur->param4;
mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point);
mavlink_wpm_send_message(&msg);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
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);
}
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");
}
}
void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count)
{
mavlink_message_t msg;
mavlink_waypoint_count_t wpc;
wpc.target_system = wpm.current_partner_sysid;
wpc.target_component = wpm.current_partner_compid;
wpc.count = count;
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);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
{
if (seq < wpm.size)
{
mavlink_message_t msg;
mavlink_waypoint_t *wp = &(wpm.waypoints[seq]);
wp->target_system = wpm.current_partner_sysid;
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);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
else
{
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n");
}
}
void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq)
{
if (seq < wpm.max_size)
{
mavlink_message_t msg;
mavlink_waypoint_request_t wpr;
wpr.target_system = wpm.current_partner_sysid;
wpr.target_component = wpm.current_partner_compid;
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);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
else
{
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n");
}
}
/*
* @brief emits a message that a waypoint reached
*
* This function broadcasts a message that a waypoint is reached.
*
* @param seq The waypoint sequence number the MAV has reached.
*/
void mavlink_wpm_send_waypoint_reached(uint16_t seq)
{
mavlink_message_t msg;
mavlink_waypoint_reached_t wp_reached;
wp_reached.seq = 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);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
//float mavlink_wpm_distance_to_segment(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);
//
// // seq not the second last waypoint
// if ((uint16_t)(seq+1) < wpm.size)
// {
// mavlink_waypoint_t *next = waypoints->at(seq+1);
// const PxVector3 B(next->x, next->y, next->z);
// const float r = (B-A).dot(C-A) / (B-A).lengthSquared();
// if (r >= 0 && r <= 1)
// {
// const PxVector3 P(A + r*(B-A));
// return (P-C).length();
// }
// else if (r < 0.f)
// {
// return (C-A).length();
// }
// else
// {
// return (C-B).length();
// }
// }
// else
// {
// return (C-A).length();
// }
// }
// else
// {
// 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");
// }
return -1.f;
}
static 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;
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;
wpm.current_count = 0;
wpm.current_partner_sysid = 0;
wpm.current_partner_compid = 0;
wpm.current_wp_id = -1;
if(wpm.size == 0)
{
wpm.current_active_wp_id = -1;
}
}
if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size)
{
mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
}
switch(msg->msgid)
{
case MAVLINK_MSG_ID_ATTITUDE:
{
if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
{
mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED)
{
mavlink_attitude_t att;
mavlink_msg_attitude_decode(msg, &att);
float yaw_tolerance = wpm.accept_range_yaw;
//compare current yaw
if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI)
{
if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4)
wpm.yaw_reached = true;
}
else if(att.yaw - yaw_tolerance < 0.0f)
{
float lowerBound = 360.0f + att.yaw - yaw_tolerance;
if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance)
wpm.yaw_reached = true;
}
else
{
float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI;
if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound)
wpm.yaw_reached = true;
}
}
}
break;
}
case MAVLINK_MSG_ID_LOCAL_POSITION:
{
if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size)
{
mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]);
if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED)
{
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);
wpm.pos_reached = false;
// compare current position (given in message) with current waypoint
float orbit = wp->param1;
float dist;
if (wp->param2 == 0)
{
// FIXME segment distance
//dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z);
}
else
{
dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z);
}
if (dist >= 0.f && dist <= orbit && wpm.yaw_reached)
{
wpm.pos_reached = true;
}
}
}
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))
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
{
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;
wpm.current_wp_id = 0;
}
}
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT:
{
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)
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
if (wpc.seq < wpm.size)
{
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++)
{
if (i == wpm.current_active_wp_id)
{
wpm.waypoints[i].current = true;
}
else
{
wpm.waypoints[i].current = false;
}
}
if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id);
wpm.yaw_reached = false;
wpm.pos_reached = false;
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
wpm.timestamp_firstinside_orbit = 0;
}
else
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
}
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
}
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST:
{
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)
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
{
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;
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);
}
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);
}
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_REQUEST:
{
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)
{
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);
wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
wpm.current_wp_id = wpr.seq;
mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq);
}
else
{
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; }
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);
}
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);
}
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
}
}
}
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 (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);
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
}
}
break;
}
case MAVLINK_MSG_ID_WAYPOINT_COUNT:
{
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)
{
wpm.timestamp_lastaction = now;
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0))
{
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);
wpm.current_state = MAVLINK_WPM_STATE_GETLIST;
wpm.current_wp_id = 0;
wpm.current_partner_sysid = msg->sysid;
wpm.current_partner_compid = msg->compid;
wpm.current_count = wpc.count;
printf("clearing receive buffer and readying for receiving waypoints\n");
wpm.rcv_size = 0;
//while(waypoints_receive_buffer->size() > 0)
// {
// 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");
wpm.rcv_size = 0;
//while(waypoints_receive_buffer->size() > 0)
// {
// delete waypoints->back();
// waypoints->pop_back();
// }
wpm.current_active_wp_id = -1;
wpm.yaw_reached = false;
wpm.pos_reached = false;
break;
}
else
{
if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
}
}
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");
}
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
}
}
break;
case MAVLINK_MSG_ID_WAYPOINT:
{
mavlink_waypoint_t wp;
mavlink_msg_waypoint_decode(msg, &wp);
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))
{
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;
mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]);
memcpy(newwp, &wp, sizeof(mavlink_waypoint_t));
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(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);
mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
if (wpm.current_active_wp_id > wpm.rcv_size-1)
{
wpm.current_active_wp_id = wpm.rcv_size-1;
}
// switch the waypoints list
// FIXME CHECK!!!
for (int i = 0; i < wpm.current_count; ++i)
{
wpm.waypoints[i] = wpm.rcv_waypoints[i];
}
wpm.size = wpm.current_count;
//get the new current waypoint
uint32_t i;
for(i = 0; i < wpm.size; i++)
{
if (wpm.waypoints[i].current == 1)
{
wpm.current_active_wp_id = i;
//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);
mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
wpm.timestamp_firstinside_orbit = 0;
break;
}
}
if (i == wpm.size)
{
wpm.current_active_wp_id = -1;
wpm.yaw_reached = false;
wpm.pos_reached = false;
wpm.timestamp_firstinside_orbit = 0;
}
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
}
else
{
mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
}
}
else
{
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
//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");
}
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; }
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);
}
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);
}
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 (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)
{
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;
}
case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL:
{
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)
{
wpm.timestamp_lastaction = now;
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)
{
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");
break;
}
}
//check if the current waypoint was reached
if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle)
{
if (wpm.current_active_wp_id < wpm.size)
{
mavlink_waypoint_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]);
if (wpm.timestamp_firstinside_orbit == 0)
{
// Announce that last waypoint was reached
if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq);
mavlink_wpm_send_waypoint_reached(cur_wp->seq);
wpm.timestamp_firstinside_orbit = now;
}
// check if the MAV was long enough inside the waypoint orbit
//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000)
{
if (cur_wp->autocontinue)
{
cur_wp->current = 0;
if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1)
{
//the last waypoint was reached, if auto continue is
//activated restart the waypoint list from the beginning
wpm.current_active_wp_id = 1;
}
else
{
if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size)
wpm.current_active_wp_id++;
}
// Fly to next waypoint
wpm.timestamp_firstinside_orbit = 0;
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
mavlink_wpm_send_setpoint(wpm.current_active_wp_id);
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);
}
}
}
}
else
{
wpm.timestamp_lastoutside_orbit = now;
}
}

91
libraries/GCS_MAVLink/missionlib/waypoints.h Normal file
View File

@ -0,0 +1,91 @@
/*******************************************************************************
Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
****************************************************************************/
/* This assumes you have the mavlink headers on your include path
or in the same folder as this source file */
#include <mavlink.h>
#include <stdbool.h>
// FIXME XXX - TO BE MOVED TO XML
enum MAVLINK_WPM_STATES
{
MAVLINK_WPM_STATE_IDLE = 0,
MAVLINK_WPM_STATE_SENDLIST,
MAVLINK_WPM_STATE_SENDLIST_SENDWPS,
MAVLINK_WPM_STATE_GETLIST,
MAVLINK_WPM_STATE_GETLIST_GETWPS,
MAVLINK_WPM_STATE_GETLIST_GOTALL,
MAVLINK_WPM_STATE_ENUM_END
};
enum MAVLINK_WPM_CODES
{
MAVLINK_WPM_CODE_OK = 0,
MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED,
MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS,
MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED,
MAVLINK_WPM_CODE_ENUM_END
};
/* WAYPOINT MANAGER - MISSION LIB */
#define MAVLINK_WPM_MAX_WP_COUNT 100
#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
#define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text
#define MAVLINK_WPM_SYSTEM_ID 1
#define MAVLINK_WPM_COMPONENT_ID 1
#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000
#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000
#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40
struct mavlink_wpm_storage {
mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints
#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE
mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints
#endif
uint16_t size;
uint16_t max_size;
uint16_t rcv_size;
enum MAVLINK_WPM_STATES current_state;
uint16_t current_wp_id; ///< Waypoint in current transmission
uint16_t current_active_wp_id; ///< Waypoint the system is currently heading towards
uint16_t current_count;
uint8_t current_partner_sysid;
uint8_t current_partner_compid;
uint64_t timestamp_lastaction;
uint64_t timestamp_last_send_setpoint;
uint64_t timestamp_firstinside_orbit;
uint64_t timestamp_lastoutside_orbit;
uint32_t timeout;
uint32_t delay_setpoint;
float accept_range_yaw;
float accept_range_distance;
bool yaw_reached;
bool pos_reached;
bool idle;
};
typedef struct mavlink_wpm_storage mavlink_wpm_storage;
void mavlink_wpm_init(mavlink_wpm_storage* state);
void mavlink_wpm_message_handler(const mavlink_message_t* msg);

5
libraries/GCS_MAVLink/sync
View File

@ -1,8 +1,5 @@
#!/bin/sh
branch="dev"
# temporarily using roi branch instead of master
# this branch will be deleted soon so switch it back
# to master then
branch="master"
trap exit ERR
rm -rf _tmp
git clone git://github.com/pixhawk/mavlink.git -b $branch _tmp