Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-11 18:38:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink: pull in latest changes from upstream MAVLink

Browse Source 
this gives us the new SENSOR_OFFSETS message for debugging sensor
calibration

git-svn-id: https://arducopter.googlecode.com/svn/trunk@3080 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
tridge60@gmail.com 2011-08-13 04:45:48 +00:00
parent 1d9bc92d41
commit 6205752740
10 changed files with 1288 additions and 1069 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://qgroundcontrol.org/mavlink/
* Generated on Friday, August 5 2011, 07:37 UTC
* Generated on Saturday, August 13 2011, 04:23 UTC
*/
#ifndef ARDUPILOTMEGA_H
#define ARDUPILOTMEGA_H
@ -33,12 +33,13 @@ extern "C" {
// MESSAGE DEFINITIONS
#include "./mavlink_msg_sensor_offsets.h"
// MESSAGE LENGTHS
#undef MAVLINK_MESSAGE_LENGTHS
#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
#ifdef __cplusplus
}

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 Friday, August 5 2011, 07:37 UTC
* Generated on Saturday, August 13 2011, 04:23 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H

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

@ -1,7 +1,7 @@
/** @file
* @brief MAVLink comm protocol.
* @see http://qgroundcontrol.org/mavlink/
* Generated on Friday, August 5 2011, 08:13 UTC
* Generated on Saturday, August 13 2011, 04:23 UTC
*/
#ifndef COMMON_H
#define COMMON_H
@ -138,16 +138,17 @@ enum MAV_ROI
#include "./mavlink_msg_control_status.h"
#include "./mavlink_msg_safety_set_allowed_area.h"
#include "./mavlink_msg_safety_allowed_area.h"
#include "./mavlink_msg_set_roll_pitch_yaw.h"
#include "./mavlink_msg_set_roll_pitch_yaw_speed.h"
#include "./mavlink_msg_attitude_controller_output.h"
#include "./mavlink_msg_position_controller_output.h"
#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h"
#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h"
#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h"
#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h"
#include "./mavlink_msg_nav_controller_output.h"
#include "./mavlink_msg_position_target.h"
#include "./mavlink_msg_state_correction.h"
#include "./mavlink_msg_set_altitude.h"
#include "./mavlink_msg_request_data_stream.h"
#include "./mavlink_msg_full_state.h"
#include "./mavlink_msg_hil_state.h"
#include "./mavlink_msg_hil_controls.h"
#include "./mavlink_msg_manual_control.h"
#include "./mavlink_msg_rc_channels_override.h"
#include "./mavlink_msg_global_position_int.h"
@ -164,7 +165,7 @@ enum MAV_ROI
// MESSAGE LENGTHS
#undef MAVLINK_MESSAGE_LENGTHS
#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
#ifdef __cplusplus
}

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 Friday, August 5 2011, 08:13 UTC
* Generated on Saturday, August 13 2011, 04:23 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H

14
libraries/GCS_MAVLink/include/common/mavlink_msg_request_data_stream.h
View File

@ -7,7 +7,7 @@ typedef struct __mavlink_request_data_stream_t
uint8_t target_system; ///< The target requested to send the message stream.
uint8_t target_component; ///< The target requested to send the message stream.
uint8_t req_stream_id; ///< The ID of the requested message type
uint16_t req_message_rate; ///< The requested interval between two messages of this type
uint16_t req_message_rate; ///< Update rate in Hertz
uint8_t start_stop; ///< 1 to start sending, 0 to stop sending.
} mavlink_request_data_stream_t;
@ -23,7 +23,7 @@ typedef struct __mavlink_request_data_stream_t
* @param target_system The target requested to send the message stream.
* @param target_component The target requested to send the message stream.
* @param req_stream_id The ID of the requested message type
* @param req_message_rate The requested interval between two messages of this type
* @param req_message_rate Update rate in Hertz
* @param start_stop 1 to start sending, 0 to stop sending.
* @return length of the message in bytes (excluding serial stream start sign)
*/
@ -35,7 +35,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u
i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream.
i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream.
i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type
i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // The requested interval between two messages of this type
i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // Update rate in Hertz
i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending.
return mavlink_finalize_message(msg, system_id, component_id, i);
@ -50,7 +50,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack(uint8_t system_id, u
* @param target_system The target requested to send the message stream.
* @param target_component The target requested to send the message stream.
* @param req_stream_id The ID of the requested message type
* @param req_message_rate The requested interval between two messages of this type
* @param req_message_rate Update rate in Hertz
* @param start_stop 1 to start sending, 0 to stop sending.
* @return length of the message in bytes (excluding serial stream start sign)
*/
@ -62,7 +62,7 @@ static inline uint16_t mavlink_msg_request_data_stream_pack_chan(uint8_t system_
i += put_uint8_t_by_index(target_system, i, msg->payload); // The target requested to send the message stream.
i += put_uint8_t_by_index(target_component, i, msg->payload); // The target requested to send the message stream.
i += put_uint8_t_by_index(req_stream_id, i, msg->payload); // The ID of the requested message type
i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // The requested interval between two messages of this type
i += put_uint16_t_by_index(req_message_rate, i, msg->payload); // Update rate in Hertz
i += put_uint8_t_by_index(start_stop, i, msg->payload); // 1 to start sending, 0 to stop sending.
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i);
@ -88,7 +88,7 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id,
* @param target_system The target requested to send the message stream.
* @param target_component The target requested to send the message stream.
* @param req_stream_id The ID of the requested message type
* @param req_message_rate The requested interval between two messages of this type
* @param req_message_rate Update rate in Hertz
* @param start_stop 1 to start sending, 0 to stop sending.
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
@ -136,7 +136,7 @@ static inline uint8_t mavlink_msg_request_data_stream_get_req_stream_id(const ma
/**
* @brief Get field req_message_rate from request_data_stream message
*
* @return The requested interval between two messages of this type
* @return Update rate in Hertz
*/
static inline uint16_t mavlink_msg_request_data_stream_get_req_message_rate(const mavlink_message_t* msg)
{

25
libraries/GCS_MAVLink/message_definitions/ardupilotmega.xml
View File

@ -1,6 +1,29 @@
<?xml version="1.0"?>
<?xml version='1.0'?>
<mavlink>
<include>common.xml</include>
<!-- note that APM specific messages should use the command id
range from 150 to 250, to leave plenty of room for growth
of common.xml
If you prototype a message here, then you should consider if it
is general enough to move into common.xml later
-->
<messages>
<message id="150" name="SENSOR_OFFSETS">
<description>Offsets and calibrations values for hardware
sensors. This makes it easier to debug the calibration process.</description>
<field type="int16_t" name="mag_ofs_x">magnetometer X offset</field>
<field type="int16_t" name="mag_ofs_y">magnetometer Y offset</field>
<field type="int16_t" name="mag_ofs_z">magnetometer Z offset</field>
<field type="float" name="mag_declination">magnetic declination (radians)</field>
<field type="int32_t" name="raw_press">raw pressure from barometer</field>
<field type="int32_t" name="raw_temp">raw temperature from barometer</field>
<field type="float" name="gyro_cal_x">gyro X calibration</field>
<field type="float" name="gyro_cal_y">gyro Y calibration</field>
<field type="float" name="gyro_cal_z">gyro Z calibration</field>
<field type="float" name="accel_cal_x">accel X calibration</field>
<field type="float" name="accel_cal_y">accel Y calibration</field>
<field type="float" name="accel_cal_z">accel Z calibration</field>
</message>
</messages>
</mavlink>

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

@ -7,8 +7,7 @@
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.
</description>
ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data.</description>
<entry value="16" name="MAV_CMD_NAV_WAYPOINT">
<description>Navigate to waypoint.</description>
<param index="1">Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing)</param>
@ -83,8 +82,7 @@
<description>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.
</description>
sensors such as cameras.</description>
<param index="1">Region of intereset mode. (see MAV_ROI enum)</param>
<param index="2">Waypoint index/ target ID. (see MAV_ROI enum)</param>
<param index="3">ROI index (allows a vehicle to manage multiple ROI's)</param>
@ -278,7 +276,6 @@
<param index="6">y</param>
<param index="7">z</param>
</entry>
<entry value="240" name="MAV_CMD_DO_LAST">
<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>
@ -458,8 +455,7 @@
</message>
<message id="25" name="GPS_RAW_INT">
<description>The global position, as returned by the Global Positioning System (GPS). This is
NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame)
</description>
NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame)</description>
<field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
<field type="uint8_t" name="fix_type">0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.</field>
<field type="int32_t" name="lat">Latitude in 1E7 degrees</field>
@ -552,8 +548,7 @@
</message>
<message id="32" name="GPS_RAW">
<description>The global position, as returned by the Global Positioning System (GPS). This is
NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame)
</description>
NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame)</description>
<field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
<field type="uint8_t" name="fix_type">0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.</field>
<field type="float" name="lat">Latitude in degrees</field>
@ -611,8 +606,7 @@
</message>
<message id="39" name="WAYPOINT">
<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, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed
</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 type="uint8_t" name="target_system">System ID</field>
<field type="uint8_t" name="target_component">Component ID</field>
<field type="uint16_t" name="seq">Sequence</field>
@ -732,42 +726,43 @@
<field type="float" name="p2y">y position 2 / Longitude 2</field>
<field type="float" name="p2z">z position 2 / Altitude 2</field>
</message>
<message id="55" name="SET_ROLL_PITCH_YAW">
<message id="55" name="SET_ROLL_PITCH_YAW_THRUST">
<description>Set roll, pitch and yaw.</description>
<field type="uint8_t" name="target_system">System ID</field>
<field type="uint8_t" name="target_component">Component ID</field>
<field type="float" name="roll">Desired roll angle in radians</field>
<field type="float" name="pitch">Desired pitch angle in radians</field>
<field type="float" name="yaw">Desired yaw angle in radians</field>
<field type="float" name="thrust">Collective thrust, normalized to 0 .. 1</field>
</message>
<message id="56" name="SET_ROLL_PITCH_YAW_SPEED">
<message id="56" name="SET_ROLL_PITCH_YAW_SPEED_THRUST">
<description>Set roll, pitch and yaw.</description>
<field type="uint8_t" name="target_system">System ID</field>
<field type="uint8_t" name="target_component">Component ID</field>
<field type="float" name="roll_speed">Desired roll angular speed in rad/s</field>
<field type="float" name="pitch_speed">Desired pitch angular speed in rad/s</field>
<field type="float" name="yaw_speed">Desired yaw angular speed in rad/s</field>
<field type="float" name="thrust">Collective thrust, normalized to 0 .. 1</field>
</message>
<message id="60" name="ATTITUDE_CONTROLLER_OUTPUT">
<description>The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight.</description>
<field type="uint8_t" name="enabled">1: enabled, 0: disabled</field>
<field type="int8_t" name="roll">Attitude roll: -128: -100%, 127: +100%</field>
<field type="int8_t" name="pitch">Attitude pitch: -128: -100%, 127: +100%</field>
<field type="int8_t" name="yaw">Attitude yaw: -128: -100%, 127: +100%</field>
<field type="int8_t" name="thrust">Attitude thrust: -128: -100%, 127: +100%</field>
<message id="57" name="ROLL_PITCH_YAW_THRUST_SETPOINT">
<description>Setpoint in roll, pitch, yaw currently active on the system.</description>
<field type="uint32_t" name="time_ms">Timestamp in milliseconds since system boot</field>
<field type="float" name="roll">Desired roll angle in radians</field>
<field type="float" name="pitch">Desired pitch angle in radians</field>
<field type="float" name="yaw">Desired yaw angle in radians</field>
<field type="float" name="thrust">Collective thrust, normalized to 0 .. 1</field>
</message>
<message id="61" name="POSITION_CONTROLLER_OUTPUT">
<description>The output of the position controller. The primary use of this message is to check the response and signs of the controller before the actual flight.</description>
<field type="uint8_t" name="enabled">1: enabled, 0: disabled</field>
<field type="int8_t" name="x">Position x: -128: -100%, 127: +100%</field>
<field type="int8_t" name="y">Position y: -128: -100%, 127: +100%</field>
<field type="int8_t" name="z">Position z: -128: -100%, 127: +100%</field>
<field type="int8_t" name="yaw">Position yaw: -128: -100%, 127: +100%</field>
<message id="58" name="ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT">
<description>Setpoint in rollspeed, pitchspeed, yawspeed currently active on the system.</description>
<field type="uint32_t" name="time_ms">Timestamp in milliseconds since system boot</field>
<field type="float" name="roll_speed">Desired roll angular speed in rad/s</field>
<field type="float" name="pitch_speed">Desired pitch angular speed in rad/s</field>
<field type="float" name="yaw_speed">Desired yaw angular speed in rad/s</field>
<field type="float" name="thrust">Collective thrust, normalized to 0 .. 1</field>
</message>
<message id="62" name="NAV_CONTROLLER_OUTPUT">
<description>Outputs of the APM navigation controller. The primary use of this message is to check the response and signs
of the controller before actual flight and to assist with tuning controller parameters
</description>
of the controller before actual flight and to assist with tuning controller parameters </description>
<field type="float" name="nav_roll">Current desired roll in degrees</field>
<field type="float" name="nav_pitch">Current desired pitch in degrees</field>
<field type="int16_t" name="nav_bearing">Current desired heading in degrees</field>
@ -804,13 +799,12 @@
<field type="uint8_t" name="target_system">The target requested to send the message stream.</field>
<field type="uint8_t" name="target_component">The target requested to send the message stream.</field>
<field type="uint8_t" name="req_stream_id">The ID of the requested message type</field>
<field type="uint16_t" name="req_message_rate">The requested interval between two messages of this type</field>
<field type="uint16_t" name="req_message_rate">Update rate in Hertz</field>
<field type="uint8_t" name="start_stop">1 to start sending, 0 to stop sending.</field>
</message>
<message id="67" name="FULL_STATE">
<message id="67" name="HIL_STATE">
<description>This packet is useful for high throughput
applications such as hardware in the loop.
applications such as hardware in the loop simulations.
</description>
<field type="uint64_t" name="usec">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
<field type="float" name="roll">Roll angle (rad)</field>
@ -829,7 +823,16 @@
<field type="int16_t" name="yacc">Y acceleration (mg)</field>
<field type="int16_t" name="zacc">Z acceleration (mg)</field>
</message>
<message id="68" name="HIL_CONTROLS">
<description>Hardware in the loop control outputs</description>
<field name="time_us" type="uint64_t">Timestamp (microseconds since UNIX epoch or microseconds since system boot)</field>
<field name="roll_ailerons" type="float">Control output -3 .. 1</field>
<field name="pitch_elevator" type="float">Control output -1 .. 1</field>
<field name="yaw_rudder" type="float">Control output -1 .. 1</field>
<field name="throttle" type="float">Throttle 0 .. 1</field>
<field name="mode" type="uint8_t">System mode (MAV_MODE)</field>
<field name="nav_mode" type="uint8_t">Navigation mode (MAV_NAV_MODE)</field>
</message>
<message id="69" name="MANUAL_CONTROL">
<field type="uint8_t" name="target">The system to be controlled</field>
<field type="float" name="roll">roll</field>

63
libraries/GCS_MAVLink/missionlib/testing/main.c
View File

@ -52,32 +52,23 @@
#include <arpa/inet.h>
#endif
#include <math.h>
/* 0: Include MAVLink types */
#include <../mavlink_types.h>
/* 1: Define mavlink system storage */
mavlink_system_t mavlink_system;
/* This assumes you have the mavlink headers on your include path
or in the same folder as this source file */
/* 2: Include actual protocol, REQUIRES mavlink_system */
#include <mavlink.h>
/* 3: Define waypoint helper functions */
void mavlink_wpm_send_message(mavlink_message_t* msg);
void mavlink_wpm_send_gcs_string(const char* string);
uint64_t mavlink_wpm_get_system_timestamp();
/* Provide the interface functions for the waypoint manager */
/*
* @brief Sends a MAVLink message over UDP
*/
void mavlink_wpm_send_message(mavlink_message_t* msg)
{
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
printf("SENT %d bytes", bytes_sent);
}
#include <waypoints.h>
/* 4: Include waypoint protocol */
#include "waypoints.h"
mavlink_wpm_storage wpm;
#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why)
@ -103,6 +94,36 @@ unsigned int temp = 0;
uint64_t microsSinceEpoch();
/* Provide the interface functions for the waypoint manager */
/*
* @brief Sends a MAVLink message over UDP
*/
void mavlink_wpm_send_message(mavlink_message_t* msg)
{
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
uint16_t len = mavlink_msg_to_send_buffer(buf, msg);
uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in));
printf("SENT %d bytes", bytes_sent);
}
void mavlink_wpm_send_gcs_string(const char* string)
{
printf("%s",string);
}
uint64_t mavlink_wpm_get_system_timestamp()
{
struct timeval tv;
gettimeofday(&tv, NULL);
return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
}
int main(int argc, char* argv[])
{
// Initialize MAVLink
@ -171,7 +192,7 @@ int main(int argc, char* argv[])
{
/*Send Heartbeat */
mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_CLASS_GENERIC);
mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_HELICOPTER, MAV_CLASS_GENERIC);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
@ -220,7 +241,7 @@ int main(int argc, char* argv[])
printf("\n");
}
memset(buf, 0, BUFFER_LENGTH);
sleep(1); // Sleep one second
usleep(50000); // Sleep one second
}
}

6
libraries/GCS_MAVLink/missionlib/testing/udptest.xcodeproj/project.pbxproj
View File

@ -7,6 +7,7 @@
objects = {
/* Begin PBXBuildFile section */
34E8AFDB13F5064C001100AA /* waypoints.c in Sources */ = {isa = PBXBuildFile; fileRef = 34E8AFDA13F5064C001100AA /* waypoints.c */; };
8DD76FAC0486AB0100D96B5E /* main.c in Sources */ = {isa = PBXBuildFile; fileRef = 08FB7796FE84155DC02AAC07 /* main.c */; settings = {ATTRIBUTES = (); }; };
8DD76FB00486AB0100D96B5E /* udptest.1 in CopyFiles */ = {isa = PBXBuildFile; fileRef = C6A0FF2C0290799A04C91782 /* udptest.1 */; };
/* End PBXBuildFile section */
@ -26,6 +27,8 @@
/* Begin PBXFileReference section */
08FB7796FE84155DC02AAC07 /* main.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = main.c; sourceTree = "<group>"; };
34E8AFDA13F5064C001100AA /* waypoints.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = waypoints.c; path = ../waypoints.c; sourceTree = SOURCE_ROOT; };
34E8AFDC13F50659001100AA /* waypoints.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = waypoints.h; path = ../waypoints.h; sourceTree = SOURCE_ROOT; };
8DD76FB20486AB0100D96B5E /* udptest */ = {isa = PBXFileReference; explicitFileType = "compiled.mach-o.executable"; includeInIndex = 0; path = udptest; sourceTree = BUILT_PRODUCTS_DIR; };
C6A0FF2C0290799A04C91782 /* udptest.1 */ = {isa = PBXFileReference; lastKnownFileType = text.man; path = udptest.1; sourceTree = "<group>"; };
/* End PBXFileReference section */
@ -44,6 +47,7 @@
08FB7794FE84155DC02AAC07 /* udptest */ = {
isa = PBXGroup;
children = (
34E8AFDC13F50659001100AA /* waypoints.h */,
08FB7795FE84155DC02AAC07 /* Source */,
C6A0FF2B0290797F04C91782 /* Documentation */,
1AB674ADFE9D54B511CA2CBB /* Products */,
@ -54,6 +58,7 @@
08FB7795FE84155DC02AAC07 /* Source */ = {
isa = PBXGroup;
children = (
34E8AFDA13F5064C001100AA /* waypoints.c */,
08FB7796FE84155DC02AAC07 /* main.c */,
);
name = Source;
@ -126,6 +131,7 @@
buildActionMask = 2147483647;
files = (
8DD76FAC0486AB0100D96B5E /* main.c in Sources */,
34E8AFDB13F5064C001100AA /* waypoints.c in Sources */,
);
runOnlyForDeploymentPostprocessing = 0;
};

476
libraries/GCS_MAVLink/missionlib/waypoints.c
View File

@ -17,13 +17,21 @@
****************************************************************************/
#include <waypoints.h>
static mavlink_wpm_storage wpm;
#include "waypoints.h"
#include <math.h>
bool debug = true;
bool verbose = true;
extern mavlink_system_t mavlink_system;
extern mavlink_wpm_storage wpm;
extern void mavlink_wpm_send_message(mavlink_message_t* msg);
extern void mavlink_wpm_send_gcs_string(const char* string);
extern uint64_t mavlink_wpm_get_system_timestamp();
#define MAVLINK_WPM_NO_PRINTF
void mavlink_wpm_init(mavlink_wpm_storage* state)
{
@ -48,18 +56,6 @@ void mavlink_wpm_init(mavlink_wpm_storage* state)
}
void mavlink_wpm_send_gcs_string(const char* string)
{
printf("%s",string);
}
uint64_t mavlink_wpm_get_system_timestamp()
{
struct timeval tv;
gettimeofday(&tv, NULL);
return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
}
/*
* @brief Sends an waypoint ack message
*/
@ -79,7 +75,11 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type)
if (MAVLINK_WPM_TEXT_FEEDBACK)
{
//printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("Sent waypoint ACK");
#else
if (MAVLINK_WPM_VERBOSE) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system);
#endif
mavlink_wpm_send_gcs_string("Sent waypoint ACK");
}
}
@ -109,7 +109,7 @@ void mavlink_wpm_send_waypoint_current(uint16_t seq)
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //printf("Broadcasted new current waypoint %u\n", wpc.seq);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //// printf("Broadcasted new current waypoint %u\n", wpc.seq);
}
else
{
@ -151,14 +151,14 @@ void mavlink_wpm_send_setpoint(uint16_t seq)
}
else
{
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//// if (verbose) // printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq);
}
wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp();
}
else
{
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //if (verbose) printf("ERROR: index out of bounds\n");
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //// if (verbose) // printf("ERROR: index out of bounds\n");
}
}
@ -174,7 +174,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou
mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc);
mavlink_wpm_send_message(&msg);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
@ -189,7 +189,7 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq)
wp->target_component = wpm.current_partner_compid;
mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp);
mavlink_wpm_send_message(&msg);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
@ -210,7 +210,7 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s
wpr.seq = seq;
mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr);
mavlink_wpm_send_message(&msg);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
@ -237,7 +237,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached);
mavlink_wpm_send_message(&msg);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq);
if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq);
// FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY"));
}
@ -278,42 +278,44 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
// }
// else
// {
// if (verbose) printf("ERROR: index out of bounds\n");
// // if (verbose) // printf("ERROR: index out of bounds\n");
// }
// return -1.f;
//}
float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z)
{
// if (seq < wpm.size)
// {
// mavlink_waypoint_t *cur = waypoints->at(seq);
//
// const PxVector3 A(cur->x, cur->y, cur->z);
// const PxVector3 C(x, y, z);
//
// return (C-A).length();
// }
// else
// {
// if (verbose) printf("ERROR: index out of bounds\n");
// }
// if (seq < wpm.size)
// {
// mavlink_waypoint_t *cur = waypoints->at(seq);
//
// const PxVector3 A(cur->x, cur->y, cur->z);
// const PxVector3 C(x, y, z);
//
// return (C-A).length();
// }
// else
// {
// // if (verbose) // printf("ERROR: index out of bounds\n");
// }
return -1.f;
}
static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
// Handle param messages
//paramClient->handleMAVLinkPacket(msg);
//check for timed-out operations
struct timeval tv;
gettimeofday(&tv, NULL);
uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
uint64_t now = mavlink_wpm_get_system_timestamp();
if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
{
if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state);
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("Operation timeout switching -> IDLE");
#else
if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state);
#endif
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
wpm.current_count = 0;
wpm.current_partner_sysid = 0;
@ -376,7 +378,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
mavlink_local_position_t pos;
mavlink_msg_local_position_decode(msg, &pos);
//if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
//// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
wpm.pos_reached = false;
@ -403,52 +405,52 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
break;
}
// case MAVLINK_MSG_ID_CMD: // special action from ground station
// {
// mavlink_cmd_t action;
// mavlink_msg_cmd_decode(msg, &action);
// if(action.target == mavlink_system.sysid)
// {
// if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
// switch (action.action)
// {
// // case MAV_ACTION_LAUNCH:
// // if (verbose) std::cerr << "Launch received" << std::endl;
// // current_active_wp_id = 0;
// // if (wpm.size>0)
// // {
// // setActive(waypoints[current_active_wp_id]);
// // }
// // else
// // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
// // break;
//
// // case MAV_ACTION_CONTINUE:
// // if (verbose) std::c
// // err << "Continue received" << std::endl;
// // idle = false;
// // setActive(waypoints[current_active_wp_id]);
// // break;
//
// // case MAV_ACTION_HALT:
// // if (verbose) std::cerr << "Halt received" << std::endl;
// // idle = true;
// // break;
//
// // default:
// // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
// // break;
// }
// }
// break;
// }
// case MAVLINK_MSG_ID_CMD: // special action from ground station
// {
// mavlink_cmd_t action;
// mavlink_msg_cmd_decode(msg, &action);
// if(action.target == mavlink_system.sysid)
// {
// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
// switch (action.action)
// {
// // case MAV_ACTION_LAUNCH:
// // // if (verbose) std::cerr << "Launch received" << std::endl;
// // current_active_wp_id = 0;
// // if (wpm.size>0)
// // {
// // setActive(waypoints[current_active_wp_id]);
// // }
// // else
// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
// // break;
//
// // case MAV_ACTION_CONTINUE:
// // // if (verbose) std::c
// // err << "Continue received" << std::endl;
// // idle = false;
// // setActive(waypoints[current_active_wp_id]);
// // break;
//
// // case MAV_ACTION_HALT:
// // // if (verbose) std::cerr << "Halt received" << std::endl;
// // idle = true;
// // break;
//
// // default:
// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
// // break;
// }
// }
// break;
// }
case MAVLINK_MSG_ID_WAYPOINT_ACK:
{
mavlink_waypoint_ack_t wpa;
mavlink_msg_waypoint_ack_decode(msg, &wpa);
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid && wpa.target_component == mavlink_system.compid))
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_system.compid*/))
{
wpm.timestamp_lastaction = now;
@ -456,7 +458,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
if (wpm.current_wp_id == wpm.size-1)
{
if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("Got last WP ACK state -> IDLE");
#else
if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
#endif
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
wpm.current_wp_id = 0;
}
@ -464,7 +470,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
break;
}
@ -474,7 +484,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
mavlink_waypoint_set_current_t wpc;
mavlink_msg_waypoint_set_current_decode(msg, &wpc);
if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid)
if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_system.compid*/)
{
wpm.timestamp_lastaction = now;
@ -482,7 +492,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
if (wpc.seq < wpm.size)
{
if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
// if (verbose) // printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
wpm.current_active_wp_id = wpc.seq;
uint32_t i;
for(i = 0; i < wpm.size; i++)
@ -496,7 +506,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
wpm.waypoints[i].current = false;
}
}
if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id);
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("NEW WP SET");
#else
if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id);
#endif
wpm.yaw_reached = false;
wpm.pos_reached = false;
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
@ -505,17 +519,29 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
}
else
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Not in list");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
#endif
}
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
#endif
}
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
break;
}
@ -524,7 +550,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
mavlink_waypoint_request_list_t wprl;
mavlink_msg_waypoint_request_list_decode(msg, &wprl);
if(wprl.target_system == mavlink_system.sysid && wprl.target_component == mavlink_system.compid)
if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_system.compid*/)
{
wpm.timestamp_lastaction = now;
@ -532,8 +558,8 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
if (wpm.size > 0)
{
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
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;
@ -541,19 +567,19 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
}
else
{
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
// if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
}
wpm.current_count = wpm.size;
mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count);
}
else
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state);
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state);
}
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
// if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
}
break;
@ -563,16 +589,37 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
mavlink_waypoint_request_t wpr;
mavlink_msg_waypoint_request_decode(msg, &wpr);
if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid)
if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/)
{
wpm.timestamp_lastaction = now;
//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size))
{
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("GOT WP REQ, state -> SEND");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
}
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("GOT 2nd WP REQ");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
}
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("GOT 2nd WP REQ");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
#endif
}
wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
wpm.current_wp_id = wpr.seq;
@ -580,32 +627,76 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
}
else
{
if (verbose)
// if (verbose)
{
if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; }
if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS))
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state);
#endif
break;
}
else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
{
if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
if (wpr.seq != 0)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: First id != 0");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
#endif
}
}
else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
{
if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1);
else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1);
#endif
}
else if (wpr.seq >= wpm.size)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP not in list");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
#endif
}
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: ?");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
#endif
}
else 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((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid))
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid);
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid);
#endif
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
}
@ -616,7 +707,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
mavlink_waypoint_count_t wpc;
mavlink_msg_waypoint_count_decode(msg, &wpc);
if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid)
if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_system.compid*/)
{
wpm.timestamp_lastaction = now;
@ -624,8 +715,22 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
if (wpc.count > 0)
{
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("WP CMD OK: state -> GETLIST");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
#endif
}
if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("WP CMD OK AGAIN");
#else
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
#endif
}
wpm.current_state = MAVLINK_WPM_STATE_GETLIST;
wpm.current_wp_id = 0;
@ -633,25 +738,33 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
wpm.current_partner_compid = msg->compid;
wpm.current_count = wpc.count;
printf("clearing receive buffer and readying for receiving waypoints\n");
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("CLR RCV BUF: READY");
#else
if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n");
#endif
wpm.rcv_size = 0;
//while(waypoints_receive_buffer->size() > 0)
// {
// delete waypoints_receive_buffer->back();
// waypoints_receive_buffer->pop_back();
// }
// {
// delete waypoints_receive_buffer->back();
// waypoints_receive_buffer->pop_back();
// }
mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
}
else if (wpc.count == 0)
{
printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("COUNT 0");
#else
if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
#endif
wpm.rcv_size = 0;
//while(waypoints_receive_buffer->size() > 0)
// {
// delete waypoints->back();
// waypoints->pop_back();
// }
// {
// delete waypoints->back();
// waypoints->pop_back();
// }
wpm.current_active_wp_id = -1;
wpm.yaw_reached = false;
wpm.pos_reached = false;
@ -660,19 +773,48 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
}
else
{
if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("IGN WP CMD");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
#endif
}
}
else
{
if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state);
else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST))
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state);
#endif
}
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0)
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id);
#endif
}
else
{
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: ?");
#else
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
#endif
}
}
}
else
{
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#ifdef MAVLINK_WPM_NO_PRINTF
mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch");
#else
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
#endif
}
}
@ -683,18 +825,18 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
mavlink_waypoint_t wp;
mavlink_msg_waypoint_decode(msg, &wp);
if (verbose) printf("GOT WAYPOINT!");
// if (verbose) // printf("GOT WAYPOINT!");
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid))
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/))
{
wpm.timestamp_lastaction = now;
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count))
{
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
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]);
@ -702,11 +844,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
wpm.current_wp_id = wp.seq + 1;
if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
{
if (verbose) printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count);
// if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count);
mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
@ -730,7 +872,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
if (wpm.waypoints[i].current == 1)
{
wpm.current_active_wp_id = i;
//if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
wpm.yaw_reached = false;
wpm.pos_reached = false;
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
@ -761,36 +903,58 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
{
//we're done receiving waypoints, answer with ack.
mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
// printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
}
if (verbose)
// if (verbose)
{
if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; }
if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS))
{
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state);
break;
}
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
{
if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
if(!(wp.seq == 0))
{
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
}
else
{
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
}
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
{
if (!(wp.seq == wpm.current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id);
else if (!(wp.seq < wpm.current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
if (!(wp.seq == wpm.current_wp_id))
{
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id);
}
else if (!(wp.seq < wpm.current_count))
{
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
}
else
{
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
}
else
{
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
}
}
}
else
{
//we we're target but already communicating with someone else
if((wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid);
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid);
}
else if(wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid)
else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_system.compid*/)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
}
}
break;
@ -801,27 +965,27 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
mavlink_waypoint_clear_all_t wpca;
mavlink_msg_waypoint_clear_all_decode(msg, &wpca);
if(wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state == MAVLINK_WPM_STATE_IDLE)
if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE)
{
wpm.timestamp_lastaction = now;
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
// if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
// Delete all waypoints
wpm.size = 0;
wpm.current_active_wp_id = -1;
wpm.yaw_reached = false;
wpm.pos_reached = false;
}
else if (wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
{
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state);
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state);
}
break;
}
default:
{
if (debug) printf("Waypoint: received message of unknown type");
// if (debug) // printf("Waypoint: received message of unknown type");
break;
}
}
@ -836,7 +1000,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
if (wpm.timestamp_firstinside_orbit == 0)
{
// Announce that last waypoint was reached
if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq);
// if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq);
mavlink_wpm_send_waypoint_reached(cur_wp->seq);
wpm.timestamp_firstinside_orbit = now;
}
@ -867,7 +1031,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
wpm.waypoints[wpm.current_active_wp_id].current = true;
wpm.pos_reached = false;
wpm.yaw_reached = false;
if (verbose) printf("Set new waypoint (%u)\n", wpm.current_active_wp_id);
// if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id);
}
}
}