mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 10:28:29 -04:00
MAVLink: pull in latest changes from upstream MAVLink
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
parent
1d9bc92d41
commit
6205752740
@ -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
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
/** @file
|
||||
* @brief MAVLink comm protocol.
|
||||
* @see http://qgroundcontrol.org/mavlink/
|
||||
* Generated on Friday, August 5 2011, 08:13 UTC
|
||||
* Generated on Saturday, August 13 2011, 04:23 UTC
|
||||
*/
|
||||
#ifndef COMMON_H
|
||||
#define COMMON_H
|
||||
@ -28,7 +28,7 @@ extern "C" {
|
||||
|
||||
// ENUM DEFINITIONS
|
||||
|
||||
/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */
|
||||
/** @brief Commands to be executed by the MAV. They can be executed on user request, or as part of a mission script. If the action is used in a mission, the parameter mapping to the waypoint/mission message is as follows: Param 1, Param 2, Param 3, Param 4, X: Param 5, Y:Param 6, Z:Param 7. This command list is similar what ARINC 424 is for commercial aircraft: A data format how to interpret waypoint/mission data. */
|
||||
enum MAV_CMD
|
||||
{
|
||||
MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint. | Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) | Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) | 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. | Desired yaw angle at waypoint (rotary wing) | Latitude | Longitude | Altitude | */
|
||||
@ -63,7 +63,7 @@ enum MAV_CMD
|
||||
MAV_CMD_ENUM_END
|
||||
};
|
||||
|
||||
/** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a recommendation to the autopilot software. Individual autopilots may or may not obey the recommended messages. */
|
||||
/** @brief Data stream IDs. A data stream is not a fixed set of messages, but rather a recommendation to the autopilot software. Individual autopilots may or may not obey the recommended messages. */
|
||||
enum MAV_DATA_STREAM
|
||||
{
|
||||
MAV_DATA_STREAM_ALL=0, /* Enable all data streams | */
|
||||
@ -138,16 +138,17 @@ enum MAV_ROI
|
||||
#include "./mavlink_msg_control_status.h"
|
||||
#include "./mavlink_msg_safety_set_allowed_area.h"
|
||||
#include "./mavlink_msg_safety_allowed_area.h"
|
||||
#include "./mavlink_msg_set_roll_pitch_yaw.h"
|
||||
#include "./mavlink_msg_set_roll_pitch_yaw_speed.h"
|
||||
#include "./mavlink_msg_attitude_controller_output.h"
|
||||
#include "./mavlink_msg_position_controller_output.h"
|
||||
#include "./mavlink_msg_set_roll_pitch_yaw_thrust.h"
|
||||
#include "./mavlink_msg_set_roll_pitch_yaw_speed_thrust.h"
|
||||
#include "./mavlink_msg_roll_pitch_yaw_thrust_setpoint.h"
|
||||
#include "./mavlink_msg_roll_pitch_yaw_speed_thrust_setpoint.h"
|
||||
#include "./mavlink_msg_nav_controller_output.h"
|
||||
#include "./mavlink_msg_position_target.h"
|
||||
#include "./mavlink_msg_state_correction.h"
|
||||
#include "./mavlink_msg_set_altitude.h"
|
||||
#include "./mavlink_msg_request_data_stream.h"
|
||||
#include "./mavlink_msg_full_state.h"
|
||||
#include "./mavlink_msg_hil_state.h"
|
||||
#include "./mavlink_msg_hil_controls.h"
|
||||
#include "./mavlink_msg_manual_control.h"
|
||||
#include "./mavlink_msg_rc_channels_override.h"
|
||||
#include "./mavlink_msg_global_position_int.h"
|
||||
@ -164,7 +165,7 @@ enum MAV_ROI
|
||||
// MESSAGE LENGTHS
|
||||
|
||||
#undef MAVLINK_MESSAGE_LENGTHS
|
||||
#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 14, 14, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
|
||||
#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 }
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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)
|
||||
{
|
||||
|
||||
@ -1,6 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml version='1.0'?>
|
||||
<mavlink>
|
||||
<include>common.xml</include>
|
||||
<messages>
|
||||
</messages>
|
||||
<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>
|
||||
|
||||
File diff suppressed because it is too large
Load Diff
@ -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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
@ -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,43 +278,45 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq)
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// if (verbose) printf("ERROR: index out of bounds\n");
|
||||
// // if (verbose) // printf("ERROR: index out of bounds\n");
|
||||
// }
|
||||
// return -1.f;
|
||||
//}
|
||||
|
||||
float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z)
|
||||
{
|
||||
// if (seq < wpm.size)
|
||||
// {
|
||||
// mavlink_waypoint_t *cur = waypoints->at(seq);
|
||||
//
|
||||
// const PxVector3 A(cur->x, cur->y, cur->z);
|
||||
// const PxVector3 C(x, y, z);
|
||||
//
|
||||
// return (C-A).length();
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// if (verbose) printf("ERROR: index out of bounds\n");
|
||||
// }
|
||||
// if (seq < wpm.size)
|
||||
// {
|
||||
// mavlink_waypoint_t *cur = waypoints->at(seq);
|
||||
//
|
||||
// const PxVector3 A(cur->x, cur->y, cur->z);
|
||||
// const PxVector3 C(x, y, z);
|
||||
//
|
||||
// return (C-A).length();
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// // if (verbose) // printf("ERROR: index out of bounds\n");
|
||||
// }
|
||||
return -1.f;
|
||||
}
|
||||
|
||||
|
||||
static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
{
|
||||
// Handle param messages
|
||||
//paramClient->handleMAVLinkPacket(msg);
|
||||
|
||||
//check for timed-out operations
|
||||
struct timeval tv;
|
||||
gettimeofday(&tv, NULL);
|
||||
uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec;
|
||||
uint64_t now = mavlink_wpm_get_system_timestamp();
|
||||
if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
|
||||
{
|
||||
if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state);
|
||||
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("Operation timeout switching -> IDLE");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state);
|
||||
#endif
|
||||
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
|
||||
wpm.current_count = 0;
|
||||
wpm.current_partner_sysid = 0;
|
||||
wpm.current_partner_compid = 0;
|
||||
@ -376,7 +378,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
{
|
||||
mavlink_local_position_t pos;
|
||||
mavlink_msg_local_position_decode(msg, &pos);
|
||||
//if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
|
||||
//// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z);
|
||||
|
||||
wpm.pos_reached = false;
|
||||
|
||||
@ -403,52 +405,52 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
break;
|
||||
}
|
||||
|
||||
// case MAVLINK_MSG_ID_CMD: // special action from ground station
|
||||
// {
|
||||
// mavlink_cmd_t action;
|
||||
// mavlink_msg_cmd_decode(msg, &action);
|
||||
// if(action.target == mavlink_system.sysid)
|
||||
// {
|
||||
// if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
|
||||
// switch (action.action)
|
||||
// {
|
||||
// // case MAV_ACTION_LAUNCH:
|
||||
// // if (verbose) std::cerr << "Launch received" << std::endl;
|
||||
// // current_active_wp_id = 0;
|
||||
// // if (wpm.size>0)
|
||||
// // {
|
||||
// // setActive(waypoints[current_active_wp_id]);
|
||||
// // }
|
||||
// // else
|
||||
// // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
|
||||
// // break;
|
||||
//
|
||||
// // case MAV_ACTION_CONTINUE:
|
||||
// // if (verbose) std::c
|
||||
// // err << "Continue received" << std::endl;
|
||||
// // idle = false;
|
||||
// // setActive(waypoints[current_active_wp_id]);
|
||||
// // break;
|
||||
//
|
||||
// // case MAV_ACTION_HALT:
|
||||
// // if (verbose) std::cerr << "Halt received" << std::endl;
|
||||
// // idle = true;
|
||||
// // break;
|
||||
//
|
||||
// // default:
|
||||
// // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
|
||||
// // break;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// }
|
||||
// case MAVLINK_MSG_ID_CMD: // special action from ground station
|
||||
// {
|
||||
// mavlink_cmd_t action;
|
||||
// mavlink_msg_cmd_decode(msg, &action);
|
||||
// if(action.target == mavlink_system.sysid)
|
||||
// {
|
||||
// // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl;
|
||||
// switch (action.action)
|
||||
// {
|
||||
// // case MAV_ACTION_LAUNCH:
|
||||
// // // if (verbose) std::cerr << "Launch received" << std::endl;
|
||||
// // current_active_wp_id = 0;
|
||||
// // if (wpm.size>0)
|
||||
// // {
|
||||
// // setActive(waypoints[current_active_wp_id]);
|
||||
// // }
|
||||
// // else
|
||||
// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
|
||||
// // break;
|
||||
//
|
||||
// // case MAV_ACTION_CONTINUE:
|
||||
// // // if (verbose) std::c
|
||||
// // err << "Continue received" << std::endl;
|
||||
// // idle = false;
|
||||
// // setActive(waypoints[current_active_wp_id]);
|
||||
// // break;
|
||||
//
|
||||
// // case MAV_ACTION_HALT:
|
||||
// // // if (verbose) std::cerr << "Halt received" << std::endl;
|
||||
// // idle = true;
|
||||
// // break;
|
||||
//
|
||||
// // default:
|
||||
// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
|
||||
// // break;
|
||||
// }
|
||||
// }
|
||||
// break;
|
||||
// }
|
||||
|
||||
case MAVLINK_MSG_ID_WAYPOINT_ACK:
|
||||
{
|
||||
mavlink_waypoint_ack_t wpa;
|
||||
mavlink_msg_waypoint_ack_decode(msg, &wpa);
|
||||
|
||||
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid && wpa.target_component == mavlink_system.compid))
|
||||
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_system.compid*/))
|
||||
{
|
||||
wpm.timestamp_lastaction = now;
|
||||
|
||||
@ -456,15 +458,23 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
{
|
||||
if (wpm.current_wp_id == wpm.size-1)
|
||||
{
|
||||
if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
|
||||
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("Got last WP ACK state -> IDLE");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n");
|
||||
#endif
|
||||
wpm.current_state = MAVLINK_WPM_STATE_IDLE;
|
||||
wpm.current_wp_id = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: curr partner id mismatch");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
}
|
||||
@ -474,7 +484,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
mavlink_waypoint_set_current_t wpc;
|
||||
mavlink_msg_waypoint_set_current_decode(msg, &wpc);
|
||||
|
||||
if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid)
|
||||
if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_system.compid*/)
|
||||
{
|
||||
wpm.timestamp_lastaction = now;
|
||||
|
||||
@ -482,7 +492,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
{
|
||||
if (wpc.seq < wpm.size)
|
||||
{
|
||||
if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
|
||||
// if (verbose) // printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n");
|
||||
wpm.current_active_wp_id = wpc.seq;
|
||||
uint32_t i;
|
||||
for(i = 0; i < wpm.size; i++)
|
||||
@ -496,7 +506,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
wpm.waypoints[i].current = false;
|
||||
}
|
||||
}
|
||||
if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id);
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("NEW WP SET");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id);
|
||||
#endif
|
||||
wpm.yaw_reached = false;
|
||||
wpm.pos_reached = false;
|
||||
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
|
||||
@ -505,17 +519,29 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Not in list");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("IGN WP CURR CMD: Busy");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
|
||||
#endif
|
||||
}
|
||||
break;
|
||||
}
|
||||
@ -524,7 +550,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
{
|
||||
mavlink_waypoint_request_list_t wprl;
|
||||
mavlink_msg_waypoint_request_list_decode(msg, &wprl);
|
||||
if(wprl.target_system == mavlink_system.sysid && wprl.target_component == mavlink_system.compid)
|
||||
if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_system.compid*/)
|
||||
{
|
||||
wpm.timestamp_lastaction = now;
|
||||
|
||||
@ -532,30 +558,30 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
{
|
||||
if (wpm.size > 0)
|
||||
{
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
|
||||
wpm.current_state = MAVLINK_WPM_STATE_SENDLIST;
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid);
|
||||
wpm.current_state = MAVLINK_WPM_STATE_SENDLIST;
|
||||
wpm.current_wp_id = 0;
|
||||
wpm.current_partner_sysid = msg->sysid;
|
||||
wpm.current_partner_compid = msg->compid;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
|
||||
// if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid);
|
||||
}
|
||||
wpm.current_count = wpm.size;
|
||||
mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state);
|
||||
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
|
||||
// if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n");
|
||||
}
|
||||
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
@ -563,16 +589,37 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
{
|
||||
mavlink_waypoint_request_t wpr;
|
||||
mavlink_msg_waypoint_request_decode(msg, &wpr);
|
||||
if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid)
|
||||
if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/)
|
||||
{
|
||||
wpm.timestamp_lastaction = now;
|
||||
|
||||
//ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint)
|
||||
if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size))
|
||||
{
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
|
||||
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("GOT WP REQ, state -> SEND");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
|
||||
#endif
|
||||
}
|
||||
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1)
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("GOT 2nd WP REQ");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
|
||||
#endif
|
||||
}
|
||||
if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id)
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("GOT 2nd WP REQ");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid);
|
||||
#endif
|
||||
}
|
||||
|
||||
wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS;
|
||||
wpm.current_wp_id = wpr.seq;
|
||||
@ -580,34 +627,78 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose)
|
||||
// if (verbose)
|
||||
{
|
||||
if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; }
|
||||
if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS))
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state);
|
||||
#endif
|
||||
break;
|
||||
}
|
||||
else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST)
|
||||
{
|
||||
if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
|
||||
if (wpr.seq != 0)
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: First id != 0");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)
|
||||
{
|
||||
if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1);
|
||||
else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
|
||||
if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1)
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP was unexpected");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1);
|
||||
#endif
|
||||
}
|
||||
else if (wpr.seq >= wpm.size)
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: Req. WP not in list");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
|
||||
else
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: ?");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//we we're target but already communicating with someone else
|
||||
if((wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid))
|
||||
if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid))
|
||||
{
|
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid);
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid);
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
break;
|
||||
}
|
||||
@ -616,7 +707,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
{
|
||||
mavlink_waypoint_count_t wpc;
|
||||
mavlink_msg_waypoint_count_decode(msg, &wpc);
|
||||
if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid)
|
||||
if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_system.compid*/)
|
||||
{
|
||||
wpm.timestamp_lastaction = now;
|
||||
|
||||
@ -624,8 +715,22 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
{
|
||||
if (wpc.count > 0)
|
||||
{
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
|
||||
if (wpm.current_state == MAVLINK_WPM_STATE_IDLE)
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("WP CMD OK: state -> GETLIST");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid);
|
||||
#endif
|
||||
}
|
||||
if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("WP CMD OK AGAIN");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid);
|
||||
#endif
|
||||
}
|
||||
|
||||
wpm.current_state = MAVLINK_WPM_STATE_GETLIST;
|
||||
wpm.current_wp_id = 0;
|
||||
@ -633,25 +738,33 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
wpm.current_partner_compid = msg->compid;
|
||||
wpm.current_count = wpc.count;
|
||||
|
||||
printf("clearing receive buffer and readying for receiving waypoints\n");
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("CLR RCV BUF: READY");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n");
|
||||
#endif
|
||||
wpm.rcv_size = 0;
|
||||
//while(waypoints_receive_buffer->size() > 0)
|
||||
// {
|
||||
// delete waypoints_receive_buffer->back();
|
||||
// waypoints_receive_buffer->pop_back();
|
||||
// }
|
||||
// {
|
||||
// delete waypoints_receive_buffer->back();
|
||||
// waypoints_receive_buffer->pop_back();
|
||||
// }
|
||||
|
||||
mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id);
|
||||
}
|
||||
else if (wpc.count == 0)
|
||||
{
|
||||
printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("COUNT 0");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n");
|
||||
#endif
|
||||
wpm.rcv_size = 0;
|
||||
//while(waypoints_receive_buffer->size() > 0)
|
||||
// {
|
||||
// delete waypoints->back();
|
||||
// waypoints->pop_back();
|
||||
// }
|
||||
// {
|
||||
// delete waypoints->back();
|
||||
// waypoints->pop_back();
|
||||
// }
|
||||
wpm.current_active_wp_id = -1;
|
||||
wpm.yaw_reached = false;
|
||||
wpm.pos_reached = false;
|
||||
@ -660,19 +773,48 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("IGN WP CMD");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state);
|
||||
else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id);
|
||||
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
|
||||
if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST))
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state);
|
||||
#endif
|
||||
}
|
||||
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0)
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id);
|
||||
#endif
|
||||
}
|
||||
else
|
||||
{
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: ?");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n");
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
|
||||
#ifdef MAVLINK_WPM_NO_PRINTF
|
||||
mavlink_wpm_send_gcs_string("REJ. WP CMD: target id mismatch");
|
||||
#else
|
||||
if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n");
|
||||
#endif
|
||||
}
|
||||
|
||||
}
|
||||
@ -683,30 +825,30 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
mavlink_waypoint_t wp;
|
||||
mavlink_msg_waypoint_decode(msg, &wp);
|
||||
|
||||
if (verbose) printf("GOT WAYPOINT!");
|
||||
// if (verbose) // printf("GOT WAYPOINT!");
|
||||
|
||||
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid))
|
||||
if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/))
|
||||
{
|
||||
wpm.timestamp_lastaction = now;
|
||||
|
||||
//ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids
|
||||
if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count))
|
||||
{
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
|
||||
|
||||
wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid);
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid);
|
||||
if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid);
|
||||
|
||||
wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS;
|
||||
mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]);
|
||||
memcpy(newwp, &wp, sizeof(mavlink_waypoint_t));
|
||||
|
||||
wpm.current_wp_id = wp.seq + 1;
|
||||
wpm.current_wp_id = wp.seq + 1;
|
||||
|
||||
if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
|
||||
// if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4);
|
||||
|
||||
if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
|
||||
{
|
||||
if (verbose) printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count);
|
||||
// if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count);
|
||||
|
||||
mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
|
||||
|
||||
@ -730,7 +872,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
if (wpm.waypoints[i].current == 1)
|
||||
{
|
||||
wpm.current_active_wp_id = i;
|
||||
//if (verbose) printf("New current waypoint %u\n", current_active_wp_id);
|
||||
//// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id);
|
||||
wpm.yaw_reached = false;
|
||||
wpm.pos_reached = false;
|
||||
mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id);
|
||||
@ -761,36 +903,58 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
{
|
||||
//we're done receiving waypoints, answer with ack.
|
||||
mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0);
|
||||
printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
|
||||
// printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n");
|
||||
}
|
||||
if (verbose)
|
||||
// if (verbose)
|
||||
{
|
||||
if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; }
|
||||
if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS))
|
||||
{
|
||||
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state);
|
||||
break;
|
||||
}
|
||||
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST)
|
||||
{
|
||||
if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
|
||||
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
|
||||
if(!(wp.seq == 0))
|
||||
{
|
||||
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq);
|
||||
}
|
||||
else
|
||||
{
|
||||
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
|
||||
}
|
||||
}
|
||||
else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)
|
||||
{
|
||||
if (!(wp.seq == wpm.current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id);
|
||||
else if (!(wp.seq < wpm.current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
|
||||
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
|
||||
if (!(wp.seq == wpm.current_wp_id))
|
||||
{
|
||||
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id);
|
||||
}
|
||||
else if (!(wp.seq < wpm.current_count))
|
||||
{
|
||||
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq);
|
||||
}
|
||||
else
|
||||
{
|
||||
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
|
||||
}
|
||||
}
|
||||
else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
|
||||
else
|
||||
{
|
||||
// printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
//we we're target but already communicating with someone else
|
||||
if((wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
|
||||
if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
|
||||
{
|
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid);
|
||||
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid);
|
||||
}
|
||||
else if(wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid)
|
||||
else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_system.compid*/)
|
||||
{
|
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
|
||||
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid);
|
||||
}
|
||||
}
|
||||
break;
|
||||
@ -801,27 +965,27 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
mavlink_waypoint_clear_all_t wpca;
|
||||
mavlink_msg_waypoint_clear_all_decode(msg, &wpca);
|
||||
|
||||
if(wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state == MAVLINK_WPM_STATE_IDLE)
|
||||
if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE)
|
||||
{
|
||||
wpm.timestamp_lastaction = now;
|
||||
|
||||
if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
|
||||
// if (verbose) // printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid);
|
||||
// Delete all waypoints
|
||||
wpm.size = 0;
|
||||
wpm.current_active_wp_id = -1;
|
||||
wpm.yaw_reached = false;
|
||||
wpm.pos_reached = false;
|
||||
}
|
||||
else if (wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
|
||||
else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE)
|
||||
{
|
||||
if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state);
|
||||
// if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
{
|
||||
if (debug) printf("Waypoint: received message of unknown type");
|
||||
// if (debug) // printf("Waypoint: received message of unknown type");
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -836,7 +1000,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
if (wpm.timestamp_firstinside_orbit == 0)
|
||||
{
|
||||
// Announce that last waypoint was reached
|
||||
if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq);
|
||||
// if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq);
|
||||
mavlink_wpm_send_waypoint_reached(cur_wp->seq);
|
||||
wpm.timestamp_firstinside_orbit = now;
|
||||
}
|
||||
@ -867,7 +1031,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
|
||||
wpm.waypoints[wpm.current_active_wp_id].current = true;
|
||||
wpm.pos_reached = false;
|
||||
wpm.yaw_reached = false;
|
||||
if (verbose) printf("Set new waypoint (%u)\n", wpm.current_active_wp_id);
|
||||
// if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user