Better rc override

comms/mavlink/mavlink.cpp

@@ -260,7 +260,7 @@ void Mavlink::send_sonar(uint8_t system_id,
   send_message(msg);
 }
 
-void Mavlink::send_status(uint8_t system_id, bool armed, bool failsafe, bool rc_override,
+void Mavlink::send_status(uint8_t system_id, bool armed, bool failsafe, uint16_t rc_override,
                           bool offboard, uint8_t error_code, uint8_t control_mode,
                           int16_t num_errors, int16_t loop_time_us)
 {

comms/mavlink/mavlink.h

@@ -79,7 +79,7 @@ class Mavlink : public CommLinkInterface
   void send_sonar(uint8_t system_id,
                   /* TODO enum type*/ uint8_t type, float range, float max_range,
                   float min_range) override;
-  void send_status(uint8_t system_id, bool armed, bool failsafe, bool rc_override, bool offboard,
+  void send_status(uint8_t system_id, bool armed, bool failsafe, uint16_t rc_override, bool offboard,
                    uint8_t error_code, uint8_t control_mode, int16_t num_errors,
                    int16_t loop_time_us) override;
   void send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1) override;

comms/mavlink/rosflight.xml

@@ -283,7 +283,7 @@ python3 -m pymavlink.tools.mavgen --lang=C --wire-protocol=1.0 --output=generate
 			<description>Flight Status</description>
 			<field type="uint8_t" name="armed"/>
 			<field type="uint8_t" name="failsafe"/>
-			<field type="uint8_t" name="rc_override"/>
+			<field type="uint16_t" name="rc_override"/>
 			<field type="uint8_t" name="offboard"/>
 			<field type="uint8_t" name="error_code" enum="ROSFLIGHT_ERROR_CODE"/>
 			<field type="uint8_t" name="control_mode" enum="OFFBOARD_CONTROL_MODE"/>

comms/mavlink/v1.0/checksum.h

@@ -23,7 +23,7 @@ extern "C" {
 
 #ifndef HAVE_CRC_ACCUMULATE
 /**
- * @brief Accumulate the MCRF4XX CRC16 by adding one char at a time.
+ * @brief Accumulate the X.25 CRC by adding one char at a time.
  *
  * The checksum function adds the hash of one char at a time to the
  * 16 bit checksum (uint16_t).
@@ -44,9 +44,9 @@ static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
 
 
 /**
- * @brief Initialize the buffer for the MCRF4XX CRC16
+ * @brief Initiliaze the buffer for the X.25 CRC
  *
- * @param crcAccum the 16 bit MCRF4XX CRC16
+ * @param crcAccum the 16 bit X.25 CRC
  */
 static inline void crc_init(uint16_t* crcAccum)
 {
@@ -55,7 +55,7 @@ static inline void crc_init(uint16_t* crcAccum)
 
 
 /**
- * @brief Calculates the MCRF4XX CRC16 checksum on a byte buffer
+ * @brief Calculates the X.25 checksum on a byte buffer
  *
  * @param  pBuffer buffer containing the byte array to hash
  * @param  length  length of the byte array
@@ -73,7 +73,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length)
 
 
 /**
- * @brief Accumulate the MCRF4XX CRC16 CRC by adding an array of bytes
+ * @brief Accumulate the X.25 CRC by adding an array of bytes
  *
  * The checksum function adds the hash of one char at a time to the
  * 16 bit checksum (uint16_t).

comms/mavlink/v1.0/mavlink_conversions.h

@@ -1,8 +1,6 @@
 #ifndef  _MAVLINK_CONVERSIONS_H_
 #define  _MAVLINK_CONVERSIONS_H_
 
-#ifndef MAVLINK_NO_CONVERSION_HELPERS
-
 /* enable math defines on Windows */
 #ifdef _MSC_VER
 #ifndef _USE_MATH_DEFINES
@@ -210,7 +208,4 @@ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, flo
     dcm[2][2] = cosPhi * cosThe;
 }
 
-#endif // MAVLINK_NO_CONVERSION_HELPERS
-
-#endif // _MAVLINK_CONVERSIONS_H_
-
+#endif

comms/mavlink/v1.0/mavlink_helpers.h

@@ -52,46 +52,6 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
 	status->parse_state = MAVLINK_PARSE_STATE_IDLE;
 }
 
-/**
- * @brief Finalize a MAVLink message with channel assignment
- *
- * This function calculates the checksum and sets length and aircraft id correctly.
- * It assumes that the message id and the payload are already correctly set. This function
- * can also be used if the message header has already been written before (as in mavlink_msg_xxx_pack
- * instead of mavlink_msg_xxx_pack_headerless), it just introduces little extra overhead.
- *
- * @param msg Message to finalize
- * @param system_id Id of the sending (this) system, 1-127
- * @param length Message length
- */
-#if MAVLINK_CRC_EXTRA
-MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
-                                                        mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra)
-#else
-MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
-                                                        mavlink_status_t* status, uint8_t length)
-#endif
-{
-	// This is only used for the v2 protocol and we silence it here
-	(void)min_length;
-	// This code part is the same for all messages;
-	msg->magic = MAVLINK_STX;
-	msg->len = length;
-	msg->sysid = system_id;
-	msg->compid = component_id;
-    msg->seq = status->current_tx_seq;
-    status->current_tx_seq = status->current_tx_seq+1;
-	msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
-	crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
-#if MAVLINK_CRC_EXTRA
-	crc_accumulate(crc_extra, &msg->checksum);
-#endif
-	mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
-	mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
-
-	return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-}
-
 /**
  * @brief Finalize a MAVLink message with channel assignment
  *
@@ -106,14 +66,12 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg,
  */
 #if MAVLINK_CRC_EXTRA
 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						      uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
+						      uint8_t chan, uint8_t length, uint8_t crc_extra)
 #else
 MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
 						      uint8_t chan, uint8_t length)
 #endif
 {
-    // This is only used for the v2 protocol and we silence it here
-	(void)min_length;
 	// This code part is the same for all messages;
 	msg->magic = MAVLINK_STX;
 	msg->len = length;
@@ -139,9 +97,9 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
  */
 #if MAVLINK_CRC_EXTRA
 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
-						 uint8_t min_length, uint8_t length, uint8_t crc_extra)
+						 uint8_t length, uint8_t crc_extra)
 {
-    return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
+	return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length, crc_extra);
 }
 #else
 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 
@@ -159,7 +117,7 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf,
  */
 #if MAVLINK_CRC_EXTRA
 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, 
-						    uint8_t min_length, uint8_t length, uint8_t crc_extra)
+						    uint8_t length, uint8_t crc_extra)
 #else
 MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
 #endif
@@ -246,17 +204,38 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
 }
 
 /**
- * This is a variant of mavlink_frame_char() but with caller supplied
+ * This is a varient of mavlink_frame_char() but with caller supplied
  * parsing buffers. It is useful when you want to create a MAVLink
  * parser in a library that doesn't use any global variables
  *
  * @param rxmsg    parsing message buffer
- * @param status   parsing status buffer 
+ * @param status   parsing starus buffer 
  * @param c        The char to parse
  *
- * @param r_message NULL if no message could be decoded, otherwise the message data
- * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats
+ * @param returnMsg NULL if no message could be decoded, the message data else
+ * @param returnStats if a message was decoded, this is filled with the channel's stats
  * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
+ *
+ * A typical use scenario of this function call is:
+ *
+ * @code
+ * #include <mavlink.h>
+ *
+ * mavlink_message_t msg;
+ * int chan = 0;
+ *
+ *
+ * while(serial.bytesAvailable > 0)
+ * {
+ *   uint8_t byte = serial.getNextByte();
+ *   if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
+ *     {
+ *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
+ *     }
+ * }
+ *
+ *
+ * @endcode
  */
 MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, 
                                                  mavlink_status_t* status,
@@ -288,6 +267,8 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
 #endif
 #endif
 
+	int bufferIndex = 0;
+
 	status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
 
 	switch (status->parse_state)
@@ -402,7 +383,8 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
 		break;
 	}
 
-	// If a message has been successfully decoded, check index
+	bufferIndex++;
+	// If a message has been sucessfully decoded, check index
 	if (status->msg_received == MAVLINK_FRAMING_OK)
 	{
 		//while(status->current_seq != rxmsg->seq)
@@ -446,33 +428,32 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
  * 2 (MAVLINK_FRAMING_INCOMPLETE, MAVLINK_FRAMING_OK or MAVLINK_FRAMING_BAD_CRC)
  *
  * Messages are parsed into an internal buffer (one for each channel). When a complete
- * message is received it is copies into *r_message and the channel's status is
- * copied into *r_mavlink_status.
+ * message is received it is copies into *returnMsg and the channel's status is
+ * copied into *returnStats.
  *
- * @param chan     ID of the channel to be parsed.
- *                 A channel is not a physical message channel like a serial port, but a logical partition of
- *                 the communication streams. COMM_NB is the limit for the number of channels
+ * @param chan     ID of the current channel. This allows to parse different channels with this function.
+ *                 a channel is not a physical message channel like a serial port, but a logic partition of
+ *                 the communication streams in this case. COMM_NB is the limit for the number of channels
  *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
  * @param c        The char to parse
  *
- * @param r_message NULL if no message could be decoded, the message data else
- * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats
+ * @param returnMsg NULL if no message could be decoded, the message data else
+ * @param returnStats if a message was decoded, this is filled with the channel's stats
  * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC
  *
  * A typical use scenario of this function call is:
  *
  * @code
  * #include <mavlink.h>
  *
- * mavlink_status_t status;
  * mavlink_message_t msg;
  * int chan = 0;
  *
  *
  * while(serial.bytesAvailable > 0)
  * {
  *   uint8_t byte = serial.getNextByte();
- *   if (mavlink_frame_char(chan, byte, &msg, &status) != MAVLINK_FRAMING_INCOMPLETE)
+ *   if (mavlink_frame_char(chan, byte, &msg) != MAVLINK_FRAMING_INCOMPLETE)
  *     {
  *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
  *     }
@@ -497,33 +478,32 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa
  * it could be successfully decoded. This function will return 0 or 1.
  *
  * Messages are parsed into an internal buffer (one for each channel). When a complete
- * message is received it is copies into *r_message and the channel's status is
- * copied into *r_mavlink_status.
+ * message is received it is copies into *returnMsg and the channel's status is
+ * copied into *returnStats.
  *
- * @param chan     ID of the channel to be parsed.
- *                 A channel is not a physical message channel like a serial port, but a logical partition of
- *                 the communication streams. COMM_NB is the limit for the number of channels
+ * @param chan     ID of the current channel. This allows to parse different channels with this function.
+ *                 a channel is not a physical message channel like a serial port, but a logic partition of
+ *                 the communication streams in this case. COMM_NB is the limit for the number of channels
  *                 on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows
  * @param c        The char to parse
  *
- * @param r_message NULL if no message could be decoded, otherwise the message data.
- * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats
+ * @param returnMsg NULL if no message could be decoded, the message data else
+ * @param returnStats if a message was decoded, this is filled with the channel's stats
  * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC
  *
  * A typical use scenario of this function call is:
  *
  * @code
  * #include <mavlink.h>
  *
- * mavlink_status_t status;
  * mavlink_message_t msg;
  * int chan = 0;
  *
  *
  * while(serial.bytesAvailable > 0)
  * {
  *   uint8_t byte = serial.getNextByte();
- *   if (mavlink_parse_char(chan, byte, &msg, &status))
+ *   if (mavlink_parse_char(chan, byte, &msg))
  *     {
  *     printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid);
  *     }

comms/mavlink/v1.0/mavlink_types.h

@@ -170,14 +170,12 @@ typedef struct __mavlink_message_info {
 #define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
 #define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg))
 
-#ifndef HAVE_MAVLINK_CHANNEL_T
 typedef enum {
     MAVLINK_COMM_0,
     MAVLINK_COMM_1,
     MAVLINK_COMM_2,
     MAVLINK_COMM_3
 } mavlink_channel_t;
-#endif
 
 /*
  * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number

comms/mavlink/v1.0/protocol.h

@@ -45,23 +45,19 @@
     #endif
     MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
     #if MAVLINK_CRC_EXTRA
-    MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
-                                                            mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra);
     MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
-                                                          uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra);
+                                  uint8_t chan, uint8_t length, uint8_t crc_extra);
     MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
-                                                     uint8_t min_length, uint8_t length, uint8_t crc_extra);
+                             uint8_t length, uint8_t crc_extra);
     #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
     MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
-                                                        uint8_t min_length, uint8_t length, uint8_t crc_extra);
+                                uint8_t length, uint8_t crc_extra);
     #endif
     #else
-    MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
-                                                            mavlink_status_t* status, uint8_t min_length, uint8_t length);
     MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
-                                                          uint8_t chan, uint8_t length);
+                                  uint8_t chan, uint8_t length);
     MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
-                                                     uint8_t length);
+                             uint8_t length);
     #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
     MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length);
     #endif
@@ -243,9 +239,9 @@ _MAV_PUT_ARRAY(int64_t,  i64)
 _MAV_PUT_ARRAY(float,    f)
 _MAV_PUT_ARRAY(double,   d)
 
-#define _MAV_RETURN_char(msg, wire_offset)             (char)_MAV_PAYLOAD(msg)[wire_offset]
-#define _MAV_RETURN_int8_t(msg, wire_offset)   (int8_t)_MAV_PAYLOAD(msg)[wire_offset]
-#define _MAV_RETURN_uint8_t(msg, wire_offset) (uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
+#define _MAV_RETURN_char(msg, wire_offset)             (const char)_MAV_PAYLOAD(msg)[wire_offset]
+#define _MAV_RETURN_int8_t(msg, wire_offset)   (const int8_t)_MAV_PAYLOAD(msg)[wire_offset]
+#define _MAV_RETURN_uint8_t(msg, wire_offset) (const uint8_t)_MAV_PAYLOAD(msg)[wire_offset]
 
 #if MAVLINK_NEED_BYTE_SWAP
 #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \

comms/mavlink/v1.0/rosflight/mavlink.h

@@ -1,13 +1,10 @@
 /** @file
- *  @brief MAVLink comm protocol built from rosflight.xml
- *  @see http://mavlink.org
+ *	@brief MAVLink comm protocol built from rosflight.xml
+ *	@see http://mavlink.org
  */
-#pragma once
 #ifndef MAVLINK_H
 #define MAVLINK_H
 
-#define MAVLINK_PRIMARY_XML_HASH 9006179629977798615
-
 #ifndef MAVLINK_STX
 #define MAVLINK_STX 254
 #endif
@@ -24,10 +21,6 @@
 #define MAVLINK_CRC_EXTRA 1
 #endif
 
-#ifndef MAVLINK_COMMAND_24BIT
-#define MAVLINK_COMMAND_24BIT 0
-#endif
-
 #include "version.h"
 #include "rosflight.h"