Mavlink update

git-svn-id: https://arducopter.googlecode.com/svn/trunk@1757 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
mich146@hotmail.com 2011-03-08 11:07:58 +00:00
parent 967130ed7e
commit d5ab8f3a79
9 changed files with 529 additions and 128 deletions

View File

@ -1,7 +1,7 @@
/** @file /** @file
* @brief MAVLink comm protocol. * @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink * @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Wednesday, February 16 2011, 19:40 UTC * Generated on Saturday, February 26 2011, 13:25 UTC
*/ */
#ifndef COMMON_H #ifndef COMMON_H
#define COMMON_H #define COMMON_H
@ -28,22 +28,24 @@ extern "C" {
// ENUM DEFINITIONS // ENUM DEFINITIONS
/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. */ /** @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 enum MAV_CMD
{ {
MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint.Hold time (ignored by fixed wing, time to stay at waypoint for rotary wing)Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached)0 to pass through the WP, if > 0 radius to pass by WP. Allows trajectory control.Desired yaw angleLatitudeLongitudeAltitude*/ 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_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_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_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_RETURN_TO_LAUNCH=20, /* Return to launch locationEmptyEmptyEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_NAV_LAND=21, /* Land at locationEmptyEmptyEmptyDesired yaw angle.LatitudeLongitudeAltitude*/ 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_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_ORIENTATION_TARGET=80, /* Set the location the system should be heading towards (camera heads or rotary wing aircraft).EmptyEmptyEmptyEmptyLatitudeLongitudeAltitude*/
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_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_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_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_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_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumerationEmptyEmptyEmptyEmptyEmptyEmptyEmpty*/
MAV_CMD_CONDITION_ANGLE=160, /*/
MAV_CMD_DO_SET_MODE=176, /* Set system mode.Mode, as defined by ENUM MAV_MODEEmptyEmptyEmptyEmptyEmptyEmpty*/ 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_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_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*/
@ -52,7 +54,11 @@ enum MAV_CMD
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_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_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_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 cyles with a desired period.Servo numberPWM (microseconds, 1000 to 2000 typical)Cycle countCycle time (seconds)EmptyEmptyEmpty*/ 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_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: yesRadio calibration: 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_ENUM_END MAV_CMD_ENUM_END
}; };
@ -128,6 +134,8 @@ enum MAV_DATA_STREAM
#include "./mavlink_msg_manual_control.h" #include "./mavlink_msg_manual_control.h"
#include "./mavlink_msg_global_position_int.h" #include "./mavlink_msg_global_position_int.h"
#include "./mavlink_msg_vfr_hud.h" #include "./mavlink_msg_vfr_hud.h"
#include "./mavlink_msg_command.h"
#include "./mavlink_msg_command_ack.h"
#include "./mavlink_msg_debug_vect.h" #include "./mavlink_msg_debug_vect.h"
#include "./mavlink_msg_named_value_float.h" #include "./mavlink_msg_named_value_float.h"
#include "./mavlink_msg_named_value_int.h" #include "./mavlink_msg_named_value_int.h"

View File

@ -1,7 +1,7 @@
/** @file /** @file
* @brief MAVLink comm protocol. * @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink * @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Wednesday, February 16 2011, 19:40 UTC * Generated on Saturday, February 26 2011, 13:25 UTC
*/ */
#ifndef MAVLINK_H #ifndef MAVLINK_H
#define MAVLINK_H #define MAVLINK_H

View File

@ -0,0 +1,240 @@
// MESSAGE COMMAND PACKING
#define MAVLINK_MSG_ID_COMMAND 75
typedef struct __mavlink_command_t
{
uint8_t target_system; ///< System which should execute the command
uint8_t target_component; ///< Component which should execute the command, 0 for all components
uint8_t command; ///< Command ID, as defined by MAV_CMD enum.
uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
float param1; ///< Parameter 1, as defined by MAV_CMD enum.
float param2; ///< Parameter 2, as defined by MAV_CMD enum.
float param3; ///< Parameter 3, as defined by MAV_CMD enum.
float param4; ///< Parameter 4, as defined by MAV_CMD enum.
} mavlink_command_t;
/**
* @brief Pack a command 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 which should execute the command
* @param target_component Component which should execute the command, 0 for all components
* @param command Command ID, as defined by MAV_CMD enum.
* @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
* @param param1 Parameter 1, as defined by MAV_CMD enum.
* @param param2 Parameter 2, as defined by MAV_CMD enum.
* @param param3 Parameter 3, as defined by MAV_CMD enum.
* @param param4 Parameter 4, as defined by MAV_CMD enum.
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_command_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_COMMAND;
i += put_uint8_t_by_index(target_system, i, msg->payload); // System which should execute the command
i += put_uint8_t_by_index(target_component, i, msg->payload); // Component which should execute the command, 0 for all components
i += put_uint8_t_by_index(command, i, msg->payload); // Command ID, as defined by MAV_CMD enum.
i += put_uint8_t_by_index(confirmation, i, msg->payload); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
i += put_float_by_index(param1, i, msg->payload); // Parameter 1, as defined by MAV_CMD enum.
i += put_float_by_index(param2, i, msg->payload); // Parameter 2, as defined by MAV_CMD enum.
i += put_float_by_index(param3, i, msg->payload); // Parameter 3, as defined by MAV_CMD enum.
i += put_float_by_index(param4, i, msg->payload); // Parameter 4, as defined by MAV_CMD enum.
return mavlink_finalize_message(msg, system_id, component_id, i);
}
/**
* @brief Pack a command 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 which should execute the command
* @param target_component Component which should execute the command, 0 for all components
* @param command Command ID, as defined by MAV_CMD enum.
* @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
* @param param1 Parameter 1, as defined by MAV_CMD enum.
* @param param2 Parameter 2, as defined by MAV_CMD enum.
* @param param3 Parameter 3, as defined by MAV_CMD enum.
* @param param4 Parameter 4, as defined by MAV_CMD enum.
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_command_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, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_COMMAND;
i += put_uint8_t_by_index(target_system, i, msg->payload); // System which should execute the command
i += put_uint8_t_by_index(target_component, i, msg->payload); // Component which should execute the command, 0 for all components
i += put_uint8_t_by_index(command, i, msg->payload); // Command ID, as defined by MAV_CMD enum.
i += put_uint8_t_by_index(confirmation, i, msg->payload); // 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
i += put_float_by_index(param1, i, msg->payload); // Parameter 1, as defined by MAV_CMD enum.
i += put_float_by_index(param2, i, msg->payload); // Parameter 2, as defined by MAV_CMD enum.
i += put_float_by_index(param3, i, msg->payload); // Parameter 3, as defined by MAV_CMD enum.
i += put_float_by_index(param4, i, msg->payload); // Parameter 4, as defined by MAV_CMD enum.
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
/**
* @brief Encode a command 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 command C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_t* command)
{
return mavlink_msg_command_pack(system_id, component_id, msg, command->target_system, command->target_component, command->command, command->confirmation, command->param1, command->param2, command->param3, command->param4);
}
/**
* @brief Send a command message
* @param chan MAVLink channel to send the message
*
* @param target_system System which should execute the command
* @param target_component Component which should execute the command, 0 for all components
* @param command Command ID, as defined by MAV_CMD enum.
* @param confirmation 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
* @param param1 Parameter 1, as defined by MAV_CMD enum.
* @param param2 Parameter 2, as defined by MAV_CMD enum.
* @param param3 Parameter 3, as defined by MAV_CMD enum.
* @param param4 Parameter 4, as defined by MAV_CMD enum.
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4)
{
mavlink_message_t msg;
mavlink_msg_command_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, command, confirmation, param1, param2, param3, param4);
mavlink_send_uart(chan, &msg);
}
#endif
// MESSAGE COMMAND UNPACKING
/**
* @brief Get field target_system from command message
*
* @return System which should execute the command
*/
static inline uint8_t mavlink_msg_command_get_target_system(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload)[0];
}
/**
* @brief Get field target_component from command message
*
* @return Component which should execute the command, 0 for all components
*/
static inline uint8_t mavlink_msg_command_get_target_component(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload+sizeof(uint8_t))[0];
}
/**
* @brief Get field command from command message
*
* @return Command ID, as defined by MAV_CMD enum.
*/
static inline uint8_t mavlink_msg_command_get_command(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
}
/**
* @brief Get field confirmation from command message
*
* @return 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
*/
static inline uint8_t mavlink_msg_command_get_confirmation(const mavlink_message_t* msg)
{
return (uint8_t)(msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0];
}
/**
* @brief Get field param1 from command message
*
* @return Parameter 1, as defined by MAV_CMD enum.
*/
static inline float mavlink_msg_command_get_param1(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t))[3];
return (float)r.f;
}
/**
* @brief Get field param2 from command message
*
* @return Parameter 2, as defined by MAV_CMD enum.
*/
static inline float mavlink_msg_command_get_param2(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field param3 from command message
*
* @return Parameter 3, as defined by MAV_CMD enum.
*/
static inline float mavlink_msg_command_get_param3(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field param4 from command message
*
* @return Parameter 4, as defined by MAV_CMD enum.
*/
static inline float mavlink_msg_command_get_param4(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(float)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Decode a command message into a struct
*
* @param msg The message to decode
* @param command C-struct to decode the message contents into
*/
static inline void mavlink_msg_command_decode(const mavlink_message_t* msg, mavlink_command_t* command)
{
command->target_system = mavlink_msg_command_get_target_system(msg);
command->target_component = mavlink_msg_command_get_target_component(msg);
command->command = mavlink_msg_command_get_command(msg);
command->confirmation = mavlink_msg_command_get_confirmation(msg);
command->param1 = mavlink_msg_command_get_param1(msg);
command->param2 = mavlink_msg_command_get_param2(msg);
command->param3 = mavlink_msg_command_get_param3(msg);
command->param4 = mavlink_msg_command_get_param4(msg);
}

View File

@ -0,0 +1,128 @@
// MESSAGE COMMAND_ACK PACKING
#define MAVLINK_MSG_ID_COMMAND_ACK 76
typedef struct __mavlink_command_ack_t
{
float command; ///< Current airspeed in m/s
float result; ///< 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION
} mavlink_command_ack_t;
/**
* @brief Pack a command_ack 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 command Current airspeed in m/s
* @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float command, float result)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK;
i += put_float_by_index(command, i, msg->payload); // Current airspeed in m/s
i += put_float_by_index(result, i, msg->payload); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION
return mavlink_finalize_message(msg, system_id, component_id, i);
}
/**
* @brief Pack a command_ack 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 command Current airspeed in m/s
* @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float command, float result)
{
uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_COMMAND_ACK;
i += put_float_by_index(command, i, msg->payload); // Current airspeed in m/s
i += put_float_by_index(result, i, msg->payload); // 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
}
/**
* @brief Encode a command_ack 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 command_ack C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack)
{
return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result);
}
/**
* @brief Send a command_ack message
* @param chan MAVLink channel to send the message
*
* @param command Current airspeed in m/s
* @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result)
{
mavlink_message_t msg;
mavlink_msg_command_ack_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, command, result);
mavlink_send_uart(chan, &msg);
}
#endif
// MESSAGE COMMAND_ACK UNPACKING
/**
* @brief Get field command from command_ack message
*
* @return Current airspeed in m/s
*/
static inline float mavlink_msg_command_ack_get_command(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload)[0];
r.b[2] = (msg->payload)[1];
r.b[1] = (msg->payload)[2];
r.b[0] = (msg->payload)[3];
return (float)r.f;
}
/**
* @brief Get field result from command_ack message
*
* @return 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION
*/
static inline float mavlink_msg_command_ack_get_result(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Decode a command_ack message into a struct
*
* @param msg The message to decode
* @param command_ack C-struct to decode the message contents into
*/
static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, mavlink_command_ack_t* command_ack)
{
command_ack->command = mavlink_msg_command_ack_get_command(msg);
command_ack->result = mavlink_msg_command_ack_get_result(msg);
}

View File

@ -6,12 +6,9 @@ typedef struct __mavlink_gps_set_global_origin_t
{ {
uint8_t target_system; ///< System ID uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID uint8_t target_component; ///< Component ID
uint32_t global_x; ///< global x position * 1E7 uint32_t latitude; ///< global x position * 1E7
uint32_t global_y; ///< global y position * 1E7 uint32_t longitude; ///< global y position * 1E7
uint32_t global_z; ///< global z position * 1000 uint32_t altitude; ///< global z position * 1000
float local_x; ///< local x position that matches the global x position
float local_y; ///< local y position that matches the global y position
float local_z; ///< local z position that matches the global z position
} mavlink_gps_set_global_origin_t; } mavlink_gps_set_global_origin_t;
@ -25,27 +22,21 @@ typedef struct __mavlink_gps_set_global_origin_t
* *
* @param target_system System ID * @param target_system System ID
* @param target_component Component ID * @param target_component Component ID
* @param global_x global x position * 1E7 * @param latitude global x position * 1E7
* @param global_y global y position * 1E7 * @param longitude global y position * 1E7
* @param global_z global z position * 1000 * @param altitude global z position * 1000
* @param local_x local x position that matches the global x position
* @param local_y local y position that matches the global y position
* @param local_z local z position that matches the global z position
* @return length of the message in bytes (excluding serial stream start sign) * @return length of the message in bytes (excluding serial stream start sign)
*/ */
static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint32_t global_x, uint32_t global_y, uint32_t global_z, float local_x, float local_y, float local_z) static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint32_t latitude, uint32_t longitude, uint32_t altitude)
{ {
uint16_t i = 0; uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN;
i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID 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_uint8_t_by_index(target_component, i, msg->payload); // Component ID
i += put_uint32_t_by_index(global_x, i, msg->payload); // global x position * 1E7 i += put_uint32_t_by_index(latitude, i, msg->payload); // global x position * 1E7
i += put_uint32_t_by_index(global_y, i, msg->payload); // global y position * 1E7 i += put_uint32_t_by_index(longitude, i, msg->payload); // global y position * 1E7
i += put_uint32_t_by_index(global_z, i, msg->payload); // global z position * 1000 i += put_uint32_t_by_index(altitude, i, msg->payload); // global z position * 1000
i += put_float_by_index(local_x, i, msg->payload); // local x position that matches the global x position
i += put_float_by_index(local_y, i, msg->payload); // local y position that matches the global y position
i += put_float_by_index(local_z, i, msg->payload); // local z position that matches the global z position
return mavlink_finalize_message(msg, system_id, component_id, i); return mavlink_finalize_message(msg, system_id, component_id, i);
} }
@ -58,27 +49,21 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack(uint8_t system_id,
* @param msg The MAVLink message to compress the data into * @param msg The MAVLink message to compress the data into
* @param target_system System ID * @param target_system System ID
* @param target_component Component ID * @param target_component Component ID
* @param global_x global x position * 1E7 * @param latitude global x position * 1E7
* @param global_y global y position * 1E7 * @param longitude global y position * 1E7
* @param global_z global z position * 1000 * @param altitude global z position * 1000
* @param local_x local x position that matches the global x position
* @param local_y local y position that matches the global y position
* @param local_z local z position that matches the global z position
* @return length of the message in bytes (excluding serial stream start sign) * @return length of the message in bytes (excluding serial stream start sign)
*/ */
static inline uint16_t mavlink_msg_gps_set_global_origin_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, uint32_t global_x, uint32_t global_y, uint32_t global_z, float local_x, float local_y, float local_z) static inline uint16_t mavlink_msg_gps_set_global_origin_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, uint32_t latitude, uint32_t longitude, uint32_t altitude)
{ {
uint16_t i = 0; uint16_t i = 0;
msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; msg->msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN;
i += put_uint8_t_by_index(target_system, i, msg->payload); // System ID 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_uint8_t_by_index(target_component, i, msg->payload); // Component ID
i += put_uint32_t_by_index(global_x, i, msg->payload); // global x position * 1E7 i += put_uint32_t_by_index(latitude, i, msg->payload); // global x position * 1E7
i += put_uint32_t_by_index(global_y, i, msg->payload); // global y position * 1E7 i += put_uint32_t_by_index(longitude, i, msg->payload); // global y position * 1E7
i += put_uint32_t_by_index(global_z, i, msg->payload); // global z position * 1000 i += put_uint32_t_by_index(altitude, i, msg->payload); // global z position * 1000
i += put_float_by_index(local_x, i, msg->payload); // local x position that matches the global x position
i += put_float_by_index(local_y, i, msg->payload); // local y position that matches the global y position
i += put_float_by_index(local_z, i, msg->payload); // local z position that matches the global z position
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
} }
@ -93,7 +78,7 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_pack_chan(uint8_t syste
*/ */
static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_set_global_origin_t* gps_set_global_origin) static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_set_global_origin_t* gps_set_global_origin)
{ {
return mavlink_msg_gps_set_global_origin_pack(system_id, component_id, msg, gps_set_global_origin->target_system, gps_set_global_origin->target_component, gps_set_global_origin->global_x, gps_set_global_origin->global_y, gps_set_global_origin->global_z, gps_set_global_origin->local_x, gps_set_global_origin->local_y, gps_set_global_origin->local_z); return mavlink_msg_gps_set_global_origin_pack(system_id, component_id, msg, gps_set_global_origin->target_system, gps_set_global_origin->target_component, gps_set_global_origin->latitude, gps_set_global_origin->longitude, gps_set_global_origin->altitude);
} }
/** /**
@ -102,19 +87,16 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i
* *
* @param target_system System ID * @param target_system System ID
* @param target_component Component ID * @param target_component Component ID
* @param global_x global x position * 1E7 * @param latitude global x position * 1E7
* @param global_y global y position * 1E7 * @param longitude global y position * 1E7
* @param global_z global z position * 1000 * @param altitude global z position * 1000
* @param local_x local x position that matches the global x position
* @param local_y local y position that matches the global y position
* @param local_z local z position that matches the global z position
*/ */
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t global_x, uint32_t global_y, uint32_t global_z, float local_x, float local_y, float local_z) static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t latitude, uint32_t longitude, uint32_t altitude)
{ {
mavlink_message_t msg; mavlink_message_t msg;
mavlink_msg_gps_set_global_origin_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, global_x, global_y, global_z, local_x, local_y, local_z); mavlink_msg_gps_set_global_origin_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, target_system, target_component, latitude, longitude, altitude);
mavlink_send_uart(chan, &msg); mavlink_send_uart(chan, &msg);
} }
@ -142,11 +124,11 @@ static inline uint8_t mavlink_msg_gps_set_global_origin_get_target_component(con
} }
/** /**
* @brief Get field global_x from gps_set_global_origin message * @brief Get field latitude from gps_set_global_origin message
* *
* @return global x position * 1E7 * @return global x position * 1E7
*/ */
static inline uint32_t mavlink_msg_gps_set_global_origin_get_global_x(const mavlink_message_t* msg) static inline uint32_t mavlink_msg_gps_set_global_origin_get_latitude(const mavlink_message_t* msg)
{ {
generic_32bit r; generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0]; r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t))[0];
@ -157,11 +139,11 @@ static inline uint32_t mavlink_msg_gps_set_global_origin_get_global_x(const mavl
} }
/** /**
* @brief Get field global_y from gps_set_global_origin message * @brief Get field longitude from gps_set_global_origin message
* *
* @return global y position * 1E7 * @return global y position * 1E7
*/ */
static inline uint32_t mavlink_msg_gps_set_global_origin_get_global_y(const mavlink_message_t* msg) static inline uint32_t mavlink_msg_gps_set_global_origin_get_longitude(const mavlink_message_t* msg)
{ {
generic_32bit r; generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t))[0]; r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t))[0];
@ -172,11 +154,11 @@ static inline uint32_t mavlink_msg_gps_set_global_origin_get_global_y(const mavl
} }
/** /**
* @brief Get field global_z from gps_set_global_origin message * @brief Get field altitude from gps_set_global_origin message
* *
* @return global z position * 1000 * @return global z position * 1000
*/ */
static inline uint32_t mavlink_msg_gps_set_global_origin_get_global_z(const mavlink_message_t* msg) static inline uint32_t mavlink_msg_gps_set_global_origin_get_altitude(const mavlink_message_t* msg)
{ {
generic_32bit r; generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[0]; r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t))[0];
@ -186,51 +168,6 @@ static inline uint32_t mavlink_msg_gps_set_global_origin_get_global_z(const mavl
return (uint32_t)r.i; return (uint32_t)r.i;
} }
/**
* @brief Get field local_x from gps_set_global_origin message
*
* @return local x position that matches the global x position
*/
static inline float mavlink_msg_gps_set_global_origin_get_local_x(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t))[3];
return (float)r.f;
}
/**
* @brief Get field local_y from gps_set_global_origin message
*
* @return local y position that matches the global y position
*/
static inline float mavlink_msg_gps_set_global_origin_get_local_y(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float))[3];
return (float)r.f;
}
/**
* @brief Get field local_z from gps_set_global_origin message
*
* @return local z position that matches the global z position
*/
static inline float mavlink_msg_gps_set_global_origin_get_local_z(const mavlink_message_t* msg)
{
generic_32bit r;
r.b[3] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[0];
r.b[2] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[1];
r.b[1] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[2];
r.b[0] = (msg->payload+sizeof(uint8_t)+sizeof(uint8_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(uint32_t)+sizeof(float)+sizeof(float))[3];
return (float)r.f;
}
/** /**
* @brief Decode a gps_set_global_origin message into a struct * @brief Decode a gps_set_global_origin message into a struct
* *
@ -241,10 +178,7 @@ static inline void mavlink_msg_gps_set_global_origin_decode(const mavlink_messag
{ {
gps_set_global_origin->target_system = mavlink_msg_gps_set_global_origin_get_target_system(msg); gps_set_global_origin->target_system = mavlink_msg_gps_set_global_origin_get_target_system(msg);
gps_set_global_origin->target_component = mavlink_msg_gps_set_global_origin_get_target_component(msg); gps_set_global_origin->target_component = mavlink_msg_gps_set_global_origin_get_target_component(msg);
gps_set_global_origin->global_x = mavlink_msg_gps_set_global_origin_get_global_x(msg); gps_set_global_origin->latitude = mavlink_msg_gps_set_global_origin_get_latitude(msg);
gps_set_global_origin->global_y = mavlink_msg_gps_set_global_origin_get_global_y(msg); gps_set_global_origin->longitude = mavlink_msg_gps_set_global_origin_get_longitude(msg);
gps_set_global_origin->global_z = mavlink_msg_gps_set_global_origin_get_global_z(msg); gps_set_global_origin->altitude = mavlink_msg_gps_set_global_origin_get_altitude(msg);
gps_set_global_origin->local_x = mavlink_msg_gps_set_global_origin_get_local_x(msg);
gps_set_global_origin->local_y = mavlink_msg_gps_set_global_origin_get_local_y(msg);
gps_set_global_origin->local_z = mavlink_msg_gps_set_global_origin_get_local_z(msg);
} }

View File

@ -5,10 +5,15 @@
enum MAV_CLASS enum MAV_CLASS
{ {
MAV_CLASS_GENERIC = 0, MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything
MAV_CLASS_PIXHAWK = 1, MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch
MAV_CLASS_SLUGS = 2, MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu
MAV_CLASS_ARDUPILOTMEGA = 3 MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com
MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org
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_NB ///< Number of autopilot classes
}; };
enum MAV_ACTION enum MAV_ACTION

View File

@ -1,7 +1,7 @@
/** @file /** @file
* @brief MAVLink comm protocol. * @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink * @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Wednesday, February 16 2011, 19:40 UTC * Generated on Saturday, February 26 2011, 13:25 UTC
*/ */
#ifndef MAVLINK_H #ifndef MAVLINK_H
#define MAVLINK_H #define MAVLINK_H

View File

@ -1,7 +1,7 @@
/** @file /** @file
* @brief MAVLink comm protocol. * @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink * @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Wednesday, February 16 2011, 19:40 UTC * Generated on Saturday, February 26 2011, 13:25 UTC
*/ */
#ifndef PIXHAWK_H #ifndef PIXHAWK_H
#define PIXHAWK_H #define PIXHAWK_H

View File

@ -7,14 +7,14 @@
<description>Commands to be executed by the MAV. They can be executed on user request, <description>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 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: 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. Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what
</description> ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data.</description>
<entry name="MAV_CMD_NAV_WAYPOINT" value="16"> <entry name="MAV_CMD_NAV_WAYPOINT" value="16">
<description>Navigate to waypoint.</description> <description>Navigate to waypoint.</description>
<param index="1">Hold time (ignored by fixed wing, time to stay at waypoint for rotary wing)</param> <param index="1">Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing)</param>
<param index="2">Acceptance radius (if the sphere with this radius is hit, the waypoint counts as reached)</param> <param index="2">Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached)</param>
<param index="3">0 to pass through the WP, if > 0 radius to pass by WP. Allows trajectory control.</param> <param index="3">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.</param>
<param index="4">Desired yaw angle</param> <param index="4">Desired yaw angle at waypoint (rotary wing)</param>
<param index="5">Latitude</param> <param index="5">Latitude</param>
<param index="6">Longitude</param> <param index="6">Longitude</param>
<param index="7">Altitude</param> <param index="7">Altitude</param>
@ -79,6 +79,27 @@
<param index="6">Longitude</param> <param index="6">Longitude</param>
<param index="7">Altitude</param> <param index="7">Altitude</param>
</entry> </entry>
<entry name="MAV_CMD_NAV_ORIENTATION_TARGET" value="80">
<description>Set the location the system should be heading towards (camera heads or
rotary wing aircraft).</description>
<param index="1">Empty</param>
<param index="2">Empty</param>
<param index="3">Empty</param>
<param index="4">Empty</param>
<param index="5">Latitude</param>
<param index="6">Longitude</param>
<param index="7">Altitude</param>
</entry>
<entry name="MAV_CMD_NAV_PATHPLANNING" value="81">
<description>Control autonomous path planning on the MAV.</description>
<param index="1">0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning</param>
<param index="2">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</param>
<param index="3">Empty</param>
<param index="4">Yaw angle at goal, in compass degrees, [0..360]</param>
<param index="5">Latitude/X of goal</param>
<param index="6">Longitude/Y of goal</param>
<param index="7">Altitude/Z of goal</param>
</entry>
<entry name="MAV_CMD_NAV_LAST" value="95"> <entry name="MAV_CMD_NAV_LAST" value="95">
<description>NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration</description> <description>NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration</description>
<param index="1">Empty</param> <param index="1">Empty</param>
@ -119,6 +140,16 @@
<param index="6">Empty</param> <param index="6">Empty</param>
<param index="7">Empty</param> <param index="7">Empty</param>
</entry> </entry>
<entry name="MAV_CMD_CONDITION_YAW" value="115">
<description>Reach a certain target angle.</description>
<param index="1">target angle: [0-360], 0 is north</param>
<param index="2">speed during yaw change:[deg per second]</param>
<param index="3">direction: negative: counter clockwise, positive: clockwise [-1,1]</param>
<param index="4">relative offset or absolute angle: [ 1,0]</param>
<param index="5">Empty</param>
<param index="6">Empty</param>
<param index="7">Empty</param>
</entry>
<entry name="MAV_CMD_CONDITION_LAST" value="159"> <entry name="MAV_CMD_CONDITION_LAST" value="159">
<description>NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration</description> <description>NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration</description>
<param index="1">Empty</param> <param index="1">Empty</param>
@ -210,7 +241,7 @@
<param index="7">Empty</param> <param index="7">Empty</param>
</entry> </entry>
<entry name="MAV_CMD_DO_REPEAT_SERVO" value="184"> <entry name="MAV_CMD_DO_REPEAT_SERVO" value="184">
<description>Cycle a between its nominal setting and a desired PWM for a desired number of cyles with a desired period.</description> <description>Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period.</description>
<param index="1">Servo number</param> <param index="1">Servo number</param>
<param index="2">PWM (microseconds, 1000 to 2000 typical)</param> <param index="2">PWM (microseconds, 1000 to 2000 typical)</param>
<param index="3">Cycle count</param> <param index="3">Cycle count</param>
@ -219,6 +250,46 @@
<param index="6">Empty</param> <param index="6">Empty</param>
<param index="7">Empty</param> <param index="7">Empty</param>
</entry> </entry>
<entry name="MAV_CMD_DO_CONTROL_VIDEO" value="200">
<description>Control onboard camera system.</description>
<param index="1">Camera ID (-1 for all)</param>
<param index="2">Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw</param>
<param index="3">Transmission mode: 0: video stream, >0: single images every n seconds (decimal)</param>
<param index="4">Recording: 0: disabled, 1: enabled compressed, 2: enabled raw</param>
<param index="5">Empty</param>
<param index="6">Empty</param>
<param index="7">Empty</param>
</entry>
<entry name="MAV_CMD_DO_LAST" value="240">
<description>NOP - This command is only used to mark the upper limit of the DO commands in the enumeration</description>
<param index="1">Empty</param>
<param index="2">Empty</param>
<param index="3">Empty</param>
<param index="4">Empty</param>
<param index="5">Empty</param>
<param index="6">Empty</param>
<param index="7">Empty</param>
</entry>
<entry name="MAV_CMD_PREFLIGHT_CALIBRATION" value="241">
<description>Trigger calibration. This command will be only accepted if in pre-flight mode.</description>
<param index="1">Gyro calibration: 0: no, 1: yes</param>
<param index="2">Magnetometer calibration: 0: no, 1: yes</param>
<param index="3">Ground pressure: 0: no, 1: yes</param>
<param index="4">Radio calibration: 0: no, 1: yes</param>
<param index="5">Empty</param>
<param index="6">Empty</param>
<param index="7">Empty</param>
</entry>
<entry name="MAV_CMD_PREFLIGHT_STORAGE" value="245">
<description>Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode.</description>
<param index="1">Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM</param>
<param index="2">Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM</param>
<param index="3">Reserved</param>
<param index="4">Reserved</param>
<param index="5">Empty</param>
<param index="6">Empty</param>
<param index="7">Empty</param>
</entry>
</enum> </enum>
<enum name="MAV_DATA_STREAM"> <enum name="MAV_DATA_STREAM">
<description>Data stream IDs. A data stream is not a fixed set of messages, but rather a <description>Data stream IDs. A data stream is not a fixed set of messages, but rather a
@ -498,7 +569,7 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se
<message name="WAYPOINT" id="39"> <message name="WAYPOINT" id="39">
<description>Message encoding a waypoint. This message is emitted to announce <description>Message encoding a waypoint. This message is emitted to announce
the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon. The global and body frame are related as: positive Z-down, positive X(front looking north, positive Y(body:right) looking east. Therefore y encodes in global mode the latitude, whereas x encodes the longitude and z the GPS altitude (WGS84).</description> the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed</description>
<field name="target_system" type="uint8_t">System ID</field> <field name="target_system" type="uint8_t">System ID</field>
<field name="target_component" type="uint8_t">Component ID</field> <field name="target_component" type="uint8_t">Component ID</field>
<field name="seq" type="uint16_t">Sequence</field> <field name="seq" type="uint16_t">Sequence</field>
@ -510,8 +581,8 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se
<field name="param2" type="float">PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds</field> <field name="param2" type="float">PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds</field>
<field name="param3" type="float">PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.</field> <field name="param3" type="float">PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.</field>
<field name="param4" type="float">PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH</field> <field name="param4" type="float">PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH</field>
<field name="x" type="float">PARAM5 / local: x position, global: longitude</field> <field name="x" type="float">PARAM5 / local: x position, global: latitude</field>
<field name="y" type="float">PARAM6 / y position: global: latitude</field> <field name="y" type="float">PARAM6 / y position: global: longitude</field>
<field name="z" type="float">PARAM7 / z position: global: altitude</field> <field name="z" type="float">PARAM7 / z position: global: altitude</field>
</message> </message>
@ -569,12 +640,9 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se
<description>As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor.</description> <description>As local waypoints exist, the global waypoint reference allows to transform between the local coordinate frame and the global (GPS) coordinate frame. This can be necessary when e.g. in- and outdoor settings are connected and the MAV should move from in- to outdoor.</description>
<field name="target_system" type="uint8_t">System ID</field> <field name="target_system" type="uint8_t">System ID</field>
<field name="target_component" type="uint8_t">Component ID</field> <field name="target_component" type="uint8_t">Component ID</field>
<field name="global_x" type="uint32_t">global x position * 1E7</field> <field name="latitude" type="uint32_t">global position * 1E7</field>
<field name="global_y" type="uint32_t">global y position * 1E7</field> <field name="longitude" type="uint32_t">global position * 1E7</field>
<field name="global_z" type="uint32_t">global z position * 1000</field> <field name="altitude" type="uint32_t">global position * 1000</field>
<field name="local_x" type="float">local x position that matches the global x position</field>
<field name="local_y" type="float">local y position that matches the global y position</field>
<field name="local_z" type="float">local z position that matches the global z position</field>
</message> </message>
<message name="GPS_LOCAL_ORIGIN_SET" id="49"> <message name="GPS_LOCAL_ORIGIN_SET" id="49">
@ -716,8 +784,8 @@ of the controller before actual flight and to assist with tuning controller para
<message name="GLOBAL_POSITION_INT" id="73"> <message name="GLOBAL_POSITION_INT" id="73">
<description>The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up)</description> <description>The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up)</description>
<field name="lat" type="int32_t">Latitude / X Position, expressed as * 1E7</field> <field name="lat" type="int32_t">Latitude, expressed as * 1E7</field>
<field name="lon" type="int32_t">Longitude / Y Position, expressed as * 1E7</field> <field name="lon" type="int32_t">Longitude, expressed as * 1E7</field>
<field name="alt" type="int32_t">Altitude in meters, expressed as * 1000 (millimeters)</field> <field name="alt" type="int32_t">Altitude in meters, expressed as * 1000 (millimeters)</field>
<field name="vx" type="int16_t">Ground X Speed (Latitude), expressed as m/s * 100</field> <field name="vx" type="int16_t">Ground X Speed (Latitude), expressed as m/s * 100</field>
<field name="vy" type="int16_t">Ground Y Speed (Longitude), expressed as m/s * 100</field> <field name="vy" type="int16_t">Ground Y Speed (Longitude), expressed as m/s * 100</field>
@ -734,6 +802,24 @@ of the controller before actual flight and to assist with tuning controller para
<field name="climb" type="float">Current climb rate in meters/second</field> <field name="climb" type="float">Current climb rate in meters/second</field>
</message> </message>
<message name="COMMAND" id="75">
<description>Send a command with up to four parameters to the MAV</description>
<field name="target_system" type="uint8_t">System which should execute the command</field>
<field name="target_component" type="uint8_t">Component which should execute the command, 0 for all components</field>
<field name="command" type="uint8_t">Command ID, as defined by MAV_CMD enum.</field>
<field name="confirmation" type="uint8_t">0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)</field>
<field name="param1" type="float">Parameter 1, as defined by MAV_CMD enum.</field>
<field name="param2" type="float">Parameter 2, as defined by MAV_CMD enum.</field>
<field name="param3" type="float">Parameter 3, as defined by MAV_CMD enum.</field>
<field name="param4" type="float">Parameter 4, as defined by MAV_CMD enum.</field>
</message>
<message name="COMMAND_ACK" id="76">
<description>Report status of a command. Includes feedback wether the command was executed</description>
<field name="command" type="float">Current airspeed in m/s</field>
<field name="result" type="float">1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION</field>
</message>
<!-- MESSAGE IDs 80 - 250: Space for custom messages in individual projectname_messages.xml files --> <!-- MESSAGE IDs 80 - 250: Space for custom messages in individual projectname_messages.xml files -->