diff --git a/examples/dup_samples.py b/examples/dup_samples.py new file mode 100755 index 000000000..ce699d910 --- /dev/null +++ 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 index 0634f25d7..1d9a81bf1 100644 --- 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 index 3d82a4dff..73a713304 100644 --- 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 index 99a1d5b6c..ecf4a0432 100644 --- 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 index b36f51e42..75c75d078 100644 --- 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 index 1b931700a..152edec36 100644 --- 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 index 1832be9d3..3e28d8fd0 100644 --- 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 index 829f08609..d11afb31d 100644 --- 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 index 45447cd75..91c6d3336 100644 --- 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',