From 9a705c22cb07241fc042d4fb2bbb8ce98858b057 Mon Sep 17 00:00:00 2001 From: LM Date: Thu, 11 Aug 2011 11:19:13 +0200 Subject: [PATCH] Updated MAVLink --- thirdParty/mavlink/include/common/common.h | 6 +- thirdParty/mavlink/include/common/mavlink.h | 2 +- .../mavlink_msg_roll_pitch_yaw_setpoint.h | 178 +++++++++ ...avlink_msg_roll_pitch_yaw_speed_setpoint.h | 178 +++++++++ .../mavlink/message_definitions/common.xml | 14 + thirdParty/mavlink/missionlib/waypoints.c | 372 +++++++++++++----- thirdParty/mavlink/missionlib/waypoints.h | 4 +- 7 files changed, 645 insertions(+), 109 deletions(-) create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index 32dca2b89..8ce7b21f8 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @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 } diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 76c0dbcf9..7de59d0c6 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @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 diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h new file mode 100644 index 000000000..41befa920 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_setpoint.h @@ -0,0 +1,178 @@ +// 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)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h new file mode 100644 index 000000000..c920eaf34 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_roll_pitch_yaw_speed_setpoint.h @@ -0,0 +1,178 @@ +// 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)); +} diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index 95e92da0e..4d4c1e52a 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -1011,6 +1011,20 @@ Desired pitch angular speed in rad/s Desired yaw angular speed in rad/s + + Setpoint in roll, pitch, yaw currently active on the system. + Timestamp in milliseconds since system boot + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + + + Setpoint in rollspeed, pitchspeed, yawspeed currently active on the system. + Timestamp in milliseconds since system boot + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + 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. 1: enabled, 0: disabled diff --git a/thirdParty/mavlink/missionlib/waypoints.c b/thirdParty/mavlink/missionlib/waypoints.c index 335f593a9..d0e7621f7 100644 --- a/thirdParty/mavlink/missionlib/waypoints.c +++ b/thirdParty/mavlink/missionlib/waypoints.c @@ -18,12 +18,20 @@ ****************************************************************************/ #include - -static mavlink_wpm_storage wpm; +#include 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); } } } diff --git a/thirdParty/mavlink/missionlib/waypoints.h b/thirdParty/mavlink/missionlib/waypoints.h index 4bc32a14a..2c3416a82 100644 --- a/thirdParty/mavlink/missionlib/waypoints.h +++ b/thirdParty/mavlink/missionlib/waypoints.h @@ -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); -- 2.22.0