Skip to content

Q
qgroundcontrol
Commit 9a705c22 authored by LM's avatar LM
Browse files
Options

Updated MAVLink

parent 738a310e
Hide whitespace changes
Inline Side-by-side
Showing with 645 additions and 109 deletions
thirdParty/mavlink/include/common/common.h
View file @ 9a705c22
/** @file
* @brief MAVLink comm protocol.
* @see http://qgroundcontrol.org/mavlink/
* Generated on Wednesday, August 10 2011, 09:07 UTC
* Generated on Thursday, August 11 2011, 07:25 UTC
*/
#ifndef COMMON_H
#define COMMON_H
......@@ -275,6 +275,8 @@ enum MAV_CMD
#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_roll_pitch_yaw_setpoint.h"
#include "./mavlink_msg_roll_pitch_yaw_speed_setpoint.h"
#include "./mavlink_msg_attitude_controller_output.h"
#include "./mavlink_msg_position_controller_output.h"
#include "./mavlink_msg_nav_controller_output.h"
......@@ -300,7 +302,7 @@ enum MAV_CMD
// MESSAGE CRC KEYS
#undef MAVLINK_MESSAGE_KEYS
#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 127, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 }
#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 79, 252, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 138, 43, 141, 211, 144 }
#ifdef __cplusplus
}
......
thirdParty/mavlink/include/common/mavlink.h
View file @ 9a705c22
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Wednesday, August 10 2011, 09:07 UTC
* Generated on Thursday, August 11 2011, 07:25 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H
......
thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h 0 → 100644
View file @ 9a705c22
// MESSAGE ROLL_PITCH_YAW_SETPOINT PACKING
#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT 57
#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN 16
#define MAVLINK_MSG_57_LEN 16
typedef struct __mavlink_roll_pitch_yaw_setpoint_t
{
uint32_t time_ms; ///< Timestamp in milliseconds since system boot
float roll; ///< Desired roll angle in radians
float pitch; ///< Desired pitch angle in radians
float yaw; ///< Desired yaw angle in radians
} mavlink_roll_pitch_yaw_setpoint_t;
/**
* @brief Pack a roll_pitch_yaw_setpoint message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param time_ms Timestamp in milliseconds since system boot
* @param roll Desired roll angle in radians
* @param pitch Desired pitch angle in radians
* @param yaw Desired yaw angle in radians
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_roll_pitch_yaw_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_ms, float roll, float pitch, float yaw)
{
mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT;
p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot
p->roll = roll; // float:Desired roll angle in radians
p->pitch = pitch; // float:Desired pitch angle in radians
p->yaw = yaw; // float:Desired yaw angle in radians
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN);
}
/**
* @brief Pack a roll_pitch_yaw_setpoint message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param time_ms Timestamp in milliseconds since system boot
* @param roll Desired roll angle in radians
* @param pitch Desired pitch angle in radians
* @param yaw Desired yaw angle in radians
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_roll_pitch_yaw_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time_ms, float roll, float pitch, float yaw)
{
mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT;
p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot
p->roll = roll; // float:Desired roll angle in radians
p->pitch = pitch; // float:Desired pitch angle in radians
p->yaw = yaw; // float:Desired yaw angle in radians
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN);
}
/**
* @brief Encode a roll_pitch_yaw_setpoint struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param roll_pitch_yaw_setpoint C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_roll_pitch_yaw_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_setpoint_t* roll_pitch_yaw_setpoint)
{
return mavlink_msg_roll_pitch_yaw_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_setpoint->time_ms, roll_pitch_yaw_setpoint->roll, roll_pitch_yaw_setpoint->pitch, roll_pitch_yaw_setpoint->yaw);
}
/**
* @brief Send a roll_pitch_yaw_setpoint message
* @param chan MAVLink channel to send the message
*
* @param time_ms Timestamp in milliseconds since system boot
* @param roll Desired roll angle in radians
* @param pitch Desired pitch angle in radians
* @param yaw Desired yaw angle in radians
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_roll_pitch_yaw_setpoint_send(mavlink_channel_t chan, uint32_t time_ms, float roll, float pitch, float yaw)
{
mavlink_header_t hdr;
mavlink_roll_pitch_yaw_setpoint_t payload;
uint16_t checksum;
mavlink_roll_pitch_yaw_setpoint_t *p = &payload;
p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot
p->roll = roll; // float:Desired roll angle in radians
p->pitch = pitch; // float:Desired pitch angle in radians
p->yaw = yaw; // float:Desired yaw angle in radians
hdr.STX = MAVLINK_STX;
hdr.len = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT_LEN;
hdr.msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SETPOINT;
hdr.sysid = mavlink_system.sysid;
hdr.compid = mavlink_system.compid;
hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;
mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );
crc_init(&checksum);
checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);
checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );
hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);
mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);
}
#endif
// MESSAGE ROLL_PITCH_YAW_SETPOINT UNPACKING
/**
* @brief Get field time_ms from roll_pitch_yaw_setpoint message
*
* @return Timestamp in milliseconds since system boot
*/
static inline uint32_t mavlink_msg_roll_pitch_yaw_setpoint_get_time_ms(const mavlink_message_t* msg)
{
mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0];
return (uint32_t)(p->time_ms);
}
/**
* @brief Get field roll from roll_pitch_yaw_setpoint message
*
* @return Desired roll angle in radians
*/
static inline float mavlink_msg_roll_pitch_yaw_setpoint_get_roll(const mavlink_message_t* msg)
{
mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0];
return (float)(p->roll);
}
/**
* @brief Get field pitch from roll_pitch_yaw_setpoint message
*
* @return Desired pitch angle in radians
*/
static inline float mavlink_msg_roll_pitch_yaw_setpoint_get_pitch(const mavlink_message_t* msg)
{
mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0];
return (float)(p->pitch);
}
/**
* @brief Get field yaw from roll_pitch_yaw_setpoint message
*
* @return Desired yaw angle in radians
*/
static inline float mavlink_msg_roll_pitch_yaw_setpoint_get_yaw(const mavlink_message_t* msg)
{
mavlink_roll_pitch_yaw_setpoint_t *p = (mavlink_roll_pitch_yaw_setpoint_t *)&msg->payload[0];
return (float)(p->yaw);
}
/**
* @brief Decode a roll_pitch_yaw_setpoint message into a struct
*
* @param msg The message to decode
* @param roll_pitch_yaw_setpoint C-struct to decode the message contents into
*/
static inline void mavlink_msg_roll_pitch_yaw_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_setpoint_t* roll_pitch_yaw_setpoint)
{
memcpy( roll_pitch_yaw_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_setpoint_t));
}
thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h 0 → 100644
View file @ 9a705c22
// MESSAGE ROLL_PITCH_YAW_SPEED_SETPOINT PACKING
#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT 58
#define MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN 16
#define MAVLINK_MSG_58_LEN 16
typedef struct __mavlink_roll_pitch_yaw_speed_setpoint_t
{
uint32_t time_ms; ///< Timestamp in milliseconds since system boot
float roll_speed; ///< Desired roll angular speed in rad/s
float pitch_speed; ///< Desired pitch angular speed in rad/s
float yaw_speed; ///< Desired yaw angular speed in rad/s
} mavlink_roll_pitch_yaw_speed_setpoint_t;
/**
* @brief Pack a roll_pitch_yaw_speed_setpoint message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param time_ms Timestamp in milliseconds since system boot
* @param roll_speed Desired roll angular speed in rad/s
* @param pitch_speed Desired pitch angular speed in rad/s
* @param yaw_speed Desired yaw angular speed in rad/s
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_setpoint_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed)
{
mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT;
p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot
p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s
p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s
p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN);
}
/**
* @brief Pack a roll_pitch_yaw_speed_setpoint message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param time_ms Timestamp in milliseconds since system boot
* @param roll_speed Desired roll angular speed in rad/s
* @param pitch_speed Desired pitch angular speed in rad/s
* @param yaw_speed Desired yaw angular speed in rad/s
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_setpoint_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed)
{
mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT;
p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot
p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s
p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s
p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN);
}
/**
* @brief Encode a roll_pitch_yaw_speed_setpoint struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param roll_pitch_yaw_speed_setpoint C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_roll_pitch_yaw_speed_setpoint_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_roll_pitch_yaw_speed_setpoint_t* roll_pitch_yaw_speed_setpoint)
{
return mavlink_msg_roll_pitch_yaw_speed_setpoint_pack(system_id, component_id, msg, roll_pitch_yaw_speed_setpoint->time_ms, roll_pitch_yaw_speed_setpoint->roll_speed, roll_pitch_yaw_speed_setpoint->pitch_speed, roll_pitch_yaw_speed_setpoint->yaw_speed);
}
/**
* @brief Send a roll_pitch_yaw_speed_setpoint message
* @param chan MAVLink channel to send the message
*
* @param time_ms Timestamp in milliseconds since system boot
* @param roll_speed Desired roll angular speed in rad/s
* @param pitch_speed Desired pitch angular speed in rad/s
* @param yaw_speed Desired yaw angular speed in rad/s
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_roll_pitch_yaw_speed_setpoint_send(mavlink_channel_t chan, uint32_t time_ms, float roll_speed, float pitch_speed, float yaw_speed)
{
mavlink_header_t hdr;
mavlink_roll_pitch_yaw_speed_setpoint_t payload;
uint16_t checksum;
mavlink_roll_pitch_yaw_speed_setpoint_t *p = &payload;
p->time_ms = time_ms; // uint32_t:Timestamp in milliseconds since system boot
p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s
p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s
p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s
hdr.STX = MAVLINK_STX;
hdr.len = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT_LEN;
hdr.msgid = MAVLINK_MSG_ID_ROLL_PITCH_YAW_SPEED_SETPOINT;
hdr.sysid = mavlink_system.sysid;
hdr.compid = mavlink_system.compid;
hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;
mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );
crc_init(&checksum);
checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);
checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );
hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);
mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);
}
#endif
// MESSAGE ROLL_PITCH_YAW_SPEED_SETPOINT UNPACKING
/**
* @brief Get field time_ms from roll_pitch_yaw_speed_setpoint message
*
* @return Timestamp in milliseconds since system boot
*/
static inline uint32_t mavlink_msg_roll_pitch_yaw_speed_setpoint_get_time_ms(const mavlink_message_t* msg)
{
mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0];
return (uint32_t)(p->time_ms);
}
/**
* @brief Get field roll_speed from roll_pitch_yaw_speed_setpoint message
*
* @return Desired roll angular speed in rad/s
*/
static inline float mavlink_msg_roll_pitch_yaw_speed_setpoint_get_roll_speed(const mavlink_message_t* msg)
{
mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0];
return (float)(p->roll_speed);
}
/**
* @brief Get field pitch_speed from roll_pitch_yaw_speed_setpoint message
*
* @return Desired pitch angular speed in rad/s
*/
static inline float mavlink_msg_roll_pitch_yaw_speed_setpoint_get_pitch_speed(const mavlink_message_t* msg)
{
mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0];
return (float)(p->pitch_speed);
}
/**
* @brief Get field yaw_speed from roll_pitch_yaw_speed_setpoint message
*
* @return Desired yaw angular speed in rad/s
*/
static inline float mavlink_msg_roll_pitch_yaw_speed_setpoint_get_yaw_speed(const mavlink_message_t* msg)
{
mavlink_roll_pitch_yaw_speed_setpoint_t *p = (mavlink_roll_pitch_yaw_speed_setpoint_t *)&msg->payload[0];
return (float)(p->yaw_speed);
}
/**
* @brief Decode a roll_pitch_yaw_speed_setpoint message into a struct
*
* @param msg The message to decode
* @param roll_pitch_yaw_speed_setpoint C-struct to decode the message contents into
*/
static inline void mavlink_msg_roll_pitch_yaw_speed_setpoint_decode(const mavlink_message_t* msg, mavlink_roll_pitch_yaw_speed_setpoint_t* roll_pitch_yaw_speed_setpoint)
{
memcpy( roll_pitch_yaw_speed_setpoint, msg->payload, sizeof(mavlink_roll_pitch_yaw_speed_setpoint_t));
}
thirdParty/mavlink/message_definitions/common.xml
View file @ 9a705c22
......@@ -1011,6 +1011,20 @@
<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>
</message>
<message id="57" name="ROLL_PITCH_YAW_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>
</message>
<message id="58" name="ROLL_PITCH_YAW_SPEED_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>
</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>
......
thirdParty/mavlink/missionlib/waypoints.c
View file @ 9a705c22
......@@ -18,12 +18,20 @@
****************************************************************************/
#include <waypoints.h>
static mavlink_wpm_storage wpm;
#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,7 +278,7 @@ 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;
//}
......@@ -296,25 +296,27 @@ float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z)
// }
// else
// {
// if (verbose) printf("ERROR: index out of bounds\n");
// // 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;
......@@ -409,34 +411,34 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
// 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;
// // 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;
// // // 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;
// // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl;
// // break;
//
// // case MAV_ACTION_CONTINUE:
// // if (verbose) std::c
// // // 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;
// // // 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;
// // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl;
// // break;
// }
// }
......@@ -448,7 +450,7 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
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,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 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
}
}
......@@ -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,7 +738,11 @@ 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)
// {
......@@ -645,7 +754,11 @@ static void mavlink_wpm_message_handler(const mavlink_message_t* msg)
}
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)
// {
......@@ -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);
}
}
}
......
thirdParty/mavlink/missionlib/waypoints.h
View file @ 9a705c22
......@@ -47,7 +47,7 @@ enum MAVLINK_WPM_CODES
/* WAYPOINT MANAGER - MISSION LIB */
#define MAVLINK_WPM_MAX_WP_COUNT 100
#define MAVLINK_WPM_MAX_WP_COUNT 30
#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates
#define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text
#define MAVLINK_WPM_SYSTEM_ID 1
......@@ -88,4 +88,4 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage;
void mavlink_wpm_init(mavlink_wpm_storage* state);
void mavlink_wpm_message_handler(const mavlink_message_t* msg);
\ No newline at end of file
void mavlink_wpm_message_handler(const mavlink_message_t* msg);
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment