Merge branch 'ArduPilot:master' into simon-dev

shancock884 · Oct 3, 2023 · 76639e9 · 76639e9
2 parents 6c4c9c3 + f508300
commit 76639e9
Show file tree

Hide file tree

Showing 9 changed files with 154 additions and 2 deletions.
diff --git a/examples/dup_samples.py b/examples/dup_samples.py
@@ -0,0 +1,46 @@
+#!/usr/bin/env python3
+
+from argparse import ArgumentParser
+parser = ArgumentParser()
+parser.add_argument("log")
+
+args = parser.parse_args()
+
+from pymavlink import mavutil
+
+def process(logfile):
+    '''look for duplicate raw gyro samples'''
+    mlog = mavutil.mavlink_connection(logfile)
+
+    last_gyr = {}
+    dup_count = {}
+    total_dup = {}
+    while True:
+        m = mlog.recv_match(type='GYR')
+        if m is None:
+            break
+        if not m.I in last_gyr:
+            last_gyr[m.I] = m
+            dup_count[m.I] = 0
+            total_dup[m.I] = 0
+            continue
+        axes = 0
+        if last_gyr[m.I].GyrX == m.GyrX and abs(m.GyrX) >= 1:
+            axes |= 1
+        if last_gyr[m.I].GyrY == m.GyrY and abs(m.GyrY) >= 1:
+            axes |= 2
+        if last_gyr[m.I].GyrX == m.GyrZ and abs(m.GyrZ) >= 1:
+            axes |= 4
+        if axes != 0:
+            if dup_count[m.I] == 0:
+                print("%s" % str(last_gyr[m.I]))
+            dup_count[m.I] += 1
+            total_dup[m.I] += 1
+            print("%s dup=%u axes=%u" % (str(m), dup_count[m.I], axes))
+        else:
+            dup_count[m.I] = 0
+        last_gyr[m.I] = m
+    print(total_dup)
+
+process(args.log)
+
diff --git a/generator/C/include_v0.9/protocol.h b/generator/C/include_v0.9/protocol.h
@@ -44,6 +44,8 @@
 /* always include the prototypes to ensure we don't get out of sync */
 MAVLINK_HELPER mavlink_status_t* mavlink_get_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 length, uint8_t crc_extra);
 MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, 

diff --git a/generator/C/include_v1.0/mavlink_helpers.h b/generator/C/include_v1.0/mavlink_helpers.h
@@ -52,6 +52,46 @@ 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
  *
@@ -72,7 +112,7 @@ MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, ui
 						      uint8_t chan, uint8_t length)
 #endif
 {
-	// This is only used for the v2 protocol and we silence it here
+    // 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;

diff --git a/generator/C/include_v1.0/protocol.h b/generator/C/include_v1.0/protocol.h
@@ -45,6 +45,8 @@
     #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);
     MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
@@ -54,6 +56,8 @@
                                                         uint8_t min_length, 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);
     MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,

diff --git a/generator/C/include_v2.0/mavlink_helpers.h b/generator/C/include_v2.0/mavlink_helpers.h
@@ -707,7 +707,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
 		break;
 
 	case MAVLINK_PARSE_STATE_GOT_MSGID1:
-		rxmsg->msgid |= c<<8;
+		rxmsg->msgid |= ((uint32_t)c)<<8;
 		mavlink_update_checksum(rxmsg, c);
 		status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;
 		break;

diff --git a/generator/C/include_v2.0/protocol.h b/generator/C/include_v2.0/protocol.h
@@ -43,6 +43,8 @@
     MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
     #endif
     MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
+    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);
     MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,

diff --git a/generator/mavgen_c.py b/generator/mavgen_c.py
@@ -241,6 +241,44 @@ def generate_message_h(directory, m):
     return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_${name}_MIN_LEN, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
 }
 
+/**
+ * @brief Pack a ${name_lower} message
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param status MAVLink status structure
+ * @param msg The MAVLink message to compress the data into
+ *
+${{arg_fields: * @param ${name} ${units} ${description}
+}}
+ * @return length of the message in bytes (excluding serial stream start sign)
+ */
+static inline uint16_t mavlink_msg_${name_lower}_pack_status(uint8_t system_id, uint8_t component_id, mavlink_status_t *_status, mavlink_message_t* msg,
+                              ${{arg_fields: ${array_const}${type} ${array_prefix}${name},}})
+{
+#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS
+    char buf[MAVLINK_MSG_ID_${name}_LEN];
+${{scalar_fields:    _mav_put_${type}(buf, ${wire_offset}, ${putname});
+}}
+${{array_fields:    _mav_put_${type}_array(buf, ${wire_offset}, ${name}, ${array_length});
+}}
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_${name}_LEN);
+#else
+    mavlink_${name_lower}_t packet;
+${{scalar_fields:    packet.${name} = ${putname};
+}}
+${{array_fields:    mav_array_memcpy(packet.${name}, ${name}, sizeof(${type})*${array_length});
+}}
+        memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_${name}_LEN);
+#endif
+
+    msg->msgid = MAVLINK_MSG_ID_${name};
+#if MAVLINK_CRC_EXTRA
+    return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_${name}_MIN_LEN, MAVLINK_MSG_ID_${name}_LEN, MAVLINK_MSG_ID_${name}_CRC);
+#else
+    return mavlink_finalize_message_buffer(msg, system_id, component_id, _status, MAVLINK_MSG_ID_${name}_MIN_LEN, MAVLINK_MSG_ID_${name}_LEN);
+#endif
+}
+
 /**
  * @brief Pack a ${name_lower} message on a channel
  * @param system_id ID of this system
@@ -302,6 +340,20 @@ def generate_message_h(directory, m):
     return mavlink_msg_${name_lower}_pack_chan(system_id, component_id, chan, msg,${{arg_fields: ${name_lower}->${name},}});
 }
 
+/**
+ * @brief Encode a ${name_lower} struct with provided status structure
+ *
+ * @param system_id ID of this system
+ * @param component_id ID of this component (e.g. 200 for IMU)
+ * @param status MAVLink status structure
+ * @param msg The MAVLink message to compress the data into
+ * @param ${name_lower} C-struct to read the message contents from
+ */
+static inline uint16_t mavlink_msg_${name_lower}_encode_status(uint8_t system_id, uint8_t component_id, mavlink_status_t* _status, mavlink_message_t* msg, const mavlink_${name_lower}_t* ${name_lower})
+{
+    return mavlink_msg_${name_lower}_pack_status(system_id, component_id, _status, msg, ${{arg_fields: ${name_lower}->${name},}});
+}
+
 /**
  * @brief Send a ${name_lower} message
  * @param chan MAVLink channel to send the message

diff --git a/mavextra.py b/mavextra.py
@@ -1657,3 +1657,8 @@ def mm_curr(RCOU,BAT,PWM_MIN,PWM_MAX,Mfirst,Mlast):
         command = voltage*max(pwm - PWM_MIN,0)/(PWM_MAX-PWM_MIN)
         total_curr += command**2
     return total_curr
+
+def RotateMag(MAG,rotation):
+    '''rotate a MAG message by rotation enumeration'''
+    v = Vector3(MAG.MagX,MAG.MagY,MAG.MagZ)
+    return v.rotate_by_id(rotation)
diff --git a/mavutil.py b/mavutil.py
@@ -2059,6 +2059,7 @@ def mode_string_v09(msg):
     6 : 'FOLLOW',
     7 : 'SIMPLE',
     8 : 'DOCK',
+    9 : 'CIRCLE',
     10 : 'AUTO',
     11 : 'RTL',
     12 : 'SMART_RTL',