From 94cf579ec312cdb4818659b1cb60964ee370d35e Mon Sep 17 00:00:00 2001 From: oberion Date: Tue, 16 Aug 2011 08:30:22 +0200 Subject: [PATCH] changed position message --- src/uas/senseSoarMAV.cpp | 14 ++-- .../mavlink/include/SenseSoar/SenseSoar.h | 4 +- .../mavlink/include/SenseSoar/mavlink.h | 2 +- .../SenseSoar/mavlink_msg_obs_position.h | 82 +++++++++++++++---- thirdParty/mavlink/include/common/common.h | 2 +- thirdParty/mavlink/include/common/mavlink.h | 2 +- .../mavlink/message_definitions/SenseSoar.xml | 6 +- 7 files changed, 80 insertions(+), 32 deletions(-) diff --git a/src/uas/senseSoarMAV.cpp b/src/uas/senseSoarMAV.cpp index a133fe137..037fc2e9e 100644 --- a/src/uas/senseSoarMAV.cpp +++ b/src/uas/senseSoarMAV.cpp @@ -108,13 +108,13 @@ void senseSoarMAV::receiveMessage(LinkInterface *link, mavlink_message_t message mavlink_obs_position_t posMsg; mavlink_msg_obs_position_decode(&message, &posMsg); quint64 time = getUnixTime(); - this->localX = posMsg.pos[0]; - this->localY = posMsg.pos[1]; - this->localZ = posMsg.pos[2]; - emit valueChanged(uasId, "x", "m", this->localX, time); - emit valueChanged(uasId, "y", "m", this->localY, time); - emit valueChanged(uasId, "z", "m", this->localZ, time); - emit localPositionChanged(this, this->localX, this->localY, this->localZ, time); + this->longitude = posMsg.lon/(double)1E7; + this->latitude = posMsg.lat/(double)1E7; + this->altitude = posMsg.alt/1000.0; + emit valueChanged(uasId, "latitude", "deg", this->latitude, time); + emit valueChanged(uasId, "longitude", "deg", this->longitude, time); + emit valueChanged(uasId, "altitude", "m", this->altitude, time); + emit globalPositionChanged(this, this->latitude, this->longitude, this->altitude, time); break; } case MAVLINK_MSG_ID_OBS_QFF: diff --git a/thirdParty/mavlink/include/SenseSoar/SenseSoar.h b/thirdParty/mavlink/include/SenseSoar/SenseSoar.h index aca07a270..9a97b995e 100644 --- a/thirdParty/mavlink/include/SenseSoar/SenseSoar.h +++ b/thirdParty/mavlink/include/SenseSoar/SenseSoar.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Friday, August 12 2011, 12:18 UTC + * Generated on Monday, August 15 2011, 15:40 UTC */ #ifndef SENSESOAR_H #define SENSESOAR_H @@ -61,7 +61,7 @@ enum SENSESOAR_MODE // MESSAGE LENGTHS #undef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 24, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 0, 0, 0, 0, 0, 5, 5, 26, 16, 36, 5, 6, 0, 0, 21, 0, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/SenseSoar/mavlink.h b/thirdParty/mavlink/include/SenseSoar/mavlink.h index b11a10666..9933bc430 100644 --- a/thirdParty/mavlink/include/SenseSoar/mavlink.h +++ b/thirdParty/mavlink/include/SenseSoar/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Friday, August 12 2011, 12:18 UTC + * Generated on Monday, August 15 2011, 15:40 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/SenseSoar/mavlink_msg_obs_position.h b/thirdParty/mavlink/include/SenseSoar/mavlink_msg_obs_position.h index 05d1333fa..45b6acb99 100644 --- a/thirdParty/mavlink/include/SenseSoar/mavlink_msg_obs_position.h +++ b/thirdParty/mavlink/include/SenseSoar/mavlink_msg_obs_position.h @@ -4,11 +4,12 @@ typedef struct __mavlink_obs_position_t { - double pos[3]; ///< Position + int32_t lon; ///< Longitude expressed in 1E7 + int32_t lat; ///< Latitude expressed in 1E7 + int32_t alt; ///< Altitude expressed in milimeters } mavlink_obs_position_t; -#define MAVLINK_MSG_OBS_POSITION_FIELD_POS_LEN 3 /** @@ -17,15 +18,19 @@ typedef struct __mavlink_obs_position_t * @param component_id ID of this component (e.g. 200 for IMU) * @param msg The MAVLink message to compress the data into * - * @param pos Position + * @param lon Longitude expressed in 1E7 + * @param lat Latitude expressed in 1E7 + * @param alt Altitude expressed in milimeters * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const double* pos) +static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, int32_t lon, int32_t lat, int32_t alt) { uint16_t i = 0; msg->msgid = MAVLINK_MSG_ID_OBS_POSITION; - i += put_array_by_index((const int8_t*)pos, sizeof(double)*3, i, msg->payload); // Position + i += put_int32_t_by_index(lon, i, msg->payload); // Longitude expressed in 1E7 + i += put_int32_t_by_index(lat, i, msg->payload); // Latitude expressed in 1E7 + i += put_int32_t_by_index(alt, i, msg->payload); // Altitude expressed in milimeters return mavlink_finalize_message(msg, system_id, component_id, i); } @@ -36,15 +41,19 @@ static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t * @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 pos Position + * @param lon Longitude expressed in 1E7 + * @param lat Latitude expressed in 1E7 + * @param alt Altitude expressed in milimeters * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const double* pos) +static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, int32_t lon, int32_t lat, int32_t alt) { uint16_t i = 0; msg->msgid = MAVLINK_MSG_ID_OBS_POSITION; - i += put_array_by_index((const int8_t*)pos, sizeof(double)*3, i, msg->payload); // Position + i += put_int32_t_by_index(lon, i, msg->payload); // Longitude expressed in 1E7 + i += put_int32_t_by_index(lat, i, msg->payload); // Latitude expressed in 1E7 + i += put_int32_t_by_index(alt, i, msg->payload); // Altitude expressed in milimeters return mavlink_finalize_message_chan(msg, system_id, component_id, chan, i); } @@ -59,21 +68,23 @@ static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position) { - return mavlink_msg_obs_position_pack(system_id, component_id, msg, obs_position->pos); + return mavlink_msg_obs_position_pack(system_id, component_id, msg, obs_position->lon, obs_position->lat, obs_position->alt); } /** * @brief Send a obs_position message * @param chan MAVLink channel to send the message * - * @param pos Position + * @param lon Longitude expressed in 1E7 + * @param lat Latitude expressed in 1E7 + * @param alt Altitude expressed in milimeters */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, const double* pos) +static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, int32_t lon, int32_t lat, int32_t alt) { mavlink_message_t msg; - mavlink_msg_obs_position_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, pos); + mavlink_msg_obs_position_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg, lon, lat, alt); mavlink_send_uart(chan, &msg); } @@ -81,15 +92,48 @@ static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, const d // MESSAGE OBS_POSITION UNPACKING /** - * @brief Get field pos from obs_position message + * @brief Get field lon from obs_position message * - * @return Position + * @return Longitude expressed in 1E7 */ -static inline uint16_t mavlink_msg_obs_position_get_pos(const mavlink_message_t* msg, double* r_data) +static inline int32_t mavlink_msg_obs_position_get_lon(const mavlink_message_t* msg) { + generic_32bit r; + r.b[3] = (msg->payload)[0]; + r.b[2] = (msg->payload)[1]; + r.b[1] = (msg->payload)[2]; + r.b[0] = (msg->payload)[3]; + return (int32_t)r.i; +} - memcpy(r_data, msg->payload, sizeof(double)*3); - return sizeof(double)*3; +/** + * @brief Get field lat from obs_position message + * + * @return Latitude expressed in 1E7 + */ +static inline int32_t mavlink_msg_obs_position_get_lat(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int32_t))[0]; + r.b[2] = (msg->payload+sizeof(int32_t))[1]; + r.b[1] = (msg->payload+sizeof(int32_t))[2]; + r.b[0] = (msg->payload+sizeof(int32_t))[3]; + return (int32_t)r.i; +} + +/** + * @brief Get field alt from obs_position message + * + * @return Altitude expressed in milimeters + */ +static inline int32_t mavlink_msg_obs_position_get_alt(const mavlink_message_t* msg) +{ + generic_32bit r; + r.b[3] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[0]; + r.b[2] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[1]; + r.b[1] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[2]; + r.b[0] = (msg->payload+sizeof(int32_t)+sizeof(int32_t))[3]; + return (int32_t)r.i; } /** @@ -100,5 +144,7 @@ static inline uint16_t mavlink_msg_obs_position_get_pos(const mavlink_message_t* */ static inline void mavlink_msg_obs_position_decode(const mavlink_message_t* msg, mavlink_obs_position_t* obs_position) { - mavlink_msg_obs_position_get_pos(msg, obs_position->pos); + obs_position->lon = mavlink_msg_obs_position_get_lon(msg); + obs_position->lat = mavlink_msg_obs_position_get_lat(msg); + obs_position->alt = mavlink_msg_obs_position_get_alt(msg); } diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index c92dbf1ce..e7b084009 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 Friday, August 12 2011, 12:18 UTC + * Generated on Monday, August 15 2011, 15:40 UTC */ #ifndef COMMON_H #define COMMON_H diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 52a94a134..d7ddcf77a 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 Friday, August 12 2011, 12:18 UTC + * Generated on Monday, August 15 2011, 15:40 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/message_definitions/SenseSoar.xml b/thirdParty/mavlink/message_definitions/SenseSoar.xml index 4de96ac53..1f444d246 100644 --- a/thirdParty/mavlink/message_definitions/SenseSoar.xml +++ b/thirdParty/mavlink/message_definitions/SenseSoar.xml @@ -11,8 +11,10 @@ - Position estimate of the observer in NED inertial frame - Position + Position estimate of the observer in global frame + Longitude expressed in 1E7 + Latitude expressed in 1E7 + Altitude expressed in milimeters velocity estimate of the observer in NED inertial frame -- 2.22.0