From d950361a84054a1e61833ee025ff6d2d597fd10c Mon Sep 17 00:00:00 2001 From: LM Date: Tue, 9 Aug 2011 18:24:40 +0200 Subject: [PATCH] Updated MAVLink v1.0 release candidate sources --- .../generator/MAVLinkXMLParserV10.cc | 174 ++- thirdParty/mavlink/.gitignore | 1 + .../include/ardupilotmega/ardupilotmega.h | 9 +- .../mavlink/include/ardupilotmega/mavlink.h | 7 +- thirdParty/mavlink/include/common/common.h | 213 +-- thirdParty/mavlink/include/common/mavlink.h | 7 +- .../include/common/mavlink_msg_action.h | 27 +- .../include/common/mavlink_msg_action_ack.h | 26 +- .../include/common/mavlink_msg_attitude.h | 31 +- .../mavlink_msg_attitude_controller_output.h | 29 +- .../include/common/mavlink_msg_auth_key.h | 25 +- .../mavlink/include/common/mavlink_msg_boot.h | 25 +- .../mavlink_msg_change_operator_control.h | 28 +- .../mavlink_msg_change_operator_control_ack.h | 27 +- .../include/common/mavlink_msg_command.h | 40 +- .../include/common/mavlink_msg_command_ack.h | 26 +- .../common/mavlink_msg_control_status.h | 32 +- .../include/common/mavlink_msg_debug.h | 28 +- .../include/common/mavlink_msg_debug_vect.h | 31 +- .../include/common/mavlink_msg_full_state.h | 394 ++++++ .../common/mavlink_msg_global_position.h | 31 +- .../common/mavlink_msg_global_position_int.h | 30 +- .../common/mavlink_msg_gps_local_origin_set.h | 27 +- .../include/common/mavlink_msg_gps_raw.h | 35 +- .../include/common/mavlink_msg_gps_raw_int.h | 35 +- .../mavlink_msg_gps_set_global_origin.h | 33 +- .../include/common/mavlink_msg_gps_status.h | 30 +- .../include/common/mavlink_msg_heartbeat.h | 27 +- .../common/mavlink_msg_local_position.h | 31 +- .../mavlink_msg_local_position_setpoint.h | 28 +- .../mavlink_msg_local_position_setpoint_set.h | 34 +- .../common/mavlink_msg_manual_control.h | 35 +- .../common/mavlink_msg_named_value_float.h | 28 +- .../common/mavlink_msg_named_value_int.h | 28 +- .../mavlink_msg_nav_controller_output.h | 38 +- .../common/mavlink_msg_param_request_list.h | 26 +- .../common/mavlink_msg_param_request_read.h | 52 +- .../include/common/mavlink_msg_param_set.h | 52 +- .../include/common/mavlink_msg_param_value.h | 50 +- .../mavlink/include/common/mavlink_msg_ping.h | 30 +- .../mavlink_msg_position_controller_output.h | 29 +- .../common/mavlink_msg_position_target.h | 28 +- .../include/common/mavlink_msg_raw_imu.h | 34 +- .../include/common/mavlink_msg_raw_pressure.h | 29 +- .../common/mavlink_msg_rc_channels_override.h | 286 ++++ .../common/mavlink_msg_rc_channels_raw.h | 33 +- .../common/mavlink_msg_rc_channels_scaled.h | 33 +- .../common/mavlink_msg_request_data_stream.h | 31 +- .../common/mavlink_msg_safety_allowed_area.h | 33 +- .../mavlink_msg_safety_set_allowed_area.h | 39 +- .../include/common/mavlink_msg_scaled_imu.h | 34 +- .../common/mavlink_msg_scaled_pressure.h | 28 +- .../common/mavlink_msg_servo_output_raw.h | 32 +- .../include/common/mavlink_msg_set_altitude.h | 28 +- .../include/common/mavlink_msg_set_mode.h | 26 +- .../include/common/mavlink_msg_set_nav_mode.h | 26 +- .../common/mavlink_msg_set_roll_pitch_yaw.h | 196 +++ .../mavlink_msg_set_roll_pitch_yaw_speed.h | 196 +++ .../common/mavlink_msg_state_correction.h | 33 +- .../include/common/mavlink_msg_statustext.h | 42 +- .../include/common/mavlink_msg_sys_status.h | 37 +- .../include/common/mavlink_msg_system_time.h | 25 +- .../common/mavlink_msg_system_time_utc.h | 26 +- .../include/common/mavlink_msg_vfr_hud.h | 34 +- .../include/common/mavlink_msg_waypoint.h | 52 +- .../include/common/mavlink_msg_waypoint_ack.h | 27 +- .../common/mavlink_msg_waypoint_clear_all.h | 26 +- .../common/mavlink_msg_waypoint_count.h | 29 +- .../common/mavlink_msg_waypoint_current.h | 25 +- .../common/mavlink_msg_waypoint_reached.h | 25 +- .../common/mavlink_msg_waypoint_request.h | 29 +- .../mavlink_msg_waypoint_request_list.h | 26 +- .../common/mavlink_msg_waypoint_set_current.h | 29 +- thirdParty/mavlink/include/mavlink_checksum.h | 156 +++ thirdParty/mavlink/include/mavlink_data.h | 21 + thirdParty/mavlink/include/mavlink_options.h | 102 ++ thirdParty/mavlink/include/mavlink_protocol.h | 951 +++++++++++++ thirdParty/mavlink/include/mavlink_types.h | 327 +++-- thirdParty/mavlink/include/minimal/mavlink.h | 7 +- .../include/minimal/mavlink_msg_heartbeat.h | 27 +- thirdParty/mavlink/include/minimal/minimal.h | 9 +- thirdParty/mavlink/include/pixhawk/mavlink.h | 7 +- .../pixhawk/mavlink_msg_attitude_control.h | 35 +- .../include/pixhawk/mavlink_msg_aux_status.h | 30 +- .../pixhawk/mavlink_msg_brief_feature.h | 52 +- .../mavlink_msg_data_transmission_handshake.h | 31 +- .../pixhawk/mavlink_msg_encapsulated_data.h | 26 +- .../pixhawk/mavlink_msg_image_available.h | 57 +- .../mavlink_msg_image_trigger_control.h | 25 +- .../pixhawk/mavlink_msg_image_triggered.h | 36 +- .../include/pixhawk/mavlink_msg_marker.h | 33 +- .../pixhawk/mavlink_msg_pattern_detected.h | 46 +- .../pixhawk/mavlink_msg_point_of_interest.h | 62 +- ...mavlink_msg_point_of_interest_connection.h | 65 +- .../mavlink_msg_position_control_offset_set.h | 34 +- .../mavlink_msg_position_control_setpoint.h | 31 +- ...avlink_msg_position_control_setpoint_set.h | 37 +- .../include/pixhawk/mavlink_msg_raw_aux.h | 33 +- .../pixhawk/mavlink_msg_set_cam_shutter.h | 36 +- .../mavlink_msg_vicon_position_estimate.h | 31 +- .../mavlink_msg_vision_position_estimate.h | 31 +- .../mavlink_msg_vision_speed_estimate.h | 28 +- .../pixhawk/mavlink_msg_watchdog_command.h | 30 +- .../pixhawk/mavlink_msg_watchdog_heartbeat.h | 26 +- .../mavlink_msg_watchdog_process_info.h | 57 +- .../mavlink_msg_watchdog_process_status.h | 34 +- thirdParty/mavlink/include/pixhawk/pixhawk.h | 9 +- thirdParty/mavlink/include/slugs/mavlink.h | 7 +- .../include/slugs/mavlink_msg_air_data.h | 27 +- .../include/slugs/mavlink_msg_cpu_load.h | 29 +- .../include/slugs/mavlink_msg_ctrl_srfc_pt.h | 28 +- .../include/slugs/mavlink_msg_data_log.h | 30 +- .../include/slugs/mavlink_msg_diagnostic.h | 30 +- .../include/slugs/mavlink_msg_gps_date_time.h | 31 +- .../include/slugs/mavlink_msg_mid_lvl_cmds.h | 30 +- .../include/slugs/mavlink_msg_sensor_bias.h | 30 +- .../include/slugs/mavlink_msg_slugs_action.h | 29 +- .../slugs/mavlink_msg_slugs_navigation.h | 33 +- thirdParty/mavlink/include/slugs/slugs.h | 9 +- thirdParty/mavlink/include/ualberta/mavlink.h | 7 +- .../ualberta/mavlink_msg_nav_filter_bias.h | 31 +- .../ualberta/mavlink_msg_radio_calibration.h | 30 +- .../mavlink_msg_ualberta_sys_status.h | 27 +- .../mavlink/include/ualberta/ualberta.h | 9 +- .../message_definitions/ardupilotmega.xml | 6 +- .../mavlink/message_definitions/common.xml | 71 +- .../mavlink/message_definitions/minimal.xml | 20 +- .../mavlink/message_definitions/pixhawk.xml | 493 ++++--- .../mavlink/message_definitions/slugs.xml | 220 +-- .../mavlink/message_definitions/ualberta.xml | 102 +- .../mavlink/missionlib/testing/.gitignore | 1 + thirdParty/mavlink/missionlib/testing/main.c | 1221 +++++++++++++++++ thirdParty/mavlink/missionlib/testing/udp.c | 864 +++++++++++- thirdParty/mavlink/missionlib/waypoints.c | 880 ++++++++++++ thirdParty/mavlink/missionlib/waypoints.h | 91 ++ 135 files changed, 6671 insertions(+), 3703 deletions(-) create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_full_state.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h create mode 100644 thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h create mode 100755 thirdParty/mavlink/include/mavlink_checksum.h create mode 100755 thirdParty/mavlink/include/mavlink_data.h create mode 100755 thirdParty/mavlink/include/mavlink_options.h create mode 100755 thirdParty/mavlink/include/mavlink_protocol.h mode change 100644 => 100755 thirdParty/mavlink/include/mavlink_types.h create mode 100644 thirdParty/mavlink/missionlib/testing/main.c create mode 100644 thirdParty/mavlink/missionlib/waypoints.c create mode 100644 thirdParty/mavlink/missionlib/waypoints.h diff --git a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc index 4b78f9fc6..90568e469 100644 --- a/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc +++ b/src/apps/mavlinkgen/generator/MAVLinkXMLParserV10.cc @@ -97,11 +97,60 @@ void MAVLinkXMLParserV10::crcInit(uint16_t* crcAccum) } +const struct { + const char *prefix; + unsigned length; +} length_map[] = { + { "array", 1 }, + { "char", 1 }, + { "uint8", 1 }, + { "int8", 1 }, + { "uint16", 2 }, + { "int16", 2 }, + { "uint32", 4 }, + { "int32", 4 }, + { "uint64", 8 }, + { "int64", 8 }, + { "float", 4 }, + { "double", 8 }, +}; + +unsigned itemLength( const QString &s1 ) +{ + unsigned el1, i1, i2; + QString Ss1 = s1; + + Ss1 = Ss1.replace("_"," "); + Ss1 = Ss1.simplified(); + Ss1 = Ss1.section(" ",0,0); + + el1 = i1 = 0; + i2 = sizeof(length_map)/sizeof(length_map[0]); + + do { + if (Ss1.startsWith(length_map[i1].prefix)) + el1 = length_map[i1].length; + else ; + i1++; + } while ( (el1 == 0) && (i1 < i2)); + return el1; +} + +bool structSort(const QString &s1, const QString &s2) +{ + unsigned el1, el2; + + el1 = itemLength( s1 ); + el2 = itemLength( s2 ); + return el2 < el1; +} + /** * Generate C-code (C-89 compliant) out of the XML protocol specs. */ bool MAVLinkXMLParserV10::generate() { + uint16_t crc_key = X25_INIT_CRC; // Process result bool success = true; @@ -146,7 +195,15 @@ bool MAVLinkXMLParserV10::generate() QDir dir(outputDirName + "/" + messagesDirName); int mavlinkVersion = 0; - + static unsigned message_lengths[256]; + static unsigned message_key[256]; + static int highest_message_id; + static int recursion_level; + + if (recursion_level == 0) { + highest_message_id = 0; + memset(message_lengths, 0, sizeof(message_lengths)); + } // Start main header @@ -457,6 +514,7 @@ bool MAVLinkXMLParserV10::generate() QString sendArguments; QString commentLines; int calculatedLength = 0; + unsigned message_length = 0; // Get the message fields @@ -506,17 +564,17 @@ bool MAVLinkXMLParserV10::generate() QStringList itemList; // Swap field in C structure itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); cStructLines = itemList.join("\n") + "\n"; // Swap line in message_xx_pack function itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); packLines = itemList.join("\n") + "\n"; // Swap line in decode function for this type itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); decodeLines = itemList.join("\n") + "\n"; } } @@ -541,7 +599,7 @@ bool MAVLinkXMLParserV10::generate() // packLines += QString("\ti += put_%1_by_index(%2, %3, i, msg->payload); // %4\n").arg(arrayType, fieldName, QString::number(arrayLength), e2.text()); packLines += QString("\tstrncpy( p->%2, %2, sizeof(p->%2)); // %1[%3]:%4\n").arg("char", fieldName, QString::number(arrayLength), fieldText); // Add decode function for this type - decodeLines += QString("\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); + decodeLines += QString("\n\tmavlink_msg_%1_get_%2(msg, %1->%2);\n").arg(messageName, fieldName); arrayDefines += QString("#define MAVLINK_MSG_%1_FIELD_%2_LEN %3\n").arg(messageName.toUpper(), fieldName.toUpper(), QString::number(arrayLength)); if (fieldOffset != "") @@ -661,17 +719,17 @@ bool MAVLinkXMLParserV10::generate() QStringList itemList; // Swap field in C structure itemList = cStructLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); cStructLines = itemList.join("\n") + "\n"; // Swap line in message_xx_pack function itemList = packLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); packLines = itemList.join("\n") + "\n"; // Swap line in decode function for this type itemList = decodeLines.split("\n", QString::SkipEmptyParts); - if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); else ; + if (itemList.size() > 1) itemList.swap(itemList.size() - 1, itemList.size() - 2); decodeLines = itemList.join("\n") + "\n"; } if (fieldType.contains("char")) @@ -705,6 +763,26 @@ bool MAVLinkXMLParserV10::generate() } } + // message length calculation + unsigned element_multiplier = 1; + unsigned element_length = 0; + + if (fieldType.contains("[")) { + element_multiplier = fieldType.split("[").at(1).split("]").first().toInt(); + } + for (unsigned i=0; iERROR: Unable to calculate length for %2 near line %1\nAbort.").arg(QString::number(e.lineNumber()), fieldType)); + return false; + } + message_length += element_length; + // // QString unpackingCode; @@ -791,26 +869,71 @@ bool MAVLinkXMLParserV10::generate() f = f.nextSibling(); } + if (messageId > highest_message_id) { + highest_message_id = messageId; + } + message_lengths[messageId] = message_length; + + + // Sort fields to ensure 16bit-boundary aligned data + QStringList fieldList; + // Stable sort fields in C structure + fieldList = cStructLines.split("\n", QString::SkipEmptyParts); + if (fieldList.size() > 1) + { + qStableSort(fieldList.begin(), fieldList.end(), structSort); + } else ; + + // struct now sorted, do crc calc for each field + QString fieldCRCstring; + QByteArray fieldText; + crc_key = X25_INIT_CRC; + + for (int i =0; i < fieldList.size(); i++) + { + fieldCRCstring = fieldList.at(i).simplified(); + fieldCRCstring = fieldCRCstring.section(" ",0,1); // note: this has one space bewteen type and name + fieldText = fieldCRCstring.toAscii(); + for (int i = 0; i < fieldText.size(); ++i) + { + crcAccumulate((uint8_t) fieldText.at(i), &crc_key); + } + } + + // generate the key byte value + QString stringCRC; + message_key[messageId] = (crc_key&0xff)^((crc_key>>8)&0xff); + stringCRC = stringCRC.number( message_key[messageId], 16); + + // create structure + cStructLines = fieldList.join("\n") + "\n"; + + + // cStruct = cStruct.arg(cStructName, cStructLines, QString::number(calculatedLength) ); cStruct = cStruct.arg(cStructName, cStructLines ); lcmStructDefs.append("\n").append(cStruct).append("\n"); pack = pack.arg(messageName, packParameters, messageName.toUpper(), packLines); packChan = packChan.arg(messageName, packParameters, messageName.toUpper(), packLines); encode = encode.arg(messageName).arg(cStructName).arg(packArguments); - // decode = decode.arg(messageName).arg(cStructName).arg(decodeLines); +// decode = decode.arg(messageName).arg(cStructName).arg(decodeLines); decode = decode.arg(messageName).arg(cStructName); - // QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"); - QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = (mavlink_%3_t *)&msg.payload[0];\n\n%6\n\tmsg.STX = MAVLINK_STX;\n\tmsg.len = MAVLINK_MSG_ID_%4_LEN;\n\tmsg.msgid = MAVLINK_MSG_ID_%4;\n"); - QString compactSend2("\tmsg.sysid = mavlink_system.sysid;\n\tmsg.compid = mavlink_system.compid;\n\tmsg.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;\n"); - QString compactSend3("\tchecksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);\n\tmsg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\tmsg.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_msg(chan, &msg);\n}\n\n#endif"); - // compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters ); - compactSend = compactSend.arg(channelType, messageType, messageName, messageName.toUpper(), packParameters, packLines ) + compactSend2 + compactSend3; - QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); +// QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\n\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tmavlink_msg_%3_pack_chan(mavlink_system.sysid, mavlink_system.compid, chan, &msg%4);\n\tmavlink_send_uart(chan, &msg);\n}\n\n#endif"); +// QString compactSend("#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 msg;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = (mavlink_%3_t *)&msg.payload[0];\n\n%6\n\tmsg.STX = MAVLINK_STX;\n\tmsg.len = MAVLINK_MSG_ID_%4_LEN;\n\tmsg.msgid = MAVLINK_MSG_ID_%4;\n"); +// QString compactSend2("\tmsg.sysid = mavlink_system.sysid;\n\tmsg.compid = mavlink_system.compid;\n\tmsg.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;\n"); +// QString compactSend3("\tchecksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);\n\tmsg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\tmsg.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_msg(chan, &msg);\n}\n\n#endif"); +// compactSend = compactSend.arg(channelType, messageType, messageName, sendArguments, packParameters ); +// compactSend = compactSend.arg(channelType, messageType, messageName, messageName.toUpper(), packParameters, packLines ) + compactSend2 + compactSend3; +// QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); + QString compact2Send("\n\n#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS\nstatic inline void mavlink_msg_%3_send(%1 chan%5)\n{\n\t%2 hdr;\n\tmavlink_%3_t payload;\n\tuint16_t checksum;\n\tmavlink_%3_t *p = &payload;\n\n%6\n\thdr.STX = MAVLINK_STX;\n\thdr.len = MAVLINK_MSG_ID_%4_LEN;\n\thdr.msgid = MAVLINK_MSG_ID_%4;\n"); QString compact2Send2("\thdr.sysid = mavlink_system.sysid;\n\thdr.compid = mavlink_system.compid;\n\thdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;\n\tmavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );\n"); QString compact2Send3("\n\tcrc_init(&checksum);\n\tchecksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);\n\tchecksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );\n\thdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte\n\thdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte\n\n\tmavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);\n\tmavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);\n}\n\n#endif"); - compact2Send = compact2Send.arg(channelType, headerType, messageName, messageName.toUpper(), packParameters, packLines ) + compact2Send2 + compact2Send3; - QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength)) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compactSend + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; + compact2Send = compact2Send.arg(channelType, headerType, messageName, messageName.toUpper(), packParameters, packLines ) + compact2Send2 + compact2Send3.arg(stringCRC.toUpper()); +// QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compactSend + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; + QString cFile = "// MESSAGE " + messageName.toUpper() + " PACKING\n\n" + idDefine.arg(messageName.toUpper(), QString::number(messageId), QString::number(calculatedLength), stringCRC.toUpper() ) + "\n\n" + cStruct + "\n" + arrayDefines + "\n" + commentContainer.arg(messageName.toLower(), commentLines) + pack + commentPackChanContainer.arg(messageName.toLower(), commentLines) + packChan + commentEncodeContainer.arg(messageName.toLower()) + encode + "\n" + commentSendContainer.arg(messageName.toLower(), commentLines) + compact2Send + "\n" + "// MESSAGE " + messageName.toUpper() + " UNPACKING\n\n" + unpacking + commentDecodeContainer.arg(messageName.toLower()) + decode; cFiles.append(qMakePair(QString("mavlink_msg_%1.h").arg(messageName), cFile)); + + emit parseState(tr("Compiled message %1 \t(#%3) \tend near line %2, length %4, crc key 0x%5(%6)").arg(messageName, QString::number(n.lineNumber()), QString::number(messageId), QString::number(message_lengths[messageId]), stringCRC.toUpper(), QString::number(message_key[messageId]))); } // Check if tag = message } // Check if e = NULL n = n.nextSibling(); @@ -854,6 +977,16 @@ bool MAVLinkXMLParserV10::generate() mainHeader += includeLine.arg(messagesDirName + "/" + cFiles.at(i).first); } + // CRC seeds + mainHeader += "\n\n// MESSAGE CRC KEYS\n\n"; + mainHeader += "#undef MAVLINK_MESSAGE_KEYS\n"; + mainHeader += "#define MAVLINK_MESSAGE_KEYS { "; + for (int i=0; i 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. | Desired yaw angle at waypoint (rotary wing) | Latitude | Longitude | Altitude | */ - MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of time | Empty | Empty | Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise | Desired yaw angle. | Latitude | Longitude | Altitude | */ - MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns | Turns | Empty | Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise | Desired yaw angle. | Latitude | Longitude | Altitude | */ - MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this waypoint for X seconds | Seconds (decimal) | Empty | Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise | Desired yaw angle. | Latitude | Longitude | Altitude | */ - MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location | Empty | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_NAV_LAND=21, /* Land at location | Empty | Empty | Empty | Desired yaw angle. | Latitude | Longitude | Altitude | */ - MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand | Minimum pitch (if airspeed sensor present), desired pitch without sensor | Empty | Empty | Yaw angle (if magnetometer present), ignored without magnetometer | Latitude | Longitude | Altitude | */ - MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. | Region of intereset mode. (see MAV_ROI enum) | Waypoint index/ target ID. (see MAV_ROI enum) | ROI index (allows a vehicle to manage multiple ROI's) | Empty | x the location of the fixed ROI (see MAV_FRAME) | y | z | */ - MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. | 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning | 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid | Empty | Yaw angle at goal, in compass degrees, [0..360] | Latitude/X of goal | Longitude/Y of goal | Altitude/Z of goal | */ - MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration | Empty | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. | Delay in seconds (decimal) | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. | Descent / Ascend rate (m/s) | Empty | Empty | Empty | Empty | Empty | Finish Altitude | */ - MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. | Distance (meters) | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. | target angle: [0-360], 0 is north | speed during yaw change:[deg per second] | direction: negative: counter clockwise, positive: clockwise [-1,1] | relative offset or absolute angle: [ 1,0] | Empty | Empty | Empty | */ - MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration | Empty | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_DO_SET_MODE=176, /* Set system mode. | Mode, as defined by ENUM MAV_MODE | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times | Sequence number | Repeat count | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. | Speed type (0=Airspeed, 1=Ground Speed) | Speed (m/s, -1 indicates no change) | Throttle ( Percent, -1 indicates no change) | Empty | Empty | Empty | Empty | */ - MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. | Use current (1=use current location, 0=use specified location) | Empty | Empty | Empty | Latitude | Longitude | Altitude | */ - MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. | Parameter number | Parameter value | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. | Relay number | Setting (1=on, 0=off, others possible depending on system hardware) | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. | Relay number | Cycle count | Cycle time (seconds, decimal) | Empty | Empty | Empty | Empty | */ - MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. | Servo number | PWM (microseconds, 1000 to 2000 typical) | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. | Servo number | PWM (microseconds, 1000 to 2000 typical) | Cycle count | Cycle time (seconds) | Empty | Empty | Empty | */ - MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. | Camera ID (-1 for all) | Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw | Transmission mode: 0: video stream, >0: single images every n seconds (decimal) | Recording: 0: disabled, 1: enabled compressed, 2: enabled raw | Empty | Empty | Empty | */ - MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration | Empty | Empty | Empty | Empty | Empty | Empty | Empty | */ - MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. | Gyro calibration: 0: no, 1: yes | Magnetometer calibration: 0: no, 1: yes | Ground pressure: 0: no, 1: yes | Radio calibration: 0: no, 1: yes | Empty | Empty | Empty | */ - MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. | Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM | Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM | Reserved | Reserved | Empty | Empty | Empty | */ + MAV_CMD_NAV_WAYPOINT=16, /* Navigate to waypoint. | Hold time in decimal seconds. (ignored by fixed wing, time to stay at waypoint for rotary wing) | Acceptance radius in meters (if the sphere with this radius is hit, the waypoint counts as reached) | 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control. | Desired yaw angle at waypoint (rotary wing) | Latitude | Longitude | Altitude */ + MAV_CMD_NAV_LOITER_UNLIM=17, /* Loiter around this waypoint an unlimited amount of time | Empty | Empty | Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise | Desired yaw angle. | Latitude | Longitude | Altitude */ + MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns | Turns | Empty | Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise | Desired yaw angle. | Latitude | Longitude | Altitude */ + MAV_CMD_NAV_LOITER_TIME=19, /* Loiter around this waypoint for X seconds | Seconds (decimal) | Empty | Radius around waypoint, in meters. If positive loiter clockwise, else counter-clockwise | Desired yaw angle. | Latitude | Longitude | Altitude */ + MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location | Empty | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_NAV_LAND=21, /* Land at location | Empty | Empty | Empty | Desired yaw angle. | Latitude | Longitude | Altitude */ + MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand | Minimum pitch (if airspeed sensor present), desired pitch without sensor | Empty | Empty | Yaw angle (if magnetometer present), ignored without magnetometer | Latitude | Longitude | Altitude */ + MAV_CMD_NAV_ROI=80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. | Region of intereset mode. (see MAV_ROI enum) | Waypoint index/ target ID. (see MAV_ROI enum) | ROI index (allows a vehicle to manage multiple ROI's) | Empty | x the location of the fixed ROI (see MAV_FRAME) | y | z */ + MAV_CMD_NAV_PATHPLANNING=81, /* Control autonomous path planning on the MAV. | 0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning | 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid | Empty | Yaw angle at goal, in compass degrees, [0..360] | Latitude/X of goal | Longitude/Y of goal | Altitude/Z of goal */ + MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration | Empty | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. | Delay in seconds (decimal) | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_CONDITION_CHANGE_ALT=113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. | Descent / Ascend rate (m/s) | Empty | Empty | Empty | Empty | Empty | Finish Altitude */ + MAV_CMD_CONDITION_DISTANCE=114, /* Delay mission state machine until within desired distance of next NAV point. | Distance (meters) | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_CONDITION_YAW=115, /* Reach a certain target angle. | target angle: [0-360], 0 is north | speed during yaw change:[deg per second] | direction: negative: counter clockwise, positive: clockwise [-1,1] | relative offset or absolute angle: [ 1,0] | Empty | Empty | Empty */ + MAV_CMD_CONDITION_LAST=159, /* NOP - This command is only used to mark the upper limit of the CONDITION commands in the enumeration | Empty | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_DO_SET_MODE=176, /* Set system mode. | Mode, as defined by ENUM MAV_MODE | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_DO_JUMP=177, /* Jump to the desired command in the mission list. Repeat this action only the specified number of times | Sequence number | Repeat count | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_DO_CHANGE_SPEED=178, /* Change speed and/or throttle set points. | Speed type (0=Airspeed, 1=Ground Speed) | Speed (m/s, -1 indicates no change) | Throttle ( Percent, -1 indicates no change) | Empty | Empty | Empty | Empty */ + MAV_CMD_DO_SET_HOME=179, /* Changes the home location either to the current location or a specified location. | Use current (1=use current location, 0=use specified location) | Empty | Empty | Empty | Latitude | Longitude | Altitude */ + MAV_CMD_DO_SET_PARAMETER=180, /* Set a system parameter. Caution! Use of this command requires knowledge of the numeric enumeration value of the parameter. | Parameter number | Parameter value | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_DO_SET_RELAY=181, /* Set a relay to a condition. | Relay number | Setting (1=on, 0=off, others possible depending on system hardware) | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_DO_REPEAT_RELAY=182, /* Cycle a relay on and off for a desired number of cyles with a desired period. | Relay number | Cycle count | Cycle time (seconds, decimal) | Empty | Empty | Empty | Empty */ + MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. | Servo number | PWM (microseconds, 1000 to 2000 typical) | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. | Servo number | PWM (microseconds, 1000 to 2000 typical) | Cycle count | Cycle time (seconds) | Empty | Empty | Empty */ + MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. | Camera ID (-1 for all) | Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw | Transmission mode: 0: video stream, >0: single images every n seconds (decimal) | Recording: 0: disabled, 1: enabled compressed, 2: enabled raw | Empty | Empty | Empty */ + MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration | Empty | Empty | Empty | Empty | Empty | Empty | Empty */ + MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. | Gyro calibration: 0: no, 1: yes | Magnetometer calibration: 0: no, 1: yes | Ground pressure: 0: no, 1: yes | Radio calibration: 0: no, 1: yes | Empty | Empty | Empty */ + MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. | Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM | Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM | Reserved | Reserved | Empty | Empty | Empty */ MAV_CMD_ENUM_END }; @@ -274,6 +274,8 @@ enum MAV_CMD #include "./mavlink_msg_control_status.h" #include "./mavlink_msg_safety_set_allowed_area.h" #include "./mavlink_msg_safety_allowed_area.h" +#include "./mavlink_msg_set_roll_pitch_yaw.h" +#include "./mavlink_msg_set_roll_pitch_yaw_speed.h" #include "./mavlink_msg_attitude_controller_output.h" #include "./mavlink_msg_position_controller_output.h" #include "./mavlink_msg_nav_controller_output.h" @@ -281,7 +283,9 @@ enum MAV_CMD #include "./mavlink_msg_state_correction.h" #include "./mavlink_msg_set_altitude.h" #include "./mavlink_msg_request_data_stream.h" +#include "./mavlink_msg_full_state.h" #include "./mavlink_msg_manual_control.h" +#include "./mavlink_msg_rc_channels_override.h" #include "./mavlink_msg_global_position_int.h" #include "./mavlink_msg_vfr_hud.h" #include "./mavlink_msg_command.h" @@ -291,6 +295,13 @@ enum MAV_CMD #include "./mavlink_msg_named_value_int.h" #include "./mavlink_msg_statustext.h" #include "./mavlink_msg_debug.h" + + +// MESSAGE CRC KEYS + +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index ef3973fbb..29d8958ff 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,15 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sunday, July 31 2011, 15:12 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H #pragma pack(push,1) +#include "mavlink_options.h" #include "common.h" -#ifdef MAVLINK_CHECK_LENGTH -#include "lengths.h" +#ifdef MAVLINK_DATA +#include "mavlink_data.h" #endif #pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action.h b/thirdParty/mavlink/include/common/mavlink_msg_action.h index 82ec1f50e..636689544 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_action.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_action.h @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_action_encode(uint8_t system_id, uint8_t comp * @param target_component The component executing the action * @param action The action id */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_action_t *p = (mavlink_action_t *)&msg.payload[0]; - - p->target = target; // uint8_t:The system executing the action - p->target_component = target_component; // uint8_t:The component executing the action - p->action = action; // uint8_t:The action id - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ACTION_LEN; - msg.msgid = MAVLINK_MSG_ID_ACTION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h index bfb41a819..b407815e7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_action_ack.h @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_action_ack_encode(uint8_t system_id, uint8_t * @param action The action id * @param result 0: Action DENIED, 1: Action executed */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg.payload[0]; - p->action = action; // uint8_t:The action id - p->result = result; // uint8_t:0: Action DENIED, 1: Action executed - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ACTION_ACK_LEN; - msg.msgid = MAVLINK_MSG_ID_ACTION_ACK; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h index 5d395fda6..8c32cd48c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude.h @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co * @param pitchspeed Pitch angular speed (rad/s) * @param yawspeed Yaw angular speed (rad/s) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_attitude_t *p = (mavlink_attitude_t *)&msg.payload[0]; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->roll = roll; // float:Roll angle (rad) - p->pitch = pitch; // float:Pitch angle (rad) - p->yaw = yaw; // float:Yaw angle (rad) - p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) - p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) - p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ATTITUDE_LEN; - msg.msgid = MAVLINK_MSG_ID_ATTITUDE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h index 081dd82d6..80d73978e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_attitude_controller_output.h @@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_attitude_controller_output_encode(uint8_t sys * @param yaw Attitude yaw: -128: -100%, 127: +100% * @param thrust Attitude thrust: -128: -100%, 127: +100% */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg.payload[0]; - - p->enabled = enabled; // uint8_t:1: enabled, 0: disabled - p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100% - p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100% - p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100% - p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100% - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN; - msg.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h index f857ed91a..36366e2dc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_auth_key.h @@ -68,32 +68,9 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co * * @param key key */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg.payload[0]; - - memcpy(p->key, key, sizeof(p->key)); // char[32]:key - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_AUTH_KEY_LEN; - msg.msgid = MAVLINK_MSG_ID_AUTH_KEY; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_boot.h b/thirdParty/mavlink/include/common/mavlink_msg_boot.h index 00e13e291..9051bb199 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_boot.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_boot.h @@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_boot_encode(uint8_t system_id, uint8_t compon * * @param version The onboard software version */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_boot_t *p = (mavlink_boot_t *)&msg.payload[0]; - - p->version = version; // uint32_t:The onboard software version - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_BOOT_LEN; - msg.msgid = MAVLINK_MSG_ID_BOOT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h index fdc783cac..9b1a2e795 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control.h @@ -86,35 +86,9 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system * @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. * @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System the GCS requests control for - p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch. - memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-" - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN; - msg.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h index b49883dd2..8e9315675 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_change_operator_control_ack.h @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy * @param control_request 0: request control of this MAV, 1: Release control of this MAV * @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg.payload[0]; - - p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message - p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV - p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN; - msg.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command.h b/thirdParty/mavlink/include/common/mavlink_msg_command.h index c22eb7983..7b8afd94b 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command.h @@ -6,14 +6,14 @@ typedef struct __mavlink_command_t { - uint8_t target_system; ///< System which should execute the command - uint8_t target_component; ///< Component which should execute the command, 0 for all components - uint8_t command; ///< Command ID, as defined by MAV_CMD enum. - uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) float param1; ///< Parameter 1, as defined by MAV_CMD enum. float param2; ///< Parameter 2, as defined by MAV_CMD enum. float param3; ///< Parameter 3, as defined by MAV_CMD enum. float param4; ///< Parameter 4, as defined by MAV_CMD enum. + uint8_t target_system; ///< System which should execute the command + uint8_t target_component; ///< Component which should execute the command, 0 for all components + uint8_t command; ///< Command ID, as defined by MAV_CMD enum. + uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) } mavlink_command_t; @@ -109,39 +109,9 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com * @param param3 Parameter 3, as defined by MAV_CMD enum. * @param param4 Parameter 4, as defined by MAV_CMD enum. */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_command_t *p = (mavlink_command_t *)&msg.payload[0]; - - p->target_system = target_system; // uint8_t:System which should execute the command - p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components - p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum. - p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command) - p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum. - p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum. - p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum. - p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum. - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_COMMAND_LEN; - msg.msgid = MAVLINK_MSG_ID_COMMAND; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h index a82703d53..c7e4a4b0f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_command_ack.h @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t * @param command Current airspeed in m/s * @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg.payload[0]; - p->command = command; // float:Current airspeed in m/s - p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_COMMAND_ACK_LEN; - msg.msgid = MAVLINK_MSG_ID_COMMAND_ACK; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h index 00749b618..8974f07d0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_control_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_control_status.h @@ -109,39 +109,9 @@ static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint * @param control_pos_z 0: Z position control disabled, 1: enabled * @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_control_status_t *p = (mavlink_control_status_t *)&msg.payload[0]; - - p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix - p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix - p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix - p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent - p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled - p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled - p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled - p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_CONTROL_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_CONTROL_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug.h b/thirdParty/mavlink/include/common/mavlink_msg_debug.h index 992406335..419ef33e8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug.h @@ -6,8 +6,8 @@ typedef struct __mavlink_debug_t { - uint8_t ind; ///< index of debug variable float value; ///< DEBUG value + uint8_t ind; ///< index of debug variable } mavlink_debug_t; @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo * @param ind index of debug variable * @param value DEBUG value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_debug_t *p = (mavlink_debug_t *)&msg.payload[0]; - p->ind = ind; // uint8_t:index of debug variable - p->value = value; // float:DEBUG value - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_DEBUG_LEN; - msg.msgid = MAVLINK_MSG_ID_DEBUG; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h index b31289ac8..53ef8bd11 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_debug_vect.h @@ -6,11 +6,11 @@ typedef struct __mavlink_debug_vect_t { - char name[10]; ///< Name uint64_t usec; ///< Timestamp float x; ///< x float y; ///< y float z; ///< z + char name[10]; ///< Name } mavlink_debug_vect_t; #define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10 @@ -92,36 +92,9 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t * @param y y * @param z z */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg.payload[0]; - - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name - p->usec = usec; // uint64_t:Timestamp - p->x = x; // float:x - p->y = y; // float:y - p->z = z; // float:z - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_DEBUG_VECT_LEN; - msg.msgid = MAVLINK_MSG_ID_DEBUG_VECT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_full_state.h b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h new file mode 100644 index 000000000..263d3e967 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_full_state.h @@ -0,0 +1,394 @@ +// MESSAGE FULL_STATE PACKING + +#define MAVLINK_MSG_ID_FULL_STATE 67 +#define MAVLINK_MSG_ID_FULL_STATE_LEN 56 +#define MAVLINK_MSG_67_LEN 56 + +typedef struct __mavlink_full_state_t +{ + uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) + float roll; ///< Roll angle (rad) + float pitch; ///< Pitch angle (rad) + float yaw; ///< Yaw angle (rad) + float rollspeed; ///< Roll angular speed (rad/s) + float pitchspeed; ///< Pitch angular speed (rad/s) + float yawspeed; ///< Yaw angular speed (rad/s) + int32_t lat; ///< Latitude, expressed as * 1E7 + int32_t lon; ///< Longitude, expressed as * 1E7 + int32_t alt; ///< Altitude in meters, expressed as * 1000 (millimeters) + int16_t vx; ///< Ground X Speed (Latitude), expressed as m/s * 100 + int16_t vy; ///< Ground Y Speed (Longitude), expressed as m/s * 100 + int16_t vz; ///< Ground Z Speed (Altitude), expressed as m/s * 100 + int16_t xacc; ///< X acceleration (mg) + int16_t yacc; ///< Y acceleration (mg) + int16_t zacc; ///< Z acceleration (mg) + +} mavlink_full_state_t; + +/** + * @brief Pack a full_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_full_state_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_FULL_STATE; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FULL_STATE_LEN); +} + +/** + * @brief Pack a full_state message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_full_state_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_FULL_STATE; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FULL_STATE_LEN); +} + +/** + * @brief Encode a full_state struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param full_state C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_full_state_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_full_state_t* full_state) +{ + return mavlink_msg_full_state_pack(system_id, component_id, msg, full_state->usec, full_state->roll, full_state->pitch, full_state->yaw, full_state->rollspeed, full_state->pitchspeed, full_state->yawspeed, full_state->lat, full_state->lon, full_state->alt, full_state->vx, full_state->vy, full_state->vz, full_state->xacc, full_state->yacc, full_state->zacc); +} + +/** + * @brief Send a full_state message + * @param chan MAVLink channel to send the message + * + * @param usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) + * @param roll Roll angle (rad) + * @param pitch Pitch angle (rad) + * @param yaw Yaw angle (rad) + * @param rollspeed Roll angular speed (rad/s) + * @param pitchspeed Pitch angular speed (rad/s) + * @param yawspeed Yaw angular speed (rad/s) + * @param lat Latitude, expressed as * 1E7 + * @param lon Longitude, expressed as * 1E7 + * @param alt Altitude in meters, expressed as * 1000 (millimeters) + * @param vx Ground X Speed (Latitude), expressed as m/s * 100 + * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 + * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 + * @param xacc X acceleration (mg) + * @param yacc Y acceleration (mg) + * @param zacc Z acceleration (mg) + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_full_state_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz, int16_t xacc, int16_t yacc, int16_t zacc) +{ + mavlink_header_t hdr; + mavlink_full_state_t payload; + uint16_t checksum; + mavlink_full_state_t *p = &payload; + + p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) + p->roll = roll; // float:Roll angle (rad) + p->pitch = pitch; // float:Pitch angle (rad) + p->yaw = yaw; // float:Yaw angle (rad) + p->rollspeed = rollspeed; // float:Roll angular speed (rad/s) + p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s) + p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s) + p->lat = lat; // int32_t:Latitude, expressed as * 1E7 + p->lon = lon; // int32_t:Longitude, expressed as * 1E7 + p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) + p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 + p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 + p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 + p->xacc = xacc; // int16_t:X acceleration (mg) + p->yacc = yacc; // int16_t:Y acceleration (mg) + p->zacc = zacc; // int16_t:Z acceleration (mg) + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_FULL_STATE_LEN; + hdr.msgid = MAVLINK_MSG_ID_FULL_STATE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE FULL_STATE UNPACKING + +/** + * @brief Get field usec from full_state message + * + * @return Timestamp (microseconds since UNIX epoch or microseconds since system boot) + */ +static inline uint64_t mavlink_msg_full_state_get_usec(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (uint64_t)(p->usec); +} + +/** + * @brief Get field roll from full_state message + * + * @return Roll angle (rad) + */ +static inline float mavlink_msg_full_state_get_roll(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @brief Get field pitch from full_state message + * + * @return Pitch angle (rad) + */ +static inline float mavlink_msg_full_state_get_pitch(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @brief Get field yaw from full_state message + * + * @return Yaw angle (rad) + */ +static inline float mavlink_msg_full_state_get_yaw(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @brief Get field rollspeed from full_state message + * + * @return Roll angular speed (rad/s) + */ +static inline float mavlink_msg_full_state_get_rollspeed(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->rollspeed); +} + +/** + * @brief Get field pitchspeed from full_state message + * + * @return Pitch angular speed (rad/s) + */ +static inline float mavlink_msg_full_state_get_pitchspeed(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->pitchspeed); +} + +/** + * @brief Get field yawspeed from full_state message + * + * @return Yaw angular speed (rad/s) + */ +static inline float mavlink_msg_full_state_get_yawspeed(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (float)(p->yawspeed); +} + +/** + * @brief Get field lat from full_state message + * + * @return Latitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_full_state_get_lat(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int32_t)(p->lat); +} + +/** + * @brief Get field lon from full_state message + * + * @return Longitude, expressed as * 1E7 + */ +static inline int32_t mavlink_msg_full_state_get_lon(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int32_t)(p->lon); +} + +/** + * @brief Get field alt from full_state message + * + * @return Altitude in meters, expressed as * 1000 (millimeters) + */ +static inline int32_t mavlink_msg_full_state_get_alt(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int32_t)(p->alt); +} + +/** + * @brief Get field vx from full_state message + * + * @return Ground X Speed (Latitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_full_state_get_vx(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->vx); +} + +/** + * @brief Get field vy from full_state message + * + * @return Ground Y Speed (Longitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_full_state_get_vy(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->vy); +} + +/** + * @brief Get field vz from full_state message + * + * @return Ground Z Speed (Altitude), expressed as m/s * 100 + */ +static inline int16_t mavlink_msg_full_state_get_vz(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->vz); +} + +/** + * @brief Get field xacc from full_state message + * + * @return X acceleration (mg) + */ +static inline int16_t mavlink_msg_full_state_get_xacc(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->xacc); +} + +/** + * @brief Get field yacc from full_state message + * + * @return Y acceleration (mg) + */ +static inline int16_t mavlink_msg_full_state_get_yacc(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->yacc); +} + +/** + * @brief Get field zacc from full_state message + * + * @return Z acceleration (mg) + */ +static inline int16_t mavlink_msg_full_state_get_zacc(const mavlink_message_t* msg) +{ + mavlink_full_state_t *p = (mavlink_full_state_t *)&msg->payload[0]; + return (int16_t)(p->zacc); +} + +/** + * @brief Decode a full_state message into a struct + * + * @param msg The message to decode + * @param full_state C-struct to decode the message contents into + */ +static inline void mavlink_msg_full_state_decode(const mavlink_message_t* msg, mavlink_full_state_t* full_state) +{ + memcpy( full_state, msg->payload, sizeof(mavlink_full_state_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h index bb5112eba..26e2d8a13 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position.h @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin * @param vy Y Speed (in Longitude direction, positive: going east) * @param vz Z Speed (in Altitude direction, positive: going up) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_global_position_t *p = (mavlink_global_position_t *)&msg.payload[0]; - p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch) - p->lat = lat; // float:Latitude, in degrees - p->lon = lon; // float:Longitude, in degrees - p->alt = alt; // float:Absolute altitude, in meters - p->vx = vx; // float:X Speed (in Latitude direction, positive: going north) - p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east) - p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GLOBAL_POSITION_LEN; - msg.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h index fb11a417e..cc0f6e442 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_global_position_int.h @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id, * @param vy Ground Y Speed (Longitude), expressed as m/s * 100 * @param vz Ground Z Speed (Altitude), expressed as m/s * 100 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg.payload[0]; - p->lat = lat; // int32_t:Latitude, expressed as * 1E7 - p->lon = lon; // int32_t:Longitude, expressed as * 1E7 - p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters) - p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100 - p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100 - p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN; - msg.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h index 8834ee6ce..831fc6968 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_local_origin_set.h @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id * @param longitude Longitude (WGS84), expressed as * 1E7 * @param altitude Altitude(WGS84), expressed as * 1000 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg.payload[0]; - - p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7 - p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7 - p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h index ecaeb8b84..6435fa399 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw.h @@ -7,7 +7,6 @@ typedef struct __mavlink_gps_raw_t { uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. float lat; ///< Latitude in degrees float lon; ///< Longitude in degrees float alt; ///< Altitude in meters @@ -15,6 +14,7 @@ typedef struct __mavlink_gps_raw_t float epv; ///< GPS VDOP float v; ///< GPS ground speed float hdg; ///< Compass heading in degrees, 0..360 degrees + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. } mavlink_gps_raw_t; @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com * @param v GPS ground speed * @param hdg Compass heading in degrees, 0..360 degrees */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg.payload[0]; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - p->lat = lat; // float:Latitude in degrees - p->lon = lon; // float:Longitude in degrees - p->alt = alt; // float:Altitude in meters - p->eph = eph; // float:GPS HDOP - p->epv = epv; // float:GPS VDOP - p->v = v; // float:GPS ground speed - p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_RAW_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_RAW; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h index 9fc5360b1..5ec33bd64 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_raw_int.h @@ -7,7 +7,6 @@ typedef struct __mavlink_gps_raw_int_t { uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot) - uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. int32_t lat; ///< Latitude in 1E7 degrees int32_t lon; ///< Longitude in 1E7 degrees int32_t alt; ///< Altitude in 1E3 meters (millimeters) @@ -15,6 +14,7 @@ typedef struct __mavlink_gps_raw_int_t float epv; ///< GPS VDOP float v; ///< GPS ground speed (m/s) float hdg; ///< Compass heading in degrees, 0..360 degrees + uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. } mavlink_gps_raw_int_t; @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t * @param v GPS ground speed (m/s) * @param hdg Compass heading in degrees, 0..360 degrees */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg.payload[0]; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. - p->lat = lat; // int32_t:Latitude in 1E7 degrees - p->lon = lon; // int32_t:Longitude in 1E7 degrees - p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters) - p->eph = eph; // float:GPS HDOP - p->epv = epv; // float:GPS VDOP - p->v = v; // float:GPS ground speed (m/s) - p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_RAW_INT_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_RAW_INT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h index f7aaf96da..bccfdba02 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_set_global_origin.h @@ -6,11 +6,11 @@ typedef struct __mavlink_gps_set_global_origin_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID int32_t latitude; ///< global position * 1E7 int32_t longitude; ///< global position * 1E7 int32_t altitude; ///< global position * 1000 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_gps_set_global_origin_t; @@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i * @param longitude global position * 1E7 * @param altitude global position * 1000 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg.payload[0]; - - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->latitude = latitude; // int32_t:global position * 1E7 - p->longitude = longitude; // int32_t:global position * 1E7 - p->altitude = altitude; // int32_t:global position * 1000 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h index 484de2c01..a974432d6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_gps_status.h @@ -102,37 +102,9 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t * @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg. * @param satellite_snr Signal to noise ratio of satellite */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg.payload[0]; - - p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible - memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID - memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization - memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite - memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg. - memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h index fea5381e9..7c686831e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_heartbeat.h @@ -76,34 +76,9 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg.payload[0]; - - p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; - msg.msgid = MAVLINK_MSG_ID_HEARTBEAT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h index 2fbe01f2a..fb6648714 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position.h @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint * @param vy Y Speed * @param vz Z Speed */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_local_position_t *p = (mavlink_local_position_t *)&msg.payload[0]; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->x = x; // float:X Position - p->y = y; // float:Y Position - p->z = z; // float:Z Position - p->vx = vx; // float:X Speed - p->vy = vy; // float:Y Speed - p->vz = vz; // float:Z Speed - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_LEN; - msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float vx, float vy, float vz) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h index 2f6ceb808..8ea4ca458 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint.h @@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system * @param z z position * @param yaw Desired yaw angle */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg.payload[0]; - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:Desired yaw angle - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN; - msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h index 9f34862b8..d7aa0d5bc 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_local_position_setpoint_set.h @@ -6,12 +6,12 @@ typedef struct __mavlink_local_position_setpoint_set_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID float x; ///< x position float y; ///< y position float z; ///< z position float yaw; ///< Desired yaw angle + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_local_position_setpoint_set_t; @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t sy * @param z z position * @param yaw Desired yaw angle */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:Desired yaw angle - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN; - msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h index d3aede95b..eeb05487a 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_manual_control.h @@ -6,11 +6,11 @@ typedef struct __mavlink_manual_control_t { - uint8_t target; ///< The system to be controlled float roll; ///< roll float pitch; ///< pitch float yaw; ///< yaw float thrust; ///< thrust + uint8_t target; ///< The system to be controlled uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 uint8_t pitch_manual; ///< pitch auto:0, manual:1 uint8_t yaw_manual; ///< yaw auto:0, manual:1 @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint * @param yaw_manual yaw auto:0, manual:1 * @param thrust_manual thrust auto:0, manual:1 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system to be controlled - p->roll = roll; // float:roll - p->pitch = pitch; // float:pitch - p->yaw = yaw; // float:yaw - p->thrust = thrust; // float:thrust - p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 - p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 - p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 - p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_MANUAL_CONTROL_LEN; - msg.msgid = MAVLINK_MSG_ID_MANUAL_CONTROL; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h index 6f4fb8830..ce5c680ad 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_float.h @@ -6,8 +6,8 @@ typedef struct __mavlink_named_value_float_t { - char name[10]; ///< Name of the debug variable float value; ///< Floating point value + char name[10]; ///< Name of the debug variable } mavlink_named_value_float_t; #define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10 @@ -74,33 +74,9 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u * @param name Name of the debug variable * @param value Floating point value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg.payload[0]; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // float:Floating point value - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN; - msg.msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h index a793614f5..1c75a5be1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_named_value_int.h @@ -6,8 +6,8 @@ typedef struct __mavlink_named_value_int_t { - char name[10]; ///< Name of the debug variable int32_t value; ///< Signed integer value + char name[10]; ///< Name of the debug variable } mavlink_named_value_int_t; #define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10 @@ -74,33 +74,9 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin * @param name Name of the debug variable * @param value Signed integer value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg.payload[0]; - memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable - p->value = value; // int32_t:Signed integer value - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN; - msg.msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h index f19d85b0a..ef878a3f7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_nav_controller_output.h @@ -8,12 +8,12 @@ typedef struct __mavlink_nav_controller_output_t { float nav_roll; ///< Current desired roll in degrees float nav_pitch; ///< Current desired pitch in degrees - int16_t nav_bearing; ///< Current desired heading in degrees - int16_t target_bearing; ///< Bearing to current waypoint/target in degrees - uint16_t wp_dist; ///< Distance to active waypoint in meters float alt_error; ///< Current altitude error in meters float aspd_error; ///< Current airspeed error in meters/second float xtrack_error; ///< Current crosstrack error on x-y plane in meters + int16_t nav_bearing; ///< Current desired heading in degrees + int16_t target_bearing; ///< Bearing to current waypoint/target in degrees + uint16_t wp_dist; ///< Distance to active waypoint in meters } mavlink_nav_controller_output_t; @@ -109,39 +109,9 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i * @param aspd_error Current airspeed error in meters/second * @param xtrack_error Current crosstrack error on x-y plane in meters */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg.payload[0]; - - p->nav_roll = nav_roll; // float:Current desired roll in degrees - p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees - p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees - p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees - p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters - p->alt_error = alt_error; // float:Current altitude error in meters - p->aspd_error = aspd_error; // float:Current airspeed error in meters/second - p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN; - msg.msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h index d53cfe7d8..74ed51442 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_list.h @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id, * @param target_system System ID * @param target_component Component ID */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN; - msg.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h index a60179da9..07e3ae044 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_request_read.h @@ -1,18 +1,18 @@ // MESSAGE PARAM_REQUEST_READ PACKING #define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20 -#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 19 -#define MAVLINK_MSG_20_LEN 19 +#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 20 +#define MAVLINK_MSG_20_LEN 20 typedef struct __mavlink_param_request_read_t { + int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - char param_id[15]; ///< Onboard parameter id - int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier + uint8_t param_id[16]; ///< Onboard parameter id } mavlink_param_request_read_t; -#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 15 +#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16 /** * @brief Pack a param_request_read message @@ -26,14 +26,14 @@ typedef struct __mavlink_param_request_read_t * @param param_index Parameter index. Send -1 to use the param ID field as identifier * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) +static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, int16_t param_index) { mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; p->target_system = target_system; // uint8_t:System ID p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); @@ -51,14 +51,14 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui * @param param_index Parameter index. Send -1 to use the param ID field as identifier * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) +static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, int16_t param_index) { mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; p->target_system = target_system; // uint8_t:System ID p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN); @@ -86,36 +86,10 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id, * @param param_id Onboard parameter id * @param param_index Parameter index. Send -1 to use the param ID field as identifier */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id - p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN; - msg.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, int16_t param_index) { mavlink_header_t hdr; mavlink_param_request_read_t payload; @@ -124,7 +98,7 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u p->target_system = target_system; // uint8_t:System ID p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier hdr.STX = MAVLINK_STX; @@ -176,7 +150,7 @@ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, char* param_id) +static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, uint8_t* param_id) { mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h index 476c98095..396a5307e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_set.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_set.h @@ -1,18 +1,18 @@ // MESSAGE PARAM_SET PACKING #define MAVLINK_MSG_ID_PARAM_SET 23 -#define MAVLINK_MSG_ID_PARAM_SET_LEN 21 -#define MAVLINK_MSG_23_LEN 21 +#define MAVLINK_MSG_ID_PARAM_SET_LEN 22 +#define MAVLINK_MSG_23_LEN 22 typedef struct __mavlink_param_set_t { + float param_value; ///< Onboard parameter value uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - char param_id[15]; ///< Onboard parameter id - float param_value; ///< Onboard parameter value + uint8_t param_id[16]; ///< Onboard parameter id } mavlink_param_set_t; -#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 15 +#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16 /** * @brief Pack a param_set message @@ -26,14 +26,14 @@ typedef struct __mavlink_param_set_t * @param param_value Onboard parameter value * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) +static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, float param_value) { mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_SET; p->target_system = target_system; // uint8_t:System ID p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN); @@ -51,14 +51,14 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com * @param param_value Onboard parameter value * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) +static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, float param_value) { mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_SET; p->target_system = target_system; // uint8_t:System ID p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN); @@ -86,36 +86,10 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c * @param param_id Onboard parameter id * @param param_value Onboard parameter value */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_param_set_t *p = (mavlink_param_set_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PARAM_SET_LEN; - msg.msgid = MAVLINK_MSG_ID_PARAM_SET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, float param_value) { mavlink_header_t hdr; mavlink_param_set_t payload; @@ -124,7 +98,7 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta p->target_system = target_system; // uint8_t:System ID p->target_component = target_component; // uint8_t:Component ID - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value hdr.STX = MAVLINK_STX; @@ -176,7 +150,7 @@ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_m * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, char* param_id) +static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, uint8_t* param_id) { mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h index 383440845..636589cd0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_param_value.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_param_value.h @@ -1,18 +1,18 @@ // MESSAGE PARAM_VALUE PACKING #define MAVLINK_MSG_ID_PARAM_VALUE 22 -#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 23 -#define MAVLINK_MSG_22_LEN 23 +#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 24 +#define MAVLINK_MSG_22_LEN 24 typedef struct __mavlink_param_value_t { - char param_id[15]; ///< Onboard parameter id float param_value; ///< Onboard parameter value uint16_t param_count; ///< Total number of onboard parameters uint16_t param_index; ///< Index of this onboard parameter + uint8_t param_id[16]; ///< Onboard parameter id } mavlink_param_value_t; -#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 15 +#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16 /** * @brief Pack a param_value message @@ -26,12 +26,12 @@ typedef struct __mavlink_param_value_t * @param param_index Index of this onboard parameter * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const uint8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) { mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value p->param_count = param_count; // uint16_t:Total number of onboard parameters p->param_index = param_index; // uint16_t:Index of this onboard parameter @@ -51,12 +51,12 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c * @param param_index Index of this onboard parameter * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) +static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const uint8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) { mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value p->param_count = param_count; // uint16_t:Total number of onboard parameters p->param_index = param_index; // uint16_t:Index of this onboard parameter @@ -86,43 +86,17 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t * @param param_count Total number of onboard parameters * @param param_index Index of this onboard parameter */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_param_value_t *p = (mavlink_param_value_t *)&msg.payload[0]; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id - p->param_value = param_value; // float:Onboard parameter value - p->param_count = param_count; // uint16_t:Total number of onboard parameters - p->param_index = param_index; // uint16_t:Index of this onboard parameter - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PARAM_VALUE_LEN; - msg.msgid = MAVLINK_MSG_ID_PARAM_VALUE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const uint8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index) { mavlink_header_t hdr; mavlink_param_value_t payload; uint16_t checksum; mavlink_param_value_t *p = &payload; - memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id + memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id p->param_value = param_value; // float:Onboard parameter value p->param_count = param_count; // uint16_t:Total number of onboard parameters p->param_index = param_index; // uint16_t:Index of this onboard parameter @@ -154,7 +128,7 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch * * @return Onboard parameter id */ -static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, char* param_id) +static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, uint8_t* param_id) { mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_ping.h b/thirdParty/mavlink/include/common/mavlink_msg_ping.h index 2e89aa2b0..f22a195c7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_ping.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_ping.h @@ -6,10 +6,10 @@ typedef struct __mavlink_ping_t { + uint64_t time; ///< Unix timestamp in microseconds uint32_t seq; ///< PING sequence uint8_t target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system uint8_t target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - uint64_t time; ///< Unix timestamp in microseconds } mavlink_ping_t; @@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon * @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system * @param time Unix timestamp in microseconds */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_ping_t *p = (mavlink_ping_t *)&msg.payload[0]; - p->seq = seq; // uint32_t:PING sequence - p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system - p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system - p->time = time; // uint64_t:Unix timestamp in microseconds - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PING_LEN; - msg.msgid = MAVLINK_MSG_ID_PING; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h index 2935301ef..951c29323 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_controller_output.h @@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_position_controller_output_encode(uint8_t sys * @param z Position z: -128: -100%, 127: +100% * @param yaw Position yaw: -128: -100%, 127: +100% */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg.payload[0]; - - p->enabled = enabled; // uint8_t:1: enabled, 0: disabled - p->x = x; // int8_t:Position x: -128: -100%, 127: +100% - p->y = y; // int8_t:Position y: -128: -100%, 127: +100% - p->z = z; // int8_t:Position z: -128: -100%, 127: +100% - p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100% - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN; - msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h index 7d9c425a6..1a65ed0a4 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_position_target.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_position_target.h @@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uin * @param z z position * @param yaw yaw orientation in radians, 0 = NORTH */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_position_target_t *p = (mavlink_position_target_t *)&msg.payload[0]; - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POSITION_TARGET_LEN; - msg.msgid = MAVLINK_MSG_ID_POSITION_TARGET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h index a815b1fa0..aa0f30672 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_imu.h @@ -121,41 +121,9 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com * @param ymag Y Magnetic field (raw) * @param zmag Z Magnetic field (raw) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg.payload[0]; - - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->xacc = xacc; // int16_t:X acceleration (raw) - p->yacc = yacc; // int16_t:Y acceleration (raw) - p->zacc = zacc; // int16_t:Z acceleration (raw) - p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw) - p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw) - p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw) - p->xmag = xmag; // int16_t:X Magnetic field (raw) - p->ymag = ymag; // int16_t:Y Magnetic field (raw) - p->zmag = zmag; // int16_t:Z Magnetic field (raw) - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RAW_IMU_LEN; - msg.msgid = MAVLINK_MSG_ID_RAW_IMU; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h index ccd6c59ac..4d3a9ba25 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_raw_pressure.h @@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_ * @param press_diff2 Differential pressure 2 (raw) * @param temperature Raw Temperature measurement (raw) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg.payload[0]; - - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->press_abs = press_abs; // int16_t:Absolute pressure (raw) - p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw) - p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw) - p->temperature = temperature; // int16_t:Raw Temperature measurement (raw) - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RAW_PRESSURE_LEN; - msg.msgid = MAVLINK_MSG_ID_RAW_PRESSURE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h new file mode 100644 index 000000000..b9dfe85e9 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_override.h @@ -0,0 +1,286 @@ +// MESSAGE RC_CHANNELS_OVERRIDE PACKING + +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE 70 +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18 +#define MAVLINK_MSG_70_LEN 18 + +typedef struct __mavlink_rc_channels_override_t +{ + uint16_t chan1_raw; ///< RC channel 1 value, in microseconds + uint16_t chan2_raw; ///< RC channel 2 value, in microseconds + uint16_t chan3_raw; ///< RC channel 3 value, in microseconds + uint16_t chan4_raw; ///< RC channel 4 value, in microseconds + uint16_t chan5_raw; ///< RC channel 5 value, in microseconds + uint16_t chan6_raw; ///< RC channel 6 value, in microseconds + uint16_t chan7_raw; ///< RC channel 7 value, in microseconds + uint16_t chan8_raw; ///< RC channel 8 value, in microseconds + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} mavlink_rc_channels_override_t; + +/** + * @brief Pack a rc_channels_override message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +} + +/** + * @brief Pack a rc_channels_override message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); +} + +/** + * @brief Encode a rc_channels_override struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param rc_channels_override C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) +{ + return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); +} + +/** + * @brief Send a rc_channels_override message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param chan1_raw RC channel 1 value, in microseconds + * @param chan2_raw RC channel 2 value, in microseconds + * @param chan3_raw RC channel 3 value, in microseconds + * @param chan4_raw RC channel 4 value, in microseconds + * @param chan5_raw RC channel 5 value, in microseconds + * @param chan6_raw RC channel 6 value, in microseconds + * @param chan7_raw RC channel 7 value, in microseconds + * @param chan8_raw RC channel 8 value, in microseconds + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +{ + mavlink_header_t hdr; + mavlink_rc_channels_override_t payload; + uint16_t checksum; + mavlink_rc_channels_override_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds + p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds + p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds + p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds + p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds + p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds + p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds + p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN; + hdr.msgid = MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE RC_CHANNELS_OVERRIDE UNPACKING + +/** + * @brief Get field target_system from rc_channels_override message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_rc_channels_override_get_target_system(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @brief Get field target_component from rc_channels_override message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @brief Get field chan1_raw from rc_channels_override message + * + * @return RC channel 1 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan1_raw); +} + +/** + * @brief Get field chan2_raw from rc_channels_override message + * + * @return RC channel 2 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan2_raw); +} + +/** + * @brief Get field chan3_raw from rc_channels_override message + * + * @return RC channel 3 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan3_raw); +} + +/** + * @brief Get field chan4_raw from rc_channels_override message + * + * @return RC channel 4 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan4_raw); +} + +/** + * @brief Get field chan5_raw from rc_channels_override message + * + * @return RC channel 5 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan5_raw); +} + +/** + * @brief Get field chan6_raw from rc_channels_override message + * + * @return RC channel 6 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan6_raw); +} + +/** + * @brief Get field chan7_raw from rc_channels_override message + * + * @return RC channel 7 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan7_raw); +} + +/** + * @brief Get field chan8_raw from rc_channels_override message + * + * @return RC channel 8 value, in microseconds + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) +{ + mavlink_rc_channels_override_t *p = (mavlink_rc_channels_override_t *)&msg->payload[0]; + return (uint16_t)(p->chan8_raw); +} + +/** + * @brief Decode a rc_channels_override message into a struct + * + * @param msg The message to decode + * @param rc_channels_override C-struct to decode the message contents into + */ +static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message_t* msg, mavlink_rc_channels_override_t* rc_channels_override) +{ + memcpy( rc_channels_override, msg->payload, sizeof(mavlink_rc_channels_override_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h index 7f7351d65..56de4d83c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_raw.h @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin * @param chan8_raw RC channel 8 value, in microseconds * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg.payload[0]; - p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds - p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds - p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds - p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds - p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds - p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds - p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds - p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds - p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN; - msg.msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h index 16bf4791e..fd2e57ad4 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_rc_channels_scaled.h @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id, * @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 * @param rssi Receive signal strength indicator, 0: 0%, 255: 100% */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg.payload[0]; - p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000 - p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100% - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN; - msg.msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h index 4d48f0954..b779e267e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_request_data_stream.h @@ -6,10 +6,10 @@ typedef struct __mavlink_request_data_stream_t { + uint16_t req_message_rate; ///< The requested interval between two messages of this type uint8_t target_system; ///< The target requested to send the message stream. uint8_t target_component; ///< The target requested to send the message stream. uint8_t req_stream_id; ///< The ID of the requested message type - uint16_t req_message_rate; ///< The requested interval between two messages of this type uint8_t start_stop; ///< 1 to start sending, 0 to stop sending. } mavlink_request_data_stream_t; @@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id, * @param req_message_rate The requested interval between two messages of this type * @param start_stop 1 to start sending, 0 to stop sending. */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg.payload[0]; - - p->target_system = target_system; // uint8_t:The target requested to send the message stream. - p->target_component = target_component; // uint8_t:The target requested to send the message stream. - p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type - p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type - p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending. - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN; - msg.msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h index aaee3bf13..a660e8bed 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_allowed_area.h @@ -6,13 +6,13 @@ typedef struct __mavlink_safety_allowed_area_t { - uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. float p1x; ///< x position 1 / Latitude 1 float p1y; ///< y position 1 / Longitude 1 float p1z; ///< z position 1 / Altitude 1 float p2x; ///< x position 2 / Latitude 2 float p2y; ///< y position 2 / Longitude 2 float p2z; ///< z position 2 / Altitude 2 + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. } mavlink_safety_allowed_area_t; @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id, * @param p2y y position 2 / Longitude 2 * @param p2z z position 2 / Altitude 2 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg.payload[0]; - p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - p->p1x = p1x; // float:x position 1 / Latitude 1 - p->p1y = p1y; // float:y position 1 / Longitude 1 - p->p1z = p1z; // float:z position 1 / Altitude 1 - p->p2x = p2x; // float:x position 2 / Latitude 2 - p->p2y = p2y; // float:y position 2 / Longitude 2 - p->p2z = p2z; // float:z position 2 / Altitude 2 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN; - msg.msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h b/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h index 4427dbedd..9e8ef1e44 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_safety_set_allowed_area.h @@ -6,15 +6,15 @@ typedef struct __mavlink_safety_set_allowed_area_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. float p1x; ///< x position 1 / Latitude 1 float p1y; ///< y position 1 / Longitude 1 float p1z; ///< z position 1 / Altitude 1 float p2x; ///< x position 2 / Latitude 2 float p2y; ///< y position 2 / Longitude 2 float p2z; ///< z position 2 / Altitude 2 + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. } mavlink_safety_set_allowed_area_t; @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system * @param p2y y position 2 / Longitude 2 * @param p2z z position 2 / Altitude 2 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down. - p->p1x = p1x; // float:x position 1 / Latitude 1 - p->p1y = p1y; // float:y position 1 / Longitude 1 - p->p1z = p1z; // float:z position 1 / Altitude 1 - p->p2x = p2x; // float:x position 2 / Latitude 2 - p->p2y = p2y; // float:y position 2 / Longitude 2 - p->p2z = p2z; // float:z position 2 / Altitude 2 - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN; - msg.msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h index 44fe8bdbd..8f381f22c 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_imu.h @@ -121,41 +121,9 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t * @param ymag Y Magnetic field (milli tesla) * @param zmag Z Magnetic field (milli tesla) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg.payload[0]; - - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->xacc = xacc; // int16_t:X acceleration (mg) - p->yacc = yacc; // int16_t:Y acceleration (mg) - p->zacc = zacc; // int16_t:Z acceleration (mg) - p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec) - p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec) - p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec) - p->xmag = xmag; // int16_t:X Magnetic field (milli tesla) - p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla) - p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla) - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SCALED_IMU_LEN; - msg.msgid = MAVLINK_MSG_ID_SCALED_IMU; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h index bf653ad5a..39d29440f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_scaled_pressure.h @@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin * @param press_diff Differential pressure 1 (hectopascal) * @param temperature Temperature measurement (0.01 degrees celsius) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint64_t usec, float press_abs, float press_diff, int16_t temperature) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg.payload[0]; - p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot) - p->press_abs = press_abs; // float:Absolute pressure (hectopascal) - p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal) - p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SCALED_PRESSURE_LEN; - msg.msgid = MAVLINK_MSG_ID_SCALED_PRESSURE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint64_t usec, float press_abs, float press_diff, int16_t temperature) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h index f12eb29a0..5aa3cc8d0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_servo_output_raw.h @@ -109,39 +109,9 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui * @param servo7_raw Servo output 7 value, in microseconds * @param servo8_raw Servo output 8 value, in microseconds */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg.payload[0]; - - p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds - p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds - p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds - p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds - p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds - p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds - p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds - p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN; - msg.msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h index 3e32eee15..9b50027f1 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_altitude.h @@ -6,8 +6,8 @@ typedef struct __mavlink_set_altitude_t { - uint8_t target; ///< The system setting the altitude uint32_t mode; ///< The new altitude in meters + uint8_t target; ///< The system setting the altitude } mavlink_set_altitude_t; @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_ * @param target The system setting the altitude * @param mode The new altitude in meters */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system setting the altitude - p->mode = mode; // uint32_t:The new altitude in meters - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SET_ALTITUDE_LEN; - msg.msgid = MAVLINK_MSG_ID_SET_ALTITUDE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h index a74108ee8..ad796f1e0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_mode.h @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co * @param target The system setting the mode * @param mode The new mode */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system setting the mode - p->mode = mode; // uint8_t:The new mode - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SET_MODE_LEN; - msg.msgid = MAVLINK_MSG_ID_SET_MODE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h index 649fbee57..114744009 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_nav_mode.h @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_set_nav_mode_encode(uint8_t system_id, uint8_ * @param target The system setting the mode * @param nav_mode The new navigation mode */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system setting the mode - p->nav_mode = nav_mode; // uint8_t:The new navigation mode - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SET_NAV_MODE_LEN; - msg.msgid = MAVLINK_MSG_ID_SET_NAV_MODE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h new file mode 100644 index 000000000..1fc909fc4 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw.h @@ -0,0 +1,196 @@ +// MESSAGE SET_ROLL_PITCH_YAW PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW 55 +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN 14 +#define MAVLINK_MSG_55_LEN 14 + +typedef struct __mavlink_set_roll_pitch_yaw_t +{ + float roll; ///< Desired roll angle in radians + float pitch; ///< Desired pitch angle in radians + float yaw; ///< Desired yaw angle in radians + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} mavlink_set_roll_pitch_yaw_t; + +/** + * @brief Pack a set_roll_pitch_yaw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN); +} + +/** + * @brief Pack a set_roll_pitch_yaw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN); +} + +/** + * @brief Encode a set_roll_pitch_yaw struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw) +{ + return mavlink_msg_set_roll_pitch_yaw_pack(system_id, component_id, msg, set_roll_pitch_yaw->target_system, set_roll_pitch_yaw->target_component, set_roll_pitch_yaw->roll, set_roll_pitch_yaw->pitch, set_roll_pitch_yaw->yaw); +} + +/** + * @brief Send a set_roll_pitch_yaw message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll Desired roll angle in radians + * @param pitch Desired pitch angle in radians + * @param yaw Desired yaw angle in radians + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_set_roll_pitch_yaw_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw) +{ + mavlink_header_t hdr; + mavlink_set_roll_pitch_yaw_t payload; + uint16_t checksum; + mavlink_set_roll_pitch_yaw_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll = roll; // float:Desired roll angle in radians + p->pitch = pitch; // float:Desired pitch angle in radians + p->yaw = yaw; // float:Desired yaw angle in radians + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_system(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_component(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @brief Get field roll from set_roll_pitch_yaw message + * + * @return Desired roll angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_get_roll(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (float)(p->roll); +} + +/** + * @brief Get field pitch from set_roll_pitch_yaw message + * + * @return Desired pitch angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_get_pitch(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (float)(p->pitch); +} + +/** + * @brief Get field yaw from set_roll_pitch_yaw message + * + * @return Desired yaw angle in radians + */ +static inline float mavlink_msg_set_roll_pitch_yaw_get_yaw(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0]; + return (float)(p->yaw); +} + +/** + * @brief Decode a set_roll_pitch_yaw message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw) +{ + memcpy( set_roll_pitch_yaw, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h new file mode 100644 index 000000000..bb5858356 --- /dev/null +++ b/thirdParty/mavlink/include/common/mavlink_msg_set_roll_pitch_yaw_speed.h @@ -0,0 +1,196 @@ +// MESSAGE SET_ROLL_PITCH_YAW_SPEED PACKING + +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED 56 +#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN 14 +#define MAVLINK_MSG_56_LEN 14 + +typedef struct __mavlink_set_roll_pitch_yaw_speed_t +{ + float roll_speed; ///< Desired roll angular speed in rad/s + float pitch_speed; ///< Desired pitch angular speed in rad/s + float yaw_speed; ///< Desired yaw angular speed in rad/s + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + +} mavlink_set_roll_pitch_yaw_speed_t; + +/** + * @brief Pack a set_roll_pitch_yaw_speed message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN); +} + +/** + * @brief Pack a set_roll_pitch_yaw_speed message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message was sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN); +} + +/** + * @brief Encode a set_roll_pitch_yaw_speed struct into a message + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param set_roll_pitch_yaw_speed C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed) +{ + return mavlink_msg_set_roll_pitch_yaw_speed_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed->target_system, set_roll_pitch_yaw_speed->target_component, set_roll_pitch_yaw_speed->roll_speed, set_roll_pitch_yaw_speed->pitch_speed, set_roll_pitch_yaw_speed->yaw_speed); +} + +/** + * @brief Send a set_roll_pitch_yaw_speed message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param roll_speed Desired roll angular speed in rad/s + * @param pitch_speed Desired pitch angular speed in rad/s + * @param yaw_speed Desired yaw angular speed in rad/s + */ + + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_set_roll_pitch_yaw_speed_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed) +{ + mavlink_header_t hdr; + mavlink_set_roll_pitch_yaw_speed_t payload; + uint16_t checksum; + mavlink_set_roll_pitch_yaw_speed_t *p = &payload; + + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s + p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s + p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s + + hdr.STX = MAVLINK_STX; + hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN; + hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED; + hdr.sysid = mavlink_system.sysid; + hdr.compid = mavlink_system.compid; + hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1; + mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES ); + + crc_init(&checksum); + checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len ); + hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte + hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte + + mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len); + mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES); +} + +#endif +// MESSAGE SET_ROLL_PITCH_YAW_SPEED UNPACKING + +/** + * @brief Get field target_system from set_roll_pitch_yaw_speed message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (uint8_t)(p->target_system); +} + +/** + * @brief Get field target_component from set_roll_pitch_yaw_speed message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (uint8_t)(p->target_component); +} + +/** + * @brief Get field roll_speed from set_roll_pitch_yaw_speed message + * + * @return Desired roll angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (float)(p->roll_speed); +} + +/** + * @brief Get field pitch_speed from set_roll_pitch_yaw_speed message + * + * @return Desired pitch angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (float)(p->pitch_speed); +} + +/** + * @brief Get field yaw_speed from set_roll_pitch_yaw_speed message + * + * @return Desired yaw angular speed in rad/s + */ +static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(const mavlink_message_t* msg) +{ + mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0]; + return (float)(p->yaw_speed); +} + +/** + * @brief Decode a set_roll_pitch_yaw_speed message into a struct + * + * @param msg The message to decode + * @param set_roll_pitch_yaw_speed C-struct to decode the message contents into + */ +static inline void mavlink_msg_set_roll_pitch_yaw_speed_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed) +{ + memcpy( set_roll_pitch_yaw_speed, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_speed_t)); +} diff --git a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h index 6ca16a534..f101513eb 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_state_correction.h @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui * @param vyErr y velocity * @param vzErr z velocity */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg.payload[0]; - p->xErr = xErr; // float:x position error - p->yErr = yErr; // float:y position error - p->zErr = zErr; // float:z position error - p->rollErr = rollErr; // float:roll error (radians) - p->pitchErr = pitchErr; // float:pitch error (radians) - p->yawErr = yawErr; // float:yaw error (radians) - p->vxErr = vxErr; // float:x velocity - p->vyErr = vyErr; // float:y velocity - p->vzErr = vzErr; // float:z velocity - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_STATE_CORRECTION_LEN; - msg.msgid = MAVLINK_MSG_ID_STATE_CORRECTION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h index 485bbf1cc..bc9c77b8f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_statustext.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_statustext.h @@ -7,7 +7,7 @@ typedef struct __mavlink_statustext_t { uint8_t severity; ///< Severity of status, 0 = info message, 255 = critical fault - char text[50]; ///< Status text message, without null termination character + int8_t text[50]; ///< Status text message, without null termination character } mavlink_statustext_t; #define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50 @@ -22,13 +22,13 @@ typedef struct __mavlink_statustext_t * @param text Status text message, without null termination character * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t severity, const char* text) +static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t severity, const int8_t* text) { mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + memcpy(p->text, text, sizeof(p->text)); // int8_t[50]:Status text message, without null termination character return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN); } @@ -43,13 +43,13 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @param text Status text message, without null termination character * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t severity, const char* text) +static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t severity, const int8_t* text) { mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_STATUSTEXT; p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + memcpy(p->text, text, sizeof(p->text)); // int8_t[50]:Status text message, without null termination character return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN); } @@ -74,34 +74,10 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t * @param severity Severity of status, 0 = info message, 255 = critical fault * @param text Status text message, without null termination character */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_statustext_t *p = (mavlink_statustext_t *)&msg.payload[0]; - - p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; - msg.msgid = MAVLINK_MSG_ID_STATUSTEXT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const int8_t* text) { mavlink_header_t hdr; mavlink_statustext_t payload; @@ -109,7 +85,7 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s mavlink_statustext_t *p = &payload; p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault - memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character + memcpy(p->text, text, sizeof(p->text)); // int8_t[50]:Status text message, without null termination character hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_STATUSTEXT_LEN; @@ -149,7 +125,7 @@ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_ * * @return Status text message, without null termination character */ -static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char* text) +static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, int8_t* text) { mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h index a340cfaa8..1ca8cec2e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_sys_status.h @@ -6,13 +6,13 @@ typedef struct __mavlink_sys_status_t { - uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM - uint8_t status; ///< System status flag, see MAV_STATUS ENUM uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt) uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000) uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV) + uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h + uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM + uint8_t status; ///< System status flag, see MAV_STATUS ENUM } mavlink_sys_status_t; @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t * @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000) * @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg.payload[0]; - p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM - p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt) - p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000) - p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SYS_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_SYS_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h index 2213495fc..c2328a68f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time.h @@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t * * @param time_usec Timestamp of the master clock in microseconds since UNIX epoch. */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_system_time_t *p = (mavlink_system_time_t *)&msg.payload[0]; - - p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch. - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SYSTEM_TIME_LEN; - msg.msgid = MAVLINK_MSG_ID_SYSTEM_TIME; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h index 04d124f2b..414b4984f 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_system_time_utc.h @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uin * @param utc_date GPS UTC date ddmmyy * @param utc_time GPS UTC time hhmmss */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg.payload[0]; - p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy - p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN; - msg.msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h index ee92a1e34..0b757cef6 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_vfr_hud.h @@ -8,10 +8,10 @@ typedef struct __mavlink_vfr_hud_t { float airspeed; ///< Current airspeed in m/s float groundspeed; ///< Current ground speed in m/s - int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) - uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 float alt; ///< Current altitude (MSL), in meters float climb; ///< Current climb rate in meters/second + int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north) + uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100 } mavlink_vfr_hud_t; @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com * @param alt Current altitude (MSL), in meters * @param climb Current climb rate in meters/second */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg.payload[0]; - p->airspeed = airspeed; // float:Current airspeed in m/s - p->groundspeed = groundspeed; // float:Current ground speed in m/s - p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north) - p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100 - p->alt = alt; // float:Current altitude (MSL), in meters - p->climb = climb; // float:Current climb rate in meters/second - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_VFR_HUD_LEN; - msg.msgid = MAVLINK_MSG_ID_VFR_HUD; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h index 07f7a08bd..a29a26b0e 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint.h @@ -6,13 +6,6 @@ typedef struct __mavlink_waypoint_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence - uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - uint8_t current; ///< false:0, true:1 - uint8_t autocontinue; ///< autocontinue to next wp float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. @@ -20,6 +13,13 @@ typedef struct __mavlink_waypoint_t float x; ///< PARAM5 / local: x position, global: latitude float y; ///< PARAM6 / y position: global: longitude float z; ///< PARAM7 / z position: global: altitude + uint16_t seq; ///< Sequence + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID + uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h + uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs + uint8_t current; ///< false:0, true:1 + uint8_t autocontinue; ///< autocontinue to next wp } mavlink_waypoint_t; @@ -145,45 +145,9 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co * @param y PARAM6 / y position: global: longitude * @param z PARAM7 / z position: global: altitude */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence - p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h - p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs - p->current = current; // uint8_t:false:0, true:1 - p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp - p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters - p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds - p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise. - p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH - p->x = x; // float:PARAM5 / local: x position, global: latitude - p->y = y; // float:PARAM6 / y position: global: longitude - p->z = z; // float:PARAM7 / z position: global: altitude - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h index 8bdd5ae15..8a5d52ed3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_ack.h @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_ * @param target_component Component ID * @param type 0: OK, 1: Error */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg.payload[0]; - - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->type = type; // uint8_t:0: OK, 1: Error - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_ACK_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_ACK; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h index efccf3600..b96a80696 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_clear_all.h @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id, * @param target_system System ID * @param target_component Component ID */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h index a44619721..8856c3c44 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_count.h @@ -6,9 +6,9 @@ typedef struct __mavlink_waypoint_count_t { + uint16_t count; ///< Number of Waypoints in the Sequence uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint16_t count; ///< Number of Waypoints in the Sequence } mavlink_waypoint_count_t; @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint * @param target_component Component ID * @param count Number of Waypoints in the Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg.payload[0]; - - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->count = count; // uint16_t:Number of Waypoints in the Sequence - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h index 59b2f6b3a..9c75639c7 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_current.h @@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, ui * * @param seq Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg.payload[0]; - - p->seq = seq; // uint16_t:Sequence - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h index 17404e658..5e1f144e8 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_reached.h @@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, ui * * @param seq Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg.payload[0]; - - p->seq = seq; // uint16_t:Sequence - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h index c33edce8e..9f3ccdbe3 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request.h @@ -6,9 +6,9 @@ typedef struct __mavlink_waypoint_request_t { + uint16_t seq; ///< Sequence uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence } mavlink_waypoint_request_t; @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, ui * @param target_component Component ID * @param seq Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg.payload[0]; - - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h index 209b6140e..34d8276d0 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_request_list.h @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_i * @param target_system System ID * @param target_component Component ID */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h index c35fe2107..829f29982 100644 --- a/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h +++ b/thirdParty/mavlink/include/common/mavlink_msg_waypoint_set_current.h @@ -6,9 +6,9 @@ typedef struct __mavlink_waypoint_set_current_t { + uint16_t seq; ///< Sequence uint8_t target_system; ///< System ID uint8_t target_component; ///< Component ID - uint16_t seq; ///< Sequence } mavlink_waypoint_set_current_t; @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id * @param target_component Component ID * @param seq Sequence */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg.payload[0]; - - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->seq = seq; // uint16_t:Sequence - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN; - msg.msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/mavlink_checksum.h b/thirdParty/mavlink/include/mavlink_checksum.h new file mode 100755 index 000000000..0f4225174 --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_checksum.h @@ -0,0 +1,156 @@ +/** @file + * @brief MAVLink comm protocol checksum routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _CHECKSUM_H_ +#define _CHECKSUM_H_ + +#include "inttypes.h" + + +/** + * + * CALCULATE THE CHECKSUM + * + */ + +#define X25_INIT_CRC 0xffff +#define X25_VALIDATE_CRC 0xf0b8 + +/** + * @brief Accumulate the X.25 CRC by adding one char at a time. + * + * The checksum function adds the hash of one char at a time to the + * 16 bit checksum (uint16_t). + * + * @param data new char to hash + * @param crcAccum the already accumulated checksum + **/ +static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) +{ + /*Accumulate one byte of data into the CRC*/ + uint8_t tmp; + + tmp=data ^ (uint8_t)(*crcAccum &0xff); + tmp^= (tmp<<4); + *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); +// *crcAccum += data; // super simple to test +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init(uint16_t* crcAccum) +{ + *crcAccum = X25_INIT_CRC; +} + +/** + * @brief Initiliaze the buffer for the X.25 CRC to a specified value + * + * @param crcAccum the 16 bit X.25 CRC + */ +static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue) +{ + *crcAccum = crcValue; +} + + + +/** + * @brief Calculates the X.25 checksum on a byte buffer + * + * @param pBuffer buffer containing the byte array to hash + * @param length length of the byte array + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate(uint8_t* pBuffer, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=pBuffer; + + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + +/** + * @brief Calculates the X.25 checksum on a msg buffer + * + * @param pMSG buffer containing the msg to hash + * @param length number of bytes to hash + * @return the checksum over the buffer bytes + **/ +static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length) +{ + + // For a "message" of length bytes contained in the unsigned char array + // pointed to by pBuffer, calculate the CRC + // crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed + + uint16_t crcTmp; + //uint16_t tmp; + uint8_t* pTmp; + int i; + + pTmp=&pMSG->len; + + /* init crcTmp */ + crc_init(&crcTmp); + + for (i = 0; i < 5; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + pTmp=&pMSG->payload[0]; + for (; i < length; i++){ + crc_accumulate(*pTmp++, &crcTmp); + } + + /* This is currently not needed, as only the checksum over payload should be computed + tmp = crcTmp; + crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp); + crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp); + *checkConst = tmp; + */ + return(crcTmp); +} + + + + +#endif /* _CHECKSUM_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/thirdParty/mavlink/include/mavlink_data.h b/thirdParty/mavlink/include/mavlink_data.h new file mode 100755 index 000000000..ce51e2b93 --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_data.h @@ -0,0 +1,21 @@ +/** @file + * @brief Main MAVLink comm protocol data. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _ML_DATA_H_ +#define _ML_DATA_H_ + +#include "mavlink_types.h" + +#ifdef MAVLINK_CHECK_LENGTH +const uint8_t MAVLINK_CONST mavlink_msg_lengths[256] = MAVLINK_MESSAGE_LENGTHS; +#endif + +const uint8_t MAVLINK_CONST mavlink_msg_keys[256] = MAVLINK_MESSAGE_KEYS; + +mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + +#endif \ No newline at end of file diff --git a/thirdParty/mavlink/include/mavlink_options.h b/thirdParty/mavlink/include/mavlink_options.h new file mode 100755 index 000000000..646ebed02 --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_options.h @@ -0,0 +1,102 @@ +/** @file + * @brief MAVLink comm protocol option constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef _ML_OPTIONS_H_ +#define _ML_OPTIONS_H_ + + +/** + * + * Receive message length check option. On receive verify the length field + * as soon as the message ID field is received. Requires a 256 byte const + * table. Comment out the define to leave out the table and code to check it. + * + */ +#define MAVLINK_CHECK_LENGTH + +/** + * + * Receive message buffer option. This option should be used only when the + * side effects are understood but allows the underlying program access to + * the internal recieve buffer - eliminating the usual double buffering. It + * also REQUIRES changes in the return type of mavlink_parse_char so only + * enable if you make the changes required. Default DISABLED. + * + */ +//#define MAVLINK_STAIC_BUFFER + +/** + * + * Receive message buffers option. This option defines how many msg buffers + * mavlink will define, and thereby how many links it can support. A default + * will be supplied if the symbol is not pre-defined, dependant on the make + * envionment. The default is 16 for a recognised OS envionment and 1 + * otherwise. + * + */ +#if !((defined MAVLINK_COMM_NB) | (MAVLINK_COMM_NB < 1)) +#undef MAVLINK_COMM_NB + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) | (defined __APPLE__) + #define MAVLINK_COMM_NB 16 + #else + #define MAVLINK_COMM_NB 1 + #endif +#endif + + +/** + * + * Data relization option. This option controls inclusion of the file + * mavlink_data.h in the current compile unit - thus defining mavlink's + * variables. Default is ON (not defined) because typically mavlink.h is only + * included once in a system but if it was used in two files there would + * be duplicate variables at link time. Normal practice would be to define + * this symbol outside of this file as defining it here will cause missing + * symbols at link time. In other words in the first file to include mavlink.h + * do not define this sybol, then define this symbol in all other files before + * including mavlink.h + * + */ +//#define MAVLINK_NO_DATA +#ifdef MAVLINK_NO_DATA + #undef MAVLINK_DATA +#else + #define MAVLINK_DATA +#endif + +/** + * + * Custom data const data relization and access options. + * This define is placed in the form + * const uint8_t MAVLINK_CONST name[] = { ... }; + * for the keys table and (if applicable) lengths table to tell the compiler + * were to put the data. The access option is placed in the form + * variable = MAVLINK_CONST_READ( name[i] ); + * in order to allow custom read function's or accessors. + * By default MAVLINK_CONST is defined as nothing and MAVLINK_CONST_READ as + * MAVLINK_CONST_READ( a ) a + * These symbols are only defined if not already defined allowing this file + * to remain unchanged while the actual definitions are maintained in external + * files. + * + */ +#ifndef MAVLINK_CONST +#define MAVLINK_CONST +#endif +#ifndef MAVLINK_CONST_READ +#define MAVLINK_CONST_READ( a ) a +#endif + + +#endif /* _ML_OPTIONS_H_ */ + +#ifdef __cplusplus +} +#endif diff --git a/thirdParty/mavlink/include/mavlink_protocol.h b/thirdParty/mavlink/include/mavlink_protocol.h new file mode 100755 index 000000000..62163d598 --- /dev/null +++ b/thirdParty/mavlink/include/mavlink_protocol.h @@ -0,0 +1,951 @@ +/** @file + * @brief Main MAVLink comm protocol routines. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef _MAVLINK_PROTOCOL_H_ +#define _MAVLINK_PROTOCOL_H_ + +#include "mavlink_types.h" + +#include "mavlink_checksum.h" + +#ifdef MAVLINK_CHECK_LENGTH +extern const uint8_t MAVLINK_CONST mavlink_msg_lengths[256]; +#endif + +extern const uint8_t MAVLINK_CONST mavlink_msg_keys[256]; + +extern mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +extern mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + + +/** + * @brief Initialize the communication stack + * + * This function has to be called before using commParseBuffer() to initialize the different status registers. + * + * @return Will initialize the different buffers and status registers. + */ +static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) +{ + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } +} + +static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) +{ + + return &m_mavlink_status[chan]; +} + +/** + * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel + * + * 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. + * + * @warning This function implicitely assumes the message is sent over channel zero. + * if the message is sent over a different channel it will reach the receiver + * without error, BUT the sequence number might be wrong due to the wrong + * channel sequence counter. This will result is wrongly reported excessive + * packet loss. Please use @see mavlink_{pack|encode}_headerless and then + * @see mavlink_finalize_message_chan before sending for a correct channel + * assignment. Please note that the mavlink_msg_xxx_pack and encode functions + * assign channel zero as default and thus induce possible loss counter errors.\ + * They have been left to ensure code compatibility. + * + * @see mavlink_finalize_message_chan + * @param msg Message to finalize + * @param system_id Id of the sending (this) system, 1-127 + * @param length Message length, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) +{ + // This code part is the same for all messages; + uint16_t checksum; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @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, usually just the counter incremented while packing the message + */ +static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) +{ + // This code part is the same for all messages; + uint16_t checksum; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; +// checksum = crc_calculate((uint8_t*)((void*)msg), length + MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + msg->ck_a = (uint8_t)(checksum & 0xFF); ///< High byte + msg->ck_b = (uint8_t)(checksum >> 8); ///< Low byte + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; +} + +/** + * @brief Pack a message to send it over a serial byte stream + */ +static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) +{ + *(buffer+0) = MAVLINK_STX; ///< Start transmit +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header + memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +// return 0; +} + +/** + * @brief Get the required buffer size for this message + */ +static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +{ + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +} + +union checksum_ { + uint16_t s; + uint8_t c[2]; +}; + +union __mavlink_bitfield { + uint8_t uint8; + int8_t int8; + uint16_t uint16; + int16_t int16; + uint32_t uint32; + int32_t int32; +}; + + +static inline void mavlink_start_checksum(mavlink_message_t* msg) +{ + union checksum_ ck; + crc_init(&(ck.s)); + msg->ck_a = ck.c[0]; + msg->ck_b = ck.c[1]; +} + +static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) +{ + union checksum_ ck; + ck.c[0] = msg->ck_a; + ck.c[1] = msg->ck_b; + crc_accumulate(c, &(ck.s)); + msg->ck_a = ck.c[0]; + msg->ck_b = ck.c[1]; +} + +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ +#ifdef MAVLINK_STAIC_BUFFER +static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#else +static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +#endif +{ + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); +#ifdef MAVLINK_CHECK_LENGTH + if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + else ; +#endif + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + rxmsg->payload[status->packet_idx++] = c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + mavlink_update_checksum(rxmsg, + MAVLINK_CONST_READ( mavlink_msg_lengths[rxmsg->msgid] ) ); + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: + if (c != rxmsg->ck_a) + { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != rxmsg->ck_b) + {// Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; + if (status->msg_received == 1) + { + if ( r_message != NULL ) + return r_message; + else return rxmsg; + } else return NULL; +} + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +// To make MAVLink work on your MCU, define a similar function + +/* + +#include "mavlink_types.h" + +void comm_send_ch(mavlink_channel_t chan, uint8_t ch) +{ + if (chan == MAVLINK_COMM_0) + { + uart0_transmit(ch); + } + if (chan == MAVLINK_COMM_1) + { + uart1_transmit(ch); + } +} + + +static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); +} + +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) +{ + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } +} + */ +static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); +static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num); +#define mavlink_send_msg( a, b ) mavlink_send_uart( a, b ) +#endif + +#define FILE_FINISHED + +#ifndef FILE_FINISHED +/** + * This is a convenience function which handles the complete MAVLink parsing. + * the function will parse one byte at a time and return the complete packet once + * it could be successfully decoded. Checksum and other failures will be silently + * ignored. + * + * @param chan ID of the current channel. This allows to parse different channels with this function. + * a channel is not a physical message channel like a serial port, but a logic partition of + * the communication streams in this case. COMM_NB is the limit for the number of channels + * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows + * @param c The char to barse + * + * @param returnMsg NULL if no message could be decoded, the message data else + * @return 0 if no message could be decoded, 1 else + * + * A typical use scenario of this function call is: + * + * @code + * #include // For fixed-width uint8_t type + * + * mavlink_message_t msg; + * int chan = 0; + * + * + * while(serial.bytesAvailable > 0) + * { + * uint8_t byte = serial.getNextByte(); + * if (mavlink_parse_char(chan, byte, &msg)) + * { + * printf("Received message with ID %d, sequence: %d from component %d of system %d", msg.msgid, msg.seq, msg.compid, msg.sysid); + * } + * } + * + * + * @endcode + */ + +#define MAVLINK_PACKET_START_CANDIDATES 50 +/* +static inline uint8_t mavlink_parse_char_new(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) +{ + #if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_msgbuf[MAVLINK_COMM_NB_HIGH][MAVLINK_MAX_PACKET_LEN * 2]; + static uint8_t m_msgbuf_index[MAVLINK_COMM_NB_HIGH]; + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_packet_start[MAVLINK_COMM_NB_HIGH][MAVLINK_PACKET_START_CANDIDATES]; + static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB_HIGH]; + static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB_HIGH]; + #else + static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; + static uint8_t m_msgbuf[MAVLINK_COMM_NB][MAVLINK_MAX_PACKET_LEN * 2]; + static uint8_t m_msgbuf_index[MAVLINK_COMM_NB]; + static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; + static uint8_t m_packet_start[MAVLINK_COMM_NB][MAVLINK_PACKET_START_CANDIDATES]; + static uint8_t m_packet_start_index_read[MAVLINK_COMM_NB]; + static uint8_t m_packet_start_index_write[MAVLINK_COMM_NB]; + #endif + + // Set a packet start candidate index if sign is start sign + if (c == MAVLINK_STX) + { + m_packet_start[chan][++(m_packet_start_index_write[chan]) % MAVLINK_PACKET_START_CANDIDATES] = m_msgbuf_index[chan]; + } + + // Parse normally, if a CRC mismatch occurs retry with the next packet index +} +//#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) +// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB_HIGH]; +// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB_HIGH]; +//#else +// static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB]; +// static mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB]; +//#endif +//// Initializes only once, values keep unchanged after first initialization +// mavlink_parse_state_initialize(&m_mavlink_status[chan]); +// +//mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message +//mavlink_status_t* status = &m_mavlink_status[chan]; ///< The current decode status +//int bufferIndex = 0; +// +//status->msg_received = 0; +// +//switch (status->parse_state) +//{ +//case MAVLINK_PARSE_STATE_UNINIT: +//case MAVLINK_PARSE_STATE_IDLE: +// if (c == MAVLINK_STX) +// { +// status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; +// mavlink_start_checksum(rxmsg); +// } +// break; +// +//case MAVLINK_PARSE_STATE_GOT_STX: +// if (status->msg_received) +// { +// status->buffer_overrun++; +// status->parse_error++; +// status->msg_received = 0; +// status->parse_state = MAVLINK_PARSE_STATE_IDLE; +// } +// else +// { +// // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 +// rxmsg->len = c; +// status->packet_idx = 0; +// mavlink_update_checksum(rxmsg, c); +// status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; +// } +// break; +// +//case MAVLINK_PARSE_STATE_GOT_LENGTH: +// rxmsg->seq = c; +// mavlink_update_checksum(rxmsg, c); +// status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; +// break; +// +//case MAVLINK_PARSE_STATE_GOT_SEQ: +// rxmsg->sysid = c; +// mavlink_update_checksum(rxmsg, c); +// status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; +// break; +// +//case MAVLINK_PARSE_STATE_GOT_SYSID: +// rxmsg->compid = c; +// mavlink_update_checksum(rxmsg, c); +// status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; +// break; +// +//case MAVLINK_PARSE_STATE_GOT_COMPID: +// rxmsg->msgid = c; +// mavlink_update_checksum(rxmsg, c); +// if (rxmsg->len == 0) +// { +// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; +// } +// else +// { +// status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; +// } +// break; +// +//case MAVLINK_PARSE_STATE_GOT_MSGID: +// rxmsg->payload[status->packet_idx++] = c; +// mavlink_update_checksum(rxmsg, c); +// if (status->packet_idx == rxmsg->len) +// { +// status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; +// } +// break; +// +//case MAVLINK_PARSE_STATE_GOT_PAYLOAD: +// if (c != rxmsg->ck_a) +// { +// // Check first checksum byte +// status->parse_error++; +// status->msg_received = 0; +// status->parse_state = MAVLINK_PARSE_STATE_IDLE; +// } +// else +// { +// status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; +// } +// break; +// +//case MAVLINK_PARSE_STATE_GOT_CRC1: +// if (c != rxmsg->ck_b) +// {// Check second checksum byte +// status->parse_error++; +// status->msg_received = 0; +// status->parse_state = MAVLINK_PARSE_STATE_IDLE; +// } +// else +// { +// // Successfully got message +// status->msg_received = 1; +// status->parse_state = MAVLINK_PARSE_STATE_IDLE; +// memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); +// } +// break; +//} + +bufferIndex++; +// If a message has been sucessfully decoded, check index +if (status->msg_received == 1) +{ + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; +} + +r_mavlink_status->current_seq = status->current_seq+1; +r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; +r_mavlink_status->packet_rx_drop_count = status->parse_error; +return status->msg_received; +} + */ + + +typedef union __generic_16bit +{ + uint8_t b[2]; + int16_t s; +} generic_16bit; + +typedef union __generic_32bit +{ + uint8_t b[4]; + float f; + int32_t i; + int16_t s; +} generic_32bit; + +typedef union __generic_64bit +{ + uint8_t b[8]; + int64_t ll; ///< Long long (64 bit) +} generic_64bit; + +/** + * @brief Place an unsigned byte into the buffer + * + * @param b the byte to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_uint8_t_by_index(uint8_t b, uint8_t bindex, uint8_t* buffer) +{ + *(buffer + bindex) = b; + return sizeof(b); +} + +/** + * @brief Place a signed byte into the buffer + * + * @param b the byte to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_int8_t_by_index(int8_t b, int8_t bindex, uint8_t* buffer) +{ + *(buffer + bindex) = (uint8_t)b; + return sizeof(b); +} + +/** + * @brief Place two unsigned bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_uint16_t_by_index(uint16_t b, const uint8_t bindex, uint8_t* buffer) +{ + buffer[bindex] = (b>>8)&0xff; + buffer[bindex+1] = (b & 0xff); + return sizeof(b); +} + +/** + * @brief Place two signed bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_int16_t_by_index(int16_t b, uint8_t bindex, uint8_t* buffer) +{ + return put_uint16_t_by_index(b, bindex, buffer); +} + +/** + * @brief Place four unsigned bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_uint32_t_by_index(uint32_t b, const uint8_t bindex, uint8_t* buffer) +{ + buffer[bindex] = (b>>24)&0xff; + buffer[bindex+1] = (b>>16)&0xff; + buffer[bindex+2] = (b>>8)&0xff; + buffer[bindex+3] = (b & 0xff); + return sizeof(b); +} + +/** + * @brief Place four signed bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_int32_t_by_index(int32_t b, uint8_t bindex, uint8_t* buffer) +{ + buffer[bindex] = (b>>24)&0xff; + buffer[bindex+1] = (b>>16)&0xff; + buffer[bindex+2] = (b>>8)&0xff; + buffer[bindex+3] = (b & 0xff); + return sizeof(b); +} + +/** + * @brief Place four unsigned bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_uint64_t_by_index(uint64_t b, const uint8_t bindex, uint8_t* buffer) +{ + buffer[bindex] = (b>>56)&0xff; + buffer[bindex+1] = (b>>48)&0xff; + buffer[bindex+2] = (b>>40)&0xff; + buffer[bindex+3] = (b>>32)&0xff; + buffer[bindex+4] = (b>>24)&0xff; + buffer[bindex+5] = (b>>16)&0xff; + buffer[bindex+6] = (b>>8)&0xff; + buffer[bindex+7] = (b & 0xff); + return sizeof(b); +} + +/** + * @brief Place four signed bytes into the buffer + * + * @param b the bytes to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_int64_t_by_index(int64_t b, uint8_t bindex, uint8_t* buffer) +{ + return put_uint64_t_by_index(b, bindex, buffer); +} + +/** + * @brief Place a float into the buffer + * + * @param b the float to add + * @param bindex the position in the packet + * @param buffer the packet buffer + * @return the new position of the last used byte in the buffer + */ +static inline uint8_t put_float_by_index(float b, uint8_t bindex, uint8_t* buffer) +{ + generic_32bit g; + g.f = b; + return put_int32_t_by_index(g.i, bindex, buffer); +} + +/** + * @brief Place an array into the buffer + * + * @param b the array to add + * @param length size of the array (for strings: length WITH '\0' char) + * @param bindex the position in the packet + * @param buffer packet buffer + * @return new position of the last used byte in the buffer + */ +static inline uint8_t put_array_by_index(const int8_t* b, uint8_t length, uint8_t bindex, uint8_t* buffer) +{ + memcpy(buffer+bindex, b, length); + return length; +} + +/** + * @brief Place a string into the buffer + * + * @param b the string to add + * @param maxlength size of the array (for strings: length WITHOUT '\0' char) + * @param bindex the position in the packet + * @param buffer packet buffer + * @return new position of the last used byte in the buffer + */ +static inline uint8_t put_string_by_index(const char* b, uint8_t maxlength, uint8_t bindex, uint8_t* buffer) +{ + uint16_t length = 0; + // Copy string into buffer, ensuring not to exceed the buffer size + int i; + for (i = 1; i < maxlength; i++) + { + length++; + // String characters + if (i < (maxlength - 1)) + { + buffer[bindex+i] = b[i]; + // Stop at null character + if (b[i] == '\0') + { + break; + } + } + // Enforce null termination at end of buffer + else if (i == (maxlength - 1)) + { + buffer[i] = '\0'; + } + } + // Write length into first field + put_uint8_t_by_index(length, bindex, buffer); + return length; +} + +/** + * @brief Put a bitfield of length 1-32 bit into the buffer + * + * @param b the value to add, will be encoded in the bitfield + * @param bits number of bits to use to encode b, e.g. 1 for boolean, 2, 3, etc. + * @param packet_index the position in the packet (the index of the first byte to use) + * @param bit_index the position in the byte (the index of the first bit to use) + * @param buffer packet buffer to write into + * @return new position of the last used byte in the buffer + */ +static inline uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index, uint8_t* r_bit_index, uint8_t* buffer) +{ + uint16_t bits_remain = bits; + // Transform number into network order + generic_32bit bin; + generic_32bit bout; + uint8_t i_bit_index, i_byte_index, curr_bits_n; + bin.i = b; + bout.b[0] = bin.b[3]; + bout.b[1] = bin.b[2]; + bout.b[2] = bin.b[1]; + bout.b[3] = bin.b[0]; + + // buffer in + // 01100000 01000000 00000000 11110001 + // buffer out + // 11110001 00000000 01000000 01100000 + + // Existing partly filled byte (four free slots) + // 0111xxxx + + // Mask n free bits + // 00001111 = 2^0 + 2^1 + 2^2 + 2^3 = 2^n - 1 + // = ((uint32_t)(1 << n)) - 1; // = 2^n - 1 + + // Shift n bits into the right position + // out = in >> n; + + // Mask and shift bytes + i_bit_index = bit_index; + i_byte_index = packet_index; + if (bit_index > 0) + { + // If bits were available at start, they were available + // in the byte before the current index + i_byte_index--; + } + + // While bits have not been packed yet + while (bits_remain > 0) + { + // Bits still have to be packed + // there can be more than 8 bits, so + // we might have to pack them into more than one byte + + // First pack everything we can into the current 'open' byte + //curr_bits_n = bits_remain << 3; // Equals bits_remain mod 8 + //FIXME + if (bits_remain <= (8 - i_bit_index)) + { + // Enough space + curr_bits_n = bits_remain; + } + else + { + curr_bits_n = (8 - i_bit_index); + } + + // Pack these n bits into the current byte + // Mask out whatever was at that position with ones (xxx11111) + buffer[i_byte_index] &= (0xFF >> (8 - curr_bits_n)); + // Put content to this position, by masking out the non-used part + buffer[i_byte_index] |= ((0x00 << curr_bits_n) & bout.i); + + // Increment the bit index + i_bit_index += curr_bits_n; + + // Now proceed to the next byte, if necessary + bits_remain -= curr_bits_n; + if (bits_remain > 0) + { + // Offer another 8 bits / one byte + i_byte_index++; + i_bit_index = 0; + } + } + + *r_bit_index = i_bit_index; + // If a partly filled byte is present, mark this as consumed + if (i_bit_index != 7) i_byte_index++; + return i_byte_index - packet_index; +} +*/ +#endif /* FILE_FINISHED */ + +#endif /* _MAVLINK_PROTOCOL_H_ */ diff --git a/thirdParty/mavlink/include/mavlink_types.h b/thirdParty/mavlink/include/mavlink_types.h old mode 100644 new mode 100755 index 199fc56f4..3124ba621 --- a/thirdParty/mavlink/include/mavlink_types.h +++ b/thirdParty/mavlink/include/mavlink_types.h @@ -1,87 +1,240 @@ -#ifndef MAVLINK_TYPES_H_ -#define MAVLINK_TYPES_H_ - -#include "inttypes.h" - -#define MAVLINK_STX 0xD5 ///< Packet start sign -#define MAVLINK_STX_LEN 1 ///< Length of start sign -#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length - -#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum -#define MAVLINK_NUM_CHECKSUM_BYTES 2 -#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) -#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) - -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length -//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN - -typedef struct __mavlink_system { - uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function - uint8_t type; ///< Unused, can be used by user to store the system's type - uint8_t state; ///< Unused, can be used by user to store the system's state - uint8_t mode; ///< Unused, can be used by user to store the system's mode - uint8_t nav_mode; ///< Unused, can be used by user to store the system's navigation mode -} mavlink_system_t; - -typedef struct __mavlink_message { - uint8_t ck_a; ///< Checksum high byte - uint8_t ck_b; ///< Checksum low byte - uint8_t STX; ///< start of packet marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload - uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU -} mavlink_message_t; - -typedef struct __mavlink_header { - uint8_t ck_a; ///< Checksum high byte - uint8_t ck_b; ///< Checksum low byte - uint8_t STX; ///< start of packet marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload -} mavlink_header_t; - -typedef enum { - MAVLINK_COMM_0, - MAVLINK_COMM_1, - MAVLINK_COMM_2, - MAVLINK_COMM_3, - MAVLINK_COMM_NB, - MAVLINK_COMM_NB_HIGH = 16 -} mavlink_channel_t; - -typedef enum { - MAVLINK_PARSE_STATE_UNINIT=0, - MAVLINK_PARSE_STATE_IDLE, - MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, - MAVLINK_PARSE_STATE_GOT_LENGTH, - MAVLINK_PARSE_STATE_GOT_SYSID, - MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, - MAVLINK_PARSE_STATE_GOT_PAYLOAD, - MAVLINK_PARSE_STATE_GOT_CRC1 -} mavlink_parse_state_t; ///< The state machine for the comm parser - -typedef struct __mavlink_status { - uint8_t ck_a; ///< Checksum byte 1 - uint8_t ck_b; ///< Checksum byte 2 - uint8_t msg_received; ///< Number of received messages - uint8_t buffer_overrun; ///< Number of buffer overruns - uint8_t parse_error; ///< Number of parse errors - mavlink_parse_state_t parse_state; ///< Parsing state machine - uint8_t packet_idx; ///< Index in current packet - uint8_t current_rx_seq; ///< Sequence number of last packet received - uint8_t current_tx_seq; ///< Sequence number of last packet sent - uint16_t packet_rx_success_count; ///< Received packets - uint16_t packet_rx_drop_count; ///< Number of packet drops -} mavlink_status_t; - -#endif /* MAVLINK_TYPES_H_ */ +/** @file + * @brief MAVLink comm protocol enumerations / structures / constants. + * @see http://qgroundcontrol.org/mavlink/ + * Edited on Monday, August 8 2011 + */ + +#ifndef MAVLINK_TYPES_H_ +#define MAVLINK_TYPES_H_ + +#include "inttypes.h" + +enum MAV_CLASS +{ + MAV_CLASS_GENERIC = 0, ///< Generic autopilot, full support for everything + MAV_CLASS_PIXHAWK = 1, ///< PIXHAWK autopilot, http://pixhawk.ethz.ch + MAV_CLASS_SLUGS = 2, ///< SLUGS autopilot, http://slugsuav.soe.ucsc.edu + MAV_CLASS_ARDUPILOTMEGA = 3, ///< ArduPilotMega / ArduCopter, http://diydrones.com + MAV_CLASS_OPENPILOT = 4, ///< OpenPilot, http://openpilot.org + MAV_CLASS_GENERIC_MISSION_WAYPOINTS_ONLY = 5, ///< Generic autopilot only supporting simple waypoints + MAV_CLASS_GENERIC_MISSION_NAVIGATION_ONLY = 6, ///< Generic autopilot supporting waypoints and other simple navigation commands + MAV_CLASS_GENERIC_MISSION_FULL = 7, ///< Generic autopilot supporting the full mission command set + MAV_CLASS_NB ///< Number of autopilot classes +}; + +enum MAV_ACTION +{ + MAV_ACTION_HOLD = 0, + MAV_ACTION_MOTORS_START = 1, + MAV_ACTION_LAUNCH = 2, + MAV_ACTION_RETURN = 3, + MAV_ACTION_EMCY_LAND = 4, + MAV_ACTION_EMCY_KILL = 5, + MAV_ACTION_CONFIRM_KILL = 6, + MAV_ACTION_CONTINUE = 7, + MAV_ACTION_MOTORS_STOP = 8, + MAV_ACTION_HALT = 9, + MAV_ACTION_SHUTDOWN = 10, + MAV_ACTION_REBOOT = 11, + MAV_ACTION_SET_MANUAL = 12, + MAV_ACTION_SET_AUTO = 13, + MAV_ACTION_STORAGE_READ = 14, + MAV_ACTION_STORAGE_WRITE = 15, + MAV_ACTION_CALIBRATE_RC = 16, + MAV_ACTION_CALIBRATE_GYRO = 17, + MAV_ACTION_CALIBRATE_MAG = 18, + MAV_ACTION_CALIBRATE_ACC = 19, + MAV_ACTION_CALIBRATE_PRESSURE = 20, + MAV_ACTION_REC_START = 21, + MAV_ACTION_REC_PAUSE = 22, + MAV_ACTION_REC_STOP = 23, + MAV_ACTION_TAKEOFF = 24, + MAV_ACTION_NAVIGATE = 25, + MAV_ACTION_LAND = 26, + MAV_ACTION_LOITER = 27, + MAV_ACTION_SET_ORIGIN = 28, + MAV_ACTION_RELAY_ON = 29, + MAV_ACTION_RELAY_OFF = 30, + MAV_ACTION_GET_IMAGE = 31, + MAV_ACTION_VIDEO_START = 32, + MAV_ACTION_VIDEO_STOP = 33, + MAV_ACTION_RESET_MAP = 34, + MAV_ACTION_RESET_PLAN = 35, + MAV_ACTION_DELAY_BEFORE_COMMAND = 36, + MAV_ACTION_ASCEND_AT_RATE = 37, + MAV_ACTION_CHANGE_MODE = 38, + MAV_ACTION_LOITER_MAX_TURNS = 39, + MAV_ACTION_LOITER_MAX_TIME = 40, + MAV_ACTION_START_HILSIM = 41, + MAV_ACTION_STOP_HILSIM = 42, + MAV_ACTION_NB ///< Number of MAV actions +}; + +enum MAV_MODE +{ + MAV_MODE_UNINIT = 0, ///< System is in undefined state + MAV_MODE_LOCKED = 1, ///< Motors are blocked, system is safe + MAV_MODE_MANUAL = 2, ///< System is allowed to be active, under manual (RC) control + MAV_MODE_GUIDED = 3, ///< System is allowed to be active, under autonomous control, manual setpoint + MAV_MODE_AUTO = 4, ///< System is allowed to be active, under autonomous control and navigation + MAV_MODE_TEST1 = 5, ///< Generic test mode, for custom use + MAV_MODE_TEST2 = 6, ///< Generic test mode, for custom use + MAV_MODE_TEST3 = 7, ///< Generic test mode, for custom use + MAV_MODE_READY = 8, ///< System is ready, motors are unblocked, but controllers are inactive + MAV_MODE_RC_TRAINING = 9 ///< System is blocked, only RC valued are read and reported back +}; + +enum MAV_STATE +{ + MAV_STATE_UNINIT = 0, + MAV_STATE_BOOT, + MAV_STATE_CALIBRATING, + MAV_STATE_STANDBY, + MAV_STATE_ACTIVE, + MAV_STATE_CRITICAL, + MAV_STATE_EMERGENCY, + MAV_STATE_POWEROFF +}; + +enum MAV_NAV +{ + MAV_NAV_GROUNDED = 0, + MAV_NAV_LIFTOFF, + MAV_NAV_HOLD, + MAV_NAV_WAYPOINT, + MAV_NAV_VECTOR, + MAV_NAV_RETURNING, + MAV_NAV_LANDING, + MAV_NAV_LOST, + MAV_NAV_LOITER +}; + +enum MAV_TYPE +{ + MAV_GENERIC = 0, + MAV_FIXED_WING = 1, + MAV_QUADROTOR = 2, + MAV_COAXIAL = 3, + MAV_HELICOPTER = 4, + MAV_GROUND = 5, + OCU = 6 +}; + +enum MAV_AUTOPILOT_TYPE +{ + MAV_AUTOPILOT_GENERIC = 0, + MAV_AUTOPILOT_PIXHAWK = 1, + MAV_AUTOPILOT_SLUGS = 2, + MAV_AUTOPILOT_ARDUPILOTMEGA = 3 +}; + +enum MAV_COMPONENT +{ + MAV_COMP_ID_GPS, + MAV_COMP_ID_WAYPOINTPLANNER, + MAV_COMP_ID_BLOBTRACKER, + MAV_COMP_ID_PATHPLANNER, + MAV_COMP_ID_AIRSLAM, + MAV_COMP_ID_MAPPER, + MAV_COMP_ID_CAMERA, + MAV_COMP_ID_IMU = 200, + MAV_COMP_ID_UDP_BRIDGE = 240, + MAV_COMP_ID_UART_BRIDGE = 241, + MAV_COMP_ID_SYSTEM_CONTROL = 250 +}; + +enum MAV_FRAME +{ + MAV_FRAME_GLOBAL = 0, + MAV_FRAME_LOCAL = 1, + MAV_FRAME_MISSION = 2, + MAV_FRAME_GLOBAL_RELATIVE_ALT = 3 +}; + +#define MAVLINK_STX 0xD5 ///< Packet start sign +#define MAVLINK_STX_LEN 1 ///< Length of start sign +#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length + +#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + MAVLINK_STX_LEN) ///< Length of all header bytes, including core and checksum +#define MAVLINK_NUM_CHECKSUM_BYTES 2 +#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) +#define MAVLINK_NUM_NON_STX_PAYLOAD_BYTES (MAVLINK_NUM_NON_PAYLOAD_BYTES - MAVLINK_STX_LEN) + +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length +//#define MAVLINK_MAX_DATA_LEN MAVLINK_MAX_PACKET_LEN - MAVLINK_STX_LEN + +typedef struct __mavlink_system { + uint8_t sysid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t compid; ///< Used by the MAVLink message_xx_send() convenience function + uint8_t type; ///< Unused, can be used by user to store the system's type + uint8_t state; ///< Unused, can be used by user to store the system's state + uint8_t mode; ///< Unused, can be used by user to store the system's mode +} mavlink_system_t; + +typedef struct __mavlink_message { + union { + uint16_t ck; ///< Checksum high byte + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload + uint8_t payload[MAVLINK_MAX_PAYLOAD_LEN]; ///< Payload data, ALIGNMENT IMPORTANT ON MCU +} mavlink_message_t; + +typedef struct __mavlink_header { + union { + uint16_t ck; ///< Checksum high byte + uint8_t ck_a; ///< Checksum low byte + uint8_t ck_b; ///< Checksum high byte + }; + uint8_t STX; ///< start of packet marker + uint8_t len; ///< Length of payload + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint8_t msgid; ///< ID of message in payload +} mavlink_header_t; + +typedef enum { + MAVLINK_COMM_0, + MAVLINK_COMM_1, + MAVLINK_COMM_2, + MAVLINK_COMM_3, + MAVLINK_COMM_NB, + MAVLINK_COMM_NB_HIGH = 16 + } mavlink_channel_t; + +typedef enum { + MAVLINK_PARSE_STATE_UNINIT=0, + MAVLINK_PARSE_STATE_IDLE, + MAVLINK_PARSE_STATE_GOT_STX, + MAVLINK_PARSE_STATE_GOT_SEQ, + MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_SYSID, + MAVLINK_PARSE_STATE_GOT_COMPID, + MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_PAYLOAD, + MAVLINK_PARSE_STATE_GOT_CRC1 +} mavlink_parse_state_t; ///< The state machine for the comm parser + +typedef struct __mavlink_status { + uint8_t ck_a; ///< Checksum byte 1 + uint8_t ck_b; ///< Checksum byte 2 + uint8_t msg_received; ///< Number of received messages + uint8_t buffer_overrun; ///< Number of buffer overruns + uint8_t parse_error; ///< Number of parse errors + mavlink_parse_state_t parse_state; ///< Parsing state machine + uint8_t packet_idx; ///< Index in current packet + uint8_t current_rx_seq; ///< Sequence number of last packet received + uint8_t current_tx_seq; ///< Sequence number of last packet sent + uint16_t packet_rx_success_count; ///< Received packets + uint16_t packet_rx_drop_count; ///< Number of packet drops +} mavlink_status_t; + +#endif /* MAVLINK_TYPES_H_ */ diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h index 7cfe0d9d0..c8cd1940e 100644 --- a/thirdParty/mavlink/include/minimal/mavlink.h +++ b/thirdParty/mavlink/include/minimal/mavlink.h @@ -1,15 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H #pragma pack(push,1) +#include "mavlink_options.h" #include "minimal.h" -#ifdef MAVLINK_CHECK_LENGTH -#include "lengths.h" +#ifdef MAVLINK_DATA +#include "mavlink_data.h" #endif #pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h index fea5381e9..7c686831e 100644 --- a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h @@ -76,34 +76,9 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c * @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) * @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg.payload[0]; - - p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - - p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; - msg.msgid = MAVLINK_MSG_ID_HEARTBEAT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h index 47a266ccb..c3d7e2a1b 100644 --- a/thirdParty/mavlink/include/minimal/minimal.h +++ b/thirdParty/mavlink/include/minimal/minimal.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MINIMAL_H #define MINIMAL_H @@ -32,6 +32,13 @@ extern "C" { // MESSAGE DEFINITIONS #include "./mavlink_msg_heartbeat.h" + + +// MESSAGE CRC KEYS + +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink.h b/thirdParty/mavlink/include/pixhawk/mavlink.h index 963c3697a..c59c58b83 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink.h @@ -1,15 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H #pragma pack(push,1) +#include "mavlink_options.h" #include "pixhawk.h" -#ifdef MAVLINK_CHECK_LENGTH -#include "lengths.h" +#ifdef MAVLINK_DATA +#include "mavlink_data.h" #endif #pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h index 25ed69410..fd8503d97 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h @@ -6,11 +6,11 @@ typedef struct __mavlink_attitude_control_t { - uint8_t target; ///< The system to be controlled float roll; ///< roll float pitch; ///< pitch float yaw; ///< yaw float thrust; ///< thrust + uint8_t target; ///< The system to be controlled uint8_t roll_manual; ///< roll control enabled auto:0, manual:1 uint8_t pitch_manual; ///< pitch auto:0, manual:1 uint8_t yaw_manual; ///< yaw auto:0, manual:1 @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui * @param yaw_manual yaw auto:0, manual:1 * @param thrust_manual thrust auto:0, manual:1 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system to be controlled - p->roll = roll; // float:roll - p->pitch = pitch; // float:pitch - p->yaw = yaw; // float:yaw - p->thrust = thrust; // float:thrust - p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 - p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 - p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 - p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN; - msg.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h index 9cb8641e8..2fb3314c9 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_aux_status_encode(uint8_t system_id, uint8_t * @param spi1_err_count Number of I2C errors since startup * @param uart_total_err_count Number of I2C errors since startup */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg.payload[0]; - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup - p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup - p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup - p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup - p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_AUX_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_AUX_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h index 96814fcab..4e6f6b83b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h @@ -9,11 +9,11 @@ typedef struct __mavlink_brief_feature_t float x; ///< x position in m float y; ///< y position in m float z; ///< z position in m - uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true + float response; ///< Harris operator response at this location uint16_t size; ///< Size in pixels uint16_t orientation; ///< Orientation - char descriptor[32]; ///< Descriptor - float response; ///< Harris operator response at this location + uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true + uint8_t descriptor[32]; ///< Descriptor } mavlink_brief_feature_t; #define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 @@ -34,7 +34,7 @@ typedef struct __mavlink_brief_feature_t * @param response Harris operator response at this location * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const char* descriptor, float response) +static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) { mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; @@ -45,7 +45,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true p->size = size; // uint16_t:Size in pixels p->orientation = orientation; // uint16_t:Orientation - memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor p->response = response; // float:Harris operator response at this location return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t * @param response Harris operator response at this location * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const char* descriptor, float response) +static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) { mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; @@ -78,7 +78,7 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true p->size = size; // uint16_t:Size in pixels p->orientation = orientation; // uint16_t:Orientation - memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor p->response = response; // float:Harris operator response at this location return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); @@ -110,40 +110,10 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 * @param descriptor Descriptor * @param response Harris operator response at this location */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const char* descriptor, float response) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg.payload[0]; - - p->x = x; // float:x position in m - p->y = y; // float:y position in m - p->z = z; // float:z position in m - p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true - p->size = size; // uint16_t:Size in pixels - p->orientation = orientation; // uint16_t:Orientation - memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor - p->response = response; // float:Harris operator response at this location - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_BRIEF_FEATURE_LEN; - msg.msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const char* descriptor, float response) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) { mavlink_header_t hdr; mavlink_brief_feature_t payload; @@ -156,7 +126,7 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true p->size = size; // uint16_t:Size in pixels p->orientation = orientation; // uint16_t:Orientation - memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // char[32]:Descriptor + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor p->response = response; // float:Harris operator response at this location hdr.STX = MAVLINK_STX; @@ -252,7 +222,7 @@ static inline uint16_t mavlink_msg_brief_feature_get_orientation(const mavlink_m * * @return Descriptor */ -static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, char* descriptor) +static inline uint16_t mavlink_msg_brief_feature_get_descriptor(const mavlink_message_t* msg, uint8_t* descriptor) { mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h index 22116a54c..54d2070a9 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -6,8 +6,8 @@ typedef struct __mavlink_data_transmission_handshake_t { - uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) uint32_t size; ///< total data size in bytes (set on ACK only) + uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) uint8_t packets; ///< number of packets beeing sent (set on ACK only) uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) uint8_t jpg_quality; ///< JPEG quality out of [1,100] @@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) * @param jpg_quality JPEG quality out of [1,100] */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg.payload[0]; - - p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - p->size = size; // uint32_t:total data size in bytes (set on ACK only) - p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) - p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN; - msg.msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h index cd052fcb4..a5354fdde 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h @@ -74,33 +74,9 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u * @param seqnr sequence number (starting with 0 on every transmission) * @param data image data bytes */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t* data) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg.payload[0]; - p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) - memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN; - msg.msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t* data) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h index 49869a01e..0b4c9c8ef 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h @@ -7,15 +7,10 @@ typedef struct __mavlink_image_available_t { uint64_t cam_id; ///< Camera id - uint8_t cam_no; ///< Camera # (starts with 0) uint64_t timestamp; ///< Timestamp uint64_t valid_until; ///< Until which timestamp this buffer will stay valid uint32_t img_seq; ///< The image sequence number uint32_t img_buf_index; ///< Position of the image in the buffer, starts with 0 - uint16_t width; ///< Image width - uint16_t height; ///< Image height - uint16_t depth; ///< Image depth - uint8_t channels; ///< Image channels uint32_t key; ///< Shared memory area key uint32_t exposure; ///< Exposure time, in microseconds float gain; ///< Camera gain @@ -29,6 +24,11 @@ typedef struct __mavlink_image_available_t float ground_x; ///< Ground truth X float ground_y; ///< Ground truth Y float ground_z; ///< Ground truth Z + uint16_t width; ///< Image width + uint16_t height; ///< Image height + uint16_t depth; ///< Image depth + uint8_t cam_no; ///< Camera # (starts with 0) + uint8_t channels; ///< Image channels } mavlink_image_available_t; @@ -199,54 +199,9 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin * @param ground_y Ground truth Y * @param ground_z Ground truth Z */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_image_available_t *p = (mavlink_image_available_t *)&msg.payload[0]; - - p->cam_id = cam_id; // uint64_t:Camera id - p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) - p->timestamp = timestamp; // uint64_t:Timestamp - p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid - p->img_seq = img_seq; // uint32_t:The image sequence number - p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 - p->width = width; // uint16_t:Image width - p->height = height; // uint16_t:Image height - p->depth = depth; // uint16_t:Image depth - p->channels = channels; // uint8_t:Image channels - p->key = key; // uint32_t:Shared memory area key - p->exposure = exposure; // uint32_t:Exposure time, in microseconds - p->gain = gain; // float:Camera gain - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - p->local_z = local_z; // float:Local frame Z coordinate (height over ground) - p->lat = lat; // float:GPS X coordinate - p->lon = lon; // float:GPS Y coordinate - p->alt = alt; // float:Global frame altitude - p->ground_x = ground_x; // float:Ground truth X - p->ground_y = ground_y; // float:Ground truth Y - p->ground_z = ground_z; // float:Ground truth Z - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN; - msg.msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h index 0e81a6c6d..99027fea5 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h @@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i * * @param enable 0 to disable, 1 to enable */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg.payload[0]; - - p->enable = enable; // uint8_t:0 to disable, 1 to enable - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN; - msg.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h index 6087d3523..250bee3a1 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h @@ -133,43 +133,9 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin * @param ground_y Ground truth Y * @param ground_z Ground truth Z */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg.payload[0]; - - p->timestamp = timestamp; // uint64_t:Timestamp - p->seq = seq; // uint32_t:IMU seq - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - p->local_z = local_z; // float:Local frame Z coordinate (height over ground) - p->lat = lat; // float:GPS X coordinate - p->lon = lon; // float:GPS Y coordinate - p->alt = alt; // float:Global frame altitude - p->ground_x = ground_x; // float:Ground truth X - p->ground_y = ground_y; // float:Ground truth Y - p->ground_z = ground_z; // float:Ground truth Z - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN; - msg.msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h index e5519ef6d..34ebf6351 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h @@ -6,13 +6,13 @@ typedef struct __mavlink_marker_t { - uint16_t id; ///< ID float x; ///< x position float y; ///< y position float z; ///< z position float roll; ///< roll orientation float pitch; ///< pitch orientation float yaw; ///< yaw orientation + uint16_t id; ///< ID } mavlink_marker_t; @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp * @param pitch pitch orientation * @param yaw yaw orientation */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_marker_t *p = (mavlink_marker_t *)&msg.payload[0]; - p->id = id; // uint16_t:ID - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->roll = roll; // float:roll orientation - p->pitch = pitch; // float:pitch orientation - p->yaw = yaw; // float:yaw orientation - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_MARKER_LEN; - msg.msgid = MAVLINK_MSG_ID_MARKER; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index 311e3be97..b2dd6c3a0 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -6,9 +6,9 @@ typedef struct __mavlink_pattern_detected_t { - uint8_t type; ///< 0: Pattern, 1: Letter float confidence; ///< Confidence of detection - char file[100]; ///< Pattern file name + uint8_t type; ///< 0: Pattern, 1: Letter + uint8_t file[100]; ///< Pattern file name uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes } mavlink_pattern_detected_t; @@ -26,14 +26,14 @@ typedef struct __mavlink_pattern_detected_t * @param detected Accepted as true detection, 0 no, 1 yes * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, float confidence, const char* file, uint8_t detected) +static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, float confidence, const uint8_t* file, uint8_t detected) { mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; p->type = type; // uint8_t:0: Pattern, 1: Letter p->confidence = confidence; // float:Confidence of detection - memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + memcpy(p->file, file, sizeof(p->file)); // uint8_t[100]:Pattern file name p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); @@ -51,14 +51,14 @@ static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint * @param detected Accepted as true detection, 0 no, 1 yes * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, float confidence, const char* file, uint8_t detected) +static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, float confidence, const uint8_t* file, uint8_t detected) { mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; p->type = type; // uint8_t:0: Pattern, 1: Letter p->confidence = confidence; // float:Confidence of detection - memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + memcpy(p->file, file, sizeof(p->file)); // uint8_t[100]:Pattern file name p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); @@ -86,36 +86,10 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui * @param file Pattern file name * @param detected Accepted as true detection, 0 no, 1 yes */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char* file, uint8_t detected) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg.payload[0]; - p->type = type; // uint8_t:0: Pattern, 1: Letter - p->confidence = confidence; // float:Confidence of detection - memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name - p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_PATTERN_DETECTED_LEN; - msg.msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char* file, uint8_t detected) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const uint8_t* file, uint8_t detected) { mavlink_header_t hdr; mavlink_pattern_detected_t payload; @@ -124,7 +98,7 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin p->type = type; // uint8_t:0: Pattern, 1: Letter p->confidence = confidence; // float:Confidence of detection - memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + memcpy(p->file, file, sizeof(p->file)); // uint8_t[100]:Pattern file name p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes hdr.STX = MAVLINK_STX; @@ -176,7 +150,7 @@ static inline float mavlink_msg_pattern_detected_get_confidence(const mavlink_me * * @return Pattern file name */ -static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, char* file) +static inline uint16_t mavlink_msg_pattern_detected_get_file(const mavlink_message_t* msg, uint8_t* file) { mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h index 284bd4a21..0a70ad8e2 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -1,22 +1,22 @@ // MESSAGE POINT_OF_INTEREST PACKING #define MAVLINK_MSG_ID_POINT_OF_INTEREST 161 -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 42 -#define MAVLINK_MSG_161_LEN 42 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 43 +#define MAVLINK_MSG_161_LEN 43 typedef struct __mavlink_point_of_interest_t { - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds float x; ///< X Position float y; ///< Y Position float z; ///< Z Position - char name[25]; ///< POI name + uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds + uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + uint8_t coordinate_system; ///< 0: global, 1:local + uint8_t name[26]; ///< POI name } mavlink_point_of_interest_t; -#define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 25 +#define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 26 /** * @brief Pack a point_of_interest message @@ -34,7 +34,7 @@ typedef struct __mavlink_point_of_interest_t * @param name POI name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) +static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const uint8_t* name) { mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; @@ -46,7 +46,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin p->x = x; // float:X Position p->y = y; // float:Y Position p->z = z; // float:Z Position - memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI name return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } @@ -67,7 +67,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin * @param name POI name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) +static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const uint8_t* name) { mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id p->x = x; // float:X Position p->y = y; // float:Y Position p->z = z; // float:Z Position - memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI name return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } @@ -110,40 +110,10 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u * @param z Z Position * @param name POI name */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg.payload[0]; - - p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - p->x = x; // float:X Position - p->y = y; // float:Y Position - p->z = z; // float:Z Position - memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; - msg.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const uint8_t* name) { mavlink_header_t hdr; mavlink_point_of_interest_t payload; @@ -157,7 +127,7 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui p->x = x; // float:X Position p->y = y; // float:Y Position p->z = z; // float:Z Position - memcpy(p->name, name, sizeof(p->name)); // char[25]:POI name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI name hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; @@ -263,7 +233,7 @@ static inline float mavlink_msg_point_of_interest_get_z(const mavlink_message_t* * * @return POI name */ -static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, char* name) +static inline uint16_t mavlink_msg_point_of_interest_get_name(const mavlink_message_t* msg, uint8_t* name) { mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h index 972450692..c9fe383af 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h @@ -1,25 +1,25 @@ // MESSAGE POINT_OF_INTEREST_CONNECTION PACKING #define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 162 -#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 54 -#define MAVLINK_MSG_162_LEN 54 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 55 +#define MAVLINK_MSG_162_LEN 55 typedef struct __mavlink_point_of_interest_connection_t { - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds float xp1; ///< X1 Position float yp1; ///< Y1 Position float zp1; ///< Z1 Position float xp2; ///< X2 Position float yp2; ///< Y2 Position float zp2; ///< Z2 Position - char name[25]; ///< POI connection name + uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds + uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + uint8_t coordinate_system; ///< 0: global, 1:local + uint8_t name[26]; ///< POI connection name } mavlink_point_of_interest_connection_t; -#define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 25 +#define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 26 /** * @brief Pack a point_of_interest_connection message @@ -40,7 +40,7 @@ typedef struct __mavlink_point_of_interest_connection_t * @param name POI connection name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const uint8_t* name) { mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; @@ -55,7 +55,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys p->xp2 = xp2; // float:X2 Position p->yp2 = yp2; // float:Y2 Position p->zp2 = zp2; // float:Z2 Position - memcpy(p->name, name, sizeof(p->name)); // char[25]:POI connection name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI connection name return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } @@ -79,7 +79,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys * @param name POI connection name * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const uint8_t* name) { mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; @@ -94,7 +94,7 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_ p->xp2 = xp2; // float:X2 Position p->yp2 = yp2; // float:Y2 Position p->zp2 = zp2; // float:Z2 Position - memcpy(p->name, name, sizeof(p->name)); // char[25]:POI connection name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI connection name return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } @@ -128,43 +128,10 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s * @param zp2 Z2 Position * @param name POI connection name */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg.payload[0]; - p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - p->xp1 = xp1; // float:X1 Position - p->yp1 = yp1; // float:Y1 Position - p->zp1 = zp1; // float:Z1 Position - p->xp2 = xp2; // float:X2 Position - p->yp2 = yp2; // float:Y2 Position - p->zp2 = zp2; // float:Z2 Position - memcpy(p->name, name, sizeof(p->name)); // char[25]:POI connection name - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; - msg.msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const uint8_t* name) { mavlink_header_t hdr; mavlink_point_of_interest_connection_t payload; @@ -181,7 +148,7 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel p->xp2 = xp2; // float:X2 Position p->yp2 = yp2; // float:Y2 Position p->zp2 = zp2; // float:Z2 Position - memcpy(p->name, name, sizeof(p->name)); // char[25]:POI connection name + memcpy(p->name, name, sizeof(p->name)); // uint8_t[26]:POI connection name hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; @@ -320,7 +287,7 @@ static inline float mavlink_msg_point_of_interest_connection_get_zp2(const mavli * * @return POI connection name */ -static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, char* name) +static inline uint16_t mavlink_msg_point_of_interest_connection_get_name(const mavlink_message_t* msg, uint8_t* name) { mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h index 2b508a62a..b5446931f 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h @@ -6,12 +6,12 @@ typedef struct __mavlink_position_control_offset_set_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID float x; ///< x position offset float y; ///< y position offset float z; ///< z position offset float yaw; ///< yaw orientation offset in radians, 0 = NORTH + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_position_control_offset_set_t; @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_position_control_offset_set_encode(uint8_t sy * @param z z position offset * @param yaw yaw orientation offset in radians, 0 = NORTH */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_position_control_offset_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->x = x; // float:x position offset - p->y = y; // float:y position offset - p->z = z; // float:z position offset - p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN; - msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_control_offset_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h index ee33580eb..dd3b92ca8 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h @@ -6,11 +6,11 @@ typedef struct __mavlink_position_control_setpoint_t { - uint16_t id; ///< ID of waypoint, 0 for plain position float x; ///< x position float y; ///< y position float z; ///< z position float yaw; ///< yaw orientation in radians, 0 = NORTH + uint16_t id; ///< ID of waypoint, 0 for plain position } mavlink_position_control_setpoint_t; @@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst * @param z z position * @param yaw yaw orientation in radians, 0 = NORTH */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg.payload[0]; - - p->id = id; // uint16_t:ID of waypoint, 0 for plain position - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN; - msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h index 031a73b01..f14a31c46 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h @@ -6,13 +6,13 @@ typedef struct __mavlink_position_control_setpoint_set_t { - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID - uint16_t id; ///< ID of waypoint, 0 for plain position float x; ///< x position float y; ///< y position float z; ///< z position float yaw; ///< yaw orientation in radians, 0 = NORTH + uint16_t id; ///< ID of waypoint, 0 for plain position + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_position_control_setpoint_set_t; @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_encode(uint8_t * @param z z position * @param yaw yaw orientation in radians, 0 = NORTH */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg.payload[0]; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->id = id; // uint16_t:ID of waypoint, 0 for plain position - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN; - msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h index 32b82e568..7bafeb4c9 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h @@ -6,13 +6,13 @@ typedef struct __mavlink_raw_aux_t { + int32_t baro; ///< Barometric pressure (hecto Pascal) uint16_t adc1; ///< ADC1 (J405 ADC3, LPC2148 AD0.6) uint16_t adc2; ///< ADC2 (J405 ADC5, LPC2148 AD0.2) uint16_t adc3; ///< ADC3 (J405 ADC6, LPC2148 AD0.1) uint16_t adc4; ///< ADC4 (J405 ADC7, LPC2148 AD1.3) uint16_t vbat; ///< Battery voltage int16_t temp; ///< Temperature (degrees celcius) - int32_t baro; ///< Barometric pressure (hecto Pascal) } mavlink_raw_aux_t; @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com * @param temp Temperature (degrees celcius) * @param baro Barometric pressure (hecto Pascal) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg.payload[0]; - p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) - p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) - p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) - p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) - p->vbat = vbat; // uint16_t:Battery voltage - p->temp = temp; // int16_t:Temperature (degrees celcius) - p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RAW_AUX_LEN; - msg.msgid = MAVLINK_MSG_ID_RAW_AUX; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h index 5db1aa478..5b1466236 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h @@ -6,12 +6,12 @@ typedef struct __mavlink_set_cam_shutter_t { + float gain; ///< Camera gain + uint16_t interval; ///< Shutter interval, in microseconds + uint16_t exposure; ///< Exposure time, in microseconds uint8_t cam_no; ///< Camera id uint8_t cam_mode; ///< Camera mode: 0 = auto, 1 = manual uint8_t trigger_pin; ///< Trigger pin, 0-3 for PtGrey FireFly - uint16_t interval; ///< Shutter interval, in microseconds - uint16_t exposure; ///< Exposure time, in microseconds - float gain; ///< Camera gain } mavlink_set_cam_shutter_t; @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin * @param exposure Exposure time, in microseconds * @param gain Camera gain */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg.payload[0]; - p->cam_no = cam_no; // uint8_t:Camera id - p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual - p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly - p->interval = interval; // uint16_t:Shutter interval, in microseconds - p->exposure = exposure; // uint16_t:Exposure time, in microseconds - p->gain = gain; // float:Camera gain - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN; - msg.msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h index c27e22bc2..862ea8cbe 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system * @param pitch Pitch angle in rad * @param yaw Yaw angle in rad */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg.payload[0]; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X position - p->y = y; // float:Global Y position - p->z = z; // float:Global Z position - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN; - msg.msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h index dbf29c50a..cfd0b15f7 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste * @param pitch Pitch angle in rad * @param yaw Yaw angle in rad */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg.payload[0]; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X position - p->y = y; // float:Global Y position - p->z = z; // float:Global Z position - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN; - msg.msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h index 569f7123b..e3d0755de 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h @@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i * @param y Global Y speed * @param z Global Z speed */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg.payload[0]; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X speed - p->y = y; // float:Global Y speed - p->z = z; // float:Global Z speed - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN; - msg.msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h index a039a8aca..dee5bcf0a 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h @@ -6,9 +6,9 @@ typedef struct __mavlink_watchdog_command_t { - uint8_t target_system_id; ///< Target system ID uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID + uint8_t target_system_id; ///< Target system ID uint8_t command_id; ///< Command ID } mavlink_watchdog_command_t; @@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui * @param process_id Process ID * @param command_id Command ID */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg.payload[0]; - p->target_system_id = target_system_id; // uint8_t:Target system ID - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - p->command_id = command_id; // uint8_t:Command ID - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN; - msg.msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h index 4d5c2687e..e31a060ea 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, * @param watchdog_id Watchdog ID * @param process_count Number of processes */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg.payload[0]; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_count = process_count; // uint16_t:Number of processes - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN; - msg.msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h index 44a5cf243..12273bcdd 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -6,11 +6,11 @@ typedef struct __mavlink_watchdog_process_info_t { + int32_t timeout; ///< Timeout (seconds) uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID - char name[100]; ///< Process name - char arguments[147]; ///< Process arguments - int32_t timeout; ///< Timeout (seconds) + uint8_t name[100]; ///< Process name + uint8_t arguments[147]; ///< Process arguments } mavlink_watchdog_process_info_t; #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 @@ -29,15 +29,15 @@ typedef struct __mavlink_watchdog_process_info_t * @param timeout Timeout (seconds) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) +static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const uint8_t* name, const uint8_t* arguments, int32_t timeout) { mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID p->process_id = process_id; // uint16_t:Process ID - memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + memcpy(p->name, name, sizeof(p->name)); // uint8_t[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // uint8_t[147]:Process arguments p->timeout = timeout; // int32_t:Timeout (seconds) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); @@ -56,15 +56,15 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, * @param timeout Timeout (seconds) * @return length of the message in bytes (excluding serial stream start sign) */ -static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) +static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint16_t watchdog_id, uint16_t process_id, const uint8_t* name, const uint8_t* arguments, int32_t timeout) { mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID p->process_id = process_id; // uint16_t:Process ID - memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + memcpy(p->name, name, sizeof(p->name)); // uint8_t[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // uint8_t[147]:Process arguments p->timeout = timeout; // int32_t:Timeout (seconds) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); @@ -93,37 +93,10 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i * @param arguments Process arguments * @param timeout Timeout (seconds) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg.payload[0]; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments - p->timeout = timeout; // int32_t:Timeout (seconds) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN; - msg.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL -static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS +static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const uint8_t* name, const uint8_t* arguments, int32_t timeout) { mavlink_header_t hdr; mavlink_watchdog_process_info_t payload; @@ -132,8 +105,8 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID p->process_id = process_id; // uint16_t:Process ID - memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + memcpy(p->name, name, sizeof(p->name)); // uint8_t[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // uint8_t[147]:Process arguments p->timeout = timeout; // int32_t:Timeout (seconds) hdr.STX = MAVLINK_STX; @@ -185,7 +158,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_process_id(const ma * * @return Process name */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, char* name) +static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_message_t* msg, uint8_t* name) { mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; @@ -198,7 +171,7 @@ static inline uint16_t mavlink_msg_watchdog_process_info_get_name(const mavlink_ * * @return Process arguments */ -static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, char* arguments) +static inline uint16_t mavlink_msg_watchdog_process_info_get_arguments(const mavlink_message_t* msg, uint8_t* arguments) { mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h index 01c80eb31..f6f3dd26d 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h @@ -6,12 +6,12 @@ typedef struct __mavlink_watchdog_process_status_t { + int32_t pid; ///< PID uint16_t watchdog_id; ///< Watchdog ID uint16_t process_id; ///< Process ID + uint16_t crashes; ///< Number of crashes uint8_t state; ///< Is running / finished / suspended / crashed uint8_t muted; ///< Is muted - int32_t pid; ///< PID - uint16_t crashes; ///< Number of crashes } mavlink_watchdog_process_status_t; @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system * @param pid PID * @param crashes Number of crashes */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg.payload[0]; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - p->state = state; // uint8_t:Is running / finished / suspended / crashed - p->muted = muted; // uint8_t:Is muted - p->pid = pid; // int32_t:PID - p->crashes = crashes; // uint16_t:Number of crashes - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index 618dbe87c..c2d8beffc 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -66,6 +66,13 @@ enum DATA_TYPES #include "./mavlink_msg_data_transmission_handshake.h" #include "./mavlink_msg_encapsulated_data.h" #include "./mavlink_msg_brief_feature.h" + + +// MESSAGE CRC KEYS + +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h index 1379932f2..04bffe2e3 100644 --- a/thirdParty/mavlink/include/slugs/mavlink.h +++ b/thirdParty/mavlink/include/slugs/mavlink.h @@ -1,15 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H #pragma pack(push,1) +#include "mavlink_options.h" #include "slugs.h" -#ifdef MAVLINK_CHECK_LENGTH -#include "lengths.h" +#ifdef MAVLINK_DATA +#include "mavlink_data.h" #endif #pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h index 19503c1df..b345be106 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t co * @param staticPressure Static pressure (Pa) * @param temperature Board temperature */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_air_data_t *p = (mavlink_air_data_t *)&msg.payload[0]; - - p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) - p->staticPressure = staticPressure; // float:Static pressure (Pa) - p->temperature = temperature; // uint16_t:Board temperature - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_AIR_DATA_LEN; - msg.msgid = MAVLINK_MSG_ID_AIR_DATA; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h index d3191546a..f30e1d8b6 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h @@ -6,9 +6,9 @@ typedef struct __mavlink_cpu_load_t { + uint16_t batVolt; ///< Battery Voltage in millivolts uint8_t sensLoad; ///< Sensor DSC Load uint8_t ctrlLoad; ///< Control DSC Load - uint16_t batVolt; ///< Battery Voltage in millivolts } mavlink_cpu_load_t; @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t co * @param ctrlLoad Control DSC Load * @param batVolt Battery Voltage in millivolts */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg.payload[0]; - - p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load - p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load - p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_CPU_LOAD_LEN; - msg.msgid = MAVLINK_MSG_ID_CPU_LOAD; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h index f983a544c..81b5fa59a 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h @@ -6,8 +6,8 @@ typedef struct __mavlink_ctrl_srfc_pt_t { - uint8_t target; ///< The system setting the commands uint16_t bitfieldPt; ///< Bitfield containing the PT configuration + uint8_t target; ///< The system setting the commands } mavlink_ctrl_srfc_pt_t; @@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_ * @param target The system setting the commands * @param bitfieldPt Bitfield containing the PT configuration */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system setting the commands - p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN; - msg.msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h index 018de9224..14da8b9a9 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_data_log_encode(uint8_t system_id, uint8_t co * @param fl_5 Log value 5 * @param fl_6 Log value 6 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_data_log_t *p = (mavlink_data_log_t *)&msg.payload[0]; - p->fl_1 = fl_1; // float:Log value 1 - p->fl_2 = fl_2; // float:Log value 2 - p->fl_3 = fl_3; // float:Log value 3 - p->fl_4 = fl_4; // float:Log value 4 - p->fl_5 = fl_5; // float:Log value 5 - p->fl_6 = fl_6; // float:Log value 6 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_DATA_LOG_LEN; - msg.msgid = MAVLINK_MSG_ID_DATA_LOG; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h index 4bfebe5fb..e9aa6279b 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_diagnostic_encode(uint8_t system_id, uint8_t * @param diagSh2 Diagnostic short 2 * @param diagSh3 Diagnostic short 3 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg.payload[0]; - p->diagFl1 = diagFl1; // float:Diagnostic float 1 - p->diagFl2 = diagFl2; // float:Diagnostic float 2 - p->diagFl3 = diagFl3; // float:Diagnostic float 3 - p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 - p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 - p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_DIAGNOSTIC_LEN; - msg.msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h index ca3b1a934..bab695cfa 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_gps_date_time_encode(uint8_t system_id, uint8 * @param sec Sec reported by Gps * @param visSat Visible sattelites reported by Gps */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg.payload[0]; - p->year = year; // uint8_t:Year reported by Gps - p->month = month; // uint8_t:Month reported by Gps - p->day = day; // uint8_t:Day reported by Gps - p->hour = hour; // uint8_t:Hour reported by Gps - p->min = min; // uint8_t:Min reported by Gps - p->sec = sec; // uint8_t:Sec reported by Gps - p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_GPS_DATE_TIME_LEN; - msg.msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h index c15461332..0436ae949 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h @@ -6,10 +6,10 @@ typedef struct __mavlink_mid_lvl_cmds_t { - uint8_t target; ///< The system setting the commands float hCommand; ///< Commanded Airspeed float uCommand; ///< Log value 2 float rCommand; ///< Log value 3 + uint8_t target; ///< The system setting the commands } mavlink_mid_lvl_cmds_t; @@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_encode(uint8_t system_id, uint8_ * @param uCommand Log value 2 * @param rCommand Log value 3 */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t target, float hCommand, float uCommand, float rCommand) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg.payload[0]; - p->target = target; // uint8_t:The system setting the commands - p->hCommand = hCommand; // float:Commanded Airspeed - p->uCommand = uCommand; // float:Log value 2 - p->rCommand = rCommand; // float:Log value 3 - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_MID_LVL_CMDS_LEN; - msg.msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t target, float hCommand, float uCommand, float rCommand) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h index 944ef8d41..5601a2a34 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h @@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t * @param gyBias Gyro Y bias (rad/s) * @param gzBias Gyro Z bias (rad/s) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg.payload[0]; - p->axBias = axBias; // float:Accelerometer X bias (m/s) - p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) - p->azBias = azBias; // float:Accelerometer Z bias (m/s) - p->gxBias = gxBias; // float:Gyro X bias (rad/s) - p->gyBias = gyBias; // float:Gyro Y bias (rad/s) - p->gzBias = gzBias; // float:Gyro Z bias (rad/s) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SENSOR_BIAS_LEN; - msg.msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h index d86066689..fe3399e2e 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h @@ -6,9 +6,9 @@ typedef struct __mavlink_slugs_action_t { + uint16_t actionVal; ///< Value associated with the action uint8_t target; ///< The system reporting the action uint8_t actionId; ///< Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - uint16_t actionVal; ///< Value associated with the action } mavlink_slugs_action_t; @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_slugs_action_encode(uint8_t system_id, uint8_ * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names * @param actionVal Value associated with the action */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t target, uint8_t actionId, uint16_t actionVal) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg.payload[0]; - - p->target = target; // uint8_t:The system reporting the action - p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - p->actionVal = actionVal; // uint16_t:Value associated with the action - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SLUGS_ACTION_LEN; - msg.msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t target, uint8_t actionId, uint16_t actionVal) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h index 0c8d87f00..dc69df79e 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h @@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_slugs_navigation_encode(uint8_t system_id, ui * @param fromWP Origin WP * @param toWP Destination WP */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg.payload[0]; - p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter - p->phi_c = phi_c; // float:Commanded Roll - p->theta_c = theta_c; // float:Commanded Pitch - p->psiDot_c = psiDot_c; // float:Commanded Turn rate - p->ay_body = ay_body; // float:Y component of the body acceleration - p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation - p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation - p->fromWP = fromWP; // uint8_t:Origin WP - p->toWP = toWP; // uint8_t:Destination WP - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN; - msg.msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h index e742be358..990857c72 100644 --- a/thirdParty/mavlink/include/slugs/slugs.h +++ b/thirdParty/mavlink/include/slugs/slugs.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Sunday, July 31 2011, 15:11 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef SLUGS_H #define SLUGS_H @@ -43,6 +43,13 @@ extern "C" { #include "./mavlink_msg_mid_lvl_cmds.h" #include "./mavlink_msg_ctrl_srfc_pt.h" #include "./mavlink_msg_slugs_action.h" + + +// MESSAGE CRC KEYS + +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink.h b/thirdParty/mavlink/include/ualberta/mavlink.h index 39a4c44a6..0bb9f2f90 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink.h +++ b/thirdParty/mavlink/include/ualberta/mavlink.h @@ -1,15 +1,16 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Sunday, July 31 2011, 15:12 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef MAVLINK_H #define MAVLINK_H #pragma pack(push,1) +#include "mavlink_options.h" #include "ualberta.h" -#ifdef MAVLINK_CHECK_LENGTH -#include "lengths.h" +#ifdef MAVLINK_DATA +#include "mavlink_data.h" #endif #pragma pack(pop) #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h index d278887fe..d1b777ee4 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h @@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_nav_filter_bias_encode(uint8_t system_id, uin * @param gyro_1 b_f[1] * @param gyro_2 b_f[2] */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg.payload[0]; - p->usec = usec; // uint64_t:Timestamp (microseconds) - p->accel_0 = accel_0; // float:b_f[0] - p->accel_1 = accel_1; // float:b_f[1] - p->accel_2 = accel_2; // float:b_f[2] - p->gyro_0 = gyro_0; // float:b_f[0] - p->gyro_1 = gyro_1; // float:b_f[1] - p->gyro_2 = gyro_2; // float:b_f[2] - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN; - msg.msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h index 0c5e7972e..22b139afc 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h @@ -103,37 +103,9 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u * @param pitch Pitch curve setpoints (every 25%) * @param throttle Throttle curve setpoints (every 25%) */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg.payload[0]; - - memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right - memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up - memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right - memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode - memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) - memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN; - msg.msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - mavlink_send_msg(chan, &msg); -} - -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h index 5546cdc5f..16e2ff685 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h @@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM * @param pilot Pilot mode, see UALBERTA_PILOT_MODE */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot) -{ - mavlink_message_t msg; - uint16_t checksum; - mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg.payload[0]; - - p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM - p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE - - msg.STX = MAVLINK_STX; - msg.len = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN; - msg.msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - msg.sysid = mavlink_system.sysid; - msg.compid = mavlink_system.compid; - msg.seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1; - checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN); - msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte - msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte - - mavlink_send_msg(chan, &msg); -} -#endif -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { mavlink_header_t hdr; diff --git a/thirdParty/mavlink/include/ualberta/ualberta.h b/thirdParty/mavlink/include/ualberta/ualberta.h index b957f6d73..c0c03d827 100644 --- a/thirdParty/mavlink/include/ualberta/ualberta.h +++ b/thirdParty/mavlink/include/ualberta/ualberta.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Sunday, July 31 2011, 15:12 UTC + * Generated on Tuesday, August 9 2011, 16:16 UTC */ #ifndef UALBERTA_H #define UALBERTA_H @@ -66,6 +66,13 @@ enum UALBERTA_PILOT_MODE #include "./mavlink_msg_nav_filter_bias.h" #include "./mavlink_msg_radio_calibration.h" #include "./mavlink_msg_ualberta_sys_status.h" + + +// MESSAGE CRC KEYS + +#undef MAVLINK_MESSAGE_KEYS +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 73, 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, 43, 141, 211, 166 } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/message_definitions/ardupilotmega.xml b/thirdParty/mavlink/message_definitions/ardupilotmega.xml index 39d96eab5..72bf88a7c 100644 --- a/thirdParty/mavlink/message_definitions/ardupilotmega.xml +++ b/thirdParty/mavlink/message_definitions/ardupilotmega.xml @@ -1,6 +1,6 @@ - common.xml - - + common.xml + + diff --git a/thirdParty/mavlink/message_definitions/common.xml b/thirdParty/mavlink/message_definitions/common.xml index de21876a8..3ca5f5275 100644 --- a/thirdParty/mavlink/message_definitions/common.xml +++ b/thirdParty/mavlink/message_definitions/common.xml @@ -701,7 +701,7 @@ Request to read the onboard parameter with the param_id string id. Onboard parameters are stored as key[const char*] -> value[float]. This allows to send a parameter to any other component (such as the GCS) without the need of previous knowledge of possible parameter names. Thus the same GCS can store different parameters for different autopilots. See also http://qgroundcontrol.org/parameter_interface for a full documentation of QGroundControl and IMU code. System ID Component ID - Onboard parameter id + Onboard parameter id Parameter index. Send -1 to use the param ID field as identifier @@ -711,7 +711,7 @@ Emit the value of a onboard parameter. The inclusion of param_count and param_index in the message allows the recipient to keep track of received parameters and allows him to re-request missing parameters after a loss or timeout. - Onboard parameter id + Onboard parameter id Onboard parameter value Total number of onboard parameters Index of this onboard parameter @@ -720,12 +720,13 @@ Set a parameter value TEMPORARILY to RAM. It will be reset to default on system reboot. Send the ACTION MAV_ACTION_STORAGE_WRITE to PERMANENTLY write the RAM contents to EEPROM. IMPORTANT: The receiving component should acknowledge the new parameter value by sending a param_value message to all communication partners. This will also ensure that multiple GCS all have an up-to-date list of all parameters. If the sending GCS did not receive a PARAM_VALUE message within its timeout time, it should re-send the PARAM_SET message. System ID Component ID - Onboard parameter id + Onboard parameter id Onboard parameter value The global position, as returned by the Global Positioning System (GPS). This is -NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. Latitude in 1E7 degrees @@ -818,7 +819,8 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se The global position, as returned by the Global Positioning System (GPS). This is -NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + NOT the global position estimate of the sytem, but rather a RAW sensor value. See message GLOBAL_POSITION for the global position estimate. Coordinate frame is right-handed, Z-axis up (GPS frame) + Timestamp (microseconds since UNIX epoch or microseconds since system boot) 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. Latitude in degrees @@ -876,7 +878,8 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se Message encoding a waypoint. This message is emitted to announce - the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed + the presence of a waypoint and to set a waypoint on the system. The waypoint can be either in x, y, z meters (type: LOCAL) or x:lat, y:lon, z:altitude. Local frame is Z-down, right handed, global frame is Z-up, right handed + System ID Component ID Sequence @@ -996,6 +999,22 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se y position 2 / Longitude 2 z position 2 / Altitude 2 + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angle in radians + Desired pitch angle in radians + Desired yaw angle in radians + + + Set roll, pitch and yaw. + System ID + Component ID + Desired roll angular speed in rad/s + Desired pitch angular speed in rad/s + Desired yaw angular speed in rad/s + The output of the attitude controller. This output is the control response the controller currently generates and the attitude the MAV would take if it is under control of the attitude controller. The primary use of this message is to check the response and signs of the controller before the actual flight. 1: enabled, 0: disabled @@ -1014,7 +1033,8 @@ NOT the global position estimate of the sytem, but rather a RAW sensor value. Se Outputs of the APM navigation controller. The primary use of this message is to check the response and signs -of the controller before actual flight and to assist with tuning controller parameters + of the controller before actual flight and to assist with tuning controller parameters + Current desired roll in degrees Current desired pitch in degrees Current desired heading in degrees @@ -1054,6 +1074,27 @@ of the controller before actual flight and to assist with tuning controller para The requested interval between two messages of this type 1 to start sending, 0 to stop sending. + + This packet is useful for high throughput + applications such as hardware in the loop. + + Timestamp (microseconds since UNIX epoch or microseconds since system boot) + Roll angle (rad) + Pitch angle (rad) + Yaw angle (rad) + Roll angular speed (rad/s) + Pitch angular speed (rad/s) + Yaw angular speed (rad/s) + Latitude, expressed as * 1E7 + Longitude, expressed as * 1E7 + Altitude in meters, expressed as * 1000 (millimeters) + Ground X Speed (Latitude), expressed as m/s * 100 + Ground Y Speed (Longitude), expressed as m/s * 100 + Ground Z Speed (Altitude), expressed as m/s * 100 + X acceleration (mg) + Y acceleration (mg) + Z acceleration (mg) + The system to be controlled roll @@ -1065,6 +1106,19 @@ of the controller before actual flight and to assist with tuning controller para yaw auto:0, manual:1 thrust auto:0, manual:1 + + The RAW values of the RC channels sent to the MAV to override info received from the RC radio. A value of -1 means no change to that channel. A value of 0 means control of that channel should be released back to the RC radio. The standard PPM modulation is as follows: 1000 microseconds: 0%, 2000 microseconds: 100%. Individual receivers/transmitters might violate this specification. + System ID + Component ID + RC channel 1 value, in microseconds + RC channel 2 value, in microseconds + RC channel 3 value, in microseconds + RC channel 4 value, in microseconds + RC channel 5 value, in microseconds + RC channel 6 value, in microseconds + RC channel 7 value, in microseconds + RC channel 8 value, in microseconds + The filtered global position (e.g. fused GPS and accelerometers). The position is in GPS-frame (right-handed, Z-up) Latitude, expressed as * 1E7 @@ -1120,7 +1174,7 @@ of the controller before actual flight and to assist with tuning controller para Status text message. These messages are printed in yellow in the COMM console of QGroundControl. WARNING: They consume quite some bandwidth, so use only for important status and error messages. If implemented wisely, these messages are buffered on the MCU and sent only at a limited rate (e.g. 10 Hz). Severity of status, 0 = info message, 255 = critical fault - Status text message, without null termination character + Status text message, without null termination character Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N. @@ -1128,4 +1182,5 @@ of the controller before actual flight and to assist with tuning controller para DEBUG value +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f diff --git a/thirdParty/mavlink/message_definitions/minimal.xml b/thirdParty/mavlink/message_definitions/minimal.xml index 16d26831e..5b41a4909 100644 --- a/thirdParty/mavlink/message_definitions/minimal.xml +++ b/thirdParty/mavlink/message_definitions/minimal.xml @@ -1,13 +1,13 @@ - 2 - - - - The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). - Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - MAVLink version - - + 2 + + + + The heartbeat message shows that a system is present and responding. The type of the MAV and Autopilot hardware allow the receiving system to treat further messages from this system appropriate (e.g. by laying out the user interface based on the autopilot). + Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + MAVLink version + + diff --git a/thirdParty/mavlink/message_definitions/pixhawk.xml b/thirdParty/mavlink/message_definitions/pixhawk.xml index 46f18b154..3878726aa 100644 --- a/thirdParty/mavlink/message_definitions/pixhawk.xml +++ b/thirdParty/mavlink/message_definitions/pixhawk.xml @@ -1,262 +1,235 @@ - + -common.xml - - - - Content Types for data transmission handshake - - - - - - - - - - - - The system to be controlled - roll - pitch - yaw - thrust - roll control enabled auto:0, manual:1 - pitch auto:0, manual:1 - yaw auto:0, manual:1 - thrust auto:0, manual:1 - - - - Camera id - Camera mode: 0 = auto, 1 = manual - Trigger pin, 0-3 for PtGrey FireFly - Shutter interval, in microseconds - Exposure time, in microseconds - Camera gain - - - - Timestamp - IMU seq - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - Local frame Z coordinate (height over ground) - GPS X coordinate - GPS Y coordinate - Global frame altitude - Ground truth X - Ground truth Y - Ground truth Z - - - - 0 to disable, 1 to enable - - - - Camera id - Camera # (starts with 0) - Timestamp - Until which timestamp this buffer will stay valid - The image sequence number - Position of the image in the buffer, starts with 0 - Image width - Image height - Image depth - Image channels - Shared memory area key - Exposure time, in microseconds - Camera gain - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - Local frame Z coordinate (height over ground) - GPS X coordinate - GPS Y coordinate - Global frame altitude - Ground truth X - Ground truth Y - Ground truth Z - - - - Timestamp (milliseconds) - Global X position - Global Y position - Global Z position - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - - - - Timestamp (milliseconds) - Global X position - Global Y position - Global Z position - Roll angle in rad - Pitch angle in rad - Yaw angle in rad - - - - Timestamp (milliseconds) - Global X speed - Global Y speed - Global Z speed - - - - Message sent to the MAV to set a new position as reference for the controller - System ID - Component ID - ID of waypoint, 0 for plain position - x position - y position - z position - yaw orientation in radians, 0 = NORTH - - - - Message sent to the MAV to set a new offset from the currently controlled position - System ID - Component ID - x position offset - y position offset - z position offset - yaw orientation offset in radians, 0 = NORTH - - - - - ID of waypoint, 0 for plain position - x position - y position - z position - yaw orientation in radians, 0 = NORTH - - - - ID - x position - y position - z position - roll orientation - pitch orientation - yaw orientation - - - - ADC1 (J405 ADC3, LPC2148 AD0.6) - ADC2 (J405 ADC5, LPC2148 AD0.2) - ADC3 (J405 ADC6, LPC2148 AD0.1) - ADC4 (J405 ADC7, LPC2148 AD1.3) - Battery voltage - Temperature (degrees celcius) - Barometric pressure (hecto Pascal) - - - - Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - Number of I2C errors since startup -Number of I2C errors since startup - Number of I2C errors since startup -Number of I2C errors since startup - Number of I2C errors since startup - - - - Watchdog ID - Number of processes - - - - Watchdog ID - Process ID - Process name - Process arguments - Timeout (seconds) - - - - Watchdog ID - Process ID - Is running / finished / suspended / crashed - Is muted - PID - Number of crashes - - - - Target system ID - Watchdog ID - Process ID - Command ID - - - - 0: Pattern, 1: Letter - Confidence of detection - Pattern file name - Accepted as true detection, 0 no, 1 yes - - - - Notifies the operator about a point of interest (POI). This can be anything detected by the - system. This generic message is intented to help interfacing to generic visualizations and to display - the POI on a map. - 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - 0: global, 1:local - 0: no timeout, >1: timeout in seconds - X Position - Y Position - Z Position - POI name - - - - Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the - system. This generic message is intented to help interfacing to generic visualizations and to display - the POI on a map. - 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - 0: global, 1:local - 0: no timeout, >1: timeout in seconds - X1 Position - Y1 Position - Z1 Position - X2 Position - Y2 Position - Z2 Position - POI connection name - - - - type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - total data size in bytes (set on ACK only) - number of packets beeing sent (set on ACK only) - payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - JPEG quality out of [1,100] - - - - sequence number (starting with 0 on every transmission) - image data bytes - - - - x position in m - y position in m - z position in m - Orientation assignment 0: false, 1:true - Size in pixels - Orientation - Descriptor - Harris operator response at this location - - - + common.xml + + + Content Types for data transmission handshake + + + + + + + + The system to be controlled + roll + pitch + yaw + thrust + roll control enabled auto:0, manual:1 + pitch auto:0, manual:1 + yaw auto:0, manual:1 + thrust auto:0, manual:1 + + + Camera id + Camera mode: 0 = auto, 1 = manual + Trigger pin, 0-3 for PtGrey FireFly + Shutter interval, in microseconds + Exposure time, in microseconds + Camera gain + + + Timestamp + IMU seq + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + Local frame Z coordinate (height over ground) + GPS X coordinate + GPS Y coordinate + Global frame altitude + Ground truth X + Ground truth Y + Ground truth Z + + + 0 to disable, 1 to enable + + + Camera id + Camera # (starts with 0) + Timestamp + Until which timestamp this buffer will stay valid + The image sequence number + Position of the image in the buffer, starts with 0 + Image width + Image height + Image depth + Image channels + Shared memory area key + Exposure time, in microseconds + Camera gain + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + Local frame Z coordinate (height over ground) + GPS X coordinate + GPS Y coordinate + Global frame altitude + Ground truth X + Ground truth Y + Ground truth Z + + + Timestamp (milliseconds) + Global X position + Global Y position + Global Z position + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + + + Timestamp (milliseconds) + Global X position + Global Y position + Global Z position + Roll angle in rad + Pitch angle in rad + Yaw angle in rad + + + Timestamp (milliseconds) + Global X speed + Global Y speed + Global Z speed + + + Message sent to the MAV to set a new position as reference for the controller + System ID + Component ID + ID of waypoint, 0 for plain position + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + Message sent to the MAV to set a new offset from the currently controlled position + System ID + Component ID + x position offset + y position offset + z position offset + yaw orientation offset in radians, 0 = NORTH + + + + ID of waypoint, 0 for plain position + x position + y position + z position + yaw orientation in radians, 0 = NORTH + + + ID + x position + y position + z position + roll orientation + pitch orientation + yaw orientation + + + ADC1 (J405 ADC3, LPC2148 AD0.6) + ADC2 (J405 ADC5, LPC2148 AD0.2) + ADC3 (J405 ADC6, LPC2148 AD0.1) + ADC4 (J405 ADC7, LPC2148 AD1.3) + Battery voltage + Temperature (degrees celcius) + Barometric pressure (hecto Pascal) + + + Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + Number of I2C errors since startup + Number of I2C errors since startup + Number of I2C errors since startup + Number of I2C errors since startup + Number of I2C errors since startup + + + Watchdog ID + Number of processes + + + Watchdog ID + Process ID + Process name + Process arguments + Timeout (seconds) + + + Watchdog ID + Process ID + Is running / finished / suspended / crashed + Is muted + PID + Number of crashes + + + Target system ID + Watchdog ID + Process ID + Command ID + + + 0: Pattern, 1: Letter + Confidence of detection + Pattern file name + Accepted as true detection, 0 no, 1 yes + + + Notifies the operator about a point of interest (POI). This can be anything detected by the + system. This generic message is intented to help interfacing to generic visualizations and to display + the POI on a map. + + 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + 0: global, 1:local + 0: no timeout, >1: timeout in seconds + X Position + Y Position + Z Position + POI name + + + Notifies the operator about the connection of two point of interests (POI). This can be anything detected by the + system. This generic message is intented to help interfacing to generic visualizations and to display + the POI on a map. + + 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + 0: global, 1:local + 0: no timeout, >1: timeout in seconds + X1 Position + Y1 Position + Z1 Position + X2 Position + Y2 Position + Z2 Position + POI connection name + + + type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + total data size in bytes (set on ACK only) + number of packets beeing sent (set on ACK only) + payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + JPEG quality out of [1,100] + + + sequence number (starting with 0 on every transmission) + image data bytes + + + x position in m + y position in m + z position in m + Orientation assignment 0: false, 1:true + Size in pixels + Orientation + Descriptor + Harris operator response at this location + + diff --git a/thirdParty/mavlink/message_definitions/slugs.xml b/thirdParty/mavlink/message_definitions/slugs.xml index 6de0697fe..f8644c5c4 100644 --- a/thirdParty/mavlink/message_definitions/slugs.xml +++ b/thirdParty/mavlink/message_definitions/slugs.xml @@ -1,8 +1,8 @@ -common.xml - - - - - - - Sensor and DSC control loads. - Sensor DSC Load - Control DSC Load - Battery Voltage in millivolts - - - - Air data for altitude and airspeed computation. - Dynamic pressure (Pa) - Static pressure (Pa) - Board temperature - - - - Accelerometer and gyro biases. - Accelerometer X bias (m/s) - Accelerometer Y bias (m/s) - Accelerometer Z bias (m/s) - Gyro X bias (rad/s) - Gyro Y bias (rad/s) - Gyro Z bias (rad/s) - - - - Configurable diagnostic messages. - Diagnostic float 1 - Diagnostic float 2 - Diagnostic float 3 - Diagnostic short 1 - Diagnostic short 2 - Diagnostic short 3 - - - - Data used in the navigation algorithm. - Measured Airspeed prior to the Nav Filter - Commanded Roll - Commanded Pitch - Commanded Turn rate - Y component of the body acceleration - Total Distance to Run on this leg of Navigation - Remaining distance to Run on this leg of Navigation - Origin WP - Destination WP - - - - Configurable data log probes to be used inside Simulink - Log value 1 - Log value 2 - Log value 3 - Log value 4 - Log value 5 - Log value 6 - - - - Pilot console PWM messges. - Year reported by Gps - Month reported by Gps - Day reported by Gps - Hour reported by Gps - Min reported by Gps - Sec reported by Gps - Visible sattelites reported by Gps - - - Mid Level commands sent from the GS to the autopilot. These are only sent when being opperated in mid-level commands mode from the ground; for periodic report of these commands generated from the autopilot see message XXXX. - The system setting the commands - Commanded Airspeed - Log value 2 - Log value 3 - - - -This message configures the Selective Passthrough mode. it allows to select which control surfaces the Pilot can control from his console. It is implemented as a bitfield as follows: - Position Bit Code - ================================= - 15-8 Reserved - 7 dt_pass 128 - 6 dla_pass 64 - 5 dra_pass 32 - 4 dr_pass 16 - 3 dle_pass 8 - 2 dre_pass 4 - 1 dlf_pass 2 - 0 drf_pass 1 - Where Bit 15 is the MSb. 0 = AP has control of the surface; 1 = Pilot Console has control of the surface. - The system setting the commands - Bitfield containing the PT configuration - - - - - Action messages focused on the SLUGS AP. - The system reporting the action - Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - Value associated with the action - - - - \ No newline at end of file + + + + Sensor and DSC control loads. + Sensor DSC Load + Control DSC Load + Battery Voltage in millivolts + + + + Air data for altitude and airspeed computation. + Dynamic pressure (Pa) + Static pressure (Pa) + Board temperature + + + + Accelerometer and gyro biases. + Accelerometer X bias (m/s) + Accelerometer Y bias (m/s) + Accelerometer Z bias (m/s) + Gyro X bias (rad/s) + Gyro Y bias (rad/s) + Gyro Z bias (rad/s) + + + + Configurable diagnostic messages. + Diagnostic float 1 + Diagnostic float 2 + Diagnostic float 3 + Diagnostic short 1 + Diagnostic short 2 + Diagnostic short 3 + + + + Data used in the navigation algorithm. + Measured Airspeed prior to the Nav Filter + Commanded Roll + Commanded Pitch + Commanded Turn rate + Y component of the body acceleration + Total Distance to Run on this leg of Navigation + Remaining distance to Run on this leg of Navigation + Origin WP + Destination WP + + + + Configurable data log probes to be used inside Simulink + Log value 1 + Log value 2 + Log value 3 + Log value 4 + Log value 5 + Log value 6 + + + + Pilot console PWM messges. + Year reported by Gps + Month reported by Gps + Day reported by Gps + Hour reported by Gps + Min reported by Gps + Sec reported by Gps + Visible sattelites reported by Gps + + + + Mid Level commands sent from the GS to the autopilot. These are only sent when being opperated in mid-level commands mode from the ground; for periodic report of these commands generated from the autopilot see message XXXX. + The system setting the commands + Commanded Airspeed + Log value 2 + Log value 3 + + + + + This message configures the Selective Passthrough mode. it allows to select which control surfaces the Pilot can control from his console. It is implemented as a bitfield as follows: + Position Bit Code + ================================= + 15-8 Reserved + 7 dt_pass 128 + 6 dla_pass 64 + 5 dra_pass 32 + 4 dr_pass 16 + 3 dle_pass 8 + 2 dre_pass 4 + 1 dlf_pass 2 + 0 drf_pass 1 + Where Bit 15 is the MSb. 0 = AP has control of the surface; 1 = Pilot Console has control of the surface. + The system setting the commands + Bitfield containing the PT configuration + + + + + + Action messages focused on the SLUGS AP. + The system reporting the action + Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + Value associated with the action + + + + diff --git a/thirdParty/mavlink/message_definitions/ualberta.xml b/thirdParty/mavlink/message_definitions/ualberta.xml index eaa9d9984..5e53e141e 100644 --- a/thirdParty/mavlink/message_definitions/ualberta.xml +++ b/thirdParty/mavlink/message_definitions/ualberta.xml @@ -1,54 +1,54 @@ - common.xml - - - Available autopilot modes for ualberta uav - Raw input pulse widts sent to output - Inputs are normalized using calibration, the converted back to raw pulse widths for output - dfsdfs - dfsfds - dfsdfsdfs - - - Navigation filter mode - - AHRS mode - INS/GPS initialization mode - INS/GPS mode - - - Mode currently commanded by pilot - sdf - dfs - Rotomotion mode - - - - - Accelerometer and Gyro biases from the navigation filter - Timestamp (microseconds) - b_f[0] - b_f[1] - b_f[2] - b_f[0] - b_f[1] - b_f[2] - - - Complete set of calibration parameters for the radio - Aileron setpoints: left, center, right - Elevator setpoints: nose down, center, nose up - Rudder setpoints: nose left, center, nose right - Tail gyro mode/gain setpoints: heading hold, rate mode - Pitch curve setpoints (every 25%) - Throttle curve setpoints (every 25%) - - - System status specific to ualberta uav - System mode, see UALBERTA_AUTOPILOT_MODE ENUM - Navigation mode, see UALBERTA_NAV_MODE ENUM - Pilot mode, see UALBERTA_PILOT_MODE - - + common.xml + + + Available autopilot modes for ualberta uav + Raw input pulse widts sent to output + Inputs are normalized using calibration, the converted back to raw pulse widths for output + dfsdfs + dfsfds + dfsdfsdfs + + + Navigation filter mode + + AHRS mode + INS/GPS initialization mode + INS/GPS mode + + + Mode currently commanded by pilot + sdf + dfs + Rotomotion mode + + + + + Accelerometer and Gyro biases from the navigation filter + Timestamp (microseconds) + b_f[0] + b_f[1] + b_f[2] + b_f[0] + b_f[1] + b_f[2] + + + Complete set of calibration parameters for the radio + Aileron setpoints: left, center, right + Elevator setpoints: nose down, center, nose up + Rudder setpoints: nose left, center, nose right + Tail gyro mode/gain setpoints: heading hold, rate mode + Pitch curve setpoints (every 25%) + Throttle curve setpoints (every 25%) + + + System status specific to ualberta uav + System mode, see UALBERTA_AUTOPILOT_MODE ENUM + Navigation mode, see UALBERTA_NAV_MODE ENUM + Pilot mode, see UALBERTA_PILOT_MODE + + diff --git a/thirdParty/mavlink/missionlib/testing/.gitignore b/thirdParty/mavlink/missionlib/testing/.gitignore index 4b4c3f6c4..0776965a6 100644 --- a/thirdParty/mavlink/missionlib/testing/.gitignore +++ b/thirdParty/mavlink/missionlib/testing/.gitignore @@ -1,2 +1,3 @@ *.o udptest +build diff --git a/thirdParty/mavlink/missionlib/testing/main.c b/thirdParty/mavlink/missionlib/testing/main.c new file mode 100644 index 000000000..60b653a79 --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/main.c @@ -0,0 +1,1221 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + and Bryan Godbolt godbolt ( a t ) ualberta.ca + + adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ +/* + This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets + cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from + qgroundcontrol are printed by this program along with the heartbeats. + + + I compiled this program sucessfully on Ubuntu 10.04 with the following command + + gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c + + the rt library is needed for the clock_gettime on linux + */ +/* These headers are for QNX, but should all be standard on unix/linux */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if (defined __QNX__) | (defined __QNXNTO__) +/* QNX specific headers */ +#include +#else +/* Linux / MacOS POSIX timer headers */ +#include +#include +#include +#endif + +#include +<<<<<<< HEAD +#include +======= +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f + +#include <../mavlink_types.h> + +mavlink_system_t mavlink_system; + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ +#include + + +<<<<<<< HEAD +======= +/* Provide the interface functions for the waypoint manager */ + +/* + * @brief Sends a MAVLink message over UDP + */ +void mavlink_wpm_send_message(mavlink_message_t* msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + + printf("SENT %d bytes", bytes_sent); +} + + +#include + + +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f +#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) + +char help[] = "--help"; + + +char target_ip[100]; + +float position[6] = {}; +int sock; +struct sockaddr_in gcAddr; +struct sockaddr_in locAddr; +uint8_t buf[BUFFER_LENGTH]; +ssize_t recsize; +socklen_t fromlen; +int bytes_sent; +mavlink_message_t msg; +uint16_t len; +int i = 0; +unsigned int temp = 0; + +uint64_t microsSinceEpoch(); + + +<<<<<<< HEAD +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES +{ + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES +{ + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 100 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text +#define MAVLINK_WPM_SYSTEM_ID 1 +#define MAVLINK_WPM_COMPONENT_ID 1 +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000 +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40 + + +struct mavlink_wpm_storage { + mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + uint16_t current_wp_id; ///< Waypoint in current transmission + uint16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; +}; + +typedef struct mavlink_wpm_storage mavlink_wpm_storage; + +mavlink_wpm_storage wpm; + +bool debug = true; +bool verbose = true; + + +void mavlink_wpm_init(mavlink_wpm_storage* state) +{ + // Set all waypoints to zero + + // Set count to zero + state->size = 0; + state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->current_state = MAVLINK_WPM_STATE_IDLE; + state->current_partner_sysid = 0; + state->current_partner_compid = 0; + state->timestamp_lastaction = 0; + state->timestamp_last_send_setpoint = 0; + state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; + state->idle = false; ///< indicates if the system is following the waypoints or is waiting + state->current_active_wp_id = -1; ///< id of current waypoint + state->yaw_reached = false; ///< boolean for yaw attitude reached + state->pos_reached = false; ///< boolean for position reached + state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value + state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + +} + +/* + * @brief Sends a MAVLink message over UDP + */ +void mavlink_wpm_send_message(mavlink_message_t* msg) +{ + uint16_t len = mavlink_msg_to_send_buffer(buf, msg); + uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + + printf("SENT %d bytes", bytes_sent); +} + +void mavlink_wpm_send_gcs_string(const char* string) +{ + printf("%s",string); +} + +uint64_t mavlink_wpm_get_system_timestamp() +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; +} + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_waypoint_ack_t wpa; + + wpa.target_system = wpm.current_partner_sysid; + wpa.target_component = wpm.current_partner_compid; + wpa.type = type; + + mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) + { + //printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); + mavlink_wpm_send_gcs_string("Sent waypoint ACK"); + } +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if(seq < wpm.size) + { + mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); + + mavlink_message_t msg; + mavlink_waypoint_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //printf("Broadcasted new current waypoint %u\n", wpc.seq); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: index out of bounds\n"); + } +} + +/* + * @brief Directs the MAV to fly to a position + * + * Sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_setpoint(uint16_t seq) +{ + if(seq < wpm.size) + { + mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); + + mavlink_message_t msg; + mavlink_local_position_setpoint_set_t position_control_set_point; + + // Send new NED or ENU setpoint to onbaord autopilot + if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU) + { + position_control_set_point.target_system = mavlink_system.sysid; + position_control_set_point.target_component = MAV_COMP_ID_IMU; + position_control_set_point.x = cur->x; + position_control_set_point.y = cur->y; + position_control_set_point.z = cur->z; + position_control_set_point.yaw = cur->param4; + + mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + } + + wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp(); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_waypoint_count_t wpc; + + wpc.target_system = wpm.current_partner_sysid; + wpc.target_component = wpm.current_partner_compid; + wpc.count = count; + + mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_wpm_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm.size) + { + mavlink_message_t msg; + mavlink_waypoint_t *wp = &(wpm.waypoints[seq]); + wp->target_system = wpm.current_partner_sysid; + wp->target_component = wpm.current_partner_compid; + mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); + mavlink_wpm_send_message(&msg); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm.max_size) + { + mavlink_message_t msg; + mavlink_waypoint_request_t wpr; + wpr.target_system = wpm.current_partner_sysid; + wpr.target_component = wpm.current_partner_compid; + wpr.seq = seq; + mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); + mavlink_wpm_send_message(&msg); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_waypoint_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); + mavlink_wpm_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +//{ +// if (seq < wpm.size) +// { +// mavlink_waypoint_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// // seq not the second last waypoint +// if ((uint16_t)(seq+1) < wpm.size) +// { +// mavlink_waypoint_t *next = waypoints->at(seq+1); +// const PxVector3 B(next->x, next->y, next->z); +// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); +// if (r >= 0 && r <= 1) +// { +// const PxVector3 P(A + r*(B-A)); +// return (P-C).length(); +// } +// else if (r < 0.f) +// { +// return (C-A).length(); +// } +// else +// { +// return (C-B).length(); +// } +// } +// else +// { +// return (C-A).length(); +// } +// } +// else +// { +// if (verbose) printf("ERROR: index out of bounds\n"); +// } +// return -1.f; +//} + +float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) +{ +// if (seq < wpm.size) +// { +// mavlink_waypoint_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// return (C-A).length(); +// } +// else +// { +// if (verbose) printf("ERROR: index out of bounds\n"); +// } + return -1.f; +} + + +static void mavlink_wpm_message_handler(const mavlink_message_t* msg) +{ + // Handle param messages + //paramClient->handleMAVLinkPacket(msg); + + //check for timed-out operations + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_count = 0; + wpm.current_partner_sysid = 0; + wpm.current_partner_compid = 0; + wpm.current_wp_id = -1; + + if(wpm.size == 0) + { + wpm.current_active_wp_id = -1; + } + } + + if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size) + { + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + } + + switch(msg->msgid) + { + case MAVLINK_MSG_ID_ATTITUDE: + { + if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); + if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) + { + mavlink_attitude_t att; + mavlink_msg_attitude_decode(msg, &att); + float yaw_tolerance = wpm.accept_range_yaw; + //compare current yaw + if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) + { + if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) + wpm.yaw_reached = true; + } + else if(att.yaw - yaw_tolerance < 0.0f) + { + float lowerBound = 360.0f + att.yaw - yaw_tolerance; + if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) + wpm.yaw_reached = true; + } + else + { + float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; + if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) + wpm.yaw_reached = true; + } + } + } + break; + } + + case MAVLINK_MSG_ID_LOCAL_POSITION: + { + if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); + + if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) + { + mavlink_local_position_t pos; + mavlink_msg_local_position_decode(msg, &pos); + //if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); + + wpm.pos_reached = false; + + // compare current position (given in message) with current waypoint + float orbit = wp->param1; + + float dist; + if (wp->param2 == 0) + { + // FIXME segment distance + //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); + } + else + { + dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z); + } + + if (dist >= 0.f && dist <= orbit && wpm.yaw_reached) + { + wpm.pos_reached = true; + } + } + } + break; + } + +// case MAVLINK_MSG_ID_CMD: // special action from ground station +// { +// mavlink_cmd_t action; +// mavlink_msg_cmd_decode(msg, &action); +// if(action.target == mavlink_system.sysid) +// { +// if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; +// switch (action.action) +// { +// // case MAV_ACTION_LAUNCH: +// // if (verbose) std::cerr << "Launch received" << std::endl; +// // current_active_wp_id = 0; +// // if (wpm.size>0) +// // { +// // setActive(waypoints[current_active_wp_id]); +// // } +// // else +// // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; +// // break; +// +// // case MAV_ACTION_CONTINUE: +// // if (verbose) std::c +// // err << "Continue received" << std::endl; +// // idle = false; +// // setActive(waypoints[current_active_wp_id]); +// // break; +// +// // case MAV_ACTION_HALT: +// // if (verbose) std::cerr << "Halt received" << std::endl; +// // idle = true; +// // break; +// +// // default: +// // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; +// // break; +// } +// } +// break; +// } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + mavlink_waypoint_ack_t wpa; + mavlink_msg_waypoint_ack_decode(msg, &wpa); + + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid && wpa.target_component == mavlink_system.compid)) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpm.current_wp_id == wpm.size-1) + { + if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_wp_id = 0; + } + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + { + mavlink_waypoint_set_current_t wpc; + mavlink_msg_waypoint_set_current_decode(msg, &wpc); + + if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + if (wpc.seq < wpm.size) + { + if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + wpm.current_active_wp_id = wpc.seq; + uint32_t i; + for(i = 0; i < wpm.size; i++) + { + if (i == wpm.current_active_wp_id) + { + wpm.waypoints[i].current = true; + } + else + { + wpm.waypoints[i].current = false; + } + } + if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id); + wpm.yaw_reached = false; + wpm.pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.timestamp_firstinside_orbit = 0; + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + { + mavlink_waypoint_request_list_t wprl; + mavlink_msg_waypoint_request_list_decode(msg, &wprl); + if(wprl.target_system == mavlink_system.sysid && wprl.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpm.size > 0) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; + wpm.current_wp_id = 0; + wpm.current_partner_sysid = msg->sysid; + wpm.current_partner_compid = msg->compid; + } + else + { + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + } + wpm.current_count = wpm.size; + mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + } + + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + mavlink_waypoint_request_t wpr; + mavlink_msg_waypoint_request_decode(msg, &wpr); + if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + wpm.current_wp_id = wpr.seq; + mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq); + } + else + { + if (verbose) + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); + else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + mavlink_waypoint_count_t wpc; + mavlink_msg_waypoint_count_decode(msg, &wpc); + if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0)) + { + if (wpc.count > 0) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST; + wpm.current_wp_id = 0; + wpm.current_partner_sysid = msg->sysid; + wpm.current_partner_compid = msg->compid; + wpm.current_count = wpc.count; + + printf("clearing receive buffer and readying for receiving waypoints\n"); + wpm.rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints_receive_buffer->back(); +// waypoints_receive_buffer->pop_back(); +// } + + mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); + } + else if (wpc.count == 0) + { + printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); + wpm.rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints->back(); +// waypoints->pop_back(); +// } + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + break; + + } + else + { + if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + } + } + else + { + if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); + else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + + } + break; + + case MAVLINK_MSG_ID_WAYPOINT: + { + mavlink_waypoint_t wp; + mavlink_msg_waypoint_decode(msg, &wp); + + if (verbose) printf("GOT WAYPOINT!"); + + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid)) + { + wpm.timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids + if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); + memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); + + wpm.current_wp_id = wp.seq + 1; + + if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + + if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) + { + if (verbose) printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); + + mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); + + if (wpm.current_active_wp_id > wpm.rcv_size-1) + { + wpm.current_active_wp_id = wpm.rcv_size-1; + } + + // switch the waypoints list + // FIXME CHECK!!! + for (int i = 0; i < wpm.current_count; ++i) + { + wpm.waypoints[i] = wpm.rcv_waypoints[i]; + } + wpm.size = wpm.current_count; + + //get the new current waypoint + uint32_t i; + for(i = 0; i < wpm.size; i++) + { + if (wpm.waypoints[i].current == 1) + { + wpm.current_active_wp_id = i; + //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + wpm.yaw_reached = false; + wpm.pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == wpm.size) + { + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + wpm.timestamp_firstinside_orbit = 0; + } + + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + } + else + { + mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); + } + } + else + { + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + //we're done receiving waypoints, answer with ack. + mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); + printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + } + if (verbose) + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) + { + if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) + { + if (!(wp.seq == wpm.current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); + else if (!(wp.seq < wpm.current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); + } + else if(wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + { + mavlink_waypoint_clear_all_t wpca; + mavlink_msg_waypoint_clear_all_decode(msg, &wpca); + + if(wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + wpm.timestamp_lastaction = now; + + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + // Delete all waypoints + wpm.size = 0; + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + } + else if (wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); + } + break; + } + + default: + { + if (debug) printf("Waypoint: received message of unknown type"); + break; + } + } + + //check if the current waypoint was reached + if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle) + { + if (wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]); + + if (wpm.timestamp_firstinside_orbit == 0) + { + // Announce that last waypoint was reached + if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + mavlink_wpm_send_waypoint_reached(cur_wp->seq); + wpm.timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000) + { + if (cur_wp->autocontinue) + { + cur_wp->current = 0; + if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1) + { + //the last waypoint was reached, if auto continue is + //activated restart the waypoint list from the beginning + wpm.current_active_wp_id = 1; + } + else + { + if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size) + wpm.current_active_wp_id++; + } + + // Fly to next waypoint + wpm.timestamp_firstinside_orbit = 0; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.waypoints[wpm.current_active_wp_id].current = true; + wpm.pos_reached = false; + wpm.yaw_reached = false; + if (verbose) printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); + } + } + } + } + else + { + wpm.timestamp_lastoutside_orbit = now; + } +} + + + + + + + + +======= +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f +int main(int argc, char* argv[]) +{ + // Initialize MAVLink + mavlink_wpm_init(&wpm); + mavlink_system.sysid = 1; + mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER; + + + + // Create socket + sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); + + // Check if --help flag was used + if ((argc == 2) && (strcmp(argv[1], help) == 0)) + { + printf("\n"); + printf("\tUsage:\n\n"); + printf("\t"); + printf("%s", argv[0]); + printf(" \n"); + printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); + exit(EXIT_FAILURE); + } + + + // Change the target ip if parameter was given + strcpy(target_ip, "127.0.0.1"); + if (argc == 2) + { + strcpy(target_ip, argv[1]); + } + + + memset(&locAddr, 0, sizeof(locAddr)); + locAddr.sin_family = AF_INET; + locAddr.sin_addr.s_addr = INADDR_ANY; + locAddr.sin_port = htons(14551); + + /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ + if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) + { + perror("error bind failed"); + close(sock); + exit(EXIT_FAILURE); + } + + /* Attempt to make it non blocking */ + if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) + { + fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); + close(sock); + exit(EXIT_FAILURE); + } + + + memset(&gcAddr, 0, sizeof(gcAddr)); + gcAddr.sin_family = AF_INET; + gcAddr.sin_addr.s_addr = inet_addr(target_ip); + gcAddr.sin_port = htons(14550); + + + printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); + + + for (;;) + { + + /*Send Heartbeat */ + mavlink_msg_heartbeat_pack(mavlink_system.sysid, 200, &msg, MAV_TYPE_HELICOPTER, MAV_CLASS_GENERIC); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send Status */ + mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); + +<<<<<<< HEAD +// /* Send Local Position */ +// mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), +// position[0], position[1], position[2], +// position[3], position[4], position[5]); +// len = mavlink_msg_to_send_buffer(buf, &msg); +// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); +// +// /* Send attitude */ +// mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); +// len = mavlink_msg_to_send_buffer(buf, &msg); +// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); +// +======= + /* Send Local Position */ + mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), + position[0], position[1], position[2], + position[3], position[4], position[5]); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + + /* Send attitude */ + mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); + len = mavlink_msg_to_send_buffer(buf, &msg); + bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); + +>>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f + + memset(buf, 0, BUFFER_LENGTH); + recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); + if (recsize > 0) + { + // Something received - print out all bytes and parse packet + mavlink_message_t msg; + mavlink_status_t status; + + printf("Bytes Received: %d\nDatagram: ", (int)recsize); + for (i = 0; i < recsize; ++i) + { + temp = buf[i]; + printf("%02x ", (unsigned char)temp); + if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) + { + // Packet received + printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); + + // Handle packet with waypoint component + mavlink_wpm_message_handler(&msg); + + // Handle packet with parameter component + } + } + printf("\n"); + } + memset(buf, 0, BUFFER_LENGTH); + sleep(1); // Sleep one second + } +} + + +/* QNX timer version */ +#if (defined __QNX__) | (defined __QNXNTO__) +uint64_t microsSinceEpoch() +{ + + struct timespec time; + + uint64_t micros = 0; + + clock_gettime(CLOCK_REALTIME, &time); + micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000; + + return micros; +} +#else +uint64_t microsSinceEpoch() +{ + + struct timeval tv; + + uint64_t micros = 0; + + gettimeofday(&tv, NULL); + micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + + return micros; +} +#endif \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/testing/udp.c b/thirdParty/mavlink/missionlib/testing/udp.c index 37a8a7848..dc096be51 100644 --- a/thirdParty/mavlink/missionlib/testing/udp.c +++ b/thirdParty/mavlink/missionlib/testing/udp.c @@ -1,5 +1,9 @@ /******************************************************************************* - Copyright (C) 2010 Bryan Godbolt godbolt ( a t ) ualberta.ca + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + and Bryan Godbolt godbolt ( a t ) ualberta.ca + + adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -23,7 +27,7 @@ I compiled this program sucessfully on Ubuntu 10.04 with the following command - gcc -I ../../pixhawk/mavlink/include -o udp-server udp-server-test.c + gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c the rt library is needed for the clock_gettime on linux */ @@ -57,6 +61,860 @@ uint64_t microsSinceEpoch(); + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES +{ + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES +{ + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 100 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#define MAVLINK_WPM_TEXT_FEEDBACK ///< Report back status information as text +#define MAVLINK_WPM_SYSTEM_ID 1 +#define MAVLINK_WPM_COMPONENT_ID 1 + +struct _mavlink_wpm_storage { + mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t count; + MAVLINK_WPM_STATES current_state; +} mavlink_wpm_storage; + + +void mavlink_wpm_init(mavlink_wpm_storage* state) +{ + // Set all waypoints to zero + + // Set count to zero + state->count = 0; + state->current_state = MAVLINK_WPM_STATE_IDLE; +} + + +PX_WAYPOINTPLANNER_STATES current_state = PX_WPP_IDLE; +uint16_t protocol_current_wp_id = 0; +uint16_t protocol_current_count = 0; +uint8_t protocol_current_partner_systemid = 0; +uint8_t protocol_current_partner_compid = 0; +uint64_t protocol_timestamp_lastaction = 0; + +uint64_t timestamp_last_send_setpoint = 0; + + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_waypoint_ack_t wpa; + + wpa.target_system = target_systemid; + wpa.target_component = target_compid; + wpa.type = type; + + mavlink_msg_waypoint_ack_encode(systemid, compid, &msg, &wpa); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if(seq < waypoints->size()) + { + mavlink_waypoint_t *cur = waypoints->at(seq); + + mavlink_message_t msg; + mavlink_waypoint_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_waypoint_current_encode(systemid, compid, &msg, &wpc); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq); + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +/* + * @brief Directs the MAV to fly to a position + * + * Sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_setpoint(uint16_t seq) +{ + if(seq < waypoints->size()) + { + mavlink_waypoint_t *cur = waypoints->at(seq); + + mavlink_message_t msg; + mavlink_local_position_setpoint_set_t PControlSetPoint; + + // send new set point to local IMU + if (cur->frame == 1) + { + PControlSetPoint.target_system = systemid; + PControlSetPoint.target_component = MAV_COMP_ID_IMU; + PControlSetPoint.x = cur->x; + PControlSetPoint.y = cur->y; + PControlSetPoint.z = cur->z; + PControlSetPoint.yaw = cur->param4; + + mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + } + + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + timestamp_last_send_setpoint = now; + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_waypoint_count_t wpc; + + wpc.target_system = target_systemid; + wpc.target_component = target_compid; + wpc.count = count; + + mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) +{ + if (seq < waypoints->size()) + { + mavlink_message_t msg; + mavlink_waypoint_t *wp = waypoints->at(seq); + wp->target_system = target_systemid; + wp->target_component = target_compid; + mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) +{ + if (seq < waypoints->size()) + { + mavlink_message_t msg; + mavlink_waypoint_request_t wpr; + wpr.target_system = target_systemid; + wpr.target_component = target_compid; + wpr.seq = seq; + mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_waypoint_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached); + mavlink_message_t_publish(lcm, "MAVLINK", &msg); + + if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); + + usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +{ + if (seq < waypoints->size()) + { + mavlink_waypoint_t *cur = waypoints->at(seq); + + const PxVector3 A(cur->x, cur->y, cur->z); + const PxVector3 C(x, y, z); + + // seq not the second last waypoint + if ((uint16_t)(seq+1) < waypoints->size()) + { + mavlink_waypoint_t *next = waypoints->at(seq+1); + const PxVector3 B(next->x, next->y, next->z); + const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); + if (r >= 0 && r <= 1) + { + const PxVector3 P(A + r*(B-A)); + return (P-C).length(); + } + else if (r < 0.f) + { + return (C-A).length(); + } + else + { + return (C-B).length(); + } + } + else + { + return (C-A).length(); + } + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } + return -1.f; +} + +float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) +{ + if (seq < waypoints->size()) + { + mavlink_waypoint_t *cur = waypoints->at(seq); + + const PxVector3 A(cur->x, cur->y, cur->z); + const PxVector3 C(x, y, z); + + return (C-A).length(); + } + else + { + if (verbose) printf("ERROR: index out of bounds\n"); + } + return -1.f; +} + + +static void mavlink_wpm_mavlink_handler(const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user) +{ + // Handle param messages + paramClient->handleMAVLinkPacket(msg); + + //check for timed-out operations + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + if (now-protocol_timestamp_lastaction > paramClient->getParamValue("PROTOCOLTIMEOUT") && current_state != PX_WPP_IDLE) + { + if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state); + current_state = PX_WPP_IDLE; + protocol_current_count = 0; + protocol_current_partner_systemid = 0; + protocol_current_partner_compid = 0; + protocol_current_wp_id = -1; + + if(waypoints->size() == 0) + { + current_active_wp_id = -1; + } + } + + if(now-timestamp_last_send_setpoint > paramClient->getParamValue("SETPOINTDELAY") && current_active_wp_id < waypoints->size()) + { + send_setpoint(current_active_wp_id); + } + + switch(msg->msgid) + { + case MAVLINK_MSG_ID_ATTITUDE: + { + if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) + { + mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); + if(wp->frame == 1) + { + mavlink_attitude_t att; + mavlink_msg_attitude_decode(msg, &att); + float yaw_tolerance = paramClient->getParamValue("YAWTOLERANCE"); + //compare current yaw + if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) + { + if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) + yawReached = true; + } + else if(att.yaw - yaw_tolerance < 0.0f) + { + float lowerBound = 360.0f + att.yaw - yaw_tolerance; + if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) + yawReached = true; + } + else + { + float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; + if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) + yawReached = true; + } + } + } + break; + } + + case MAVLINK_MSG_ID_LOCAL_POSITION: + { + if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) + { + mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); + + if(wp->frame == 1) + { + mavlink_local_position_t pos; + mavlink_msg_local_position_decode(msg, &pos); + if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); + + posReached = false; + + // compare current position (given in message) with current waypoint + float orbit = wp->param1; + + float dist; + if (wp->param2 == 0) + { + dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z); + } + else + { + dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z); + } + + if (dist >= 0.f && dist <= orbit && yawReached) + { + posReached = true; + } + } + } + break; + } + + case MAVLINK_MSG_ID_CMD: // special action from ground station + { + mavlink_cmd_t action; + mavlink_msg_cmd_decode(msg, &action); + if(action.target == systemid) + { + if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; + switch (action.action) + { + // case MAV_ACTION_LAUNCH: + // if (verbose) std::cerr << "Launch received" << std::endl; + // current_active_wp_id = 0; + // if (waypoints->size()>0) + // { + // setActive(waypoints[current_active_wp_id]); + // } + // else + // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; + // break; + + // case MAV_ACTION_CONTINUE: + // if (verbose) std::c + // err << "Continue received" << std::endl; + // idle = false; + // setActive(waypoints[current_active_wp_id]); + // break; + + // case MAV_ACTION_HALT: + // if (verbose) std::cerr << "Halt received" << std::endl; + // idle = true; + // break; + + // default: + // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; + // break; + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + mavlink_waypoint_ack_t wpa; + mavlink_msg_waypoint_ack_decode(msg, &wpa); + + if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid)) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS) + { + if (protocol_current_wp_id == waypoints->size()-1) + { + if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); + current_state = PX_WPP_IDLE; + protocol_current_wp_id = 0; + } + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + { + mavlink_waypoint_set_current_t wpc; + mavlink_msg_waypoint_set_current_decode(msg, &wpc); + + if(wpc.target_system == systemid && wpc.target_component == compid) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_IDLE) + { + if (wpc.seq < waypoints->size()) + { + if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + current_active_wp_id = wpc.seq; + uint32_t i; + for(i = 0; i < waypoints->size(); i++) + { + if (i == current_active_wp_id) + { + waypoints->at(i)->current = true; + } + else + { + waypoints->at(i)->current = false; + } + } + if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + yawReached = false; + posReached = false; + send_waypoint_current(current_active_wp_id); + send_setpoint(current_active_wp_id); + timestamp_firstinside_orbit = 0; + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); + } + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + { + mavlink_waypoint_request_list_t wprl; + mavlink_msg_waypoint_request_list_decode(msg, &wprl); + if(wprl.target_system == systemid && wprl.target_component == compid) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST) + { + if (waypoints->size() > 0) + { + if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); + current_state = PX_WPP_SENDLIST; + protocol_current_wp_id = 0; + protocol_current_partner_systemid = msg->sysid; + protocol_current_partner_compid = msg->compid; + } + else + { + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + } + protocol_current_count = waypoints->size(); + send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + mavlink_waypoint_request_t wpr; + mavlink_msg_waypoint_request_decode(msg, &wpr); + if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) + { + protocol_timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) + { + if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + + current_state = PX_WPP_SENDLIST_SENDWPS; + protocol_current_wp_id = wpr.seq; + send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq); + } + else + { + if (verbose) + { + if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; } + else if (current_state == PX_WPP_SENDLIST) + { + if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + } + else if (current_state == PX_WPP_SENDLIST_SENDWPS) + { + if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); + else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + mavlink_waypoint_count_t wpc; + mavlink_msg_waypoint_count_decode(msg, &wpc); + if(wpc.target_system == systemid && wpc.target_component == compid) + { + protocol_timestamp_lastaction = now; + + if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) + { + if (wpc.count > 0) + { + if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + + current_state = PX_WPP_GETLIST; + protocol_current_wp_id = 0; + protocol_current_partner_systemid = msg->sysid; + protocol_current_partner_compid = msg->compid; + protocol_current_count = wpc.count; + + printf("clearing receive buffer and readying for receiving waypoints\n"); + while(waypoints_receive_buffer->size() > 0) + { + delete waypoints_receive_buffer->back(); + waypoints_receive_buffer->pop_back(); + } + + send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); + } + else if (wpc.count == 0) + { + printf("got waypoint count of 0, clearing waypoint list and staying in state PX_WPP_IDLE\n"); + while(waypoints_receive_buffer->size() > 0) + { + delete waypoints->back(); + waypoints->pop_back(); + } + current_active_wp_id = -1; + yawReached = false; + posReached = false; + break; + + } + else + { + if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + } + } + else + { + if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state); + else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT: + { + mavlink_waypoint_t wp; + mavlink_msg_waypoint_decode(msg, &wp); + + if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) + { + protocol_timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids + if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) + { + if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + + current_state = PX_WPP_GETLIST_GETWPS; + protocol_current_wp_id = wp.seq + 1; + mavlink_waypoint_t* newwp = new mavlink_waypoint_t; + memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); + waypoints_receive_buffer->push_back(newwp); + + if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + + if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) + { + if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); + + send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); + + if (current_active_wp_id > waypoints_receive_buffer->size()-1) + { + current_active_wp_id = waypoints_receive_buffer->size() - 1; + } + + // switch the waypoints list + std::vector* waypoints_temp = waypoints; + waypoints = waypoints_receive_buffer; + waypoints_receive_buffer = waypoints_temp; + + //get the new current waypoint + uint32_t i; + for(i = 0; i < waypoints->size(); i++) + { + if (waypoints->at(i)->current == 1) + { + current_active_wp_id = i; + //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + yawReached = false; + posReached = false; + send_waypoint_current(current_active_wp_id); + send_setpoint(current_active_wp_id); + timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == waypoints->size()) + { + current_active_wp_id = -1; + yawReached = false; + posReached = false; + timestamp_firstinside_orbit = 0; + } + + current_state = PX_WPP_IDLE; + } + else + { + send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); + } + } + else + { + if (current_state == PX_WPP_IDLE) + { + //we're done receiving waypoints, answer with ack. + send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); + printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); + } + if (verbose) + { + if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; } + else if (current_state == PX_WPP_GETLIST) + { + if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else if (current_state == PX_WPP_GETLIST_GETWPS) + { + if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); + else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); + } + else if(wp.target_system == systemid && wp.target_component == compid) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + { + mavlink_waypoint_clear_all_t wpca; + mavlink_msg_waypoint_clear_all_decode(msg, &wpca); + + if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) + { + protocol_timestamp_lastaction = now; + + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + while(waypoints->size() > 0) + { + delete waypoints->back(); + waypoints->pop_back(); + } + current_active_wp_id = -1; + yawReached = false; + posReached = false; + } + else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); + } + break; + } + + default: + { + if (debug) std::cerr << "Waypoint: received message of unknown type" << std::endl; + break; + } + } + + //check if the current waypoint was reached + if ((posReached && /*yawReached &&*/ !idle)) + { + if (current_active_wp_id < waypoints->size()) + { + mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id); + + if (timestamp_firstinside_orbit == 0) + { + // Announce that last waypoint was reached + if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + send_waypoint_reached(cur_wp->seq); + timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000) + { + if (cur_wp->autocontinue) + { + cur_wp->current = 0; + if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1) + { + //the last waypoint was reached, if auto continue is + //activated restart the waypoint list from the beginning + current_active_wp_id = 1; + } + else + { + if ((uint16_t)(current_active_wp_id + 1) < waypoints->size()) + current_active_wp_id++; + } + + // Fly to next waypoint + timestamp_firstinside_orbit = 0; + send_waypoint_current(current_active_wp_id); + send_setpoint(current_active_wp_id); + waypoints->at(current_active_wp_id)->current = true; + posReached = false; + yawReached = false; + if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id); + } + } + } + } + else + { + timestamp_lastoutside_orbit = now; + } +} + + + + + + + + int main(int argc, char* argv[]) { @@ -129,6 +987,8 @@ int main(int argc, char* argv[]) gcAddr.sin_port = htons(14550); + printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); + for (;;) { diff --git a/thirdParty/mavlink/missionlib/waypoints.c b/thirdParty/mavlink/missionlib/waypoints.c new file mode 100644 index 000000000..335f593a9 --- /dev/null +++ b/thirdParty/mavlink/missionlib/waypoints.c @@ -0,0 +1,880 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +#include + +static mavlink_wpm_storage wpm; + +bool debug = true; +bool verbose = true; + + +void mavlink_wpm_init(mavlink_wpm_storage* state) +{ + // Set all waypoints to zero + + // Set count to zero + state->size = 0; + state->max_size = MAVLINK_WPM_MAX_WP_COUNT; + state->current_state = MAVLINK_WPM_STATE_IDLE; + state->current_partner_sysid = 0; + state->current_partner_compid = 0; + state->timestamp_lastaction = 0; + state->timestamp_last_send_setpoint = 0; + state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; + state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; + state->idle = false; ///< indicates if the system is following the waypoints or is waiting + state->current_active_wp_id = -1; ///< id of current waypoint + state->yaw_reached = false; ///< boolean for yaw attitude reached + state->pos_reached = false; ///< boolean for position reached + state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value + state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value + +} + +void mavlink_wpm_send_gcs_string(const char* string) +{ + printf("%s",string); +} + +uint64_t mavlink_wpm_get_system_timestamp() +{ + struct timeval tv; + gettimeofday(&tv, NULL); + return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; +} + +/* + * @brief Sends an waypoint ack message + */ +void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) +{ + mavlink_message_t msg; + mavlink_waypoint_ack_t wpa; + + wpa.target_system = wpm.current_partner_sysid; + wpa.target_component = wpm.current_partner_compid; + wpa.type = type; + + mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) + { + //printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); + mavlink_wpm_send_gcs_string("Sent waypoint ACK"); + } +} + +/* + * @brief Broadcasts the new target waypoint and directs the MAV to fly there + * + * This function broadcasts its new active waypoint sequence number and + * sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_waypoint_current(uint16_t seq) +{ + if(seq < wpm.size) + { + mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); + + mavlink_message_t msg; + mavlink_waypoint_current_t wpc; + + wpc.seq = cur->seq; + + mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //printf("Broadcasted new current waypoint %u\n", wpc.seq); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: index out of bounds\n"); + } +} + +/* + * @brief Directs the MAV to fly to a position + * + * Sends a message to the controller, advising it to fly to the coordinates + * of the waypoint with a given orientation + * + * @param seq The waypoint sequence number the MAV should fly to. + */ +void mavlink_wpm_send_setpoint(uint16_t seq) +{ + if(seq < wpm.size) + { + mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); + + mavlink_message_t msg; + mavlink_local_position_setpoint_set_t position_control_set_point; + + // Send new NED or ENU setpoint to onbaord autopilot + if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU) + { + position_control_set_point.target_system = mavlink_system.sysid; + position_control_set_point.target_component = MAV_COMP_ID_IMU; + position_control_set_point.x = cur->x; + position_control_set_point.y = cur->y; + position_control_set_point.z = cur->z; + position_control_set_point.yaw = cur->param4; + + mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point); + mavlink_wpm_send_message(&msg); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); + } + + wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp(); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //if (verbose) printf("ERROR: index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) +{ + mavlink_message_t msg; + mavlink_waypoint_count_t wpc; + + wpc.target_system = wpm.current_partner_sysid; + wpc.target_component = wpm.current_partner_compid; + wpc.count = count; + + mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_wpm_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm.size) + { + mavlink_message_t msg; + mavlink_waypoint_t *wp = &(wpm.waypoints[seq]); + wp->target_system = wpm.current_partner_sysid; + wp->target_component = wpm.current_partner_compid; + mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); + mavlink_wpm_send_message(&msg); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); + } +} + +void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +{ + if (seq < wpm.max_size) + { + mavlink_message_t msg; + mavlink_waypoint_request_t wpr; + wpr.target_system = wpm.current_partner_sysid; + wpr.target_component = wpm.current_partner_compid; + wpr.seq = seq; + mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); + mavlink_wpm_send_message(&msg); + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + } + else + { + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); + } +} + +/* + * @brief emits a message that a waypoint reached + * + * This function broadcasts a message that a waypoint is reached. + * + * @param seq The waypoint sequence number the MAV has reached. + */ +void mavlink_wpm_send_waypoint_reached(uint16_t seq) +{ + mavlink_message_t msg; + mavlink_waypoint_reached_t wp_reached; + + wp_reached.seq = seq; + + mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); + mavlink_wpm_send_message(&msg); + + if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); + + // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); +} + +//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) +//{ +// if (seq < wpm.size) +// { +// mavlink_waypoint_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// // seq not the second last waypoint +// if ((uint16_t)(seq+1) < wpm.size) +// { +// mavlink_waypoint_t *next = waypoints->at(seq+1); +// const PxVector3 B(next->x, next->y, next->z); +// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); +// if (r >= 0 && r <= 1) +// { +// const PxVector3 P(A + r*(B-A)); +// return (P-C).length(); +// } +// else if (r < 0.f) +// { +// return (C-A).length(); +// } +// else +// { +// return (C-B).length(); +// } +// } +// else +// { +// return (C-A).length(); +// } +// } +// else +// { +// if (verbose) printf("ERROR: index out of bounds\n"); +// } +// return -1.f; +//} + +float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) +{ +// if (seq < wpm.size) +// { +// mavlink_waypoint_t *cur = waypoints->at(seq); +// +// const PxVector3 A(cur->x, cur->y, cur->z); +// const PxVector3 C(x, y, z); +// +// return (C-A).length(); +// } +// else +// { +// if (verbose) printf("ERROR: index out of bounds\n"); +// } + return -1.f; +} + + +static void mavlink_wpm_message_handler(const mavlink_message_t* msg) +{ + // Handle param messages + //paramClient->handleMAVLinkPacket(msg); + + //check for timed-out operations + struct timeval tv; + gettimeofday(&tv, NULL); + uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; + if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_count = 0; + wpm.current_partner_sysid = 0; + wpm.current_partner_compid = 0; + wpm.current_wp_id = -1; + + if(wpm.size == 0) + { + wpm.current_active_wp_id = -1; + } + } + + if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size) + { + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + } + + switch(msg->msgid) + { + case MAVLINK_MSG_ID_ATTITUDE: + { + if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); + if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) + { + mavlink_attitude_t att; + mavlink_msg_attitude_decode(msg, &att); + float yaw_tolerance = wpm.accept_range_yaw; + //compare current yaw + if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) + { + if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) + wpm.yaw_reached = true; + } + else if(att.yaw - yaw_tolerance < 0.0f) + { + float lowerBound = 360.0f + att.yaw - yaw_tolerance; + if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) + wpm.yaw_reached = true; + } + else + { + float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; + if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) + wpm.yaw_reached = true; + } + } + } + break; + } + + case MAVLINK_MSG_ID_LOCAL_POSITION: + { + if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); + + if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) + { + mavlink_local_position_t pos; + mavlink_msg_local_position_decode(msg, &pos); + //if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); + + wpm.pos_reached = false; + + // compare current position (given in message) with current waypoint + float orbit = wp->param1; + + float dist; + if (wp->param2 == 0) + { + // FIXME segment distance + //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); + } + else + { + dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z); + } + + if (dist >= 0.f && dist <= orbit && wpm.yaw_reached) + { + wpm.pos_reached = true; + } + } + } + break; + } + +// case MAVLINK_MSG_ID_CMD: // special action from ground station +// { +// mavlink_cmd_t action; +// mavlink_msg_cmd_decode(msg, &action); +// if(action.target == mavlink_system.sysid) +// { +// if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; +// switch (action.action) +// { +// // case MAV_ACTION_LAUNCH: +// // if (verbose) std::cerr << "Launch received" << std::endl; +// // current_active_wp_id = 0; +// // if (wpm.size>0) +// // { +// // setActive(waypoints[current_active_wp_id]); +// // } +// // else +// // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; +// // break; +// +// // case MAV_ACTION_CONTINUE: +// // if (verbose) std::c +// // err << "Continue received" << std::endl; +// // idle = false; +// // setActive(waypoints[current_active_wp_id]); +// // break; +// +// // case MAV_ACTION_HALT: +// // if (verbose) std::cerr << "Halt received" << std::endl; +// // idle = true; +// // break; +// +// // default: +// // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; +// // break; +// } +// } +// break; +// } + + case MAVLINK_MSG_ID_WAYPOINT_ACK: + { + mavlink_waypoint_ack_t wpa; + mavlink_msg_waypoint_ack_decode(msg, &wpa); + + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid && wpa.target_component == mavlink_system.compid)) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpm.current_wp_id == wpm.size-1) + { + if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + wpm.current_wp_id = 0; + } + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: + { + mavlink_waypoint_set_current_t wpc; + mavlink_msg_waypoint_set_current_decode(msg, &wpc); + + if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + if (wpc.seq < wpm.size) + { + if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); + wpm.current_active_wp_id = wpc.seq; + uint32_t i; + for(i = 0; i < wpm.size; i++) + { + if (i == wpm.current_active_wp_id) + { + wpm.waypoints[i].current = true; + } + else + { + wpm.waypoints[i].current = false; + } + } + if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id); + wpm.yaw_reached = false; + wpm.pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.timestamp_firstinside_orbit = 0; + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: + { + mavlink_waypoint_request_list_t wprl; + mavlink_msg_waypoint_request_list_decode(msg, &wprl); + if(wprl.target_system == mavlink_system.sysid && wprl.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpm.size > 0) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; + wpm.current_wp_id = 0; + wpm.current_partner_sysid = msg->sysid; + wpm.current_partner_compid = msg->compid; + } + else + { + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); + } + wpm.current_count = wpm.size; + mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); + } + else + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); + } + + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_REQUEST: + { + mavlink_waypoint_request_t wpr; + mavlink_msg_waypoint_request_decode(msg, &wpr); + if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) + if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; + wpm.current_wp_id = wpr.seq; + mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq); + } + else + { + if (verbose) + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) + { + if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); + } + else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) + { + if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); + else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_COUNT: + { + mavlink_waypoint_count_t wpc; + mavlink_msg_waypoint_count_decode(msg, &wpc); + if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) + { + wpm.timestamp_lastaction = now; + + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0)) + { + if (wpc.count > 0) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST; + wpm.current_wp_id = 0; + wpm.current_partner_sysid = msg->sysid; + wpm.current_partner_compid = msg->compid; + wpm.current_count = wpc.count; + + printf("clearing receive buffer and readying for receiving waypoints\n"); + wpm.rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints_receive_buffer->back(); +// waypoints_receive_buffer->pop_back(); +// } + + mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); + } + else if (wpc.count == 0) + { + printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); + wpm.rcv_size = 0; + //while(waypoints_receive_buffer->size() > 0) +// { +// delete waypoints->back(); +// waypoints->pop_back(); +// } + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + break; + + } + else + { + if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); + } + } + else + { + if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); + else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); + } + } + else + { + if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); + } + + } + break; + + case MAVLINK_MSG_ID_WAYPOINT: + { + mavlink_waypoint_t wp; + mavlink_msg_waypoint_decode(msg, &wp); + + if (verbose) printf("GOT WAYPOINT!"); + + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid)) + { + wpm.timestamp_lastaction = now; + + //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids + if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) + { + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); + if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); + + wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; + mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); + memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); + + wpm.current_wp_id = wp.seq + 1; + + if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); + + if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) + { + if (verbose) printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); + + mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); + + if (wpm.current_active_wp_id > wpm.rcv_size-1) + { + wpm.current_active_wp_id = wpm.rcv_size-1; + } + + // switch the waypoints list + // FIXME CHECK!!! + for (int i = 0; i < wpm.current_count; ++i) + { + wpm.waypoints[i] = wpm.rcv_waypoints[i]; + } + wpm.size = wpm.current_count; + + //get the new current waypoint + uint32_t i; + for(i = 0; i < wpm.size; i++) + { + if (wpm.waypoints[i].current == 1) + { + wpm.current_active_wp_id = i; + //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); + wpm.yaw_reached = false; + wpm.pos_reached = false; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.timestamp_firstinside_orbit = 0; + break; + } + } + + if (i == wpm.size) + { + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + wpm.timestamp_firstinside_orbit = 0; + } + + wpm.current_state = MAVLINK_WPM_STATE_IDLE; + } + else + { + mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); + } + } + else + { + if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + //we're done receiving waypoints, answer with ack. + mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); + printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); + } + if (verbose) + { + if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) + { + if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) + { + if (!(wp.seq == wpm.current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); + else if (!(wp.seq < wpm.current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); + } + } + } + else + { + //we we're target but already communicating with someone else + if((wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); + } + else if(wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); + } + } + break; + } + + case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: + { + mavlink_waypoint_clear_all_t wpca; + mavlink_msg_waypoint_clear_all_decode(msg, &wpca); + + if(wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state == MAVLINK_WPM_STATE_IDLE) + { + wpm.timestamp_lastaction = now; + + if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); + // Delete all waypoints + wpm.size = 0; + wpm.current_active_wp_id = -1; + wpm.yaw_reached = false; + wpm.pos_reached = false; + } + else if (wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + { + if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); + } + break; + } + + default: + { + if (debug) printf("Waypoint: received message of unknown type"); + break; + } + } + + //check if the current waypoint was reached + if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle) + { + if (wpm.current_active_wp_id < wpm.size) + { + mavlink_waypoint_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]); + + if (wpm.timestamp_firstinside_orbit == 0) + { + // Announce that last waypoint was reached + if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); + mavlink_wpm_send_waypoint_reached(cur_wp->seq); + wpm.timestamp_firstinside_orbit = now; + } + + // check if the MAV was long enough inside the waypoint orbit + //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) + if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000) + { + if (cur_wp->autocontinue) + { + cur_wp->current = 0; + if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1) + { + //the last waypoint was reached, if auto continue is + //activated restart the waypoint list from the beginning + wpm.current_active_wp_id = 1; + } + else + { + if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size) + wpm.current_active_wp_id++; + } + + // Fly to next waypoint + wpm.timestamp_firstinside_orbit = 0; + mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); + mavlink_wpm_send_setpoint(wpm.current_active_wp_id); + wpm.waypoints[wpm.current_active_wp_id].current = true; + wpm.pos_reached = false; + wpm.yaw_reached = false; + if (verbose) printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); + } + } + } + } + else + { + wpm.timestamp_lastoutside_orbit = now; + } +} + diff --git a/thirdParty/mavlink/missionlib/waypoints.h b/thirdParty/mavlink/missionlib/waypoints.h new file mode 100644 index 000000000..4bc32a14a --- /dev/null +++ b/thirdParty/mavlink/missionlib/waypoints.h @@ -0,0 +1,91 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ +#include +#include + +// FIXME XXX - TO BE MOVED TO XML +enum MAVLINK_WPM_STATES +{ + MAVLINK_WPM_STATE_IDLE = 0, + MAVLINK_WPM_STATE_SENDLIST, + MAVLINK_WPM_STATE_SENDLIST_SENDWPS, + MAVLINK_WPM_STATE_GETLIST, + MAVLINK_WPM_STATE_GETLIST_GETWPS, + MAVLINK_WPM_STATE_GETLIST_GOTALL, + MAVLINK_WPM_STATE_ENUM_END +}; + +enum MAVLINK_WPM_CODES +{ + MAVLINK_WPM_CODE_OK = 0, + MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, + MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, + MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, + MAVLINK_WPM_CODE_ENUM_END +}; + + +/* WAYPOINT MANAGER - MISSION LIB */ + +#define MAVLINK_WPM_MAX_WP_COUNT 100 +#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates +#define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text +#define MAVLINK_WPM_SYSTEM_ID 1 +#define MAVLINK_WPM_COMPONENT_ID 1 +#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000 +#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 +#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40 + + +struct mavlink_wpm_storage { + mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints +#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE + mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints +#endif + uint16_t size; + uint16_t max_size; + uint16_t rcv_size; + enum MAVLINK_WPM_STATES current_state; + uint16_t current_wp_id; ///< Waypoint in current transmission + uint16_t current_active_wp_id; ///< Waypoint the system is currently heading towards + uint16_t current_count; + uint8_t current_partner_sysid; + uint8_t current_partner_compid; + uint64_t timestamp_lastaction; + uint64_t timestamp_last_send_setpoint; + uint64_t timestamp_firstinside_orbit; + uint64_t timestamp_lastoutside_orbit; + uint32_t timeout; + uint32_t delay_setpoint; + float accept_range_yaw; + float accept_range_distance; + bool yaw_reached; + bool pos_reached; + bool idle; +}; + +typedef struct mavlink_wpm_storage mavlink_wpm_storage; + +void mavlink_wpm_init(mavlink_wpm_storage* state); + +void mavlink_wpm_message_handler(const mavlink_message_t* msg); \ No newline at end of file -- 2.22.0