diff --git a/mavlink_lib.py b/mavlink_lib.py index 01285d232d1c367201367c667665f1165c16939e..6023b3c554cf86f41f54466e0a497a769a801d28 100644 --- a/mavlink_lib.py +++ b/mavlink_lib.py @@ -276,8 +276,10 @@ MAV_SYSID_RIG = 3 # enums['SysIDs'][3] = EnumEntry('MAV_SYSID_RIG', '''''') MAV_SYSID_GS = 4 # enums['SysIDs'][4] = EnumEntry('MAV_SYSID_GS', '''''') -SysIDs_ENUM_END = 5 # -enums['SysIDs'][5] = EnumEntry('SysIDs_ENUM_END', '''''') +MAV_SYSID_ARP = 5 # +enums['SysIDs'][5] = EnumEntry('MAV_SYSID_ARP', '''''') +SysIDs_ENUM_END = 6 # +enums['SysIDs'][6] = EnumEntry('SysIDs_ENUM_END', '''''') # SystemTMList enums['SystemTMList'] = {} @@ -430,8 +432,16 @@ MAV_CMD_REGISTRY_CLEAR = 21 # Command to clear the registry enums['MavCommandList'][21] = EnumEntry('MAV_CMD_REGISTRY_CLEAR', '''Command to clear the registry''') MAV_CMD_ENTER_HIL = 22 # Command to enter HIL mode at next reboot enums['MavCommandList'][22] = EnumEntry('MAV_CMD_ENTER_HIL', '''Command to enter HIL mode at next reboot''') -MavCommandList_ENUM_END = 23 # -enums['MavCommandList'][23] = EnumEntry('MavCommandList_ENUM_END', '''''') +MAV_CMD_RESET_ALGORITHM = 23 # Command to reset the set algorithm. Used for now in ARP to reset the + # follow algorithm and return to armed state. +enums['MavCommandList'][23] = EnumEntry('MAV_CMD_RESET_ALGORITHM', '''Command to reset the set algorithm. Used for now in ARP to reset the follow algorithm and return to armed state.''') +MAV_CMD_ARP_FORCE_NO_FEEDBACK = 24 # Command to force ARP system to entern the no feedback from VN300 state +enums['MavCommandList'][24] = EnumEntry('MAV_CMD_ARP_FORCE_NO_FEEDBACK', '''Command to force ARP system to entern the no feedback from VN300 state''') +MAV_CMD_ARP_FOLLOW = 25 # Command to enter in the ARP's follow state and procedure to follow the + # rocket +enums['MavCommandList'][25] = EnumEntry('MAV_CMD_ARP_FOLLOW', '''Command to enter in the ARP's follow state and procedure to follow the rocket''') +MavCommandList_ENUM_END = 26 # +enums['MavCommandList'][26] = EnumEntry('MavCommandList_ENUM_END', '''''') # ServosList enums['ServosList'] = {} @@ -2154,28 +2164,29 @@ class MAVLink_arp_tm_message(MAVLink_message): ''' id = MAVLINK_MSG_ID_ARP_TM name = 'ARP_TM' - fieldnames = ['timestamp', 'yaw', 'pitch', 'roll', 'target_yaw', 'target_pitch', 'stepperX_pos', 'stepperX_delta', 'stepperX_speed', 'stepperY_pos', 'stepperY_delta', 'stepperY_speed', 'gps_latitude', 'gps_longitude', 'gps_height', 'gps_fix', 'main_radio_present', 'main_packet_tx_error_count', 'main_tx_bitrate', 'main_packet_rx_success_count', 'main_packet_rx_drop_count', 'main_rx_bitrate', 'main_rx_rssi', 'payload_radio_present', 'payload_packet_tx_error_count', 'payload_tx_bitrate', 'payload_packet_rx_success_count', 'payload_packet_rx_drop_count', 'payload_rx_bitrate', 'payload_rx_rssi', 'ethernet_present', 'ethernet_status', 'battery_voltage'] - ordered_fieldnames = ['timestamp', 'yaw', 'pitch', 'roll', 'target_yaw', 'target_pitch', 'stepperX_pos', 'stepperX_delta', 'stepperX_speed', 'stepperY_pos', 'stepperY_delta', 'stepperY_speed', 'gps_latitude', 'gps_longitude', 'gps_height', 'main_rx_rssi', 'payload_rx_rssi', 'battery_voltage', 'main_packet_tx_error_count', 'main_tx_bitrate', 'main_packet_rx_success_count', 'main_packet_rx_drop_count', 'main_rx_bitrate', 'payload_packet_tx_error_count', 'payload_tx_bitrate', 'payload_packet_rx_success_count', 'payload_packet_rx_drop_count', 'payload_rx_bitrate', 'gps_fix', 'main_radio_present', 'payload_radio_present', 'ethernet_present', 'ethernet_status'] - fieldtypes = ['uint64_t', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'uint8_t', 'uint8_t', 'uint16_t', 'uint16_t', 'uint16_t', 'uint16_t', 'uint16_t', 'float', 'uint8_t', 'uint16_t', 'uint16_t', 'uint16_t', 'uint16_t', 'uint16_t', 'float', 'uint8_t', 'uint8_t', 'float'] + fieldnames = ['timestamp', 'state', 'yaw', 'pitch', 'roll', 'target_yaw', 'target_pitch', 'stepperX_pos', 'stepperX_delta', 'stepperX_speed', 'stepperY_pos', 'stepperY_delta', 'stepperY_speed', 'gps_latitude', 'gps_longitude', 'gps_height', 'gps_fix', 'main_radio_present', 'main_packet_tx_error_count', 'main_tx_bitrate', 'main_packet_rx_success_count', 'main_packet_rx_drop_count', 'main_rx_bitrate', 'main_rx_rssi', 'payload_radio_present', 'payload_packet_tx_error_count', 'payload_tx_bitrate', 'payload_packet_rx_success_count', 'payload_packet_rx_drop_count', 'payload_rx_bitrate', 'payload_rx_rssi', 'ethernet_present', 'ethernet_status', 'battery_voltage', 'log_number'] + ordered_fieldnames = ['timestamp', 'yaw', 'pitch', 'roll', 'target_yaw', 'target_pitch', 'stepperX_pos', 'stepperX_delta', 'stepperX_speed', 'stepperY_pos', 'stepperY_delta', 'stepperY_speed', 'gps_latitude', 'gps_longitude', 'gps_height', 'main_rx_rssi', 'payload_rx_rssi', 'battery_voltage', 'main_packet_tx_error_count', 'main_tx_bitrate', 'main_packet_rx_success_count', 'main_packet_rx_drop_count', 'main_rx_bitrate', 'payload_packet_tx_error_count', 'payload_tx_bitrate', 'payload_packet_rx_success_count', 'payload_packet_rx_drop_count', 'payload_rx_bitrate', 'log_number', 'state', 'gps_fix', 'main_radio_present', 'payload_radio_present', 'ethernet_present', 'ethernet_status'] + fieldtypes = ['uint64_t', 'uint8_t', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'float', 'uint8_t', 'uint8_t', 'uint16_t', 'uint16_t', 'uint16_t', 'uint16_t', 'uint16_t', 'float', 'uint8_t', 'uint16_t', 'uint16_t', 'uint16_t', 'uint16_t', 'uint16_t', 'float', 'uint8_t', 'uint8_t', 'float', 'int16_t'] fielddisplays_by_name = {} fieldenums_by_name = {} fieldunits_by_name = {"timestamp": "us", "yaw": "deg", "pitch": "deg", "roll": "deg", "target_yaw": "deg", "target_pitch": "deg", "stepperX_pos": "deg", "stepperX_delta": "deg", "stepperX_speed": "rps", "stepperY_pos": "deg", "stepperY_delta": "deg", "stepperY_speed": "rps", "gps_latitude": "deg", "gps_longitude": "deg", "gps_height": "m", "main_tx_bitrate": "b/s", "main_rx_bitrate": "b/s", "main_rx_rssi": "dBm", "payload_tx_bitrate": "b/s", "payload_rx_bitrate": "b/s", "payload_rx_rssi": "dBm", "battery_voltage": "V"} - format = '<QfffffffffffffffffHHHHHHHHHHBBBBB' - native_format = bytearray('<QfffffffffffffffffHHHHHHHHHHBBBBB', 'ascii') - orders = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 28, 29, 18, 19, 20, 21, 22, 15, 30, 23, 24, 25, 26, 27, 16, 31, 32, 17] - lengths = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] - array_lengths = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] - crc_extra = 22 - unpacker = struct.Struct('<QfffffffffffffffffHHHHHHHHHHBBBBB') + format = '<QfffffffffffffffffHHHHHHHHHHhBBBBBB' + native_format = bytearray('<QfffffffffffffffffHHHHHHHHHHhBBBBBB', 'ascii') + orders = [0, 29, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 30, 31, 18, 19, 20, 21, 22, 15, 32, 23, 24, 25, 26, 27, 16, 33, 34, 17, 28] + lengths = [1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1] + array_lengths = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + crc_extra = 239 + unpacker = struct.Struct('<QfffffffffffffffffHHHHHHHHHHhBBBBBB') instance_field = None instance_offset = -1 - def __init__(self, timestamp, yaw, pitch, roll, target_yaw, target_pitch, stepperX_pos, stepperX_delta, stepperX_speed, stepperY_pos, stepperY_delta, stepperY_speed, gps_latitude, gps_longitude, gps_height, gps_fix, main_radio_present, main_packet_tx_error_count, main_tx_bitrate, main_packet_rx_success_count, main_packet_rx_drop_count, main_rx_bitrate, main_rx_rssi, payload_radio_present, payload_packet_tx_error_count, payload_tx_bitrate, payload_packet_rx_success_count, payload_packet_rx_drop_count, payload_rx_bitrate, payload_rx_rssi, ethernet_present, ethernet_status, battery_voltage): + def __init__(self, timestamp, state, yaw, pitch, roll, target_yaw, target_pitch, stepperX_pos, stepperX_delta, stepperX_speed, stepperY_pos, stepperY_delta, stepperY_speed, gps_latitude, gps_longitude, gps_height, gps_fix, main_radio_present, main_packet_tx_error_count, main_tx_bitrate, main_packet_rx_success_count, main_packet_rx_drop_count, main_rx_bitrate, main_rx_rssi, payload_radio_present, payload_packet_tx_error_count, payload_tx_bitrate, payload_packet_rx_success_count, payload_packet_rx_drop_count, payload_rx_bitrate, payload_rx_rssi, ethernet_present, ethernet_status, battery_voltage, log_number): MAVLink_message.__init__(self, MAVLink_arp_tm_message.id, MAVLink_arp_tm_message.name) self._fieldnames = MAVLink_arp_tm_message.fieldnames self._instance_field = MAVLink_arp_tm_message.instance_field self._instance_offset = MAVLink_arp_tm_message.instance_offset self.timestamp = timestamp + self.state = state self.yaw = yaw self.pitch = pitch self.roll = roll @@ -2208,9 +2219,10 @@ class MAVLink_arp_tm_message(MAVLink_message): self.ethernet_present = ethernet_present self.ethernet_status = ethernet_status self.battery_voltage = battery_voltage + self.log_number = log_number def pack(self, mav, force_mavlink1=False): - return MAVLink_message.pack(self, mav, 22, struct.pack('<QfffffffffffffffffHHHHHHHHHHBBBBB', self.timestamp, self.yaw, self.pitch, self.roll, self.target_yaw, self.target_pitch, self.stepperX_pos, self.stepperX_delta, self.stepperX_speed, self.stepperY_pos, self.stepperY_delta, self.stepperY_speed, self.gps_latitude, self.gps_longitude, self.gps_height, self.main_rx_rssi, self.payload_rx_rssi, self.battery_voltage, self.main_packet_tx_error_count, self.main_tx_bitrate, self.main_packet_rx_success_count, self.main_packet_rx_drop_count, self.main_rx_bitrate, self.payload_packet_tx_error_count, self.payload_tx_bitrate, self.payload_packet_rx_success_count, self.payload_packet_rx_drop_count, self.payload_rx_bitrate, self.gps_fix, self.main_radio_present, self.payload_radio_present, self.ethernet_present, self.ethernet_status), force_mavlink1=force_mavlink1) + return MAVLink_message.pack(self, mav, 239, struct.pack('<QfffffffffffffffffHHHHHHHHHHhBBBBBB', self.timestamp, self.yaw, self.pitch, self.roll, self.target_yaw, self.target_pitch, self.stepperX_pos, self.stepperX_delta, self.stepperX_speed, self.stepperY_pos, self.stepperY_delta, self.stepperY_speed, self.gps_latitude, self.gps_longitude, self.gps_height, self.main_rx_rssi, self.payload_rx_rssi, self.battery_voltage, self.main_packet_tx_error_count, self.main_tx_bitrate, self.main_packet_rx_success_count, self.main_packet_rx_drop_count, self.main_rx_bitrate, self.payload_packet_tx_error_count, self.payload_tx_bitrate, self.payload_packet_rx_success_count, self.payload_packet_rx_drop_count, self.payload_rx_bitrate, self.log_number, self.state, self.gps_fix, self.main_radio_present, self.payload_radio_present, self.ethernet_present, self.ethernet_status), force_mavlink1=force_mavlink1) class MAVLink_sys_tm_message(MAVLink_message): ''' @@ -4443,11 +4455,12 @@ class MAVLink(object): ''' return self.send(self.registry_coord_tm_encode(timestamp, key_id, key_name, latitude, longitude), force_mavlink1=force_mavlink1) - def arp_tm_encode(self, timestamp, yaw, pitch, roll, target_yaw, target_pitch, stepperX_pos, stepperX_delta, stepperX_speed, stepperY_pos, stepperY_delta, stepperY_speed, gps_latitude, gps_longitude, gps_height, gps_fix, main_radio_present, main_packet_tx_error_count, main_tx_bitrate, main_packet_rx_success_count, main_packet_rx_drop_count, main_rx_bitrate, main_rx_rssi, payload_radio_present, payload_packet_tx_error_count, payload_tx_bitrate, payload_packet_rx_success_count, payload_packet_rx_drop_count, payload_rx_bitrate, payload_rx_rssi, ethernet_present, ethernet_status, battery_voltage): + def arp_tm_encode(self, timestamp, state, yaw, pitch, roll, target_yaw, target_pitch, stepperX_pos, stepperX_delta, stepperX_speed, stepperY_pos, stepperY_delta, stepperY_speed, gps_latitude, gps_longitude, gps_height, gps_fix, main_radio_present, main_packet_tx_error_count, main_tx_bitrate, main_packet_rx_success_count, main_packet_rx_drop_count, main_rx_bitrate, main_rx_rssi, payload_radio_present, payload_packet_tx_error_count, payload_tx_bitrate, payload_packet_rx_success_count, payload_packet_rx_drop_count, payload_rx_bitrate, payload_rx_rssi, ethernet_present, ethernet_status, battery_voltage, log_number): ''' timestamp : Timestamp [us] (type:uint64_t) + state : State Machine Controller State (type:uint8_t) yaw : Current Yaw [deg] (type:float) pitch : Current Pitch [deg] (type:float) roll : Current Roll [deg] (type:float) @@ -4480,15 +4493,17 @@ class MAVLink(object): ethernet_present : Boolean indicating the presence of the ethernet module (type:uint8_t) ethernet_status : Status flag indicating the status of the ethernet PHY (type:uint8_t) battery_voltage : Battery voltage [V] (type:float) + log_number : Currently active log file, -1 if the logger is inactive (type:int16_t) ''' - return MAVLink_arp_tm_message(timestamp, yaw, pitch, roll, target_yaw, target_pitch, stepperX_pos, stepperX_delta, stepperX_speed, stepperY_pos, stepperY_delta, stepperY_speed, gps_latitude, gps_longitude, gps_height, gps_fix, main_radio_present, main_packet_tx_error_count, main_tx_bitrate, main_packet_rx_success_count, main_packet_rx_drop_count, main_rx_bitrate, main_rx_rssi, payload_radio_present, payload_packet_tx_error_count, payload_tx_bitrate, payload_packet_rx_success_count, payload_packet_rx_drop_count, payload_rx_bitrate, payload_rx_rssi, ethernet_present, ethernet_status, battery_voltage) + return MAVLink_arp_tm_message(timestamp, state, yaw, pitch, roll, target_yaw, target_pitch, stepperX_pos, stepperX_delta, stepperX_speed, stepperY_pos, stepperY_delta, stepperY_speed, gps_latitude, gps_longitude, gps_height, gps_fix, main_radio_present, main_packet_tx_error_count, main_tx_bitrate, main_packet_rx_success_count, main_packet_rx_drop_count, main_rx_bitrate, main_rx_rssi, payload_radio_present, payload_packet_tx_error_count, payload_tx_bitrate, payload_packet_rx_success_count, payload_packet_rx_drop_count, payload_rx_bitrate, payload_rx_rssi, ethernet_present, ethernet_status, battery_voltage, log_number) - def arp_tm_send(self, timestamp, yaw, pitch, roll, target_yaw, target_pitch, stepperX_pos, stepperX_delta, stepperX_speed, stepperY_pos, stepperY_delta, stepperY_speed, gps_latitude, gps_longitude, gps_height, gps_fix, main_radio_present, main_packet_tx_error_count, main_tx_bitrate, main_packet_rx_success_count, main_packet_rx_drop_count, main_rx_bitrate, main_rx_rssi, payload_radio_present, payload_packet_tx_error_count, payload_tx_bitrate, payload_packet_rx_success_count, payload_packet_rx_drop_count, payload_rx_bitrate, payload_rx_rssi, ethernet_present, ethernet_status, battery_voltage, force_mavlink1=False): + def arp_tm_send(self, timestamp, state, yaw, pitch, roll, target_yaw, target_pitch, stepperX_pos, stepperX_delta, stepperX_speed, stepperY_pos, stepperY_delta, stepperY_speed, gps_latitude, gps_longitude, gps_height, gps_fix, main_radio_present, main_packet_tx_error_count, main_tx_bitrate, main_packet_rx_success_count, main_packet_rx_drop_count, main_rx_bitrate, main_rx_rssi, payload_radio_present, payload_packet_tx_error_count, payload_tx_bitrate, payload_packet_rx_success_count, payload_packet_rx_drop_count, payload_rx_bitrate, payload_rx_rssi, ethernet_present, ethernet_status, battery_voltage, log_number, force_mavlink1=False): ''' timestamp : Timestamp [us] (type:uint64_t) + state : State Machine Controller State (type:uint8_t) yaw : Current Yaw [deg] (type:float) pitch : Current Pitch [deg] (type:float) roll : Current Roll [deg] (type:float) @@ -4521,9 +4536,10 @@ class MAVLink(object): ethernet_present : Boolean indicating the presence of the ethernet module (type:uint8_t) ethernet_status : Status flag indicating the status of the ethernet PHY (type:uint8_t) battery_voltage : Battery voltage [V] (type:float) + log_number : Currently active log file, -1 if the logger is inactive (type:int16_t) ''' - return self.send(self.arp_tm_encode(timestamp, yaw, pitch, roll, target_yaw, target_pitch, stepperX_pos, stepperX_delta, stepperX_speed, stepperY_pos, stepperY_delta, stepperY_speed, gps_latitude, gps_longitude, gps_height, gps_fix, main_radio_present, main_packet_tx_error_count, main_tx_bitrate, main_packet_rx_success_count, main_packet_rx_drop_count, main_rx_bitrate, main_rx_rssi, payload_radio_present, payload_packet_tx_error_count, payload_tx_bitrate, payload_packet_rx_success_count, payload_packet_rx_drop_count, payload_rx_bitrate, payload_rx_rssi, ethernet_present, ethernet_status, battery_voltage), force_mavlink1=force_mavlink1) + return self.send(self.arp_tm_encode(timestamp, state, yaw, pitch, roll, target_yaw, target_pitch, stepperX_pos, stepperX_delta, stepperX_speed, stepperY_pos, stepperY_delta, stepperY_speed, gps_latitude, gps_longitude, gps_height, gps_fix, main_radio_present, main_packet_tx_error_count, main_tx_bitrate, main_packet_rx_success_count, main_packet_rx_drop_count, main_rx_bitrate, main_rx_rssi, payload_radio_present, payload_packet_tx_error_count, payload_tx_bitrate, payload_packet_rx_success_count, payload_packet_rx_drop_count, payload_rx_bitrate, payload_rx_rssi, ethernet_present, ethernet_status, battery_voltage, log_number), force_mavlink1=force_mavlink1) def sys_tm_encode(self, timestamp, logger, event_broker, radio, sensors, actuators, pin_handler, can_handler, scheduler): ''' diff --git a/mavlink_lib/lyra/lyra.h b/mavlink_lib/lyra/lyra.h index f938e5aac62a9912f7f40f77057a9730f9f5025f..00d1ef5df8e54aaf35bc6648cac2e9cb0b058737 100644 --- a/mavlink_lib/lyra/lyra.h +++ b/mavlink_lib/lyra/lyra.h @@ -11,7 +11,7 @@ #endif #undef MAVLINK_THIS_XML_HASH -#define MAVLINK_THIS_XML_HASH 4317817575156074154 +#define MAVLINK_THIS_XML_HASH -8212031089336132932 #ifdef __cplusplus extern "C" { @@ -20,11 +20,11 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {0, 8, 1, 1, 1, 1, 5, 1, 1, 4, 4, 12, 16, 8, 2, 4, 8, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5, 5, 7, 4, 4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5, 5, 5, 12, 12, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 4, 4, 74, 64, 32, 60, 32, 32, 32, 32, 56, 22, 5, 19, 36, 36, 40, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 101, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 46, 28, 20, 44, 53, 77, 29, 167, 159, 104, 88, 56, 37, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define MAVLINK_MESSAGE_LENGTHS {0, 8, 1, 1, 1, 1, 5, 1, 1, 4, 4, 12, 16, 8, 2, 4, 8, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5, 5, 7, 4, 4, 4, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 5, 5, 5, 12, 12, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 2, 4, 4, 74, 64, 32, 60, 32, 32, 32, 32, 56, 22, 5, 19, 36, 36, 40, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 104, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 46, 28, 20, 44, 53, 77, 29, 167, 159, 104, 88, 56, 37, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {0, 136, 198, 165, 248, 184, 215, 226, 160, 113, 38, 71, 168, 67, 218, 44, 81, 181, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 110, 22, 65, 79, 167, 84, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 180, 246, 173, 183, 220, 181, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 50, 251, 51, 57, 72, 87, 229, 245, 212, 140, 148, 6, 165, 87, 255, 9, 68, 234, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 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, 48, 142, 108, 39, 19, 234, 66, 11, 52, 59, 46, 64, 33, 67, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} +#define MAVLINK_MESSAGE_CRCS {0, 136, 198, 165, 248, 184, 215, 226, 160, 113, 38, 71, 168, 67, 218, 44, 81, 181, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 110, 22, 65, 79, 167, 84, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 180, 246, 173, 183, 220, 181, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 50, 251, 51, 57, 72, 87, 229, 245, 212, 140, 148, 6, 165, 87, 255, 9, 68, 234, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 239, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 48, 142, 108, 39, 19, 234, 66, 11, 52, 59, 46, 64, 33, 67, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0} #endif #include "../protocol.h" @@ -43,7 +43,8 @@ typedef enum SysIDs MAV_SYSID_PAYLOAD=2, /* | */ MAV_SYSID_RIG=3, /* | */ MAV_SYSID_GS=4, /* | */ - SysIDs_ENUM_END=5, /* | */ + MAV_SYSID_ARP=5, /* | */ + SysIDs_ENUM_END=6, /* | */ } SysIDs; #endif @@ -138,7 +139,10 @@ typedef enum MavCommandList MAV_CMD_REGISTRY_SAVE=20, /* Command to commit the registry to memory | */ MAV_CMD_REGISTRY_CLEAR=21, /* Command to clear the registry | */ MAV_CMD_ENTER_HIL=22, /* Command to enter HIL mode at next reboot | */ - MavCommandList_ENUM_END=23, /* | */ + MAV_CMD_RESET_ALGORITHM=23, /* Command to reset the set algorithm. Used for now in ARP to reset the follow algorithm and return to armed state. | */ + MAV_CMD_ARP_FORCE_NO_FEEDBACK=24, /* Command to force ARP system to entern the no feedback from VN300 state | */ + MAV_CMD_ARP_FOLLOW=25, /* Command to enter in the ARP's follow state and procedure to follow the rocket | */ + MavCommandList_ENUM_END=26, /* | */ } MavCommandList; #endif @@ -261,7 +265,7 @@ typedef enum PinsList #undef MAVLINK_THIS_XML_HASH -#define MAVLINK_THIS_XML_HASH 4317817575156074154 +#define MAVLINK_THIS_XML_HASH -8212031089336132932 #if MAVLINK_THIS_XML_HASH == MAVLINK_PRIMARY_XML_HASH # define MAVLINK_MESSAGE_INFO {{"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING_TC, MAVLINK_MESSAGE_INFO_COMMAND_TC, MAVLINK_MESSAGE_INFO_SYSTEM_TM_REQUEST_TC, MAVLINK_MESSAGE_INFO_SENSOR_TM_REQUEST_TC, MAVLINK_MESSAGE_INFO_SERVO_TM_REQUEST_TC, MAVLINK_MESSAGE_INFO_SET_SERVO_ANGLE_TC, MAVLINK_MESSAGE_INFO_RESET_SERVO_TC, MAVLINK_MESSAGE_INFO_WIGGLE_SERVO_TC, MAVLINK_MESSAGE_INFO_SET_REFERENCE_ALTITUDE_TC, MAVLINK_MESSAGE_INFO_SET_REFERENCE_TEMPERATURE_TC, MAVLINK_MESSAGE_INFO_SET_ORIENTATION_TC, MAVLINK_MESSAGE_INFO_SET_ORIENTATION_QUAT_TC, MAVLINK_MESSAGE_INFO_SET_COORDINATES_TC, MAVLINK_MESSAGE_INFO_RAW_EVENT_TC, MAVLINK_MESSAGE_INFO_SET_DEPLOYMENT_ALTITUDE_TC, MAVLINK_MESSAGE_INFO_SET_TARGET_COORDINATES_TC, MAVLINK_MESSAGE_INFO_SET_ALGORITHM_TC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_ATOMIC_VALVE_TIMING_TC, MAVLINK_MESSAGE_INFO_SET_VALVE_MAXIMUM_APERTURE_TC, MAVLINK_MESSAGE_INFO_CONRIG_STATE_TC, MAVLINK_MESSAGE_INFO_SET_IGNITION_TIME_TC, MAVLINK_MESSAGE_INFO_SET_NITROGEN_TIME_TC, MAVLINK_MESSAGE_INFO_SET_COOLING_TIME_TC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_STEPPER_ANGLE_TC, MAVLINK_MESSAGE_INFO_SET_STEPPER_STEPS_TC, MAVLINK_MESSAGE_INFO_SET_STEPPER_MULTIPLIER_TC, MAVLINK_MESSAGE_INFO_SET_ANTENNA_COORDINATES_ARP_TC, MAVLINK_MESSAGE_INFO_SET_ROCKET_COORDINATES_ARP_TC, MAVLINK_MESSAGE_INFO_ARP_COMMAND_TC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ACK_TM, MAVLINK_MESSAGE_INFO_NACK_TM, MAVLINK_MESSAGE_INFO_WACK_TM, MAVLINK_MESSAGE_INFO_GPS_TM, MAVLINK_MESSAGE_INFO_IMU_TM, MAVLINK_MESSAGE_INFO_PRESSURE_TM, MAVLINK_MESSAGE_INFO_ADC_TM, MAVLINK_MESSAGE_INFO_VOLTAGE_TM, MAVLINK_MESSAGE_INFO_CURRENT_TM, MAVLINK_MESSAGE_INFO_TEMP_TM, MAVLINK_MESSAGE_INFO_LOAD_TM, MAVLINK_MESSAGE_INFO_ATTITUDE_TM, MAVLINK_MESSAGE_INFO_SENSOR_STATE_TM, MAVLINK_MESSAGE_INFO_SERVO_TM, MAVLINK_MESSAGE_INFO_PIN_TM, MAVLINK_MESSAGE_INFO_REGISTRY_FLOAT_TM, MAVLINK_MESSAGE_INFO_REGISTRY_INT_TM, MAVLINK_MESSAGE_INFO_REGISTRY_COORD_TM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ARP_TM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_TM, MAVLINK_MESSAGE_INFO_LOGGER_TM, MAVLINK_MESSAGE_INFO_MAVLINK_STATS_TM, MAVLINK_MESSAGE_INFO_CAN_STATS_TM, MAVLINK_MESSAGE_INFO_TASK_STATS_TM, MAVLINK_MESSAGE_INFO_ADA_TM, MAVLINK_MESSAGE_INFO_NAS_TM, MAVLINK_MESSAGE_INFO_MEA_TM, MAVLINK_MESSAGE_INFO_ROCKET_FLIGHT_TM, MAVLINK_MESSAGE_INFO_PAYLOAD_FLIGHT_TM, MAVLINK_MESSAGE_INFO_ROCKET_STATS_TM, MAVLINK_MESSAGE_INFO_PAYLOAD_STATS_TM, MAVLINK_MESSAGE_INFO_GSE_TM, MAVLINK_MESSAGE_INFO_MOTOR_TM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} diff --git a/mavlink_lib/lyra/mavlink.h b/mavlink_lib/lyra/mavlink.h index 0f32071e03b0a7fc527179b21f669b3a7f30cc32..a5dc5b92376f295c71ac3f27f3635e39a58d705b 100644 --- a/mavlink_lib/lyra/mavlink.h +++ b/mavlink_lib/lyra/mavlink.h @@ -6,7 +6,7 @@ #ifndef MAVLINK_H #define MAVLINK_H -#define MAVLINK_PRIMARY_XML_HASH 4317817575156074154 +#define MAVLINK_PRIMARY_XML_HASH -8212031089336132932 #ifndef MAVLINK_STX #define MAVLINK_STX 254 diff --git a/mavlink_lib/lyra/mavlink_msg_arp_tm.h b/mavlink_lib/lyra/mavlink_msg_arp_tm.h index dc4045d218fe505f05fce9d5d18e940f32bb8a7e..b09667d4fa9046d7ceeba28c518c47ef1d4c7d27 100644 --- a/mavlink_lib/lyra/mavlink_msg_arp_tm.h +++ b/mavlink_lib/lyra/mavlink_msg_arp_tm.h @@ -33,6 +33,8 @@ typedef struct __mavlink_arp_tm_t { uint16_t payload_packet_rx_success_count; /*< Number of succesfull received mavlink packets*/ uint16_t payload_packet_rx_drop_count; /*< Number of dropped mavlink packets*/ uint16_t payload_rx_bitrate; /*< [b/s] Receive bitrate*/ + int16_t log_number; /*< Currently active log file, -1 if the logger is inactive*/ + uint8_t state; /*< State Machine Controller State*/ uint8_t gps_fix; /*< Wether the GPS has a FIX*/ uint8_t main_radio_present; /*< Boolean indicating the presence of the main radio*/ uint8_t payload_radio_present; /*< Boolean indicating the presence of the payload radio*/ @@ -40,13 +42,13 @@ typedef struct __mavlink_arp_tm_t { uint8_t ethernet_status; /*< Status flag indicating the status of the ethernet PHY*/ } mavlink_arp_tm_t; -#define MAVLINK_MSG_ID_ARP_TM_LEN 101 -#define MAVLINK_MSG_ID_ARP_TM_MIN_LEN 101 -#define MAVLINK_MSG_ID_150_LEN 101 -#define MAVLINK_MSG_ID_150_MIN_LEN 101 +#define MAVLINK_MSG_ID_ARP_TM_LEN 104 +#define MAVLINK_MSG_ID_ARP_TM_MIN_LEN 104 +#define MAVLINK_MSG_ID_150_LEN 104 +#define MAVLINK_MSG_ID_150_MIN_LEN 104 -#define MAVLINK_MSG_ID_ARP_TM_CRC 22 -#define MAVLINK_MSG_ID_150_CRC 22 +#define MAVLINK_MSG_ID_ARP_TM_CRC 239 +#define MAVLINK_MSG_ID_150_CRC 239 @@ -54,8 +56,9 @@ typedef struct __mavlink_arp_tm_t { #define MAVLINK_MESSAGE_INFO_ARP_TM { \ 150, \ "ARP_TM", \ - 33, \ + 35, \ { { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_arp_tm_t, timestamp) }, \ + { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 98, offsetof(mavlink_arp_tm_t, state) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_arp_tm_t, yaw) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_arp_tm_t, pitch) }, \ { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_arp_tm_t, roll) }, \ @@ -70,31 +73,33 @@ typedef struct __mavlink_arp_tm_t { { "gps_latitude", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_arp_tm_t, gps_latitude) }, \ { "gps_longitude", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_arp_tm_t, gps_longitude) }, \ { "gps_height", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_arp_tm_t, gps_height) }, \ - { "gps_fix", NULL, MAVLINK_TYPE_UINT8_T, 0, 96, offsetof(mavlink_arp_tm_t, gps_fix) }, \ - { "main_radio_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 97, offsetof(mavlink_arp_tm_t, main_radio_present) }, \ + { "gps_fix", NULL, MAVLINK_TYPE_UINT8_T, 0, 99, offsetof(mavlink_arp_tm_t, gps_fix) }, \ + { "main_radio_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 100, offsetof(mavlink_arp_tm_t, main_radio_present) }, \ { "main_packet_tx_error_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 76, offsetof(mavlink_arp_tm_t, main_packet_tx_error_count) }, \ { "main_tx_bitrate", NULL, MAVLINK_TYPE_UINT16_T, 0, 78, offsetof(mavlink_arp_tm_t, main_tx_bitrate) }, \ { "main_packet_rx_success_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 80, offsetof(mavlink_arp_tm_t, main_packet_rx_success_count) }, \ { "main_packet_rx_drop_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 82, offsetof(mavlink_arp_tm_t, main_packet_rx_drop_count) }, \ { "main_rx_bitrate", NULL, MAVLINK_TYPE_UINT16_T, 0, 84, offsetof(mavlink_arp_tm_t, main_rx_bitrate) }, \ { "main_rx_rssi", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_arp_tm_t, main_rx_rssi) }, \ - { "payload_radio_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 98, offsetof(mavlink_arp_tm_t, payload_radio_present) }, \ + { "payload_radio_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 101, offsetof(mavlink_arp_tm_t, payload_radio_present) }, \ { "payload_packet_tx_error_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 86, offsetof(mavlink_arp_tm_t, payload_packet_tx_error_count) }, \ { "payload_tx_bitrate", NULL, MAVLINK_TYPE_UINT16_T, 0, 88, offsetof(mavlink_arp_tm_t, payload_tx_bitrate) }, \ { "payload_packet_rx_success_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 90, offsetof(mavlink_arp_tm_t, payload_packet_rx_success_count) }, \ { "payload_packet_rx_drop_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 92, offsetof(mavlink_arp_tm_t, payload_packet_rx_drop_count) }, \ { "payload_rx_bitrate", NULL, MAVLINK_TYPE_UINT16_T, 0, 94, offsetof(mavlink_arp_tm_t, payload_rx_bitrate) }, \ { "payload_rx_rssi", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_arp_tm_t, payload_rx_rssi) }, \ - { "ethernet_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 99, offsetof(mavlink_arp_tm_t, ethernet_present) }, \ - { "ethernet_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 100, offsetof(mavlink_arp_tm_t, ethernet_status) }, \ + { "ethernet_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 102, offsetof(mavlink_arp_tm_t, ethernet_present) }, \ + { "ethernet_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 103, offsetof(mavlink_arp_tm_t, ethernet_status) }, \ { "battery_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_arp_tm_t, battery_voltage) }, \ + { "log_number", NULL, MAVLINK_TYPE_INT16_T, 0, 96, offsetof(mavlink_arp_tm_t, log_number) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_ARP_TM { \ "ARP_TM", \ - 33, \ + 35, \ { { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_arp_tm_t, timestamp) }, \ + { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 98, offsetof(mavlink_arp_tm_t, state) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_arp_tm_t, yaw) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_arp_tm_t, pitch) }, \ { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_arp_tm_t, roll) }, \ @@ -109,24 +114,25 @@ typedef struct __mavlink_arp_tm_t { { "gps_latitude", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_arp_tm_t, gps_latitude) }, \ { "gps_longitude", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_arp_tm_t, gps_longitude) }, \ { "gps_height", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_arp_tm_t, gps_height) }, \ - { "gps_fix", NULL, MAVLINK_TYPE_UINT8_T, 0, 96, offsetof(mavlink_arp_tm_t, gps_fix) }, \ - { "main_radio_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 97, offsetof(mavlink_arp_tm_t, main_radio_present) }, \ + { "gps_fix", NULL, MAVLINK_TYPE_UINT8_T, 0, 99, offsetof(mavlink_arp_tm_t, gps_fix) }, \ + { "main_radio_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 100, offsetof(mavlink_arp_tm_t, main_radio_present) }, \ { "main_packet_tx_error_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 76, offsetof(mavlink_arp_tm_t, main_packet_tx_error_count) }, \ { "main_tx_bitrate", NULL, MAVLINK_TYPE_UINT16_T, 0, 78, offsetof(mavlink_arp_tm_t, main_tx_bitrate) }, \ { "main_packet_rx_success_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 80, offsetof(mavlink_arp_tm_t, main_packet_rx_success_count) }, \ { "main_packet_rx_drop_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 82, offsetof(mavlink_arp_tm_t, main_packet_rx_drop_count) }, \ { "main_rx_bitrate", NULL, MAVLINK_TYPE_UINT16_T, 0, 84, offsetof(mavlink_arp_tm_t, main_rx_bitrate) }, \ { "main_rx_rssi", NULL, MAVLINK_TYPE_FLOAT, 0, 64, offsetof(mavlink_arp_tm_t, main_rx_rssi) }, \ - { "payload_radio_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 98, offsetof(mavlink_arp_tm_t, payload_radio_present) }, \ + { "payload_radio_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 101, offsetof(mavlink_arp_tm_t, payload_radio_present) }, \ { "payload_packet_tx_error_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 86, offsetof(mavlink_arp_tm_t, payload_packet_tx_error_count) }, \ { "payload_tx_bitrate", NULL, MAVLINK_TYPE_UINT16_T, 0, 88, offsetof(mavlink_arp_tm_t, payload_tx_bitrate) }, \ { "payload_packet_rx_success_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 90, offsetof(mavlink_arp_tm_t, payload_packet_rx_success_count) }, \ { "payload_packet_rx_drop_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 92, offsetof(mavlink_arp_tm_t, payload_packet_rx_drop_count) }, \ { "payload_rx_bitrate", NULL, MAVLINK_TYPE_UINT16_T, 0, 94, offsetof(mavlink_arp_tm_t, payload_rx_bitrate) }, \ { "payload_rx_rssi", NULL, MAVLINK_TYPE_FLOAT, 0, 68, offsetof(mavlink_arp_tm_t, payload_rx_rssi) }, \ - { "ethernet_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 99, offsetof(mavlink_arp_tm_t, ethernet_present) }, \ - { "ethernet_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 100, offsetof(mavlink_arp_tm_t, ethernet_status) }, \ + { "ethernet_present", NULL, MAVLINK_TYPE_UINT8_T, 0, 102, offsetof(mavlink_arp_tm_t, ethernet_present) }, \ + { "ethernet_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 103, offsetof(mavlink_arp_tm_t, ethernet_status) }, \ { "battery_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 72, offsetof(mavlink_arp_tm_t, battery_voltage) }, \ + { "log_number", NULL, MAVLINK_TYPE_INT16_T, 0, 96, offsetof(mavlink_arp_tm_t, log_number) }, \ } \ } #endif @@ -138,6 +144,7 @@ typedef struct __mavlink_arp_tm_t { * @param msg The MAVLink message to compress the data into * * @param timestamp [us] Timestamp + * @param state State Machine Controller State * @param yaw [deg] Current Yaw * @param pitch [deg] Current Pitch * @param roll [deg] Current Roll @@ -170,10 +177,11 @@ typedef struct __mavlink_arp_tm_t { * @param ethernet_present Boolean indicating the presence of the ethernet module * @param ethernet_status Status flag indicating the status of the ethernet PHY * @param battery_voltage [V] Battery voltage + * @param log_number Currently active log file, -1 if the logger is inactive * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_arp_tm_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t timestamp, float yaw, float pitch, float roll, float target_yaw, float target_pitch, float stepperX_pos, float stepperX_delta, float stepperX_speed, float stepperY_pos, float stepperY_delta, float stepperY_speed, float gps_latitude, float gps_longitude, float gps_height, uint8_t gps_fix, uint8_t main_radio_present, uint16_t main_packet_tx_error_count, uint16_t main_tx_bitrate, uint16_t main_packet_rx_success_count, uint16_t main_packet_rx_drop_count, uint16_t main_rx_bitrate, float main_rx_rssi, uint8_t payload_radio_present, uint16_t payload_packet_tx_error_count, uint16_t payload_tx_bitrate, uint16_t payload_packet_rx_success_count, uint16_t payload_packet_rx_drop_count, uint16_t payload_rx_bitrate, float payload_rx_rssi, uint8_t ethernet_present, uint8_t ethernet_status, float battery_voltage) + uint64_t timestamp, uint8_t state, float yaw, float pitch, float roll, float target_yaw, float target_pitch, float stepperX_pos, float stepperX_delta, float stepperX_speed, float stepperY_pos, float stepperY_delta, float stepperY_speed, float gps_latitude, float gps_longitude, float gps_height, uint8_t gps_fix, uint8_t main_radio_present, uint16_t main_packet_tx_error_count, uint16_t main_tx_bitrate, uint16_t main_packet_rx_success_count, uint16_t main_packet_rx_drop_count, uint16_t main_rx_bitrate, float main_rx_rssi, uint8_t payload_radio_present, uint16_t payload_packet_tx_error_count, uint16_t payload_tx_bitrate, uint16_t payload_packet_rx_success_count, uint16_t payload_packet_rx_drop_count, uint16_t payload_rx_bitrate, float payload_rx_rssi, uint8_t ethernet_present, uint8_t ethernet_status, float battery_voltage, int16_t log_number) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ARP_TM_LEN]; @@ -205,11 +213,13 @@ static inline uint16_t mavlink_msg_arp_tm_pack(uint8_t system_id, uint8_t compon _mav_put_uint16_t(buf, 90, payload_packet_rx_success_count); _mav_put_uint16_t(buf, 92, payload_packet_rx_drop_count); _mav_put_uint16_t(buf, 94, payload_rx_bitrate); - _mav_put_uint8_t(buf, 96, gps_fix); - _mav_put_uint8_t(buf, 97, main_radio_present); - _mav_put_uint8_t(buf, 98, payload_radio_present); - _mav_put_uint8_t(buf, 99, ethernet_present); - _mav_put_uint8_t(buf, 100, ethernet_status); + _mav_put_int16_t(buf, 96, log_number); + _mav_put_uint8_t(buf, 98, state); + _mav_put_uint8_t(buf, 99, gps_fix); + _mav_put_uint8_t(buf, 100, main_radio_present); + _mav_put_uint8_t(buf, 101, payload_radio_present); + _mav_put_uint8_t(buf, 102, ethernet_present); + _mav_put_uint8_t(buf, 103, ethernet_status); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ARP_TM_LEN); #else @@ -242,6 +252,8 @@ static inline uint16_t mavlink_msg_arp_tm_pack(uint8_t system_id, uint8_t compon packet.payload_packet_rx_success_count = payload_packet_rx_success_count; packet.payload_packet_rx_drop_count = payload_packet_rx_drop_count; packet.payload_rx_bitrate = payload_rx_bitrate; + packet.log_number = log_number; + packet.state = state; packet.gps_fix = gps_fix; packet.main_radio_present = main_radio_present; packet.payload_radio_present = payload_radio_present; @@ -262,6 +274,7 @@ static inline uint16_t mavlink_msg_arp_tm_pack(uint8_t system_id, uint8_t compon * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param timestamp [us] Timestamp + * @param state State Machine Controller State * @param yaw [deg] Current Yaw * @param pitch [deg] Current Pitch * @param roll [deg] Current Roll @@ -294,11 +307,12 @@ static inline uint16_t mavlink_msg_arp_tm_pack(uint8_t system_id, uint8_t compon * @param ethernet_present Boolean indicating the presence of the ethernet module * @param ethernet_status Status flag indicating the status of the ethernet PHY * @param battery_voltage [V] Battery voltage + * @param log_number Currently active log file, -1 if the logger is inactive * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_arp_tm_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t timestamp,float yaw,float pitch,float roll,float target_yaw,float target_pitch,float stepperX_pos,float stepperX_delta,float stepperX_speed,float stepperY_pos,float stepperY_delta,float stepperY_speed,float gps_latitude,float gps_longitude,float gps_height,uint8_t gps_fix,uint8_t main_radio_present,uint16_t main_packet_tx_error_count,uint16_t main_tx_bitrate,uint16_t main_packet_rx_success_count,uint16_t main_packet_rx_drop_count,uint16_t main_rx_bitrate,float main_rx_rssi,uint8_t payload_radio_present,uint16_t payload_packet_tx_error_count,uint16_t payload_tx_bitrate,uint16_t payload_packet_rx_success_count,uint16_t payload_packet_rx_drop_count,uint16_t payload_rx_bitrate,float payload_rx_rssi,uint8_t ethernet_present,uint8_t ethernet_status,float battery_voltage) + uint64_t timestamp,uint8_t state,float yaw,float pitch,float roll,float target_yaw,float target_pitch,float stepperX_pos,float stepperX_delta,float stepperX_speed,float stepperY_pos,float stepperY_delta,float stepperY_speed,float gps_latitude,float gps_longitude,float gps_height,uint8_t gps_fix,uint8_t main_radio_present,uint16_t main_packet_tx_error_count,uint16_t main_tx_bitrate,uint16_t main_packet_rx_success_count,uint16_t main_packet_rx_drop_count,uint16_t main_rx_bitrate,float main_rx_rssi,uint8_t payload_radio_present,uint16_t payload_packet_tx_error_count,uint16_t payload_tx_bitrate,uint16_t payload_packet_rx_success_count,uint16_t payload_packet_rx_drop_count,uint16_t payload_rx_bitrate,float payload_rx_rssi,uint8_t ethernet_present,uint8_t ethernet_status,float battery_voltage,int16_t log_number) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ARP_TM_LEN]; @@ -330,11 +344,13 @@ static inline uint16_t mavlink_msg_arp_tm_pack_chan(uint8_t system_id, uint8_t c _mav_put_uint16_t(buf, 90, payload_packet_rx_success_count); _mav_put_uint16_t(buf, 92, payload_packet_rx_drop_count); _mav_put_uint16_t(buf, 94, payload_rx_bitrate); - _mav_put_uint8_t(buf, 96, gps_fix); - _mav_put_uint8_t(buf, 97, main_radio_present); - _mav_put_uint8_t(buf, 98, payload_radio_present); - _mav_put_uint8_t(buf, 99, ethernet_present); - _mav_put_uint8_t(buf, 100, ethernet_status); + _mav_put_int16_t(buf, 96, log_number); + _mav_put_uint8_t(buf, 98, state); + _mav_put_uint8_t(buf, 99, gps_fix); + _mav_put_uint8_t(buf, 100, main_radio_present); + _mav_put_uint8_t(buf, 101, payload_radio_present); + _mav_put_uint8_t(buf, 102, ethernet_present); + _mav_put_uint8_t(buf, 103, ethernet_status); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ARP_TM_LEN); #else @@ -367,6 +383,8 @@ static inline uint16_t mavlink_msg_arp_tm_pack_chan(uint8_t system_id, uint8_t c packet.payload_packet_rx_success_count = payload_packet_rx_success_count; packet.payload_packet_rx_drop_count = payload_packet_rx_drop_count; packet.payload_rx_bitrate = payload_rx_bitrate; + packet.log_number = log_number; + packet.state = state; packet.gps_fix = gps_fix; packet.main_radio_present = main_radio_present; packet.payload_radio_present = payload_radio_present; @@ -390,7 +408,7 @@ static inline uint16_t mavlink_msg_arp_tm_pack_chan(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_arp_tm_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_arp_tm_t* arp_tm) { - return mavlink_msg_arp_tm_pack(system_id, component_id, msg, arp_tm->timestamp, arp_tm->yaw, arp_tm->pitch, arp_tm->roll, arp_tm->target_yaw, arp_tm->target_pitch, arp_tm->stepperX_pos, arp_tm->stepperX_delta, arp_tm->stepperX_speed, arp_tm->stepperY_pos, arp_tm->stepperY_delta, arp_tm->stepperY_speed, arp_tm->gps_latitude, arp_tm->gps_longitude, arp_tm->gps_height, arp_tm->gps_fix, arp_tm->main_radio_present, arp_tm->main_packet_tx_error_count, arp_tm->main_tx_bitrate, arp_tm->main_packet_rx_success_count, arp_tm->main_packet_rx_drop_count, arp_tm->main_rx_bitrate, arp_tm->main_rx_rssi, arp_tm->payload_radio_present, arp_tm->payload_packet_tx_error_count, arp_tm->payload_tx_bitrate, arp_tm->payload_packet_rx_success_count, arp_tm->payload_packet_rx_drop_count, arp_tm->payload_rx_bitrate, arp_tm->payload_rx_rssi, arp_tm->ethernet_present, arp_tm->ethernet_status, arp_tm->battery_voltage); + return mavlink_msg_arp_tm_pack(system_id, component_id, msg, arp_tm->timestamp, arp_tm->state, arp_tm->yaw, arp_tm->pitch, arp_tm->roll, arp_tm->target_yaw, arp_tm->target_pitch, arp_tm->stepperX_pos, arp_tm->stepperX_delta, arp_tm->stepperX_speed, arp_tm->stepperY_pos, arp_tm->stepperY_delta, arp_tm->stepperY_speed, arp_tm->gps_latitude, arp_tm->gps_longitude, arp_tm->gps_height, arp_tm->gps_fix, arp_tm->main_radio_present, arp_tm->main_packet_tx_error_count, arp_tm->main_tx_bitrate, arp_tm->main_packet_rx_success_count, arp_tm->main_packet_rx_drop_count, arp_tm->main_rx_bitrate, arp_tm->main_rx_rssi, arp_tm->payload_radio_present, arp_tm->payload_packet_tx_error_count, arp_tm->payload_tx_bitrate, arp_tm->payload_packet_rx_success_count, arp_tm->payload_packet_rx_drop_count, arp_tm->payload_rx_bitrate, arp_tm->payload_rx_rssi, arp_tm->ethernet_present, arp_tm->ethernet_status, arp_tm->battery_voltage, arp_tm->log_number); } /** @@ -404,7 +422,7 @@ static inline uint16_t mavlink_msg_arp_tm_encode(uint8_t system_id, uint8_t comp */ static inline uint16_t mavlink_msg_arp_tm_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_arp_tm_t* arp_tm) { - return mavlink_msg_arp_tm_pack_chan(system_id, component_id, chan, msg, arp_tm->timestamp, arp_tm->yaw, arp_tm->pitch, arp_tm->roll, arp_tm->target_yaw, arp_tm->target_pitch, arp_tm->stepperX_pos, arp_tm->stepperX_delta, arp_tm->stepperX_speed, arp_tm->stepperY_pos, arp_tm->stepperY_delta, arp_tm->stepperY_speed, arp_tm->gps_latitude, arp_tm->gps_longitude, arp_tm->gps_height, arp_tm->gps_fix, arp_tm->main_radio_present, arp_tm->main_packet_tx_error_count, arp_tm->main_tx_bitrate, arp_tm->main_packet_rx_success_count, arp_tm->main_packet_rx_drop_count, arp_tm->main_rx_bitrate, arp_tm->main_rx_rssi, arp_tm->payload_radio_present, arp_tm->payload_packet_tx_error_count, arp_tm->payload_tx_bitrate, arp_tm->payload_packet_rx_success_count, arp_tm->payload_packet_rx_drop_count, arp_tm->payload_rx_bitrate, arp_tm->payload_rx_rssi, arp_tm->ethernet_present, arp_tm->ethernet_status, arp_tm->battery_voltage); + return mavlink_msg_arp_tm_pack_chan(system_id, component_id, chan, msg, arp_tm->timestamp, arp_tm->state, arp_tm->yaw, arp_tm->pitch, arp_tm->roll, arp_tm->target_yaw, arp_tm->target_pitch, arp_tm->stepperX_pos, arp_tm->stepperX_delta, arp_tm->stepperX_speed, arp_tm->stepperY_pos, arp_tm->stepperY_delta, arp_tm->stepperY_speed, arp_tm->gps_latitude, arp_tm->gps_longitude, arp_tm->gps_height, arp_tm->gps_fix, arp_tm->main_radio_present, arp_tm->main_packet_tx_error_count, arp_tm->main_tx_bitrate, arp_tm->main_packet_rx_success_count, arp_tm->main_packet_rx_drop_count, arp_tm->main_rx_bitrate, arp_tm->main_rx_rssi, arp_tm->payload_radio_present, arp_tm->payload_packet_tx_error_count, arp_tm->payload_tx_bitrate, arp_tm->payload_packet_rx_success_count, arp_tm->payload_packet_rx_drop_count, arp_tm->payload_rx_bitrate, arp_tm->payload_rx_rssi, arp_tm->ethernet_present, arp_tm->ethernet_status, arp_tm->battery_voltage, arp_tm->log_number); } /** @@ -412,6 +430,7 @@ static inline uint16_t mavlink_msg_arp_tm_encode_chan(uint8_t system_id, uint8_t * @param chan MAVLink channel to send the message * * @param timestamp [us] Timestamp + * @param state State Machine Controller State * @param yaw [deg] Current Yaw * @param pitch [deg] Current Pitch * @param roll [deg] Current Roll @@ -444,10 +463,11 @@ static inline uint16_t mavlink_msg_arp_tm_encode_chan(uint8_t system_id, uint8_t * @param ethernet_present Boolean indicating the presence of the ethernet module * @param ethernet_status Status flag indicating the status of the ethernet PHY * @param battery_voltage [V] Battery voltage + * @param log_number Currently active log file, -1 if the logger is inactive */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_arp_tm_send(mavlink_channel_t chan, uint64_t timestamp, float yaw, float pitch, float roll, float target_yaw, float target_pitch, float stepperX_pos, float stepperX_delta, float stepperX_speed, float stepperY_pos, float stepperY_delta, float stepperY_speed, float gps_latitude, float gps_longitude, float gps_height, uint8_t gps_fix, uint8_t main_radio_present, uint16_t main_packet_tx_error_count, uint16_t main_tx_bitrate, uint16_t main_packet_rx_success_count, uint16_t main_packet_rx_drop_count, uint16_t main_rx_bitrate, float main_rx_rssi, uint8_t payload_radio_present, uint16_t payload_packet_tx_error_count, uint16_t payload_tx_bitrate, uint16_t payload_packet_rx_success_count, uint16_t payload_packet_rx_drop_count, uint16_t payload_rx_bitrate, float payload_rx_rssi, uint8_t ethernet_present, uint8_t ethernet_status, float battery_voltage) +static inline void mavlink_msg_arp_tm_send(mavlink_channel_t chan, uint64_t timestamp, uint8_t state, float yaw, float pitch, float roll, float target_yaw, float target_pitch, float stepperX_pos, float stepperX_delta, float stepperX_speed, float stepperY_pos, float stepperY_delta, float stepperY_speed, float gps_latitude, float gps_longitude, float gps_height, uint8_t gps_fix, uint8_t main_radio_present, uint16_t main_packet_tx_error_count, uint16_t main_tx_bitrate, uint16_t main_packet_rx_success_count, uint16_t main_packet_rx_drop_count, uint16_t main_rx_bitrate, float main_rx_rssi, uint8_t payload_radio_present, uint16_t payload_packet_tx_error_count, uint16_t payload_tx_bitrate, uint16_t payload_packet_rx_success_count, uint16_t payload_packet_rx_drop_count, uint16_t payload_rx_bitrate, float payload_rx_rssi, uint8_t ethernet_present, uint8_t ethernet_status, float battery_voltage, int16_t log_number) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ARP_TM_LEN]; @@ -479,11 +499,13 @@ static inline void mavlink_msg_arp_tm_send(mavlink_channel_t chan, uint64_t time _mav_put_uint16_t(buf, 90, payload_packet_rx_success_count); _mav_put_uint16_t(buf, 92, payload_packet_rx_drop_count); _mav_put_uint16_t(buf, 94, payload_rx_bitrate); - _mav_put_uint8_t(buf, 96, gps_fix); - _mav_put_uint8_t(buf, 97, main_radio_present); - _mav_put_uint8_t(buf, 98, payload_radio_present); - _mav_put_uint8_t(buf, 99, ethernet_present); - _mav_put_uint8_t(buf, 100, ethernet_status); + _mav_put_int16_t(buf, 96, log_number); + _mav_put_uint8_t(buf, 98, state); + _mav_put_uint8_t(buf, 99, gps_fix); + _mav_put_uint8_t(buf, 100, main_radio_present); + _mav_put_uint8_t(buf, 101, payload_radio_present); + _mav_put_uint8_t(buf, 102, ethernet_present); + _mav_put_uint8_t(buf, 103, ethernet_status); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ARP_TM, buf, MAVLINK_MSG_ID_ARP_TM_MIN_LEN, MAVLINK_MSG_ID_ARP_TM_LEN, MAVLINK_MSG_ID_ARP_TM_CRC); #else @@ -516,6 +538,8 @@ static inline void mavlink_msg_arp_tm_send(mavlink_channel_t chan, uint64_t time packet.payload_packet_rx_success_count = payload_packet_rx_success_count; packet.payload_packet_rx_drop_count = payload_packet_rx_drop_count; packet.payload_rx_bitrate = payload_rx_bitrate; + packet.log_number = log_number; + packet.state = state; packet.gps_fix = gps_fix; packet.main_radio_present = main_radio_present; packet.payload_radio_present = payload_radio_present; @@ -534,7 +558,7 @@ static inline void mavlink_msg_arp_tm_send(mavlink_channel_t chan, uint64_t time static inline void mavlink_msg_arp_tm_send_struct(mavlink_channel_t chan, const mavlink_arp_tm_t* arp_tm) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_arp_tm_send(chan, arp_tm->timestamp, arp_tm->yaw, arp_tm->pitch, arp_tm->roll, arp_tm->target_yaw, arp_tm->target_pitch, arp_tm->stepperX_pos, arp_tm->stepperX_delta, arp_tm->stepperX_speed, arp_tm->stepperY_pos, arp_tm->stepperY_delta, arp_tm->stepperY_speed, arp_tm->gps_latitude, arp_tm->gps_longitude, arp_tm->gps_height, arp_tm->gps_fix, arp_tm->main_radio_present, arp_tm->main_packet_tx_error_count, arp_tm->main_tx_bitrate, arp_tm->main_packet_rx_success_count, arp_tm->main_packet_rx_drop_count, arp_tm->main_rx_bitrate, arp_tm->main_rx_rssi, arp_tm->payload_radio_present, arp_tm->payload_packet_tx_error_count, arp_tm->payload_tx_bitrate, arp_tm->payload_packet_rx_success_count, arp_tm->payload_packet_rx_drop_count, arp_tm->payload_rx_bitrate, arp_tm->payload_rx_rssi, arp_tm->ethernet_present, arp_tm->ethernet_status, arp_tm->battery_voltage); + mavlink_msg_arp_tm_send(chan, arp_tm->timestamp, arp_tm->state, arp_tm->yaw, arp_tm->pitch, arp_tm->roll, arp_tm->target_yaw, arp_tm->target_pitch, arp_tm->stepperX_pos, arp_tm->stepperX_delta, arp_tm->stepperX_speed, arp_tm->stepperY_pos, arp_tm->stepperY_delta, arp_tm->stepperY_speed, arp_tm->gps_latitude, arp_tm->gps_longitude, arp_tm->gps_height, arp_tm->gps_fix, arp_tm->main_radio_present, arp_tm->main_packet_tx_error_count, arp_tm->main_tx_bitrate, arp_tm->main_packet_rx_success_count, arp_tm->main_packet_rx_drop_count, arp_tm->main_rx_bitrate, arp_tm->main_rx_rssi, arp_tm->payload_radio_present, arp_tm->payload_packet_tx_error_count, arp_tm->payload_tx_bitrate, arp_tm->payload_packet_rx_success_count, arp_tm->payload_packet_rx_drop_count, arp_tm->payload_rx_bitrate, arp_tm->payload_rx_rssi, arp_tm->ethernet_present, arp_tm->ethernet_status, arp_tm->battery_voltage, arp_tm->log_number); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ARP_TM, (const char *)arp_tm, MAVLINK_MSG_ID_ARP_TM_MIN_LEN, MAVLINK_MSG_ID_ARP_TM_LEN, MAVLINK_MSG_ID_ARP_TM_CRC); #endif @@ -548,7 +572,7 @@ static inline void mavlink_msg_arp_tm_send_struct(mavlink_channel_t chan, const is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_arp_tm_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t timestamp, float yaw, float pitch, float roll, float target_yaw, float target_pitch, float stepperX_pos, float stepperX_delta, float stepperX_speed, float stepperY_pos, float stepperY_delta, float stepperY_speed, float gps_latitude, float gps_longitude, float gps_height, uint8_t gps_fix, uint8_t main_radio_present, uint16_t main_packet_tx_error_count, uint16_t main_tx_bitrate, uint16_t main_packet_rx_success_count, uint16_t main_packet_rx_drop_count, uint16_t main_rx_bitrate, float main_rx_rssi, uint8_t payload_radio_present, uint16_t payload_packet_tx_error_count, uint16_t payload_tx_bitrate, uint16_t payload_packet_rx_success_count, uint16_t payload_packet_rx_drop_count, uint16_t payload_rx_bitrate, float payload_rx_rssi, uint8_t ethernet_present, uint8_t ethernet_status, float battery_voltage) +static inline void mavlink_msg_arp_tm_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t timestamp, uint8_t state, float yaw, float pitch, float roll, float target_yaw, float target_pitch, float stepperX_pos, float stepperX_delta, float stepperX_speed, float stepperY_pos, float stepperY_delta, float stepperY_speed, float gps_latitude, float gps_longitude, float gps_height, uint8_t gps_fix, uint8_t main_radio_present, uint16_t main_packet_tx_error_count, uint16_t main_tx_bitrate, uint16_t main_packet_rx_success_count, uint16_t main_packet_rx_drop_count, uint16_t main_rx_bitrate, float main_rx_rssi, uint8_t payload_radio_present, uint16_t payload_packet_tx_error_count, uint16_t payload_tx_bitrate, uint16_t payload_packet_rx_success_count, uint16_t payload_packet_rx_drop_count, uint16_t payload_rx_bitrate, float payload_rx_rssi, uint8_t ethernet_present, uint8_t ethernet_status, float battery_voltage, int16_t log_number) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -580,11 +604,13 @@ static inline void mavlink_msg_arp_tm_send_buf(mavlink_message_t *msgbuf, mavlin _mav_put_uint16_t(buf, 90, payload_packet_rx_success_count); _mav_put_uint16_t(buf, 92, payload_packet_rx_drop_count); _mav_put_uint16_t(buf, 94, payload_rx_bitrate); - _mav_put_uint8_t(buf, 96, gps_fix); - _mav_put_uint8_t(buf, 97, main_radio_present); - _mav_put_uint8_t(buf, 98, payload_radio_present); - _mav_put_uint8_t(buf, 99, ethernet_present); - _mav_put_uint8_t(buf, 100, ethernet_status); + _mav_put_int16_t(buf, 96, log_number); + _mav_put_uint8_t(buf, 98, state); + _mav_put_uint8_t(buf, 99, gps_fix); + _mav_put_uint8_t(buf, 100, main_radio_present); + _mav_put_uint8_t(buf, 101, payload_radio_present); + _mav_put_uint8_t(buf, 102, ethernet_present); + _mav_put_uint8_t(buf, 103, ethernet_status); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ARP_TM, buf, MAVLINK_MSG_ID_ARP_TM_MIN_LEN, MAVLINK_MSG_ID_ARP_TM_LEN, MAVLINK_MSG_ID_ARP_TM_CRC); #else @@ -617,6 +643,8 @@ static inline void mavlink_msg_arp_tm_send_buf(mavlink_message_t *msgbuf, mavlin packet->payload_packet_rx_success_count = payload_packet_rx_success_count; packet->payload_packet_rx_drop_count = payload_packet_rx_drop_count; packet->payload_rx_bitrate = payload_rx_bitrate; + packet->log_number = log_number; + packet->state = state; packet->gps_fix = gps_fix; packet->main_radio_present = main_radio_present; packet->payload_radio_present = payload_radio_present; @@ -643,6 +671,16 @@ static inline uint64_t mavlink_msg_arp_tm_get_timestamp(const mavlink_message_t* return _MAV_RETURN_uint64_t(msg, 0); } +/** + * @brief Get field state from arp_tm message + * + * @return State Machine Controller State + */ +static inline uint8_t mavlink_msg_arp_tm_get_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 98); +} + /** * @brief Get field yaw from arp_tm message * @@ -790,7 +828,7 @@ static inline float mavlink_msg_arp_tm_get_gps_height(const mavlink_message_t* m */ static inline uint8_t mavlink_msg_arp_tm_get_gps_fix(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 96); + return _MAV_RETURN_uint8_t(msg, 99); } /** @@ -800,7 +838,7 @@ static inline uint8_t mavlink_msg_arp_tm_get_gps_fix(const mavlink_message_t* ms */ static inline uint8_t mavlink_msg_arp_tm_get_main_radio_present(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 97); + return _MAV_RETURN_uint8_t(msg, 100); } /** @@ -870,7 +908,7 @@ static inline float mavlink_msg_arp_tm_get_main_rx_rssi(const mavlink_message_t* */ static inline uint8_t mavlink_msg_arp_tm_get_payload_radio_present(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 98); + return _MAV_RETURN_uint8_t(msg, 101); } /** @@ -940,7 +978,7 @@ static inline float mavlink_msg_arp_tm_get_payload_rx_rssi(const mavlink_message */ static inline uint8_t mavlink_msg_arp_tm_get_ethernet_present(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 99); + return _MAV_RETURN_uint8_t(msg, 102); } /** @@ -950,7 +988,7 @@ static inline uint8_t mavlink_msg_arp_tm_get_ethernet_present(const mavlink_mess */ static inline uint8_t mavlink_msg_arp_tm_get_ethernet_status(const mavlink_message_t* msg) { - return _MAV_RETURN_uint8_t(msg, 100); + return _MAV_RETURN_uint8_t(msg, 103); } /** @@ -963,6 +1001,16 @@ static inline float mavlink_msg_arp_tm_get_battery_voltage(const mavlink_message return _MAV_RETURN_float(msg, 72); } +/** + * @brief Get field log_number from arp_tm message + * + * @return Currently active log file, -1 if the logger is inactive + */ +static inline int16_t mavlink_msg_arp_tm_get_log_number(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 96); +} + /** * @brief Decode a arp_tm message into a struct * @@ -1000,6 +1048,8 @@ static inline void mavlink_msg_arp_tm_decode(const mavlink_message_t* msg, mavli arp_tm->payload_packet_rx_success_count = mavlink_msg_arp_tm_get_payload_packet_rx_success_count(msg); arp_tm->payload_packet_rx_drop_count = mavlink_msg_arp_tm_get_payload_packet_rx_drop_count(msg); arp_tm->payload_rx_bitrate = mavlink_msg_arp_tm_get_payload_rx_bitrate(msg); + arp_tm->log_number = mavlink_msg_arp_tm_get_log_number(msg); + arp_tm->state = mavlink_msg_arp_tm_get_state(msg); arp_tm->gps_fix = mavlink_msg_arp_tm_get_gps_fix(msg); arp_tm->main_radio_present = mavlink_msg_arp_tm_get_main_radio_present(msg); arp_tm->payload_radio_present = mavlink_msg_arp_tm_get_payload_radio_present(msg); diff --git a/mavlink_lib/lyra/testsuite.h b/mavlink_lib/lyra/testsuite.h index 92f9bf1c989853a6d3530f9c08d5681c8f1f5c61..8a38bd02e5e454149ba72a7d5ad2d0d58c140e03 100644 --- a/mavlink_lib/lyra/testsuite.h +++ b/mavlink_lib/lyra/testsuite.h @@ -2904,7 +2904,7 @@ static void mavlink_test_arp_tm(uint8_t system_id, uint8_t component_id, mavlink uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_arp_tm_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,437.0,465.0,493.0,521.0,21187,21291,21395,21499,21603,21707,21811,21915,22019,22123,37,104,171,238,49 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,437.0,465.0,493.0,521.0,21187,21291,21395,21499,21603,21707,21811,21915,22019,22123,22227,171,238,49,116,183,250 }; mavlink_arp_tm_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2936,6 +2936,8 @@ static void mavlink_test_arp_tm(uint8_t system_id, uint8_t component_id, mavlink packet1.payload_packet_rx_success_count = packet_in.payload_packet_rx_success_count; packet1.payload_packet_rx_drop_count = packet_in.payload_packet_rx_drop_count; packet1.payload_rx_bitrate = packet_in.payload_rx_bitrate; + packet1.log_number = packet_in.log_number; + packet1.state = packet_in.state; packet1.gps_fix = packet_in.gps_fix; packet1.main_radio_present = packet_in.main_radio_present; packet1.payload_radio_present = packet_in.payload_radio_present; @@ -2955,12 +2957,12 @@ static void mavlink_test_arp_tm(uint8_t system_id, uint8_t component_id, mavlink MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_arp_tm_pack(system_id, component_id, &msg , packet1.timestamp , packet1.yaw , packet1.pitch , packet1.roll , packet1.target_yaw , packet1.target_pitch , packet1.stepperX_pos , packet1.stepperX_delta , packet1.stepperX_speed , packet1.stepperY_pos , packet1.stepperY_delta , packet1.stepperY_speed , packet1.gps_latitude , packet1.gps_longitude , packet1.gps_height , packet1.gps_fix , packet1.main_radio_present , packet1.main_packet_tx_error_count , packet1.main_tx_bitrate , packet1.main_packet_rx_success_count , packet1.main_packet_rx_drop_count , packet1.main_rx_bitrate , packet1.main_rx_rssi , packet1.payload_radio_present , packet1.payload_packet_tx_error_count , packet1.payload_tx_bitrate , packet1.payload_packet_rx_success_count , packet1.payload_packet_rx_drop_count , packet1.payload_rx_bitrate , packet1.payload_rx_rssi , packet1.ethernet_present , packet1.ethernet_status , packet1.battery_voltage ); + mavlink_msg_arp_tm_pack(system_id, component_id, &msg , packet1.timestamp , packet1.state , packet1.yaw , packet1.pitch , packet1.roll , packet1.target_yaw , packet1.target_pitch , packet1.stepperX_pos , packet1.stepperX_delta , packet1.stepperX_speed , packet1.stepperY_pos , packet1.stepperY_delta , packet1.stepperY_speed , packet1.gps_latitude , packet1.gps_longitude , packet1.gps_height , packet1.gps_fix , packet1.main_radio_present , packet1.main_packet_tx_error_count , packet1.main_tx_bitrate , packet1.main_packet_rx_success_count , packet1.main_packet_rx_drop_count , packet1.main_rx_bitrate , packet1.main_rx_rssi , packet1.payload_radio_present , packet1.payload_packet_tx_error_count , packet1.payload_tx_bitrate , packet1.payload_packet_rx_success_count , packet1.payload_packet_rx_drop_count , packet1.payload_rx_bitrate , packet1.payload_rx_rssi , packet1.ethernet_present , packet1.ethernet_status , packet1.battery_voltage , packet1.log_number ); mavlink_msg_arp_tm_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_arp_tm_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.timestamp , packet1.yaw , packet1.pitch , packet1.roll , packet1.target_yaw , packet1.target_pitch , packet1.stepperX_pos , packet1.stepperX_delta , packet1.stepperX_speed , packet1.stepperY_pos , packet1.stepperY_delta , packet1.stepperY_speed , packet1.gps_latitude , packet1.gps_longitude , packet1.gps_height , packet1.gps_fix , packet1.main_radio_present , packet1.main_packet_tx_error_count , packet1.main_tx_bitrate , packet1.main_packet_rx_success_count , packet1.main_packet_rx_drop_count , packet1.main_rx_bitrate , packet1.main_rx_rssi , packet1.payload_radio_present , packet1.payload_packet_tx_error_count , packet1.payload_tx_bitrate , packet1.payload_packet_rx_success_count , packet1.payload_packet_rx_drop_count , packet1.payload_rx_bitrate , packet1.payload_rx_rssi , packet1.ethernet_present , packet1.ethernet_status , packet1.battery_voltage ); + mavlink_msg_arp_tm_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.timestamp , packet1.state , packet1.yaw , packet1.pitch , packet1.roll , packet1.target_yaw , packet1.target_pitch , packet1.stepperX_pos , packet1.stepperX_delta , packet1.stepperX_speed , packet1.stepperY_pos , packet1.stepperY_delta , packet1.stepperY_speed , packet1.gps_latitude , packet1.gps_longitude , packet1.gps_height , packet1.gps_fix , packet1.main_radio_present , packet1.main_packet_tx_error_count , packet1.main_tx_bitrate , packet1.main_packet_rx_success_count , packet1.main_packet_rx_drop_count , packet1.main_rx_bitrate , packet1.main_rx_rssi , packet1.payload_radio_present , packet1.payload_packet_tx_error_count , packet1.payload_tx_bitrate , packet1.payload_packet_rx_success_count , packet1.payload_packet_rx_drop_count , packet1.payload_rx_bitrate , packet1.payload_rx_rssi , packet1.ethernet_present , packet1.ethernet_status , packet1.battery_voltage , packet1.log_number ); mavlink_msg_arp_tm_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2973,7 +2975,7 @@ static void mavlink_test_arp_tm(uint8_t system_id, uint8_t component_id, mavlink MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_arp_tm_send(MAVLINK_COMM_1 , packet1.timestamp , packet1.yaw , packet1.pitch , packet1.roll , packet1.target_yaw , packet1.target_pitch , packet1.stepperX_pos , packet1.stepperX_delta , packet1.stepperX_speed , packet1.stepperY_pos , packet1.stepperY_delta , packet1.stepperY_speed , packet1.gps_latitude , packet1.gps_longitude , packet1.gps_height , packet1.gps_fix , packet1.main_radio_present , packet1.main_packet_tx_error_count , packet1.main_tx_bitrate , packet1.main_packet_rx_success_count , packet1.main_packet_rx_drop_count , packet1.main_rx_bitrate , packet1.main_rx_rssi , packet1.payload_radio_present , packet1.payload_packet_tx_error_count , packet1.payload_tx_bitrate , packet1.payload_packet_rx_success_count , packet1.payload_packet_rx_drop_count , packet1.payload_rx_bitrate , packet1.payload_rx_rssi , packet1.ethernet_present , packet1.ethernet_status , packet1.battery_voltage ); + mavlink_msg_arp_tm_send(MAVLINK_COMM_1 , packet1.timestamp , packet1.state , packet1.yaw , packet1.pitch , packet1.roll , packet1.target_yaw , packet1.target_pitch , packet1.stepperX_pos , packet1.stepperX_delta , packet1.stepperX_speed , packet1.stepperY_pos , packet1.stepperY_delta , packet1.stepperY_speed , packet1.gps_latitude , packet1.gps_longitude , packet1.gps_height , packet1.gps_fix , packet1.main_radio_present , packet1.main_packet_tx_error_count , packet1.main_tx_bitrate , packet1.main_packet_rx_success_count , packet1.main_packet_rx_drop_count , packet1.main_rx_bitrate , packet1.main_rx_rssi , packet1.payload_radio_present , packet1.payload_packet_tx_error_count , packet1.payload_tx_bitrate , packet1.payload_packet_rx_success_count , packet1.payload_packet_rx_drop_count , packet1.payload_rx_bitrate , packet1.payload_rx_rssi , packet1.ethernet_present , packet1.ethernet_status , packet1.battery_voltage , packet1.log_number ); mavlink_msg_arp_tm_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); diff --git a/mavlink_lib/lyra/version.h b/mavlink_lib/lyra/version.h index d4beaede7c6ea046a4c9f09b5fee9c9169d1b8da..cc05bb70ad7f35e5c50a6a946bd72b4c9886b494 100644 --- a/mavlink_lib/lyra/version.h +++ b/mavlink_lib/lyra/version.h @@ -7,7 +7,7 @@ #ifndef MAVLINK_VERSION_H #define MAVLINK_VERSION_H -#define MAVLINK_BUILD_DATE "Thu Jul 11 2024" +#define MAVLINK_BUILD_DATE "Sat Jul 13 2024" #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 167 diff --git a/message_definitions/lyra.xml b/message_definitions/lyra.xml index c13321d797aa462438d9a864d4bb271bd511f692..19e6e15c00987fce98e7917f6977f9afbe9f926c 100644 --- a/message_definitions/lyra.xml +++ b/message_definitions/lyra.xml @@ -8,6 +8,8 @@ <entry name="MAV_SYSID_PAYLOAD" value="2"></entry> <entry name="MAV_SYSID_RIG" value="3"></entry> <entry name="MAV_SYSID_GS" value="4"></entry> + <entry name="MAV_SYSID_ARP" value="5"></entry> + </enum> <enum name="SystemTMList"> <description>Enum list for all the telemetries that can be requested</description> @@ -226,6 +228,15 @@ <entry name="MAV_CMD_ENTER_HIL" value="22"> <description>Command to enter HIL mode at next reboot</description> </entry> + <entry name="MAV_CMD_RESET_ALGORITHM" value="23"> + <description>Command to reset the set algorithm. Used for now in ARP to reset the follow algorithm and return to armed state.</description> + </entry> + <entry name="MAV_CMD_ARP_FORCE_NO_FEEDBACK" value="24"> + <description>Command to force ARP system to entern the no feedback from VN300 state</description> + </entry> + <entry name="MAV_CMD_ARP_FOLLOW" value="25"> + <description>Command to enter in the ARP's follow state and procedure to follow the rocket</description> + </entry> </enum> <enum name="ServosList"> <description>Enum of all the servos</description> @@ -545,6 +556,7 @@ <message id="150" name="ARP_TM"> <description></description> <field name="timestamp" type="uint64_t" units="us">Timestamp</field> + <field name="state" type="uint8_t">State Machine Controller State</field> <field name="yaw" type="float" units="deg">Current Yaw</field> <field name="pitch" type="float" units="deg">Current Pitch</field> <field name="roll" type="float" units="deg">Current Roll</field> @@ -577,6 +589,7 @@ <field name="ethernet_present" type="uint8_t">Boolean indicating the presence of the ethernet module</field> <field name="ethernet_status" type="uint8_t">Status flag indicating the status of the ethernet PHY</field> <field name="battery_voltage" type="float" units="V">Battery voltage</field> + <field name="log_number" type="int16_t">Currently active log file, -1 if the logger is inactive</field> </message> <!-- FROM ROCKET TO GROUND: SPECIFIC -->