From c73f7ef3ab66b4aa0e610a72533d7a2d5c1b568f Mon Sep 17 00:00:00 2001 From: "Andreas M. Antonopoulos" Date: Sat, 14 Jul 2012 19:26:17 -0700 Subject: [PATCH] AP_Limits library, provides modular "limits" such as altitude and geo-fencing. --- ArduCopter/APM_Config.h | 17 + ArduCopter/ArduCopter.pde | 30 + ArduCopter/GCS_Mavlink.pde | 55 ++ ArduCopter/Parameters.h | 8 +- ArduCopter/Parameters.pde | 7 + ArduCopter/defines.h | 9 +- ArduCopter/limits.pde | 341 +++++++++ ArduCopter/motors.pde | 26 +- ArduCopter/system.pde | 34 + libraries/AP_Limits/AP_Limit_Altitude.cpp | 78 ++ libraries/AP_Limits/AP_Limit_Altitude.h | 38 + libraries/AP_Limits/AP_Limit_GPSLock.cpp | 46 ++ libraries/AP_Limits/AP_Limit_GPSLock.h | 33 + libraries/AP_Limits/AP_Limit_Geofence.cpp | 204 ++++++ libraries/AP_Limits/AP_Limit_Geofence.h | 71 ++ libraries/AP_Limits/AP_Limit_Module.cpp | 74 ++ libraries/AP_Limits/AP_Limit_Module.h | 57 ++ libraries/AP_Limits/AP_Limits.cpp | 196 +++++ libraries/AP_Limits/AP_Limits.h | 103 +++ .../GCS_MAVLink/include/mavlink/config.h | 1 - .../v0.9/ardupilotmega/ardupilotmega.h | 34 +- .../ardupilotmega/mavlink_msg_limits_status.h | 320 +++++++++ .../mavlink/v0.9/ardupilotmega/testsuite.h | 60 ++ .../mavlink/v0.9/ardupilotmega/version.h | 2 +- .../include/mavlink/v0.9/checksum.h | 0 .../include/mavlink/v0.9/common/version.h | 2 +- .../include/mavlink/v0.9/mavlink_helpers.h | 0 .../include/mavlink/v0.9/mavlink_types.h | 0 .../include/mavlink/v0.9/protocol.h | 0 .../v1.0/ardupilotmega/ardupilotmega.h | 34 +- .../ardupilotmega/mavlink_msg_limits_status.h | 320 +++++++++ .../mavlink/v1.0/ardupilotmega/testsuite.h | 60 ++ .../mavlink/v1.0/ardupilotmega/version.h | 2 +- .../include/mavlink/v1.0/checksum.h | 0 .../include/mavlink/v1.0/common/version.h | 2 +- .../mavlink/v1.0/matrixpilot/matrixpilot.h | 53 -- .../mavlink/v1.0/matrixpilot/mavlink.h | 27 - .../mavlink/v1.0/matrixpilot/testsuite.h | 36 - .../mavlink/v1.0/matrixpilot/version.h | 12 - .../include/mavlink/v1.0/mavlink_helpers.h | 0 .../mavlink/v1.0/mavlink_protobuf_manager.hpp | 0 .../include/mavlink/v1.0/mavlink_types.h | 0 .../include/mavlink/v1.0/protocol.h | 0 .../include/mavlink/v1.0/sensesoar/mavlink.h | 27 - .../sensesoar/mavlink_msg_cmd_airspeed_ack.h | 166 ----- .../sensesoar/mavlink_msg_cmd_airspeed_chng.h | 166 ----- .../v1.0/sensesoar/mavlink_msg_filt_rot_vel.h | 144 ---- .../v1.0/sensesoar/mavlink_msg_llc_out.h | 167 ----- .../v1.0/sensesoar/mavlink_msg_obs_air_temp.h | 144 ---- .../sensesoar/mavlink_msg_obs_air_velocity.h | 188 ----- .../v1.0/sensesoar/mavlink_msg_obs_attitude.h | 144 ---- .../v1.0/sensesoar/mavlink_msg_obs_bias.h | 167 ----- .../v1.0/sensesoar/mavlink_msg_obs_position.h | 188 ----- .../v1.0/sensesoar/mavlink_msg_obs_qff.h | 144 ---- .../v1.0/sensesoar/mavlink_msg_obs_velocity.h | 144 ---- .../v1.0/sensesoar/mavlink_msg_obs_wind.h | 144 ---- .../v1.0/sensesoar/mavlink_msg_pm_elec.h | 182 ----- .../v1.0/sensesoar/mavlink_msg_sys_stat.h | 210 ------ .../mavlink/v1.0/sensesoar/sensesoar.h | 77 -- .../mavlink/v1.0/sensesoar/testsuite.h | 676 ------------------ .../include/mavlink/v1.0/sensesoar/version.h | 12 - .../message_definitions/ardupilotmega.xml | 35 + .../ardupilotmega.xml | 35 +- 63 files changed, 2319 insertions(+), 3233 deletions(-) create mode 100644 ArduCopter/limits.pde create mode 100644 libraries/AP_Limits/AP_Limit_Altitude.cpp create mode 100644 libraries/AP_Limits/AP_Limit_Altitude.h create mode 100644 libraries/AP_Limits/AP_Limit_GPSLock.cpp create mode 100644 libraries/AP_Limits/AP_Limit_GPSLock.h create mode 100644 libraries/AP_Limits/AP_Limit_Geofence.cpp create mode 100644 libraries/AP_Limits/AP_Limit_Geofence.h create mode 100644 libraries/AP_Limits/AP_Limit_Module.cpp create mode 100644 libraries/AP_Limits/AP_Limit_Module.h create mode 100644 libraries/AP_Limits/AP_Limits.cpp create mode 100644 libraries/AP_Limits/AP_Limits.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/config.h create mode 100644 libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_limits_status.h mode change 100644 => 100755 libraries/GCS_MAVLink/include/mavlink/v0.9/checksum.h mode change 100644 => 100755 libraries/GCS_MAVLink/include/mavlink/v0.9/mavlink_helpers.h mode change 100644 => 100755 libraries/GCS_MAVLink/include/mavlink/v0.9/mavlink_types.h mode change 100644 => 100755 libraries/GCS_MAVLink/include/mavlink/v0.9/protocol.h create mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h mode change 100644 => 100755 libraries/GCS_MAVLink/include/mavlink/v1.0/checksum.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/matrixpilot/matrixpilot.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/matrixpilot/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/matrixpilot/testsuite.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/matrixpilot/version.h mode change 100644 => 100755 libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_helpers.h mode change 100644 => 100755 libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_protobuf_manager.hpp mode change 100644 => 100755 libraries/GCS_MAVLink/include/mavlink/v1.0/mavlink_types.h mode change 100644 => 100755 libraries/GCS_MAVLink/include/mavlink/v1.0/protocol.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_ack.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/sensesoar.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/testsuite.h delete mode 100644 libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/version.h diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index 56b2e0ea0c..34a8b2b39e 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -109,3 +109,20 @@ // #define MOT_6 CH_4 // #define MOT_7 CH_7 // #define MOT_8 CH_8 + + +/// +// +// AP_Limits +// +// + + +// Enable/disable AP_Limits +#define AP_LIMITS ENABLED + +// Use PIN for displaying LIMITS status. 0 is disabled. +#define LIMITS_TRIGGERED_PIN 0 + +// PWM of "on" state for LIM_CHANNEL +#define LIMITS_ENABLE_PWM 1800 diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0846019e9d..4b0c20dc87 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -113,6 +113,13 @@ http://code.google.com/p/ardupilot-mega/downloads/list #include // ArduPilot Mega Declination Helper Library +// Limits library - Puts limits on the vehicle, and takes recovery actions +#include +#include // a limits library module +#include // a limits library module +#include // a limits library module + + //////////////////////////////////////////////////////////////////////////////// // Serial ports //////////////////////////////////////////////////////////////////////////////// @@ -945,6 +952,21 @@ AP_Mount camera_mount(¤t_loc, g_gps, &ahrs); //pinMode(camtrig, OUTPUT); // these are free pins PE3(5), PH3(15), PH6(18), PB4(23), PB5(24), PL1(36), PL3(38), PA6(72), PA7(71), PK0(89), PK1(88), PK2(87), PK3(86), PK4(83), PK5(84), PK6(83), PK7(82) #endif +//////////////////////////////////////////////////////////////////////////////// +// Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters +//////////////////////////////////////////////////////////////////////////////// +#ifdef AP_LIMITS + + AP_Limits limits; + + AP_Limit_GPSLock gpslock_limit(g_gps); + + AP_Limit_Geofence geofence_limit(FENCE_START_BYTE, FENCE_WP_SIZE, MAX_FENCEPOINTS, g_gps, &home, ¤t_loc); + + AP_Limit_Altitude altitude_limit(¤t_loc); + +#endif + //////////////////////////////////////////////////////////////////////////////// // Top-level logic //////////////////////////////////////////////////////////////////////////////// @@ -1256,6 +1278,14 @@ static void fifty_hz_loop() static void slow_loop() { + +#ifdef AP_LIMITS + + // Run the AP_Limits main loop + limits_loop(); + +#endif // AP_LIMITS_ENABLED + // This is the slow (3 1/3 Hz) loop pieces //---------------------------------------- switch (slow_loopCounter){ diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index d57a867db7..6816116dd2 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -93,6 +93,14 @@ static NOINLINE void send_attitude(mavlink_channel_t chan) omega.z); } +#ifdef AP_LIMITS +static NOINLINE void send_limits_status(mavlink_channel_t chan) +{ + limits_send_mavlink_status(chan); +} +#endif + + static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t packet_drops) { uint32_t control_sensors_present = 0; @@ -577,6 +585,15 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id, send_statustext(chan); break; +#ifdef AP_LIMITS + + case MSG_LIMITS_STATUS: + CHECK_PAYLOAD_SIZE(LIMITS_STATUS); + send_limits_status(chan); + break; + +#endif + case MSG_AHRS: #if HIL_MODE != HIL_MODE_ATTITUDE CHECK_PAYLOAD_SIZE(AHRS); @@ -845,6 +862,8 @@ GCS_MAVLINK::data_stream_send(void) send_message(MSG_CURRENT_WAYPOINT); send_message(MSG_GPS_RAW); // TODO - remove this message after location message is working send_message(MSG_NAV_CONTROLLER_OUTPUT); + send_message(MSG_LIMITS_STATUS); + if (last_gps_satellites != g_gps->num_sats) { // this message is mostly a huge waste of bandwidth, @@ -1765,6 +1784,42 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) break; } +#ifdef AP_LIMITS + + // receive an AP_Limits fence point from GCS and store in EEPROM + // receive a fence point from GCS and store in EEPROM + case MAVLINK_MSG_ID_FENCE_POINT: { + mavlink_fence_point_t packet; + mavlink_msg_fence_point_decode(msg, &packet); + if (packet.count != geofence_limit.fence_total()) { + send_text(SEVERITY_LOW,PSTR("bad fence point")); + } else { + Vector2l point; + point.x = packet.lat*1.0e7; + point.y = packet.lng*1.0e7; + geofence_limit.set_fence_point_with_index(point, packet.idx); + } + break; + } + // send a fence point to GCS + case MAVLINK_MSG_ID_FENCE_FETCH_POINT: { + mavlink_fence_fetch_point_t packet; + mavlink_msg_fence_fetch_point_decode(msg, &packet); + if (mavlink_check_target(packet.target_system, packet.target_component)) + break; + if (packet.idx >= geofence_limit.fence_total()) { + send_text(SEVERITY_LOW,PSTR("bad fence point")); + } else { + Vector2l point = geofence_limit.get_fence_point_with_index(packet.idx); + mavlink_msg_fence_point_send(chan, 0, 0, packet.idx, geofence_limit.fence_total(), + point.x*1.0e-7, point.y*1.0e-7); + } + break; + } + + +#endif // AP_LIMITS ENABLED + } // end switch } // end handle mavlink diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 5328212339..5ef641c36b 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -60,7 +60,13 @@ public: k_param_log_last_filenumber, // *** Deprecated - remove with next eeprom number change k_param_toy_yaw_rate, // THOR The memory location for the Yaw Rate 1 = fast, 2 = med, 3 = slow - // + // 65: AP_Limits Library + k_param_limits = 65, + k_param_gpslock_limit, + k_param_geofence_limit, + k_param_altitude_limit, + + // // 80: Heli // k_param_heli_servo_1 = 80, diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 43f0955dd2..3f1fd77775 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -316,6 +316,13 @@ static const AP_Param::Info var_info[] PROGMEM = { GOBJECT(sitl, "SIM_", SITL), #endif + //@Group: LIMITS_ + //@Path: ,../libraries/AP_Limits/AP_Limits.cpp,../libraries/AP_Limits/AP_Limit_GPSLock.cpp, ../libraries/AP_Limits/AP_Limit_Geofence.cpp, ../libraries/AP_Limits/AP_Limit_Altitude.cpp, ../libraries/AP_Limits/AP_Limit_Module.cpp + GOBJECT(limits, "LIM_", AP_Limits), + GOBJECT(gpslock_limit, "LIM_", AP_Limit_GPSLock), + GOBJECT(geofence_limit, "LIM_", AP_Limit_Geofence), + GOBJECT(altitude_limit, "LIM_", AP_Limit_Altitude), + #if FRAME_CONFIG == HELI_FRAME // @Group: H_ // @Path: ../libraries/AP_Motors/AP_MotorsHeli.cpp diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 25f98f6115..30fbe21fb0 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -244,6 +244,7 @@ enum ap_message { MSG_NEXT_WAYPOINT, MSG_NEXT_PARAM, MSG_STATUSTEXT, + MSG_LIMITS_STATUS, MSG_AHRS, MSG_SIMSTATE, MSG_HWSTATUS, @@ -367,7 +368,13 @@ enum gcs_severity { #define WP_SIZE 15 #define ONBOARD_PARAM_NAME_LENGTH 15 -#define MAX_WAYPOINTS ((EEPROM_MAX_ADDR - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe + +// fence points are stored at the end of the EEPROM +#define MAX_FENCEPOINTS 20 +#define FENCE_WP_SIZE sizeof(Vector2l) +#define FENCE_START_BYTE (EEPROM_MAX_ADDR-(MAX_FENCEPOINTS*FENCE_WP_SIZE)) + +#define MAX_WAYPOINTS ((FENCE_START_BYTE - WP_START_BYTE) / WP_SIZE) - 1 // - 1 to be safe // mark a function as not to be inlined #define NOINLINE __attribute__((noinline)) diff --git a/ArduCopter/limits.pde b/ArduCopter/limits.pde new file mode 100644 index 0000000000..a2d20943cf --- /dev/null +++ b/ArduCopter/limits.pde @@ -0,0 +1,341 @@ +// Main state machine loop for AP_Limits. Called from slow or superslow loop. + +#if AP_LIMITS == ENABLED + +uint8_t lim_state = 0, lim_old_state = 0; + +void set_recovery_home_alt() { + + uint32_t return_altitude_cm_ahl = 0; // in centimeters above home level. + uint32_t amin_meters_ahl, amax_meters_ahl; + + // for flying vehicles only + if (altitude_limit.enabled()) { + + amin_meters_ahl = (uint32_t) (altitude_limit.min_alt()); + amax_meters_ahl = (uint32_t) (altitude_limit.max_alt()); + + // See if we have a meaningful setting + if (amax_meters_ahl && ((amax_meters_ahl - amin_meters_ahl) > 1)) { + // there is a max_alt set + // set a return altitude that is halfway between the minimum and maximum altitude setting. + // return_altitude is in centimeters, not meters, so we multiply + return_altitude_cm_ahl = (uint32_t) (home.alt + (100 * (uint16_t) ((amax_meters_ahl - amin_meters_ahl) / 2))); + } + } else { + return_altitude_cm_ahl = (uint32_t) (home.alt + g.RTL_altitude*100); + } + // final sanity check + // if our return is less than 4 meters from ground, set it to 4m, to clear "people" height. + if ((return_altitude_cm_ahl - (uint32_t) home.alt) < 400) { + return_altitude_cm_ahl = home.alt + 400; + } + guided_WP.id = 0; + guided_WP.p1 = 0; + guided_WP.options = 0; + guided_WP.lat = home.lat; + guided_WP.lng = home.lng; + guided_WP.alt = return_altitude_cm_ahl; +} + +static void limits_loop() { + + lim_state = limits.state(); + + // Use limits channel to determine LIMITS_ENABLED or LIMITS_DISABLED state + if (lim_state != LIMITS_DISABLED && limits.channel() !=0 && APM_RC.InputCh(limits.channel()-1) < LIMITS_ENABLE_PWM) { + limits.set_state(LIMITS_DISABLED); + } + else if (lim_state == LIMITS_DISABLED && limits.channel() !=0 && APM_RC.InputCh(limits.channel()-1) >= LIMITS_ENABLE_PWM) { + limits.set_state(LIMITS_ENABLED); + } + + if ((uint32_t) millis() - (uint32_t) limits.last_status_update > 1000) { // more than a second has passed - time for an update + gcs_send_message(MSG_LIMITS_STATUS); + } + + if (lim_state != lim_old_state) { // state changed + lim_old_state = lim_state; // we only use lim_oldstate here, for reporting purposes. So, reset it. + gcs_send_message(MSG_LIMITS_STATUS); + + if (limits.debug()) switch (lim_state) { + case LIMITS_INIT: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: INIT")); break; + case LIMITS_DISABLED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: DISABLED")); break; + case LIMITS_ENABLED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: ENABLED")); break; + case LIMITS_TRIGGERED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: TRIGGERED")); break; + case LIMITS_RECOVERING: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: RECOVERING")); break; + case LIMITS_RECOVERED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: RECOVERED")); break; + default: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: UNKNOWN")); break; + } + } + + switch (limits.state()) { + + // have not initialized yet + case LIMITS_INIT: + if (limits.init()) { // initialize system + + // See what the "master" on/off swith is and go to the appropriate start state + if (!limits.enabled()) { + limits.set_state(LIMITS_DISABLED); + } + else { + limits.set_state(LIMITS_ENABLED); + } + } + break; + + // We have been switched off + case LIMITS_DISABLED: + + // check if we have been switched on + if (limits.enabled()) { + limits.set_state(LIMITS_ENABLED); + break; + } + break; + + // Limits module is enabled + case LIMITS_ENABLED: + + // check if we've been switched off + if (!limits.enabled()) { + limits.set_state(LIMITS_DISABLED); + break; + } + + // Until motors are armed, do nothing, just wait in ENABLED state + if (!motors.armed()) { + + // we are waiting for motors to arm + // do nothing + break; + } + + bool required_only; + + required_only = (limits.last_clear == 0); // if we haven't yet 'cleared' all limits, check required limits only + + // check if any limits have been breached and trigger if they have + if (limits.check_triggered(required_only)) { + + // + // TRIGGER - BREACH OF LIMITS + // + // make a note of which limits triggered, so if we know if we recovered them + limits.mods_recovering = limits.mods_triggered; + + limits.last_action = 0; + limits.last_trigger = millis(); + limits.breach_count++; + + limits.set_state(LIMITS_TRIGGERED); + break; + } + + if (motors.armed() && limits.enabled() && !limits.mods_triggered) { + + // All clear. + if (limits.debug()) gcs_send_text(SEVERITY_LOW, "Limits - All Clear"); + limits.last_clear = millis(); + } + + break; + + // Limits have been triggered + case LIMITS_TRIGGERED: + + // check if we've been switched off + if (!limits.enabled()) { + limits.set_state(LIMITS_DISABLED); + break; + } + +#if LIMITS_TRIGGERED_PIN > 0 + digitalWrite(LIMITS_TRIGGERED_PIN, HIGH); +#endif + + if (limits.debug()) { + if (limits.mods_triggered & LIMIT_GPSLOCK) gcs_send_text(SEVERITY_LOW, "!GPSLock"); + if (limits.mods_triggered & LIMIT_GEOFENCE) gcs_send_text(SEVERITY_LOW, "!Geofence"); + if (limits.mods_triggered & LIMIT_ALTITUDE) gcs_send_text(SEVERITY_LOW, "!Altitude"); + } + + // If the motors are not armed, we have triggered pre-arm checks. Do nothing + if (motors.armed() == false) { + limits.set_state(LIMITS_ENABLED); // go back to checking limits + break; + } + + // If we are triggered but no longer in breach, that means we recovered + // somehow, via auto recovery or pilot action + if (!limits.check_all()) { + limits.last_recovery = millis(); + limits.set_state(LIMITS_RECOVERED); + break; + } + else { + limits.set_state(LIMITS_RECOVERING); + limits.last_action = 0; // reset timer + // We are about to take action on a real breach. Make sure we notify immediately + gcs_send_message(MSG_LIMITS_STATUS); + break; + } + break; + + // Take action to recover + case LIMITS_RECOVERING: + // If the motors are not armed, we have triggered pre-arm checks. Do nothing + if (motors.armed() == false) { + limits.set_state(LIMITS_ENABLED); // go back to checking limits + break; + } + + // check if we've been switched off + if (!limits.enabled() && limits.old_mode_switch == oldSwitchPosition) { + limits.old_mode_switch = 0; + reset_control_switch(); + limits.set_state(LIMITS_DISABLED); + break; + } + + // Still need action? + if (limits.check_all() == 0) { // all triggers clear + limits.set_state(LIMITS_RECOVERED); + break; + } + + if (limits.mods_triggered != limits.mods_recovering) { // if any *new* triggers, hit the trigger again + // + // TRIGGER - BREACH OF LIMITS + // + // make a note of which limits triggered, so if we know if we recovered them + limits.mods_recovering = limits.mods_triggered; + + limits.last_action = 0; + limits.last_trigger = millis(); + limits.breach_count++; + + limits.set_state(LIMITS_TRIGGERED); + limits.set_state(LIMITS_TRIGGERED); + break; + } + + // Recovery Action + // if there was no previous action, take action, take note of time send GCS. + if (limits.last_action == 0) { + + // save mode switch + limits.old_mode_switch = oldSwitchPosition; + + + // Take action + // This ensures no "radical" RTL, like a full throttle take-off,happens if something triggers at ground level + if ((uint32_t) current_loc.alt < ((uint32_t)home.alt * 200) ) { // we're under 2m (200cm), already at "people" height or on the ground + if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action: near ground - LAND")); + // TODO: Will this work for a plane? Does it make sense in general? + + set_mode(LAND); + limits.last_action = millis(); // start counter + gcs_send_message(MSG_LIMITS_STATUS); + + break; + } + + + // TODO: This applies only to planes - hold for porting +// if (control_mode == MANUAL && g.auto_trim) { +// // make sure we don't auto trim the surfaces on this change +// control_mode = STABILIZE; +// } + + + + if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action - GUIDED to home")); + set_recovery_home_alt(); + set_mode(GUIDED); + limits.last_action = millis(); + gcs_send_message(MSG_LIMITS_STATUS); + + +// if (!nav_ok) { // we don't have navigational data, now what? +// +// if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits No NAV for recovery!")); +// +// // flying vehicles - land? +// //set_mode(ALT_HOLD); +// +// // other vehicles - TODO +// } + break; + } + + // ESCALATE We have not recovered after 5 minutes of recovery action + + if (((uint32_t)millis() - (uint32_t)limits.last_action) > 300000 ) { + + // TODO: Secondary recovery + if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Recovery Escalation: RTL")); + set_mode(RTL); + limits.last_action = millis(); + break; + } + break; + + // Have recovered, relinquish control and re-enable + case LIMITS_RECOVERED: + + + // check if we've been switched off + if (!limits.enabled()) { + limits.set_state(LIMITS_DISABLED); + break; + } + +#if LIMITS_TRIGGERED_PIN > 0 + digitalWrite(LIMITS_TRIGGERED_PIN, LOW); +#endif + + // Reset action counter + limits.last_action = 0; + + if (((uint32_t)millis() - (uint32_t)limits.last_recovery) > (uint32_t)(limits.safetime() * 1000)) { // Wait "safetime" seconds of recovery before we give back control + + // Our recovery action worked. + limits.set_state(LIMITS_ENABLED); + + // Switch to stabilize + if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - Returning controls")); + set_mode(STABILIZE); limits.last_recovery = millis(); + + break; + } + break; + + default: + if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits: unknown state")); + break; + } +} + +// This function below, should really be in the AP_Limits class, but it is impossible to untangle the mavlink includes. + +void limits_send_mavlink_status(mavlink_channel_t chan) { + + limits.last_status_update = millis(); + + if (limits.enabled()) { + mavlink_msg_limits_status_send(chan, + (uint8_t) limits.state(), + (uint32_t) limits.last_trigger, + (uint32_t) limits.last_action, + (uint32_t) limits.last_recovery, + (uint32_t) limits.last_clear, + (uint16_t) limits.breach_count, + (LimitModuleBits) limits.mods_enabled, + (LimitModuleBits) limits.mods_required, + (LimitModuleBits) limits.mods_triggered); + } +} + +#endif diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index ee3809ec17..23f02fe6d6 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -28,7 +28,31 @@ static void arm_motors() }else if (arming_counter == ARM_DELAY){ if(motors.armed() == false){ // arm the motors and configure for flight - init_arm_motors(); + +//////////////////////////////////////////////////////////////////////////////// +// Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters +//////////////////////////////////////////////////////////////////////////////// +#ifdef AP_LIMITS + if (limits.enabled() && limits.required()) { + gcs_send_text_P(SEVERITY_LOW, PSTR("Limits - Running pre-arm checks")); + + // check only pre-arm required modules + if (limits.check_required()) { + gcs_send_text_P(SEVERITY_LOW, PSTR("ARMING PREVENTED - Limit Breached")); + limits.set_state(LIMITS_TRIGGERED); + gcs_send_message(MSG_LIMITS_STATUS); + + arming_counter++; // restart timer by cycling + } + else { + init_arm_motors(); + } + } else init_arm_motors(); + +#else // without AP_LIMITS, just arm motors + init_arm_motors(); +#endif //AP_LIMITS_ENABLED + } // keep going up arming_counter++; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 2c1c5a01cb..6fef2c15e2 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -333,6 +333,40 @@ static void init_ardupilot() Log_Write_Data(24, (float)g.pid_loiter_rate_lon.kD()); #endif + +/////////////////////////////////////////////////////////////////////////////// +// Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters +//////////////////////////////////////////////////////////////////////////////// +#ifdef AP_LIMITS + + // AP_Limits modules are stored as a _linked list_. That allows us to define an infinite number of modules + // and also to allocate no space until we actually need to. + + // The linked list looks (logically) like this + // [limits module] -> [first limit module] -> [second limit module] -> [third limit module] -> NULL + + + // The details of the linked list are handled by the methods + // modules_first, modules_current, modules_next, modules_last, modules_add + // in limits + + limits.modules_add(&gpslock_limit); + limits.modules_add(&geofence_limit); + limits.modules_add(&altitude_limit); + + + if (limits.debug()) { + gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Modules Loaded")); + + AP_Limit_Module *m = limits.modules_first(); + while (m) { + gcs_send_text_P(SEVERITY_LOW, get_module_name(m->get_module_id())); + m = limits.modules_next(); + } + } + +#endif + SendDebug("\nReady to FLY "); } diff --git a/libraries/AP_Limits/AP_Limit_Altitude.cpp b/libraries/AP_Limits/AP_Limit_Altitude.cpp new file mode 100644 index 0000000000..f07a4318ff --- /dev/null +++ b/libraries/AP_Limits/AP_Limit_Altitude.cpp @@ -0,0 +1,78 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + + +/// @file limits.cpp +/// @brief Imposes limits on location (geofence), altitude and other parameters. +/// Each limit breach will trigger an action or set of actions to recover. Adapted from geofence. +/// @author Andrew Tridgell +/// Andreas Antonopoulos + +#include + +const AP_Param::GroupInfo AP_Limit_Altitude::var_info[] PROGMEM = { + // @Param: ALT_ON + // @DisplayName: Enable altitude + // @Description: Setting this to Enabled(1) will enable the altitude. Setting this to Disabled(0) will disable the altitude + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("ALT_ON", 0, AP_Limit_Altitude, _enabled), + + // @Param: ALT_REQD + // @DisplayName: Require altitude + // @Description: Setting this to Enabled(1) will make being inside the altitude a required check before arming the vehicle. + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("ALT_REQ", 1, AP_Limit_Altitude, _required), + + // @Param: ALT_MIN + // @DisplayName: Minimum Altitude + // @Description: Minimum Altitude. Zero to disable. Sets a "floor" that your vehicle will try to stay above. IF the vehicle is crossing the threshold at speed, it will take a while to recover, so give yourself enough room. Caution: minimum altitude limits can cause unexpected behaviour, such as inability to land, or sudden takeoff. Read the wiki instructions before setting. + // @Units: Meters + // @Range: 0 250000 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("ALT_MIN", 2, AP_Limit_Altitude, _min_alt), + + // @Param: ALT_MAX + // @DisplayName: Maximum Altitude + // @Description: Maximum Altitude. Zero to disable. Sets a "ceiling" that your vehicle will try to stay below. IF the vehicle is crossing the threshold at speed, it will take a while to recover. + // @Units: Meters + // @Range: 0 250000 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("ALT_MAX", 3, AP_Limit_Altitude, _max_alt), + AP_GROUPEND +}; + +AP_Limit_Altitude::AP_Limit_Altitude(struct Location *current_loc) : + AP_Limit_Module(AP_LIMITS_ALTITUDE) // enabled and required +{ + _current_loc = current_loc; +} + +bool AP_Limit_Altitude::triggered() +{ + _triggered = false; // reset trigger before checking + + // _max_alt is zero if disabled + // convert _max_alt to centimeters to compare to actual altitude + if (_max_alt > 0 && _current_loc->alt > _max_alt*100 ) { + _triggered = true; + } + + // _min_alt is zero if disabled + // convert _min_alt to centimeters to compare to actual altitude + if (_min_alt > 0 && _current_loc->alt < _min_alt*100 ) { + _triggered = true; + } + return _triggered; +} + +AP_Int32 AP_Limit_Altitude::max_alt() { + return _max_alt; +} + +AP_Int32 AP_Limit_Altitude::min_alt() { + return _min_alt; +} + diff --git a/libraries/AP_Limits/AP_Limit_Altitude.h b/libraries/AP_Limits/AP_Limit_Altitude.h new file mode 100644 index 0000000000..c7f293d3dd --- /dev/null +++ b/libraries/AP_Limits/AP_Limit_Altitude.h @@ -0,0 +1,38 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file limits.h +/// @brief Imposes limits on location (geofence), altitude and other parameters. +/// Each limit breach will trigger an action or set of actions to recover. Adapted from geofence. +/// @author Andrew Tridgell +/// Andreas Antonopoulos + +#include +#include +#include +#include +#include + +#ifndef AP_Limit_Altitude_H +#define AP_Limit_Altitude_H +#endif // AP_Limit_Altitude_H + +class AP_Limit_Altitude: public AP_Limit_Module { + +public: + AP_Limit_Altitude(struct Location *current_loc); + + AP_Int32 min_alt(); + AP_Int32 max_alt(); + + bool init(); + bool triggered(); + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + struct Location *_current_loc; + AP_Int32 _min_alt; + AP_Int32 _max_alt; + + +}; diff --git a/libraries/AP_Limits/AP_Limit_GPSLock.cpp b/libraries/AP_Limits/AP_Limit_GPSLock.cpp new file mode 100644 index 0000000000..19457eb442 --- /dev/null +++ b/libraries/AP_Limits/AP_Limit_GPSLock.cpp @@ -0,0 +1,46 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + + +/// @file limits.cpp +/// @brief Imposes limits on location (geofence), altitude and other parameters. +/// Each limit breach will trigger an action or set of actions to recover. Adapted from geofence. +/// @author Andrew Tridgell +/// Andreas Antonopoulos + +#include + +const AP_Param::GroupInfo AP_Limit_GPSLock::var_info[] PROGMEM = { + // @Param: GPSLCK_ON + // @DisplayName: Enable gpslock + // @Description: Setting this to Enabled(1) will enable the gpslock. Setting this to Disabled(0) will disable the gpslock + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("GPSLCK_ON", 0, AP_Limit_GPSLock, _enabled), + + // @Param: GPSLCK_REQ + // @DisplayName: Require gpslock + // @Description: Setting this to Enabled(1) will make being inside the gpslock a required check before arming the vehicle. + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("GPSLCK_REQ", 1, AP_Limit_GPSLock, _required), + AP_GROUPEND + +}; + +AP_Limit_GPSLock::AP_Limit_GPSLock(GPS *&gps) : + AP_Limit_Module(AP_LIMITS_GPSLOCK), // enabled and required + _gps(gps) +{ +} + + +bool AP_Limit_GPSLock::triggered() { + _triggered = false; // reset trigger before checking + + if (!_gps || !_gps->fix) { + _triggered = true; + } + + return _triggered; +} + diff --git a/libraries/AP_Limits/AP_Limit_GPSLock.h b/libraries/AP_Limits/AP_Limit_GPSLock.h new file mode 100644 index 0000000000..47bd48c3b2 --- /dev/null +++ b/libraries/AP_Limits/AP_Limit_GPSLock.h @@ -0,0 +1,33 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file limits.h +/// @brief Imposes limits on location (geofence), altitude and other parameters. +/// Each limit breach will trigger an action or set of actions to recover. Adapted from geofence. +/// @author Andrew Tridgell +/// Andreas Antonopoulos + +#include +#include + +#ifndef GPS_h +#include +#endif + +#ifndef AP_Limit_GPSLock_H +#define AP_Limit_GPSLock_H +#endif // AP_Limit_GPSLock_H + +class AP_Limit_GPSLock: public AP_Limit_Module { + +public: + AP_Limit_GPSLock(GPS *&gps); + bool init(); + bool triggered(); + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + GPS *&_gps; + + +}; diff --git a/libraries/AP_Limits/AP_Limit_Geofence.cpp b/libraries/AP_Limits/AP_Limit_Geofence.cpp new file mode 100644 index 0000000000..c60fe735c0 --- /dev/null +++ b/libraries/AP_Limits/AP_Limit_Geofence.cpp @@ -0,0 +1,204 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/// @file limits.cpp +/// @brief Imposes limits on location (geofence), altitude and other parameters. +/// Each limit breach will trigger an action or set of actions to recover. Adapted from geofence. +/// @author Andrew Tridgell +/// Andreas Antonopoulos + +#include + +const AP_Param::GroupInfo AP_Limit_Geofence::var_info[] PROGMEM = { + // @Param: FNC_ON + // @DisplayName: Enable Geofence + // @Description: Setting this to Enabled(1) will enable the geofence. Setting this to Disabled(0) will disable the geofence + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("FNC_ON", 0, AP_Limit_Geofence, _enabled), + + // @Param: FNC_REQ + // @DisplayName: Require Geofence + // @Description: Setting this to Enabled(1) will make being inside the geofence a required check before arming the vehicle. + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("FNC_REQ", 1, AP_Limit_Geofence, _required), + + // @Param: FNC_SMPL + // @DisplayName: Require Geofence + // @Description: "Simple" geofence (enabled - 1) is based on a radius from the home position, "Complex" (disabled - 0) define a complex fence by lat/long positions + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("FNC_SMPL", 2, AP_Limit_Geofence, _simple), + + // @Param: FNC_RAD + // @DisplayName: Require Geofence + // @Description: Radius of fenced area in meters. A value of 20 creates a 20-meter radius circle (40-meter diameter) from the home point. + // @Units: Meters + // @Range: 0 32767 + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("FNC_RAD", 3, AP_Limit_Geofence, _radius), + + AP_GROUPINFO("FNC_TOT", 4, AP_Limit_Geofence, _fence_total), + AP_GROUPEND + + +}; + + +AP_Limit_Geofence::AP_Limit_Geofence(uint32_t efs, uint8_t f_wp_s, uint8_t max_fp, GPS *&gps, struct Location *h_loc, struct Location *c_loc) : + AP_Limit_Module(AP_LIMITS_GEOFENCE), + _eeprom_fence_start(efs), + _fence_wp_size(f_wp_s), + _max_fence_points(max_fp), + _boundary_uptodate(false), + _current_loc(c_loc), + _home(h_loc), + _gps(gps) +{ + update_boundary(); +} + + + + +bool AP_Limit_Geofence::triggered() { + + // reset trigger before checking + _triggered = false; + + // never trigger while disabled + if (!_enabled) return false; + + // if Geofence is required and we don't know where we are, trigger. + if (_required && (!_gps || !_gps->fix || !_home || !_current_loc)) { + + // TRIGGER + _triggered = true; + } + + uint32_t distance; + + if (_simple && _current_loc && _home) { // simple mode, pointers to current and home exist. + distance = (uint32_t) get_distance_meters(_current_loc, _home); + if (distance > 0 && distance > (uint16_t) _radius) { + + // TRIGGER + _triggered = true; + } + } + else { + + // COMPLEX GEOFENCE mode + + // check boundary and update if necessary + if (!_boundary_uptodate) { + update_boundary(); + } + + // if boundary is correct, and current_loc exists check if we are inside the fence. + if (boundary_correct() && _current_loc) { + Vector2l location; + location.x = _current_loc->lat; + location.y = _current_loc->lng; + if (Polygon_outside(location, &_boundary[1], _fence_total-1)) {// trigger if outside + + // TRIGGER + _triggered = true; + } + } else { // boundary incorrect + + // If geofence is required and our boundary fence is incorrect, we trigger. + if (_required) { + + // TRIGGER + _triggered = true; + } + } + } + return _triggered; +} + + + +uint32_t get_distance_meters(struct Location *loc1, struct Location *loc2) // distance in meters between two locations +{ + if (!loc1 || !loc2) + return -1; // pointers missing (dangling) + if(loc1->lat == 0 || loc1->lng == 0) + return -1; // do not trigger a false positive on erroneous location data + if(loc2->lat == 0 || loc2->lng == 0) + return -1; // do not trigger a false positive on erroneous location data + + float dlat = (float)(loc2->lat - loc1->lat); + float dlong = (float)(loc2->lng - loc1->lng); + return (sqrt(sq(dlat) + sq(dlong)) * .01113195); +} + + + +AP_Int8 AP_Limit_Geofence::fence_total() { + return _fence_total; +} + +// save a fence point +void AP_Limit_Geofence::set_fence_point_with_index(Vector2l &point, uint8_t i) +{ + uint32_t mem; + + if (i >= (unsigned)fence_total()) { + // not allowed + return; + } + + mem = _eeprom_fence_start + (i * _fence_wp_size); + + eeprom_write_dword((uint32_t *)mem, point.x); + mem += sizeof(uint32_t); + eeprom_write_dword((uint32_t *)mem, point.y); + + _boundary_uptodate = false; +} + +/* + fence boundaries fetch/store + */ +Vector2l AP_Limit_Geofence::get_fence_point_with_index(uint8_t i) +{ + uint32_t mem; + Vector2l ret; + + if (i > (unsigned) fence_total()) { + return Vector2l(0,0); + } + + // read fence point + mem = _eeprom_fence_start + (i * _fence_wp_size); + ret.x = eeprom_read_dword((uint32_t *)mem); + mem += sizeof(uint32_t); + ret.y = eeprom_read_dword((uint32_t *)mem); + + return ret; +} + +void AP_Limit_Geofence::update_boundary() { + if (!_simple && _fence_total > 0) { + + for (uint8_t i = 0; i < (uint8_t) _fence_total; i++) { + _boundary[i] = get_fence_point_with_index(i); + } + + _boundary_uptodate = true; + + } +} + +bool AP_Limit_Geofence::boundary_correct() { + + if (Polygon_complete(&_boundary[1], _fence_total - 1) && + !Polygon_outside(_boundary[0], &_boundary[1], _fence_total - 1)) { + return true; + } else return false; +} + + diff --git a/libraries/AP_Limits/AP_Limit_Geofence.h b/libraries/AP_Limits/AP_Limit_Geofence.h new file mode 100644 index 0000000000..52bc3bc2e5 --- /dev/null +++ b/libraries/AP_Limits/AP_Limit_Geofence.h @@ -0,0 +1,71 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file limits.h +/// @brief Imposes limits on location (geofence), altitude and other parameters. +/// Each limit breach will trigger an action or set of actions to recover. Adapted from geofence. +/// @author Andrew Tridgell +/// Andreas Antonopoulos + +#include +#include +#include +#include +#include + +#ifndef AP_Limit_Geofence_H +#define AP_Limit_Geofence_H +#endif // AP_Limit_Geofence_H + +#ifndef GPS_h +#include +#endif + + +#define MAX_FENCEPOINTS 20 + +class AP_Limit_Geofence: public AP_Limit_Module { + +public: + AP_Limit_Geofence(uint32_t _eeprom_fence_start, uint8_t fpsize, uint8_t max_fp, GPS *&gps, struct Location *home_loc, struct Location *current_loc); + bool init(); + bool triggered(); + + AP_Int8 fence_total(); + void set_fence_point_with_index(Vector2l &point, uint8_t i); + Vector2l get_fence_point_with_index(uint8_t i); + void update_boundary(); + bool boundary_correct(); + + + + static const struct AP_Param::GroupInfo var_info[]; + +protected: + + // pointers to gps, current location and home + GPS *&_gps; + struct Location *_current_loc; + struct Location *_home; + + + // Simple mode, just radius + AP_Int8 _simple; // 1 = simple, 0 = complex + AP_Int16 _radius; // in meters, for simple mode + + // Complex mode, defined fence points + AP_Int8 _fence_total; + AP_Int8 _num_points; + +private: + const uint32_t _eeprom_fence_start; + const unsigned _fence_wp_size; + const unsigned _max_fence_points; + bool _boundary_uptodate; + Vector2l _boundary[MAX_FENCEPOINTS]; // complex mode fence + +}; + + +// Helper functions +uint32_t get_distance_meters(struct Location *loc1, struct Location *loc2); // distance in meters between two locations + diff --git a/libraries/AP_Limits/AP_Limit_Module.cpp b/libraries/AP_Limits/AP_Limit_Module.cpp new file mode 100644 index 0000000000..fb4b6bfb4d --- /dev/null +++ b/libraries/AP_Limits/AP_Limit_Module.cpp @@ -0,0 +1,74 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + + +/// @brief Imposes limits on location (geofence), altitude and other parameters. +/// Each breach will trigger an action or set of actions to recover. Adapted from geofence. +/// @author Andrew Tridgell +/// Andreas Antonopoulos + + +#include + +extern const prog_char_t *get_module_name(enum moduleid i) { + + switch (i) { + case AP_LIMITS_GPSLOCK: + return PSTR("GPSLock Limit"); + break; + case AP_LIMITS_GEOFENCE: + return PSTR("Geofence Limit"); + break; + case AP_LIMITS_ALTITUDE: + return PSTR("Altitude Limit"); + break; + default: + return PSTR("ERROR"); + break; + } +} + +AP_Limit_Module::AP_Limit_Module(enum moduleid i) { + _id = i; + _next = NULL; +} + +bool AP_Limit_Module::init() { + + _triggered = false; + return true; +}; + +moduleid AP_Limit_Module::get_module_id() { + return(_id); +} + +bool AP_Limit_Module::enabled() { + return(_enabled); +} + +bool AP_Limit_Module::required() { + return(_required); +} + +AP_Limit_Module *AP_Limit_Module::next() { + return(_next); +} + +void AP_Limit_Module::link(AP_Limit_Module *m) { + _next = m; +} + +bool AP_Limit_Module::triggered() { + return(_triggered); +} + +void AP_Limit_Module::action() { + // do nothing +} + + + + + + + diff --git a/libraries/AP_Limits/AP_Limit_Module.h b/libraries/AP_Limits/AP_Limit_Module.h new file mode 100644 index 0000000000..7ffeecfc97 --- /dev/null +++ b/libraries/AP_Limits/AP_Limit_Module.h @@ -0,0 +1,57 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + + +/// @brief Imposes limits on location (geofence), altitude and other parameters. +/// Each breach will trigger an action or set of actions to recover. Adapted from geofence. +/// @author Andrew Tridgell +/// Andreas Antonopoulos + +#ifndef AP_LIMIT_MODULE_H_ +#define AP_LIMIT_MODULE_H_ + +#include + +// The module IDs are defined as powers of 2, to make a bit-field +enum moduleid { + AP_LIMITS_NULLMODULE = 0, // not a module - used to set the "zero" value + AP_LIMITS_GPSLOCK = (1 << 0), // a GPS-lock-required limit + AP_LIMITS_GEOFENCE = (1 << 1), // fence within a set of coordinates + AP_LIMITS_ALTITUDE = (1 << 2) // altitude limits +}; + +extern const prog_char_t *get_module_name(enum moduleid i); + +// an integer type big enough to fit a bit field for all modules. We have 3 modules, so 8-bit int is enough. +typedef uint8_t LimitModuleBits; + +class AP_Limit_Module { + +public: + AP_Limit_Module(enum moduleid id); // initialize a new module + + + bool init(); // initialize self + + virtual moduleid get_module_id(); + virtual bool enabled(); + virtual bool required(); + virtual AP_Limit_Module *next(); // return next module in linked list + + virtual void link(AP_Limit_Module *m); // link the next module in the linked list + + virtual bool triggered(); // trigger check function + + virtual void action(); // recovery action + +protected: + AP_Int8 _enabled; // often exposed as a MAVLink parameter + AP_Int8 _required; + AP_Int8 _triggered; + +private: + enum moduleid _id; + AP_Limit_Module *_next; +}; + + +#endif /* AP_LIMIT_MODULE_H_ */ diff --git a/libraries/AP_Limits/AP_Limits.cpp b/libraries/AP_Limits/AP_Limits.cpp new file mode 100644 index 0000000000..6d56d69367 --- /dev/null +++ b/libraries/AP_Limits/AP_Limits.cpp @@ -0,0 +1,196 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/// @file ap_limits.cpp +/// @brief Imposes limits on location (geofence), altitude and other parameters. +/// Each breach will trigger an action or set of actions to recover. Adapted from geofence. +/// @author Andrew Tridgell +/// Andreas Antonopoulos + +#include +#include + +const AP_Param::GroupInfo AP_Limits::var_info[] PROGMEM = { + + // @Param: ENABLED + // @DisplayName: Enable Limits Library + // @Description: Setting this to Enabled(1) will enable the limits system + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("ENABLED", 0, AP_Limits, _enabled), + + // @Param: REQUIRED + // @DisplayName: Limits Library Required + // @Description: Setting this to 1 will enable the limits pre-arm checklist + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("REQUIRED", 1, AP_Limits, _required), + + // @Param: DEBUG + // @DisplayName: Enable Limits Debug + // @Description: Setting this to 1 will turn on debugging messages on the console and via MAVLink STATUSTEXT messages + // @Values: 0:Disabled,1:Enabled + // @User: Standard + AP_GROUPINFO("DEBUG", 2, AP_Limits, _debug), + + // @Param: SAFETIME + // @DisplayName: Limits Safetime + // @Description: Automatic return of controls to pilot. Set to 0 to disable (full RTL) or a number of seconds after complete recovery to return the controls to the pilot in STABILIZE + // @Values: 0:Disabled,1-255:Seconds before returning control + // @User: Standard + AP_GROUPINFO("SAFETIME", 3, AP_Limits, _safetime), + + // @Param: CHANNEL + // @DisplayName: Limits Channel + // @Description: Channel for Limits on/off control. If channel exceeds LIMITS_ENABLE_PWM, it turns limits on, and vice-versa. + // @Range: 1-8 + // @User: Standard + AP_GROUPINFO("CHANNEL", 4, AP_Limits, _channel), + AP_GROUPEND + +}; + +AP_Limits::AP_Limits() { + last_trigger = 0; + last_action = 0; + last_recovery = 0; + last_clear = 0; + last_status_update = 0; + breach_count = 0; + mods_enabled = 0; + mods_required = 0; + mods_triggered = 0; + mods_recovering = 0; + old_mode_switch = 0; + _channel = 0; + _state = LIMITS_INIT; +} + +void AP_Limits::modules(AP_Limit_Module *m) +{ + _modules_head = m; +} + +bool AP_Limits::init() { + AP_Limit_Module *m = modules_first(); + + while (m) { + m->init(); + m = modules_next(); + } + return true; +} + + +bool AP_Limits::enabled() { + return _enabled; +} + +bool AP_Limits::debug() { + return _debug; +} + + +AP_Limit_Module *AP_Limits::modules_first() { + _modules_current = _modules_head; // reset current to head of list + return _modules_head; +} + +AP_Limit_Module *AP_Limits::modules_current() { + return _modules_current; +} + +AP_Limit_Module *AP_Limits::modules_next() { + if (_modules_current) { + _modules_current = _modules_current->next(); // move to "next" (which might be NULL) + } + return _modules_current; +} + +uint8_t AP_Limits::modules_count() { + _modules_count = 0; + AP_Limit_Module *m = _modules_head; + + while (m) { + _modules_count++; + m = m->next(); + } + return _modules_count; +} + +AP_Limit_Module *AP_Limits::modules_last() { + AP_Limit_Module *m = _modules_head; + while (m && m->next()) { + m = m->next(); + } + return m; +} + +void AP_Limits::modules_add(AP_Limit_Module *m) { + if (_modules_head) { // if there is a module linked + modules_last()->link(m); // add to the end of the list + } + else { + _modules_head = m; // otherwise, this will be the "head" + } +} + +bool AP_Limits::required() { + return _required; +} + +bool AP_Limits::check_all() { + return (bool) check_triggered(false); // required=false, means "all" +} + +bool AP_Limits::check_required() { + return (bool) check_triggered(true); // required=true, means "only required modules" +} + +bool AP_Limits::check_triggered(bool only_required) { + + // check all enabled modules for triggered limits + AP_Limit_Module *mod = _modules_head; + + // reset bit fields + mods_triggered = 0; + mods_enabled = 0; + mods_required = 0; + + while (mod) { + + unsigned module_id = mod->get_module_id(); + + // We check enabled, triggered and required across all modules + // We set all the bit-fields each time we check, keeping them up to date + + if (mod->enabled()) { + mods_enabled |= module_id; + + if (mod->required()) mods_required |= module_id; + if (mod->triggered()) mods_triggered |= module_id; + } + + mod = mod->next(); + } + + if (only_required) { + return (bool) (mods_required & mods_triggered); // just modules that are both required AND triggered. (binary AND) + } + else return (bool) (mods_triggered); +} + +AP_Int8 AP_Limits::state() { + return _state; +} + +AP_Int8 AP_Limits::safetime() { + return _safetime; +} + +void AP_Limits::set_state(int s) { + _state = (int) s; +} + + +AP_Int8 AP_Limits::channel() { + return _channel; +} diff --git a/libraries/AP_Limits/AP_Limits.h b/libraries/AP_Limits/AP_Limits.h new file mode 100644 index 0000000000..8336cd66b1 --- /dev/null +++ b/libraries/AP_Limits/AP_Limits.h @@ -0,0 +1,103 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @brief Imposes limits on location (geofence), altitude and other parameters. +/// Each breach will trigger an action or set of actions to recover. Adapted from geofence. +/// @author Andrew Tridgell +/// Andreas Antonopoulos + +#ifndef AP_LIMITS_H +#define AP_LIMITS_H + +#ifndef AP_LIMITS +#define AP_LIMITS ENABLED +#endif + +#include +#include +#include + +// MAVLink messages, trying to pull into library +//#include "../GCS_MAVLink/include/mavlink/v1.0/protocol.h" +//#include "../GCS_MAVLink/include/mavlink/v1.0/mavlink_types.h" +//#include "../GCS_MAVLink/include/mavlink/v1.0/common/mavlink.h" +//#include "../GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/mavlink_msg_limits_status.h" + + + +#ifndef HAVE_ENUM_LIMITS_STATE +#define HAVE_ENUM_LIMITS_STATE +enum LimitState { + LIMITS_INIT, // pre-initialization + LIMITS_DISABLED, // disabled + LIMITS_ENABLED, // checking limits + LIMITS_TRIGGERED, // a limit has been breached + LIMITS_RECOVERING, // taking action, eg. RTL + LIMITS_RECOVERED, // we're no longer in breach of a limit +}; +#endif + +class AP_Limits { + +public: + + AP_Limits(); + + // access functions + bool enabled(); + bool debug(); + bool required(); + AP_Int8 state(); + AP_Int8 safetime(); + AP_Int8 channel(); + + // module linked list management methods + void modules(AP_Limit_Module *m); // pointer to the first module in linked list of modules + AP_Limit_Module *modules_first(); + AP_Limit_Module *modules_current(); + AP_Limit_Module *modules_last(); + AP_Limit_Module *modules_next(); + void modules_add(AP_Limit_Module *m); + uint8_t modules_count(); + + // main limit methods + bool init(); // initialize self and all modules + void set_state(int s); // change state + bool check_all(); // check if any limit is triggered + bool check_required(); // check if any of the required modules is triggered + bool check_triggered(bool required); // the function that does the checking for the two above + + uint32_t last_trigger; // time of last limit breach (trigger) + uint32_t last_action; // time of last recovery action taken + uint32_t last_recovery; // time of last recovery success + uint32_t last_status_update; // time of last recovery success + uint32_t last_clear; // last time all triggers were clear (or 0 if never clear) + + uint16_t breach_count; // count of total breaches + + byte old_mode_switch; + + LimitModuleBits mods_enabled; + LimitModuleBits mods_required; + LimitModuleBits mods_triggered; + LimitModuleBits mods_recovering; + + + static const struct AP_Param::GroupInfo var_info[]; // module parameters + +protected: + AP_Int8 _enabled; // the entire AP_Limits system on/off switch + AP_Int8 _required; // master switch for pre-arm checks of limits. Will not allow vehicle to "arm" if breaching limits. + AP_Int8 _debug; // enable debug console messages + AP_Int8 _safetime; // how long after recovered before switching to stabilize + AP_Int8 _state; // overall state - used in the main loop state machine inside the vehicle's slow loop. + AP_Int8 _channel; // channel used for switching limits on/off + +private: + AP_Limit_Module *_modules_head; // points to linked list of modules + AP_Limit_Module *_modules_current; // points to linked list of modules + uint8_t _modules_count; + +}; + + +#endif // AP_LIMITS_H diff --git a/libraries/GCS_MAVLink/include/mavlink/config.h b/libraries/GCS_MAVLink/include/mavlink/config.h deleted file mode 100644 index db7db0d7d6..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/config.h +++ /dev/null @@ -1 +0,0 @@ -#define MAVLINK_VERSION "1.0.7" diff --git a/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/ardupilotmega.h b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/ardupilotmega.h index 311858ffd8..16102550c7 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/ardupilotmega.h +++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/ardupilotmega.h @@ -12,15 +12,15 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 36, 3, 9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} +#define MAVLINK_MESSAGE_LENGTHS {3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 3, 2, 2, 0, 0, 0, 0, 0, 0, 0, 19, 2, 23, 21, 0, 37, 26, 101, 26, 16, 32, 32, 37, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 24, 24, 0, 0, 0, 26, 16, 36, 5, 6, 56, 26, 21, 18, 0, 0, 18, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 18, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 8, 4, 12, 15, 13, 6, 15, 14, 0, 12, 3, 8, 28, 36, 3, 9, 0, 0, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 30, 14, 14, 51, 5} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 127, 42, 21, 93, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} +#define MAVLINK_MESSAGE_CRCS {72, 39, 190, 92, 191, 217, 104, 119, 0, 219, 60, 186, 10, 0, 0, 0, 0, 0, 0, 0, 89, 159, 162, 121, 0, 149, 222, 110, 179, 136, 66, 126, 185, 147, 112, 252, 162, 215, 229, 128, 9, 106, 101, 213, 4, 229, 21, 214, 215, 14, 206, 50, 157, 126, 108, 213, 95, 5, 127, 0, 0, 0, 57, 126, 130, 119, 193, 191, 236, 158, 143, 0, 0, 104, 123, 131, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 174, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155, 0, 0, 0, 0, 0, 0, 0, 0, 0, 143, 29, 208, 188, 118, 242, 19, 97, 233, 0, 18, 68, 136, 127, 42, 21, 93, 0, 0, 45, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178, 224, 60, 106, 7} #endif #ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} +#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_BOOT, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_SYSTEM_TIME_UTC, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ACTION_ACK, MAVLINK_MESSAGE_INFO_ACTION, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_SET_NAV_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_LOCAL_POSITION, MAVLINK_MESSAGE_INFO_GPS_RAW, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_WAYPOINT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST, MAVLINK_MESSAGE_INFO_WAYPOINT_SET_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_CURRENT, MAVLINK_MESSAGE_INFO_WAYPOINT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_WAYPOINT_COUNT, MAVLINK_MESSAGE_INFO_WAYPOINT_CLEAR_ALL, MAVLINK_MESSAGE_INFO_WAYPOINT_REACHED, MAVLINK_MESSAGE_INFO_WAYPOINT_ACK, MAVLINK_MESSAGE_INFO_GPS_SET_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_LOCAL_ORIGIN_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT_SET, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_CONTROL_STATUS, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_POSITION_TARGET, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, MAVLINK_MESSAGE_INFO_SET_ALTITUDE, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBJECT_DETECTION_EVENT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SENSOR_OFFSETS, MAVLINK_MESSAGE_INFO_SET_MAG_OFFSETS, MAVLINK_MESSAGE_INFO_MEMINFO, MAVLINK_MESSAGE_INFO_AP_ADC, MAVLINK_MESSAGE_INFO_DIGICAM_CONFIGURE, MAVLINK_MESSAGE_INFO_DIGICAM_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_CONFIGURE, MAVLINK_MESSAGE_INFO_MOUNT_CONTROL, MAVLINK_MESSAGE_INFO_MOUNT_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_POINT, MAVLINK_MESSAGE_INFO_FENCE_FETCH_POINT, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_AHRS, MAVLINK_MESSAGE_INFO_SIMSTATE, MAVLINK_MESSAGE_INFO_HWSTATUS, MAVLINK_MESSAGE_INFO_RADIO, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LIMITS_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG} #endif #include "../protocol.h" @@ -117,6 +117,33 @@ enum FENCE_BREACH }; #endif +/** @brief */ +#ifndef HAVE_ENUM_LIMITS_STATE +#define HAVE_ENUM_LIMITS_STATE +enum LIMITS_STATE +{ + LIMITS_INIT=0, /* pre-initialization | */ + LIMITS_DISABLED=1, /* disabled | */ + LIMITS_ENABLED=2, /* checking limits | */ + LIMITS_TRIGGERED=3, /* a limit has been breached | */ + LIMITS_RECOVERING=4, /* taking action eg. RTL | */ + LIMITS_RECOVERED=5, /* we're no longer in breach of a limit | */ + LIMITS_STATE_ENUM_END=6, /* | */ +}; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_LIMIT_MODULE +#define HAVE_ENUM_LIMIT_MODULE +enum LIMIT_MODULE +{ + LIMIT_GPSLOCK=1, /* pre-initialization | */ + LIMIT_GEOFENCE=2, /* disabled | */ + LIMIT_ALTITUDE=4, /* checking limits | */ + LIMIT_MODULE_ENUM_END=5, /* | */ +}; +#endif + #include "../common/common.h" // MAVLINK VERSION @@ -147,6 +174,7 @@ enum FENCE_BREACH #include "./mavlink_msg_simstate.h" #include "./mavlink_msg_hwstatus.h" #include "./mavlink_msg_radio.h" +#include "./mavlink_msg_limits_status.h" #ifdef __cplusplus } diff --git a/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_limits_status.h b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_limits_status.h new file mode 100644 index 0000000000..ae769c0ad0 --- /dev/null +++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/mavlink_msg_limits_status.h @@ -0,0 +1,320 @@ +// MESSAGE LIMITS_STATUS PACKING + +#define MAVLINK_MSG_ID_LIMITS_STATUS 169 + +typedef struct __mavlink_limits_status_t +{ + uint8_t limits_state; ///< state of AP_Limits, (see enum LimitState, LIMITS_STATE) + uint32_t last_trigger; ///< time of last breach in milliseconds since boot + uint32_t last_action; ///< time of last recovery action in milliseconds since boot + uint32_t last_recovery; ///< time of last successful recovery in milliseconds since boot + uint32_t last_clear; ///< time of last all-clear in milliseconds since boot + uint16_t breach_count; ///< number of fence breaches + uint8_t mods_enabled; ///< AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + uint8_t mods_required; ///< AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + uint8_t mods_triggered; ///< AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) +} mavlink_limits_status_t; + +#define MAVLINK_MSG_ID_LIMITS_STATUS_LEN 22 +#define MAVLINK_MSG_ID_169_LEN 22 + + + +#define MAVLINK_MESSAGE_INFO_LIMITS_STATUS { \ + "LIMITS_STATUS", \ + 9, \ + { { "limits_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_limits_status_t, limits_state) }, \ + { "last_trigger", NULL, MAVLINK_TYPE_UINT32_T, 0, 1, offsetof(mavlink_limits_status_t, last_trigger) }, \ + { "last_action", NULL, MAVLINK_TYPE_UINT32_T, 0, 5, offsetof(mavlink_limits_status_t, last_action) }, \ + { "last_recovery", NULL, MAVLINK_TYPE_UINT32_T, 0, 9, offsetof(mavlink_limits_status_t, last_recovery) }, \ + { "last_clear", NULL, MAVLINK_TYPE_UINT32_T, 0, 13, offsetof(mavlink_limits_status_t, last_clear) }, \ + { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 17, offsetof(mavlink_limits_status_t, breach_count) }, \ + { "mods_enabled", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_limits_status_t, mods_enabled) }, \ + { "mods_required", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_limits_status_t, mods_required) }, \ + { "mods_triggered", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_limits_status_t, mods_triggered) }, \ + } \ +} + + +/** + * @brief Pack a limits_status 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 limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) + * @param last_trigger time of last breach in milliseconds since boot + * @param last_action time of last recovery action in milliseconds since boot + * @param last_recovery time of last successful recovery in milliseconds since boot + * @param last_clear time of last all-clear in milliseconds since boot + * @param breach_count number of fence breaches + * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_limits_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint8_t(buf, 0, limits_state); + _mav_put_uint32_t(buf, 1, last_trigger); + _mav_put_uint32_t(buf, 5, last_action); + _mav_put_uint32_t(buf, 9, last_recovery); + _mav_put_uint32_t(buf, 13, last_clear); + _mav_put_uint16_t(buf, 17, breach_count); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22); +#else + mavlink_limits_status_t packet; + packet.limits_state = limits_state; + packet.last_trigger = last_trigger; + packet.last_action = last_action; + packet.last_recovery = last_recovery; + packet.last_clear = last_clear; + packet.breach_count = breach_count; + packet.mods_enabled = mods_enabled; + packet.mods_required = mods_required; + packet.mods_triggered = mods_triggered; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22); +#endif + + msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, 22); +} + +/** + * @brief Pack a limits_status message on a channel + * @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 limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) + * @param last_trigger time of last breach in milliseconds since boot + * @param last_action time of last recovery action in milliseconds since boot + * @param last_recovery time of last successful recovery in milliseconds since boot + * @param last_clear time of last all-clear in milliseconds since boot + * @param breach_count number of fence breaches + * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t limits_state,uint32_t last_trigger,uint32_t last_action,uint32_t last_recovery,uint32_t last_clear,uint16_t breach_count,uint8_t mods_enabled,uint8_t mods_required,uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint8_t(buf, 0, limits_state); + _mav_put_uint32_t(buf, 1, last_trigger); + _mav_put_uint32_t(buf, 5, last_action); + _mav_put_uint32_t(buf, 9, last_recovery); + _mav_put_uint32_t(buf, 13, last_clear); + _mav_put_uint16_t(buf, 17, breach_count); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22); +#else + mavlink_limits_status_t packet; + packet.limits_state = limits_state; + packet.last_trigger = last_trigger; + packet.last_action = last_action; + packet.last_recovery = last_recovery; + packet.last_clear = last_clear; + packet.breach_count = breach_count; + packet.mods_enabled = mods_enabled; + packet.mods_required = mods_required; + packet.mods_triggered = mods_triggered; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22); +#endif + + msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22); +} + +/** + * @brief Encode a limits_status 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 limits_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) +{ + return mavlink_msg_limits_status_pack(system_id, component_id, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); +} + +/** + * @brief Send a limits_status message + * @param chan MAVLink channel to send the message + * + * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) + * @param last_trigger time of last breach in milliseconds since boot + * @param last_action time of last recovery action in milliseconds since boot + * @param last_recovery time of last successful recovery in milliseconds since boot + * @param last_clear time of last all-clear in milliseconds since boot + * @param breach_count number of fence breaches + * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint8_t(buf, 0, limits_state); + _mav_put_uint32_t(buf, 1, last_trigger); + _mav_put_uint32_t(buf, 5, last_action); + _mav_put_uint32_t(buf, 9, last_recovery); + _mav_put_uint32_t(buf, 13, last_clear); + _mav_put_uint16_t(buf, 17, breach_count); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, 22); +#else + mavlink_limits_status_t packet; + packet.limits_state = limits_state; + packet.last_trigger = last_trigger; + packet.last_action = last_action; + packet.last_recovery = last_recovery; + packet.last_clear = last_clear; + packet.breach_count = breach_count; + packet.mods_enabled = mods_enabled; + packet.mods_required = mods_required; + packet.mods_triggered = mods_triggered; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, 22); +#endif +} + +#endif + +// MESSAGE LIMITS_STATUS UNPACKING + + +/** + * @brief Get field limits_state from limits_status message + * + * @return state of AP_Limits, (see enum LimitState, LIMITS_STATE) + */ +static inline uint8_t mavlink_msg_limits_status_get_limits_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field last_trigger from limits_status message + * + * @return time of last breach in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_trigger(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 1); +} + +/** + * @brief Get field last_action from limits_status message + * + * @return time of last recovery action in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_action(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 5); +} + +/** + * @brief Get field last_recovery from limits_status message + * + * @return time of last successful recovery in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_recovery(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 9); +} + +/** + * @brief Get field last_clear from limits_status message + * + * @return time of last all-clear in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_clear(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 13); +} + +/** + * @brief Get field breach_count from limits_status message + * + * @return number of fence breaches + */ +static inline uint16_t mavlink_msg_limits_status_get_breach_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 17); +} + +/** + * @brief Get field mods_enabled from limits_status message + * + * @return AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + */ +static inline uint8_t mavlink_msg_limits_status_get_mods_enabled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 19); +} + +/** + * @brief Get field mods_required from limits_status message + * + * @return AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + */ +static inline uint8_t mavlink_msg_limits_status_get_mods_required(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field mods_triggered from limits_status message + * + * @return AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + */ +static inline uint8_t mavlink_msg_limits_status_get_mods_triggered(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Decode a limits_status message into a struct + * + * @param msg The message to decode + * @param limits_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_limits_status_decode(const mavlink_message_t* msg, mavlink_limits_status_t* limits_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + limits_status->limits_state = mavlink_msg_limits_status_get_limits_state(msg); + limits_status->last_trigger = mavlink_msg_limits_status_get_last_trigger(msg); + limits_status->last_action = mavlink_msg_limits_status_get_last_action(msg); + limits_status->last_recovery = mavlink_msg_limits_status_get_last_recovery(msg); + limits_status->last_clear = mavlink_msg_limits_status_get_last_clear(msg); + limits_status->breach_count = mavlink_msg_limits_status_get_breach_count(msg); + limits_status->mods_enabled = mavlink_msg_limits_status_get_mods_enabled(msg); + limits_status->mods_required = mavlink_msg_limits_status_get_mods_required(msg); + limits_status->mods_triggered = mavlink_msg_limits_status_get_mods_triggered(msg); +#else + memcpy(limits_status, _MAV_PAYLOAD(msg), 22); +#endif +} diff --git a/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/testsuite.h index a3ac5476ca..46c6fe9079 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/mavlink/v0.9/ardupilotmega/testsuite.h @@ -882,6 +882,65 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_limits_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_limits_status_t packet_in = { + 5, + 963497516, + 963497724, + 963497932, + 963498140, + 18119, + 254, + 65, + 132, + }; + mavlink_limits_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.limits_state = packet_in.limits_state; + packet1.last_trigger = packet_in.last_trigger; + packet1.last_action = packet_in.last_action; + packet1.last_recovery = packet_in.last_recovery; + packet1.last_clear = packet_in.last_clear; + packet1.breach_count = packet_in.breach_count; + packet1.mods_enabled = packet_in.mods_enabled; + packet1.mods_required = packet_in.mods_required; + packet1.mods_triggered = packet_in.mods_triggered; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_limits_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_limits_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_limits_status_pack(system_id, component_id, &msg , packet1.limits_state , packet1.last_trigger , packet1.last_action , packet1.last_recovery , packet1.last_clear , packet1.breach_count , packet1.mods_enabled , packet1.mods_required , packet1.mods_triggered ); + mavlink_msg_limits_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_limits_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.limits_state , packet1.last_trigger , packet1.last_action , packet1.last_recovery , packet1.last_clear , packet1.breach_count , packet1.mods_enabled , packet1.mods_required , packet1.mods_triggered ); + mavlink_msg_limits_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; imsgid = MAVLINK_MSG_ID_LIMITS_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, 22, 144); +} + +/** + * @brief Pack a limits_status message on a channel + * @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 limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) + * @param last_trigger time of last breach in milliseconds since boot + * @param last_action time of last recovery action in milliseconds since boot + * @param last_recovery time of last successful recovery in milliseconds since boot + * @param last_clear time of last all-clear in milliseconds since boot + * @param breach_count number of fence breaches + * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_limits_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t limits_state,uint32_t last_trigger,uint32_t last_action,uint32_t last_recovery,uint32_t last_clear,uint16_t breach_count,uint8_t mods_enabled,uint8_t mods_required,uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, last_trigger); + _mav_put_uint32_t(buf, 4, last_action); + _mav_put_uint32_t(buf, 8, last_recovery); + _mav_put_uint32_t(buf, 12, last_clear); + _mav_put_uint16_t(buf, 16, breach_count); + _mav_put_uint8_t(buf, 18, limits_state); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 22); +#else + mavlink_limits_status_t packet; + packet.last_trigger = last_trigger; + packet.last_action = last_action; + packet.last_recovery = last_recovery; + packet.last_clear = last_clear; + packet.breach_count = breach_count; + packet.limits_state = limits_state; + packet.mods_enabled = mods_enabled; + packet.mods_required = mods_required; + packet.mods_triggered = mods_triggered; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 22); +#endif + + msg->msgid = MAVLINK_MSG_ID_LIMITS_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 22, 144); +} + +/** + * @brief Encode a limits_status 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 limits_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_limits_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_limits_status_t* limits_status) +{ + return mavlink_msg_limits_status_pack(system_id, component_id, msg, limits_status->limits_state, limits_status->last_trigger, limits_status->last_action, limits_status->last_recovery, limits_status->last_clear, limits_status->breach_count, limits_status->mods_enabled, limits_status->mods_required, limits_status->mods_triggered); +} + +/** + * @brief Send a limits_status message + * @param chan MAVLink channel to send the message + * + * @param limits_state state of AP_Limits, (see enum LimitState, LIMITS_STATE) + * @param last_trigger time of last breach in milliseconds since boot + * @param last_action time of last recovery action in milliseconds since boot + * @param last_recovery time of last successful recovery in milliseconds since boot + * @param last_clear time of last all-clear in milliseconds since boot + * @param breach_count number of fence breaches + * @param mods_enabled AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_required AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + * @param mods_triggered AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_limits_status_send(mavlink_channel_t chan, uint8_t limits_state, uint32_t last_trigger, uint32_t last_action, uint32_t last_recovery, uint32_t last_clear, uint16_t breach_count, uint8_t mods_enabled, uint8_t mods_required, uint8_t mods_triggered) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[22]; + _mav_put_uint32_t(buf, 0, last_trigger); + _mav_put_uint32_t(buf, 4, last_action); + _mav_put_uint32_t(buf, 8, last_recovery); + _mav_put_uint32_t(buf, 12, last_clear); + _mav_put_uint16_t(buf, 16, breach_count); + _mav_put_uint8_t(buf, 18, limits_state); + _mav_put_uint8_t(buf, 19, mods_enabled); + _mav_put_uint8_t(buf, 20, mods_required); + _mav_put_uint8_t(buf, 21, mods_triggered); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, buf, 22, 144); +#else + mavlink_limits_status_t packet; + packet.last_trigger = last_trigger; + packet.last_action = last_action; + packet.last_recovery = last_recovery; + packet.last_clear = last_clear; + packet.breach_count = breach_count; + packet.limits_state = limits_state; + packet.mods_enabled = mods_enabled; + packet.mods_required = mods_required; + packet.mods_triggered = mods_triggered; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LIMITS_STATUS, (const char *)&packet, 22, 144); +#endif +} + +#endif + +// MESSAGE LIMITS_STATUS UNPACKING + + +/** + * @brief Get field limits_state from limits_status message + * + * @return state of AP_Limits, (see enum LimitState, LIMITS_STATE) + */ +static inline uint8_t mavlink_msg_limits_status_get_limits_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Get field last_trigger from limits_status message + * + * @return time of last breach in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_trigger(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field last_action from limits_status message + * + * @return time of last recovery action in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_action(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field last_recovery from limits_status message + * + * @return time of last successful recovery in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_recovery(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field last_clear from limits_status message + * + * @return time of last all-clear in milliseconds since boot + */ +static inline uint32_t mavlink_msg_limits_status_get_last_clear(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Get field breach_count from limits_status message + * + * @return number of fence breaches + */ +static inline uint16_t mavlink_msg_limits_status_get_breach_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field mods_enabled from limits_status message + * + * @return AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + */ +static inline uint8_t mavlink_msg_limits_status_get_mods_enabled(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 19); +} + +/** + * @brief Get field mods_required from limits_status message + * + * @return AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + */ +static inline uint8_t mavlink_msg_limits_status_get_mods_required(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field mods_triggered from limits_status message + * + * @return AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + */ +static inline uint8_t mavlink_msg_limits_status_get_mods_triggered(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Decode a limits_status message into a struct + * + * @param msg The message to decode + * @param limits_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_limits_status_decode(const mavlink_message_t* msg, mavlink_limits_status_t* limits_status) +{ +#if MAVLINK_NEED_BYTE_SWAP + limits_status->last_trigger = mavlink_msg_limits_status_get_last_trigger(msg); + limits_status->last_action = mavlink_msg_limits_status_get_last_action(msg); + limits_status->last_recovery = mavlink_msg_limits_status_get_last_recovery(msg); + limits_status->last_clear = mavlink_msg_limits_status_get_last_clear(msg); + limits_status->breach_count = mavlink_msg_limits_status_get_breach_count(msg); + limits_status->limits_state = mavlink_msg_limits_status_get_limits_state(msg); + limits_status->mods_enabled = mavlink_msg_limits_status_get_mods_enabled(msg); + limits_status->mods_required = mavlink_msg_limits_status_get_mods_required(msg); + limits_status->mods_triggered = mavlink_msg_limits_status_get_mods_triggered(msg); +#else + memcpy(limits_status, _MAV_PAYLOAD(msg), 22); +#endif +} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h index aa83889d66..08221a87c2 100644 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h +++ b/libraries/GCS_MAVLink/include/mavlink/v1.0/ardupilotmega/testsuite.h @@ -882,6 +882,65 @@ static void mavlink_test_radio(uint8_t system_id, uint8_t component_id, mavlink_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_limits_status(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_limits_status_t packet_in = { + 963497464, + 963497672, + 963497880, + 963498088, + 18067, + 187, + 254, + 65, + 132, + }; + mavlink_limits_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.last_trigger = packet_in.last_trigger; + packet1.last_action = packet_in.last_action; + packet1.last_recovery = packet_in.last_recovery; + packet1.last_clear = packet_in.last_clear; + packet1.breach_count = packet_in.breach_count; + packet1.limits_state = packet_in.limits_state; + packet1.mods_enabled = packet_in.mods_enabled; + packet1.mods_required = packet_in.mods_required; + packet1.mods_triggered = packet_in.mods_triggered; + + + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_limits_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_limits_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_limits_status_pack(system_id, component_id, &msg , packet1.limits_state , packet1.last_trigger , packet1.last_action , packet1.last_recovery , packet1.last_clear , packet1.breach_count , packet1.mods_enabled , packet1.mods_required , packet1.mods_triggered ); + mavlink_msg_limits_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_limits_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.limits_state , packet1.last_trigger , packet1.last_action , packet1.last_recovery , packet1.last_clear , packet1.breach_count , packet1.mods_enabled , packet1.mods_required , packet1.mods_triggered ); + mavlink_msg_limits_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; imsgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK; - return mavlink_finalize_message(msg, system_id, component_id, 5, 243); -} - -/** - * @brief Pack a cmd_airspeed_ack message on a channel - * @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 spCmd commanded airspeed - * @param ack 0:ack, 1:nack - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cmd_airspeed_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float spCmd,uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[5]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, ack); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); -#else - mavlink_cmd_airspeed_ack_t packet; - packet.spCmd = spCmd; - packet.ack = ack; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); -#endif - - msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_ACK; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5, 243); -} - -/** - * @brief Encode a cmd_airspeed_ack 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 cmd_airspeed_ack C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cmd_airspeed_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) -{ - return mavlink_msg_cmd_airspeed_ack_pack(system_id, component_id, msg, cmd_airspeed_ack->spCmd, cmd_airspeed_ack->ack); -} - -/** - * @brief Send a cmd_airspeed_ack message - * @param chan MAVLink channel to send the message - * - * @param spCmd commanded airspeed - * @param ack 0:ack, 1:nack - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_cmd_airspeed_ack_send(mavlink_channel_t chan, float spCmd, uint8_t ack) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[5]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, ack); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, buf, 5, 243); -#else - mavlink_cmd_airspeed_ack_t packet; - packet.spCmd = spCmd; - packet.ack = ack; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_ACK, (const char *)&packet, 5, 243); -#endif -} - -#endif - -// MESSAGE CMD_AIRSPEED_ACK UNPACKING - - -/** - * @brief Get field spCmd from cmd_airspeed_ack message - * - * @return commanded airspeed - */ -static inline float mavlink_msg_cmd_airspeed_ack_get_spCmd(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field ack from cmd_airspeed_ack message - * - * @return 0:ack, 1:nack - */ -static inline uint8_t mavlink_msg_cmd_airspeed_ack_get_ack(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Decode a cmd_airspeed_ack message into a struct - * - * @param msg The message to decode - * @param cmd_airspeed_ack C-struct to decode the message contents into - */ -static inline void mavlink_msg_cmd_airspeed_ack_decode(const mavlink_message_t* msg, mavlink_cmd_airspeed_ack_t* cmd_airspeed_ack) -{ -#if MAVLINK_NEED_BYTE_SWAP - cmd_airspeed_ack->spCmd = mavlink_msg_cmd_airspeed_ack_get_spCmd(msg); - cmd_airspeed_ack->ack = mavlink_msg_cmd_airspeed_ack_get_ack(msg); -#else - memcpy(cmd_airspeed_ack, _MAV_PAYLOAD(msg), 5); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h deleted file mode 100644 index d5b21b8d79..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_cmd_airspeed_chng.h +++ /dev/null @@ -1,166 +0,0 @@ -// MESSAGE CMD_AIRSPEED_CHNG PACKING - -#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG 192 - -typedef struct __mavlink_cmd_airspeed_chng_t -{ - float spCmd; ///< commanded airspeed - uint8_t target; ///< Target ID -} mavlink_cmd_airspeed_chng_t; - -#define MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG_LEN 5 -#define MAVLINK_MSG_ID_192_LEN 5 - - - -#define MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG { \ - "CMD_AIRSPEED_CHNG", \ - 2, \ - { { "spCmd", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_cmd_airspeed_chng_t, spCmd) }, \ - { "target", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_cmd_airspeed_chng_t, target) }, \ - } \ -} - - -/** - * @brief Pack a cmd_airspeed_chng 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 Target ID - * @param spCmd commanded airspeed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target, float spCmd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[5]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, target); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); -#else - mavlink_cmd_airspeed_chng_t packet; - packet.spCmd = spCmd; - packet.target = target; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); -#endif - - msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG; - return mavlink_finalize_message(msg, system_id, component_id, 5, 209); -} - -/** - * @brief Pack a cmd_airspeed_chng message on a channel - * @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 Target ID - * @param spCmd commanded airspeed - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t target,float spCmd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[5]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, target); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 5); -#else - mavlink_cmd_airspeed_chng_t packet; - packet.spCmd = spCmd; - packet.target = target; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 5); -#endif - - msg->msgid = MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 5, 209); -} - -/** - * @brief Encode a cmd_airspeed_chng 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 cmd_airspeed_chng C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_cmd_airspeed_chng_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) -{ - return mavlink_msg_cmd_airspeed_chng_pack(system_id, component_id, msg, cmd_airspeed_chng->target, cmd_airspeed_chng->spCmd); -} - -/** - * @brief Send a cmd_airspeed_chng message - * @param chan MAVLink channel to send the message - * - * @param target Target ID - * @param spCmd commanded airspeed - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_cmd_airspeed_chng_send(mavlink_channel_t chan, uint8_t target, float spCmd) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[5]; - _mav_put_float(buf, 0, spCmd); - _mav_put_uint8_t(buf, 4, target); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, buf, 5, 209); -#else - mavlink_cmd_airspeed_chng_t packet; - packet.spCmd = spCmd; - packet.target = target; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CMD_AIRSPEED_CHNG, (const char *)&packet, 5, 209); -#endif -} - -#endif - -// MESSAGE CMD_AIRSPEED_CHNG UNPACKING - - -/** - * @brief Get field target from cmd_airspeed_chng message - * - * @return Target ID - */ -static inline uint8_t mavlink_msg_cmd_airspeed_chng_get_target(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 4); -} - -/** - * @brief Get field spCmd from cmd_airspeed_chng message - * - * @return commanded airspeed - */ -static inline float mavlink_msg_cmd_airspeed_chng_get_spCmd(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a cmd_airspeed_chng message into a struct - * - * @param msg The message to decode - * @param cmd_airspeed_chng C-struct to decode the message contents into - */ -static inline void mavlink_msg_cmd_airspeed_chng_decode(const mavlink_message_t* msg, mavlink_cmd_airspeed_chng_t* cmd_airspeed_chng) -{ -#if MAVLINK_NEED_BYTE_SWAP - cmd_airspeed_chng->spCmd = mavlink_msg_cmd_airspeed_chng_get_spCmd(msg); - cmd_airspeed_chng->target = mavlink_msg_cmd_airspeed_chng_get_target(msg); -#else - memcpy(cmd_airspeed_chng, _MAV_PAYLOAD(msg), 5); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h deleted file mode 100644 index f5fef06f04..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_filt_rot_vel.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE FILT_ROT_VEL PACKING - -#define MAVLINK_MSG_ID_FILT_ROT_VEL 184 - -typedef struct __mavlink_filt_rot_vel_t -{ - float rotVel[3]; ///< rotational velocity -} mavlink_filt_rot_vel_t; - -#define MAVLINK_MSG_ID_FILT_ROT_VEL_LEN 12 -#define MAVLINK_MSG_ID_184_LEN 12 - -#define MAVLINK_MSG_FILT_ROT_VEL_FIELD_ROTVEL_LEN 3 - -#define MAVLINK_MESSAGE_INFO_FILT_ROT_VEL { \ - "FILT_ROT_VEL", \ - 1, \ - { { "rotVel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_filt_rot_vel_t, rotVel) }, \ - } \ -} - - -/** - * @brief Pack a filt_rot_vel 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 rotVel rotational velocity - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_filt_rot_vel_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *rotVel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_float_array(buf, 0, rotVel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_filt_rot_vel_t packet; - - mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL; - return mavlink_finalize_message(msg, system_id, component_id, 12, 79); -} - -/** - * @brief Pack a filt_rot_vel message on a channel - * @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 rotVel rotational velocity - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_filt_rot_vel_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *rotVel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_float_array(buf, 0, rotVel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_filt_rot_vel_t packet; - - mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_FILT_ROT_VEL; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 79); -} - -/** - * @brief Encode a filt_rot_vel 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 filt_rot_vel C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_filt_rot_vel_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_filt_rot_vel_t* filt_rot_vel) -{ - return mavlink_msg_filt_rot_vel_pack(system_id, component_id, msg, filt_rot_vel->rotVel); -} - -/** - * @brief Send a filt_rot_vel message - * @param chan MAVLink channel to send the message - * - * @param rotVel rotational velocity - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_filt_rot_vel_send(mavlink_channel_t chan, const float *rotVel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_float_array(buf, 0, rotVel, 3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, buf, 12, 79); -#else - mavlink_filt_rot_vel_t packet; - - mav_array_memcpy(packet.rotVel, rotVel, sizeof(float)*3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FILT_ROT_VEL, (const char *)&packet, 12, 79); -#endif -} - -#endif - -// MESSAGE FILT_ROT_VEL UNPACKING - - -/** - * @brief Get field rotVel from filt_rot_vel message - * - * @return rotational velocity - */ -static inline uint16_t mavlink_msg_filt_rot_vel_get_rotVel(const mavlink_message_t* msg, float *rotVel) -{ - return _MAV_RETURN_float_array(msg, rotVel, 3, 0); -} - -/** - * @brief Decode a filt_rot_vel message into a struct - * - * @param msg The message to decode - * @param filt_rot_vel C-struct to decode the message contents into - */ -static inline void mavlink_msg_filt_rot_vel_decode(const mavlink_message_t* msg, mavlink_filt_rot_vel_t* filt_rot_vel) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_filt_rot_vel_get_rotVel(msg, filt_rot_vel->rotVel); -#else - memcpy(filt_rot_vel, _MAV_PAYLOAD(msg), 12); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h deleted file mode 100644 index 5ee3d5a19f..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_llc_out.h +++ /dev/null @@ -1,167 +0,0 @@ -// MESSAGE LLC_OUT PACKING - -#define MAVLINK_MSG_ID_LLC_OUT 186 - -typedef struct __mavlink_llc_out_t -{ - int16_t servoOut[4]; ///< Servo signal - int16_t MotorOut[2]; ///< motor signal -} mavlink_llc_out_t; - -#define MAVLINK_MSG_ID_LLC_OUT_LEN 12 -#define MAVLINK_MSG_ID_186_LEN 12 - -#define MAVLINK_MSG_LLC_OUT_FIELD_SERVOOUT_LEN 4 -#define MAVLINK_MSG_LLC_OUT_FIELD_MOTOROUT_LEN 2 - -#define MAVLINK_MESSAGE_INFO_LLC_OUT { \ - "LLC_OUT", \ - 2, \ - { { "servoOut", NULL, MAVLINK_TYPE_INT16_T, 4, 0, offsetof(mavlink_llc_out_t, servoOut) }, \ - { "MotorOut", NULL, MAVLINK_TYPE_INT16_T, 2, 8, offsetof(mavlink_llc_out_t, MotorOut) }, \ - } \ -} - - -/** - * @brief Pack a llc_out 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 servoOut Servo signal - * @param MotorOut motor signal - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_llc_out_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const int16_t *servoOut, const int16_t *MotorOut) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_int16_t_array(buf, 0, servoOut, 4); - _mav_put_int16_t_array(buf, 8, MotorOut, 2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_llc_out_t packet; - - mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); - mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_LLC_OUT; - return mavlink_finalize_message(msg, system_id, component_id, 12, 5); -} - -/** - * @brief Pack a llc_out message on a channel - * @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 servoOut Servo signal - * @param MotorOut motor signal - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_llc_out_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const int16_t *servoOut,const int16_t *MotorOut) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_int16_t_array(buf, 0, servoOut, 4); - _mav_put_int16_t_array(buf, 8, MotorOut, 2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_llc_out_t packet; - - mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); - mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_LLC_OUT; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 5); -} - -/** - * @brief Encode a llc_out 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 llc_out C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_llc_out_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_llc_out_t* llc_out) -{ - return mavlink_msg_llc_out_pack(system_id, component_id, msg, llc_out->servoOut, llc_out->MotorOut); -} - -/** - * @brief Send a llc_out message - * @param chan MAVLink channel to send the message - * - * @param servoOut Servo signal - * @param MotorOut motor signal - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_llc_out_send(mavlink_channel_t chan, const int16_t *servoOut, const int16_t *MotorOut) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_int16_t_array(buf, 0, servoOut, 4); - _mav_put_int16_t_array(buf, 8, MotorOut, 2); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, buf, 12, 5); -#else - mavlink_llc_out_t packet; - - mav_array_memcpy(packet.servoOut, servoOut, sizeof(int16_t)*4); - mav_array_memcpy(packet.MotorOut, MotorOut, sizeof(int16_t)*2); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LLC_OUT, (const char *)&packet, 12, 5); -#endif -} - -#endif - -// MESSAGE LLC_OUT UNPACKING - - -/** - * @brief Get field servoOut from llc_out message - * - * @return Servo signal - */ -static inline uint16_t mavlink_msg_llc_out_get_servoOut(const mavlink_message_t* msg, int16_t *servoOut) -{ - return _MAV_RETURN_int16_t_array(msg, servoOut, 4, 0); -} - -/** - * @brief Get field MotorOut from llc_out message - * - * @return motor signal - */ -static inline uint16_t mavlink_msg_llc_out_get_MotorOut(const mavlink_message_t* msg, int16_t *MotorOut) -{ - return _MAV_RETURN_int16_t_array(msg, MotorOut, 2, 8); -} - -/** - * @brief Decode a llc_out message into a struct - * - * @param msg The message to decode - * @param llc_out C-struct to decode the message contents into - */ -static inline void mavlink_msg_llc_out_decode(const mavlink_message_t* msg, mavlink_llc_out_t* llc_out) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_llc_out_get_servoOut(msg, llc_out->servoOut); - mavlink_msg_llc_out_get_MotorOut(msg, llc_out->MotorOut); -#else - memcpy(llc_out, _MAV_PAYLOAD(msg), 12); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h deleted file mode 100644 index 60cf018454..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_temp.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE OBS_AIR_TEMP PACKING - -#define MAVLINK_MSG_ID_OBS_AIR_TEMP 183 - -typedef struct __mavlink_obs_air_temp_t -{ - float airT; ///< Air Temperatur -} mavlink_obs_air_temp_t; - -#define MAVLINK_MSG_ID_OBS_AIR_TEMP_LEN 4 -#define MAVLINK_MSG_ID_183_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP { \ - "OBS_AIR_TEMP", \ - 1, \ - { { "airT", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_temp_t, airT) }, \ - } \ -} - - -/** - * @brief Pack a obs_air_temp 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 airT Air Temperatur - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_temp_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float airT) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_float(buf, 0, airT); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); -#else - mavlink_obs_air_temp_t packet; - packet.airT = airT; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP; - return mavlink_finalize_message(msg, system_id, component_id, 4, 248); -} - -/** - * @brief Pack a obs_air_temp message on a channel - * @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 airT Air Temperatur - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_temp_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float airT) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_float(buf, 0, airT); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); -#else - mavlink_obs_air_temp_t packet; - packet.airT = airT; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_TEMP; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 248); -} - -/** - * @brief Encode a obs_air_temp 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 obs_air_temp C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_air_temp_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_temp_t* obs_air_temp) -{ - return mavlink_msg_obs_air_temp_pack(system_id, component_id, msg, obs_air_temp->airT); -} - -/** - * @brief Send a obs_air_temp message - * @param chan MAVLink channel to send the message - * - * @param airT Air Temperatur - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_air_temp_send(mavlink_channel_t chan, float airT) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_float(buf, 0, airT); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, buf, 4, 248); -#else - mavlink_obs_air_temp_t packet; - packet.airT = airT; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_TEMP, (const char *)&packet, 4, 248); -#endif -} - -#endif - -// MESSAGE OBS_AIR_TEMP UNPACKING - - -/** - * @brief Get field airT from obs_air_temp message - * - * @return Air Temperatur - */ -static inline float mavlink_msg_obs_air_temp_get_airT(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a obs_air_temp message into a struct - * - * @param msg The message to decode - * @param obs_air_temp C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_air_temp_decode(const mavlink_message_t* msg, mavlink_obs_air_temp_t* obs_air_temp) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_air_temp->airT = mavlink_msg_obs_air_temp_get_airT(msg); -#else - memcpy(obs_air_temp, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h deleted file mode 100644 index ef56c5c617..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_air_velocity.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE OBS_AIR_VELOCITY PACKING - -#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY 178 - -typedef struct __mavlink_obs_air_velocity_t -{ - float magnitude; ///< Air speed - float aoa; ///< angle of attack - float slip; ///< slip angle -} mavlink_obs_air_velocity_t; - -#define MAVLINK_MSG_ID_OBS_AIR_VELOCITY_LEN 12 -#define MAVLINK_MSG_ID_178_LEN 12 - - - -#define MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY { \ - "OBS_AIR_VELOCITY", \ - 3, \ - { { "magnitude", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_air_velocity_t, magnitude) }, \ - { "aoa", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_obs_air_velocity_t, aoa) }, \ - { "slip", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_obs_air_velocity_t, slip) }, \ - } \ -} - - -/** - * @brief Pack a obs_air_velocity 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 magnitude Air speed - * @param aoa angle of attack - * @param slip slip angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float magnitude, float aoa, float slip) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_float(buf, 0, magnitude); - _mav_put_float(buf, 4, aoa); - _mav_put_float(buf, 8, slip); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_obs_air_velocity_t packet; - packet.magnitude = magnitude; - packet.aoa = aoa; - packet.slip = slip; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY; - return mavlink_finalize_message(msg, system_id, component_id, 12, 32); -} - -/** - * @brief Pack a obs_air_velocity message on a channel - * @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 magnitude Air speed - * @param aoa angle of attack - * @param slip slip angle - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_air_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float magnitude,float aoa,float slip) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_float(buf, 0, magnitude); - _mav_put_float(buf, 4, aoa); - _mav_put_float(buf, 8, slip); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_obs_air_velocity_t packet; - packet.magnitude = magnitude; - packet.aoa = aoa; - packet.slip = slip; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_AIR_VELOCITY; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 32); -} - -/** - * @brief Encode a obs_air_velocity 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 obs_air_velocity C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_air_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_air_velocity_t* obs_air_velocity) -{ - return mavlink_msg_obs_air_velocity_pack(system_id, component_id, msg, obs_air_velocity->magnitude, obs_air_velocity->aoa, obs_air_velocity->slip); -} - -/** - * @brief Send a obs_air_velocity message - * @param chan MAVLink channel to send the message - * - * @param magnitude Air speed - * @param aoa angle of attack - * @param slip slip angle - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_air_velocity_send(mavlink_channel_t chan, float magnitude, float aoa, float slip) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_float(buf, 0, magnitude); - _mav_put_float(buf, 4, aoa); - _mav_put_float(buf, 8, slip); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, buf, 12, 32); -#else - mavlink_obs_air_velocity_t packet; - packet.magnitude = magnitude; - packet.aoa = aoa; - packet.slip = slip; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_AIR_VELOCITY, (const char *)&packet, 12, 32); -#endif -} - -#endif - -// MESSAGE OBS_AIR_VELOCITY UNPACKING - - -/** - * @brief Get field magnitude from obs_air_velocity message - * - * @return Air speed - */ -static inline float mavlink_msg_obs_air_velocity_get_magnitude(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field aoa from obs_air_velocity message - * - * @return angle of attack - */ -static inline float mavlink_msg_obs_air_velocity_get_aoa(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field slip from obs_air_velocity message - * - * @return slip angle - */ -static inline float mavlink_msg_obs_air_velocity_get_slip(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 8); -} - -/** - * @brief Decode a obs_air_velocity message into a struct - * - * @param msg The message to decode - * @param obs_air_velocity C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_air_velocity_decode(const mavlink_message_t* msg, mavlink_obs_air_velocity_t* obs_air_velocity) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_air_velocity->magnitude = mavlink_msg_obs_air_velocity_get_magnitude(msg); - obs_air_velocity->aoa = mavlink_msg_obs_air_velocity_get_aoa(msg); - obs_air_velocity->slip = mavlink_msg_obs_air_velocity_get_slip(msg); -#else - memcpy(obs_air_velocity, _MAV_PAYLOAD(msg), 12); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h deleted file mode 100644 index 7a3e6d0ba4..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_attitude.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE OBS_ATTITUDE PACKING - -#define MAVLINK_MSG_ID_OBS_ATTITUDE 174 - -typedef struct __mavlink_obs_attitude_t -{ - double quat[4]; ///< Quaternion re;im -} mavlink_obs_attitude_t; - -#define MAVLINK_MSG_ID_OBS_ATTITUDE_LEN 32 -#define MAVLINK_MSG_ID_174_LEN 32 - -#define MAVLINK_MSG_OBS_ATTITUDE_FIELD_QUAT_LEN 4 - -#define MAVLINK_MESSAGE_INFO_OBS_ATTITUDE { \ - "OBS_ATTITUDE", \ - 1, \ - { { "quat", NULL, MAVLINK_TYPE_DOUBLE, 4, 0, offsetof(mavlink_obs_attitude_t, quat) }, \ - } \ -} - - -/** - * @brief Pack a obs_attitude 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 quat Quaternion re;im - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const double *quat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - - _mav_put_double_array(buf, 0, quat, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); -#else - mavlink_obs_attitude_t packet; - - mav_array_memcpy(packet.quat, quat, sizeof(double)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE; - return mavlink_finalize_message(msg, system_id, component_id, 32, 146); -} - -/** - * @brief Pack a obs_attitude message on a channel - * @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 quat Quaternion re;im - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const double *quat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - - _mav_put_double_array(buf, 0, quat, 4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 32); -#else - mavlink_obs_attitude_t packet; - - mav_array_memcpy(packet.quat, quat, sizeof(double)*4); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 32); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_ATTITUDE; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 32, 146); -} - -/** - * @brief Encode a obs_attitude 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 obs_attitude C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_attitude_t* obs_attitude) -{ - return mavlink_msg_obs_attitude_pack(system_id, component_id, msg, obs_attitude->quat); -} - -/** - * @brief Send a obs_attitude message - * @param chan MAVLink channel to send the message - * - * @param quat Quaternion re;im - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_attitude_send(mavlink_channel_t chan, const double *quat) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[32]; - - _mav_put_double_array(buf, 0, quat, 4); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, buf, 32, 146); -#else - mavlink_obs_attitude_t packet; - - mav_array_memcpy(packet.quat, quat, sizeof(double)*4); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_ATTITUDE, (const char *)&packet, 32, 146); -#endif -} - -#endif - -// MESSAGE OBS_ATTITUDE UNPACKING - - -/** - * @brief Get field quat from obs_attitude message - * - * @return Quaternion re;im - */ -static inline uint16_t mavlink_msg_obs_attitude_get_quat(const mavlink_message_t* msg, double *quat) -{ - return _MAV_RETURN_double_array(msg, quat, 4, 0); -} - -/** - * @brief Decode a obs_attitude message into a struct - * - * @param msg The message to decode - * @param obs_attitude C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_attitude_decode(const mavlink_message_t* msg, mavlink_obs_attitude_t* obs_attitude) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_attitude_get_quat(msg, obs_attitude->quat); -#else - memcpy(obs_attitude, _MAV_PAYLOAD(msg), 32); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h deleted file mode 100644 index 565ab0cd86..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_bias.h +++ /dev/null @@ -1,167 +0,0 @@ -// MESSAGE OBS_BIAS PACKING - -#define MAVLINK_MSG_ID_OBS_BIAS 180 - -typedef struct __mavlink_obs_bias_t -{ - float accBias[3]; ///< accelerometer bias - float gyroBias[3]; ///< gyroscope bias -} mavlink_obs_bias_t; - -#define MAVLINK_MSG_ID_OBS_BIAS_LEN 24 -#define MAVLINK_MSG_ID_180_LEN 24 - -#define MAVLINK_MSG_OBS_BIAS_FIELD_ACCBIAS_LEN 3 -#define MAVLINK_MSG_OBS_BIAS_FIELD_GYROBIAS_LEN 3 - -#define MAVLINK_MESSAGE_INFO_OBS_BIAS { \ - "OBS_BIAS", \ - 2, \ - { { "accBias", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_bias_t, accBias) }, \ - { "gyroBias", NULL, MAVLINK_TYPE_FLOAT, 3, 12, offsetof(mavlink_obs_bias_t, gyroBias) }, \ - } \ -} - - -/** - * @brief Pack a obs_bias 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 accBias accelerometer bias - * @param gyroBias gyroscope bias - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_bias_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *accBias, const float *gyroBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[24]; - - _mav_put_float_array(buf, 0, accBias, 3); - _mav_put_float_array(buf, 12, gyroBias, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24); -#else - mavlink_obs_bias_t packet; - - mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); - mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_BIAS; - return mavlink_finalize_message(msg, system_id, component_id, 24, 159); -} - -/** - * @brief Pack a obs_bias message on a channel - * @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 accBias accelerometer bias - * @param gyroBias gyroscope bias - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_bias_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *accBias,const float *gyroBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[24]; - - _mav_put_float_array(buf, 0, accBias, 3); - _mav_put_float_array(buf, 12, gyroBias, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 24); -#else - mavlink_obs_bias_t packet; - - mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); - mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 24); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_BIAS; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 24, 159); -} - -/** - * @brief Encode a obs_bias 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 obs_bias C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_bias_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_bias_t* obs_bias) -{ - return mavlink_msg_obs_bias_pack(system_id, component_id, msg, obs_bias->accBias, obs_bias->gyroBias); -} - -/** - * @brief Send a obs_bias message - * @param chan MAVLink channel to send the message - * - * @param accBias accelerometer bias - * @param gyroBias gyroscope bias - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_bias_send(mavlink_channel_t chan, const float *accBias, const float *gyroBias) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[24]; - - _mav_put_float_array(buf, 0, accBias, 3); - _mav_put_float_array(buf, 12, gyroBias, 3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, buf, 24, 159); -#else - mavlink_obs_bias_t packet; - - mav_array_memcpy(packet.accBias, accBias, sizeof(float)*3); - mav_array_memcpy(packet.gyroBias, gyroBias, sizeof(float)*3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_BIAS, (const char *)&packet, 24, 159); -#endif -} - -#endif - -// MESSAGE OBS_BIAS UNPACKING - - -/** - * @brief Get field accBias from obs_bias message - * - * @return accelerometer bias - */ -static inline uint16_t mavlink_msg_obs_bias_get_accBias(const mavlink_message_t* msg, float *accBias) -{ - return _MAV_RETURN_float_array(msg, accBias, 3, 0); -} - -/** - * @brief Get field gyroBias from obs_bias message - * - * @return gyroscope bias - */ -static inline uint16_t mavlink_msg_obs_bias_get_gyroBias(const mavlink_message_t* msg, float *gyroBias) -{ - return _MAV_RETURN_float_array(msg, gyroBias, 3, 12); -} - -/** - * @brief Decode a obs_bias message into a struct - * - * @param msg The message to decode - * @param obs_bias C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_bias_decode(const mavlink_message_t* msg, mavlink_obs_bias_t* obs_bias) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_bias_get_accBias(msg, obs_bias->accBias); - mavlink_msg_obs_bias_get_gyroBias(msg, obs_bias->gyroBias); -#else - memcpy(obs_bias, _MAV_PAYLOAD(msg), 24); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h deleted file mode 100644 index e886c8c24f..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_position.h +++ /dev/null @@ -1,188 +0,0 @@ -// MESSAGE OBS_POSITION PACKING - -#define MAVLINK_MSG_ID_OBS_POSITION 170 - -typedef struct __mavlink_obs_position_t -{ - int32_t lon; ///< Longitude expressed in 1E7 - int32_t lat; ///< Latitude expressed in 1E7 - int32_t alt; ///< Altitude expressed in milimeters -} mavlink_obs_position_t; - -#define MAVLINK_MSG_ID_OBS_POSITION_LEN 12 -#define MAVLINK_MSG_ID_170_LEN 12 - - - -#define MAVLINK_MESSAGE_INFO_OBS_POSITION { \ - "OBS_POSITION", \ - 3, \ - { { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_obs_position_t, lon) }, \ - { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_obs_position_t, lat) }, \ - { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_obs_position_t, alt) }, \ - } \ -} - - -/** - * @brief Pack a obs_position 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 lon Longitude expressed in 1E7 - * @param lat Latitude expressed in 1E7 - * @param alt Altitude expressed in milimeters - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int32_t lon, int32_t lat, int32_t alt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_int32_t(buf, 0, lon); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, alt); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_obs_position_t packet; - packet.lon = lon; - packet.lat = lat; - packet.alt = alt; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_POSITION; - return mavlink_finalize_message(msg, system_id, component_id, 12, 15); -} - -/** - * @brief Pack a obs_position message on a channel - * @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 lon Longitude expressed in 1E7 - * @param lat Latitude expressed in 1E7 - * @param alt Altitude expressed in milimeters - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - int32_t lon,int32_t lat,int32_t alt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_int32_t(buf, 0, lon); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, alt); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_obs_position_t packet; - packet.lon = lon; - packet.lat = lat; - packet.alt = alt; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_POSITION; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 15); -} - -/** - * @brief Encode a obs_position 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 obs_position C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_position_t* obs_position) -{ - return mavlink_msg_obs_position_pack(system_id, component_id, msg, obs_position->lon, obs_position->lat, obs_position->alt); -} - -/** - * @brief Send a obs_position message - * @param chan MAVLink channel to send the message - * - * @param lon Longitude expressed in 1E7 - * @param lat Latitude expressed in 1E7 - * @param alt Altitude expressed in milimeters - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_position_send(mavlink_channel_t chan, int32_t lon, int32_t lat, int32_t alt) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - _mav_put_int32_t(buf, 0, lon); - _mav_put_int32_t(buf, 4, lat); - _mav_put_int32_t(buf, 8, alt); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, buf, 12, 15); -#else - mavlink_obs_position_t packet; - packet.lon = lon; - packet.lat = lat; - packet.alt = alt; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_POSITION, (const char *)&packet, 12, 15); -#endif -} - -#endif - -// MESSAGE OBS_POSITION UNPACKING - - -/** - * @brief Get field lon from obs_position message - * - * @return Longitude expressed in 1E7 - */ -static inline int32_t mavlink_msg_obs_position_get_lon(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 0); -} - -/** - * @brief Get field lat from obs_position message - * - * @return Latitude expressed in 1E7 - */ -static inline int32_t mavlink_msg_obs_position_get_lat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 4); -} - -/** - * @brief Get field alt from obs_position message - * - * @return Altitude expressed in milimeters - */ -static inline int32_t mavlink_msg_obs_position_get_alt(const mavlink_message_t* msg) -{ - return _MAV_RETURN_int32_t(msg, 8); -} - -/** - * @brief Decode a obs_position message into a struct - * - * @param msg The message to decode - * @param obs_position C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_position_decode(const mavlink_message_t* msg, mavlink_obs_position_t* obs_position) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_position->lon = mavlink_msg_obs_position_get_lon(msg); - obs_position->lat = mavlink_msg_obs_position_get_lat(msg); - obs_position->alt = mavlink_msg_obs_position_get_alt(msg); -#else - memcpy(obs_position, _MAV_PAYLOAD(msg), 12); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h deleted file mode 100644 index 4ab10ca07c..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_qff.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE OBS_QFF PACKING - -#define MAVLINK_MSG_ID_OBS_QFF 182 - -typedef struct __mavlink_obs_qff_t -{ - float qff; ///< Wind -} mavlink_obs_qff_t; - -#define MAVLINK_MSG_ID_OBS_QFF_LEN 4 -#define MAVLINK_MSG_ID_182_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_OBS_QFF { \ - "OBS_QFF", \ - 1, \ - { { "qff", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_obs_qff_t, qff) }, \ - } \ -} - - -/** - * @brief Pack a obs_qff 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 qff Wind - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_qff_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float qff) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_float(buf, 0, qff); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); -#else - mavlink_obs_qff_t packet; - packet.qff = qff; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_QFF; - return mavlink_finalize_message(msg, system_id, component_id, 4, 24); -} - -/** - * @brief Pack a obs_qff message on a channel - * @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 qff Wind - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_qff_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float qff) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_float(buf, 0, qff); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); -#else - mavlink_obs_qff_t packet; - packet.qff = qff; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_QFF; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 24); -} - -/** - * @brief Encode a obs_qff 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 obs_qff C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_qff_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_qff_t* obs_qff) -{ - return mavlink_msg_obs_qff_pack(system_id, component_id, msg, obs_qff->qff); -} - -/** - * @brief Send a obs_qff message - * @param chan MAVLink channel to send the message - * - * @param qff Wind - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_qff_send(mavlink_channel_t chan, float qff) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_float(buf, 0, qff); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, buf, 4, 24); -#else - mavlink_obs_qff_t packet; - packet.qff = qff; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_QFF, (const char *)&packet, 4, 24); -#endif -} - -#endif - -// MESSAGE OBS_QFF UNPACKING - - -/** - * @brief Get field qff from obs_qff message - * - * @return Wind - */ -static inline float mavlink_msg_obs_qff_get_qff(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Decode a obs_qff message into a struct - * - * @param msg The message to decode - * @param obs_qff C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_qff_decode(const mavlink_message_t* msg, mavlink_obs_qff_t* obs_qff) -{ -#if MAVLINK_NEED_BYTE_SWAP - obs_qff->qff = mavlink_msg_obs_qff_get_qff(msg); -#else - memcpy(obs_qff, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h deleted file mode 100644 index e5ace9f85e..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_velocity.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE OBS_VELOCITY PACKING - -#define MAVLINK_MSG_ID_OBS_VELOCITY 172 - -typedef struct __mavlink_obs_velocity_t -{ - float vel[3]; ///< Velocity -} mavlink_obs_velocity_t; - -#define MAVLINK_MSG_ID_OBS_VELOCITY_LEN 12 -#define MAVLINK_MSG_ID_172_LEN 12 - -#define MAVLINK_MSG_OBS_VELOCITY_FIELD_VEL_LEN 3 - -#define MAVLINK_MESSAGE_INFO_OBS_VELOCITY { \ - "OBS_VELOCITY", \ - 1, \ - { { "vel", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_velocity_t, vel) }, \ - } \ -} - - -/** - * @brief Pack a obs_velocity 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 vel Velocity - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_velocity_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *vel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_float_array(buf, 0, vel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_obs_velocity_t packet; - - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY; - return mavlink_finalize_message(msg, system_id, component_id, 12, 108); -} - -/** - * @brief Pack a obs_velocity message on a channel - * @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 vel Velocity - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_velocity_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *vel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_float_array(buf, 0, vel, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_obs_velocity_t packet; - - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_VELOCITY; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 108); -} - -/** - * @brief Encode a obs_velocity 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 obs_velocity C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_velocity_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_velocity_t* obs_velocity) -{ - return mavlink_msg_obs_velocity_pack(system_id, component_id, msg, obs_velocity->vel); -} - -/** - * @brief Send a obs_velocity message - * @param chan MAVLink channel to send the message - * - * @param vel Velocity - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_velocity_send(mavlink_channel_t chan, const float *vel) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_float_array(buf, 0, vel, 3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, buf, 12, 108); -#else - mavlink_obs_velocity_t packet; - - mav_array_memcpy(packet.vel, vel, sizeof(float)*3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_VELOCITY, (const char *)&packet, 12, 108); -#endif -} - -#endif - -// MESSAGE OBS_VELOCITY UNPACKING - - -/** - * @brief Get field vel from obs_velocity message - * - * @return Velocity - */ -static inline uint16_t mavlink_msg_obs_velocity_get_vel(const mavlink_message_t* msg, float *vel) -{ - return _MAV_RETURN_float_array(msg, vel, 3, 0); -} - -/** - * @brief Decode a obs_velocity message into a struct - * - * @param msg The message to decode - * @param obs_velocity C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_velocity_decode(const mavlink_message_t* msg, mavlink_obs_velocity_t* obs_velocity) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_velocity_get_vel(msg, obs_velocity->vel); -#else - memcpy(obs_velocity, _MAV_PAYLOAD(msg), 12); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h deleted file mode 100644 index 6011a1caad..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_obs_wind.h +++ /dev/null @@ -1,144 +0,0 @@ -// MESSAGE OBS_WIND PACKING - -#define MAVLINK_MSG_ID_OBS_WIND 176 - -typedef struct __mavlink_obs_wind_t -{ - float wind[3]; ///< Wind -} mavlink_obs_wind_t; - -#define MAVLINK_MSG_ID_OBS_WIND_LEN 12 -#define MAVLINK_MSG_ID_176_LEN 12 - -#define MAVLINK_MSG_OBS_WIND_FIELD_WIND_LEN 3 - -#define MAVLINK_MESSAGE_INFO_OBS_WIND { \ - "OBS_WIND", \ - 1, \ - { { "wind", NULL, MAVLINK_TYPE_FLOAT, 3, 0, offsetof(mavlink_obs_wind_t, wind) }, \ - } \ -} - - -/** - * @brief Pack a obs_wind 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 wind Wind - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_wind_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - const float *wind) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_float_array(buf, 0, wind, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_obs_wind_t packet; - - mav_array_memcpy(packet.wind, wind, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_WIND; - return mavlink_finalize_message(msg, system_id, component_id, 12, 16); -} - -/** - * @brief Pack a obs_wind message on a channel - * @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 wind Wind - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_obs_wind_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - const float *wind) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_float_array(buf, 0, wind, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 12); -#else - mavlink_obs_wind_t packet; - - mav_array_memcpy(packet.wind, wind, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 12); -#endif - - msg->msgid = MAVLINK_MSG_ID_OBS_WIND; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 12, 16); -} - -/** - * @brief Encode a obs_wind 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 obs_wind C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_obs_wind_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obs_wind_t* obs_wind) -{ - return mavlink_msg_obs_wind_pack(system_id, component_id, msg, obs_wind->wind); -} - -/** - * @brief Send a obs_wind message - * @param chan MAVLink channel to send the message - * - * @param wind Wind - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_obs_wind_send(mavlink_channel_t chan, const float *wind) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[12]; - - _mav_put_float_array(buf, 0, wind, 3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, buf, 12, 16); -#else - mavlink_obs_wind_t packet; - - mav_array_memcpy(packet.wind, wind, sizeof(float)*3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBS_WIND, (const char *)&packet, 12, 16); -#endif -} - -#endif - -// MESSAGE OBS_WIND UNPACKING - - -/** - * @brief Get field wind from obs_wind message - * - * @return Wind - */ -static inline uint16_t mavlink_msg_obs_wind_get_wind(const mavlink_message_t* msg, float *wind) -{ - return _MAV_RETURN_float_array(msg, wind, 3, 0); -} - -/** - * @brief Decode a obs_wind message into a struct - * - * @param msg The message to decode - * @param obs_wind C-struct to decode the message contents into - */ -static inline void mavlink_msg_obs_wind_decode(const mavlink_message_t* msg, mavlink_obs_wind_t* obs_wind) -{ -#if MAVLINK_NEED_BYTE_SWAP - mavlink_msg_obs_wind_get_wind(msg, obs_wind->wind); -#else - memcpy(obs_wind, _MAV_PAYLOAD(msg), 12); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h deleted file mode 100644 index d194dae9bc..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_pm_elec.h +++ /dev/null @@ -1,182 +0,0 @@ -// MESSAGE PM_ELEC PACKING - -#define MAVLINK_MSG_ID_PM_ELEC 188 - -typedef struct __mavlink_pm_elec_t -{ - float PwCons; ///< current power consumption - float BatStat; ///< battery status - float PwGen[3]; ///< Power generation from each module -} mavlink_pm_elec_t; - -#define MAVLINK_MSG_ID_PM_ELEC_LEN 20 -#define MAVLINK_MSG_ID_188_LEN 20 - -#define MAVLINK_MSG_PM_ELEC_FIELD_PWGEN_LEN 3 - -#define MAVLINK_MESSAGE_INFO_PM_ELEC { \ - "PM_ELEC", \ - 3, \ - { { "PwCons", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_pm_elec_t, PwCons) }, \ - { "BatStat", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_pm_elec_t, BatStat) }, \ - { "PwGen", NULL, MAVLINK_TYPE_FLOAT, 3, 8, offsetof(mavlink_pm_elec_t, PwGen) }, \ - } \ -} - - -/** - * @brief Pack a pm_elec 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 PwCons current power consumption - * @param BatStat battery status - * @param PwGen Power generation from each module - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pm_elec_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - float PwCons, float BatStat, const float *PwGen) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _mav_put_float(buf, 0, PwCons); - _mav_put_float(buf, 4, BatStat); - _mav_put_float_array(buf, 8, PwGen, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 20); -#else - mavlink_pm_elec_t packet; - packet.PwCons = PwCons; - packet.BatStat = BatStat; - mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20); -#endif - - msg->msgid = MAVLINK_MSG_ID_PM_ELEC; - return mavlink_finalize_message(msg, system_id, component_id, 20, 170); -} - -/** - * @brief Pack a pm_elec message on a channel - * @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 PwCons current power consumption - * @param BatStat battery status - * @param PwGen Power generation from each module - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_pm_elec_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - float PwCons,float BatStat,const float *PwGen) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _mav_put_float(buf, 0, PwCons); - _mav_put_float(buf, 4, BatStat); - _mav_put_float_array(buf, 8, PwGen, 3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 20); -#else - mavlink_pm_elec_t packet; - packet.PwCons = PwCons; - packet.BatStat = BatStat; - mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 20); -#endif - - msg->msgid = MAVLINK_MSG_ID_PM_ELEC; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 20, 170); -} - -/** - * @brief Encode a pm_elec 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 pm_elec C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_pm_elec_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_pm_elec_t* pm_elec) -{ - return mavlink_msg_pm_elec_pack(system_id, component_id, msg, pm_elec->PwCons, pm_elec->BatStat, pm_elec->PwGen); -} - -/** - * @brief Send a pm_elec message - * @param chan MAVLink channel to send the message - * - * @param PwCons current power consumption - * @param BatStat battery status - * @param PwGen Power generation from each module - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_pm_elec_send(mavlink_channel_t chan, float PwCons, float BatStat, const float *PwGen) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[20]; - _mav_put_float(buf, 0, PwCons); - _mav_put_float(buf, 4, BatStat); - _mav_put_float_array(buf, 8, PwGen, 3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, buf, 20, 170); -#else - mavlink_pm_elec_t packet; - packet.PwCons = PwCons; - packet.BatStat = BatStat; - mav_array_memcpy(packet.PwGen, PwGen, sizeof(float)*3); - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PM_ELEC, (const char *)&packet, 20, 170); -#endif -} - -#endif - -// MESSAGE PM_ELEC UNPACKING - - -/** - * @brief Get field PwCons from pm_elec message - * - * @return current power consumption - */ -static inline float mavlink_msg_pm_elec_get_PwCons(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 0); -} - -/** - * @brief Get field BatStat from pm_elec message - * - * @return battery status - */ -static inline float mavlink_msg_pm_elec_get_BatStat(const mavlink_message_t* msg) -{ - return _MAV_RETURN_float(msg, 4); -} - -/** - * @brief Get field PwGen from pm_elec message - * - * @return Power generation from each module - */ -static inline uint16_t mavlink_msg_pm_elec_get_PwGen(const mavlink_message_t* msg, float *PwGen) -{ - return _MAV_RETURN_float_array(msg, PwGen, 3, 8); -} - -/** - * @brief Decode a pm_elec message into a struct - * - * @param msg The message to decode - * @param pm_elec C-struct to decode the message contents into - */ -static inline void mavlink_msg_pm_elec_decode(const mavlink_message_t* msg, mavlink_pm_elec_t* pm_elec) -{ -#if MAVLINK_NEED_BYTE_SWAP - pm_elec->PwCons = mavlink_msg_pm_elec_get_PwCons(msg); - pm_elec->BatStat = mavlink_msg_pm_elec_get_BatStat(msg); - mavlink_msg_pm_elec_get_PwGen(msg, pm_elec->PwGen); -#else - memcpy(pm_elec, _MAV_PAYLOAD(msg), 20); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h deleted file mode 100644 index 597fdf911f..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/mavlink_msg_sys_stat.h +++ /dev/null @@ -1,210 +0,0 @@ -// MESSAGE SYS_Stat PACKING - -#define MAVLINK_MSG_ID_SYS_Stat 190 - -typedef struct __mavlink_sys_stat_t -{ - uint8_t gps; ///< gps status - uint8_t act; ///< actuator status - uint8_t mod; ///< module status - uint8_t commRssi; ///< module status -} mavlink_sys_stat_t; - -#define MAVLINK_MSG_ID_SYS_Stat_LEN 4 -#define MAVLINK_MSG_ID_190_LEN 4 - - - -#define MAVLINK_MESSAGE_INFO_SYS_Stat { \ - "SYS_Stat", \ - 4, \ - { { "gps", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_sys_stat_t, gps) }, \ - { "act", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_sys_stat_t, act) }, \ - { "mod", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_sys_stat_t, mod) }, \ - { "commRssi", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_sys_stat_t, commRssi) }, \ - } \ -} - - -/** - * @brief Pack a sys_stat 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 gps gps status - * @param act actuator status - * @param mod module status - * @param commRssi module status - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sys_stat_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint8_t(buf, 0, gps); - _mav_put_uint8_t(buf, 1, act); - _mav_put_uint8_t(buf, 2, mod); - _mav_put_uint8_t(buf, 3, commRssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); -#else - mavlink_sys_stat_t packet; - packet.gps = gps; - packet.act = act; - packet.mod = mod; - packet.commRssi = commRssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYS_Stat; - return mavlink_finalize_message(msg, system_id, component_id, 4, 157); -} - -/** - * @brief Pack a sys_stat message on a channel - * @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 gps gps status - * @param act actuator status - * @param mod module status - * @param commRssi module status - * @return length of the message in bytes (excluding serial stream start sign) - */ -static inline uint16_t mavlink_msg_sys_stat_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, - mavlink_message_t* msg, - uint8_t gps,uint8_t act,uint8_t mod,uint8_t commRssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint8_t(buf, 0, gps); - _mav_put_uint8_t(buf, 1, act); - _mav_put_uint8_t(buf, 2, mod); - _mav_put_uint8_t(buf, 3, commRssi); - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, 4); -#else - mavlink_sys_stat_t packet; - packet.gps = gps; - packet.act = act; - packet.mod = mod; - packet.commRssi = commRssi; - - memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, 4); -#endif - - msg->msgid = MAVLINK_MSG_ID_SYS_Stat; - return mavlink_finalize_message_chan(msg, system_id, component_id, chan, 4, 157); -} - -/** - * @brief Encode a sys_stat 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 sys_stat C-struct to read the message contents from - */ -static inline uint16_t mavlink_msg_sys_stat_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_sys_stat_t* sys_stat) -{ - return mavlink_msg_sys_stat_pack(system_id, component_id, msg, sys_stat->gps, sys_stat->act, sys_stat->mod, sys_stat->commRssi); -} - -/** - * @brief Send a sys_stat message - * @param chan MAVLink channel to send the message - * - * @param gps gps status - * @param act actuator status - * @param mod module status - * @param commRssi module status - */ -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS - -static inline void mavlink_msg_sys_stat_send(mavlink_channel_t chan, uint8_t gps, uint8_t act, uint8_t mod, uint8_t commRssi) -{ -#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - char buf[4]; - _mav_put_uint8_t(buf, 0, gps); - _mav_put_uint8_t(buf, 1, act); - _mav_put_uint8_t(buf, 2, mod); - _mav_put_uint8_t(buf, 3, commRssi); - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, buf, 4, 157); -#else - mavlink_sys_stat_t packet; - packet.gps = gps; - packet.act = act; - packet.mod = mod; - packet.commRssi = commRssi; - - _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SYS_Stat, (const char *)&packet, 4, 157); -#endif -} - -#endif - -// MESSAGE SYS_Stat UNPACKING - - -/** - * @brief Get field gps from sys_stat message - * - * @return gps status - */ -static inline uint8_t mavlink_msg_sys_stat_get_gps(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 0); -} - -/** - * @brief Get field act from sys_stat message - * - * @return actuator status - */ -static inline uint8_t mavlink_msg_sys_stat_get_act(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 1); -} - -/** - * @brief Get field mod from sys_stat message - * - * @return module status - */ -static inline uint8_t mavlink_msg_sys_stat_get_mod(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 2); -} - -/** - * @brief Get field commRssi from sys_stat message - * - * @return module status - */ -static inline uint8_t mavlink_msg_sys_stat_get_commRssi(const mavlink_message_t* msg) -{ - return _MAV_RETURN_uint8_t(msg, 3); -} - -/** - * @brief Decode a sys_stat message into a struct - * - * @param msg The message to decode - * @param sys_stat C-struct to decode the message contents into - */ -static inline void mavlink_msg_sys_stat_decode(const mavlink_message_t* msg, mavlink_sys_stat_t* sys_stat) -{ -#if MAVLINK_NEED_BYTE_SWAP - sys_stat->gps = mavlink_msg_sys_stat_get_gps(msg); - sys_stat->act = mavlink_msg_sys_stat_get_act(msg); - sys_stat->mod = mavlink_msg_sys_stat_get_mod(msg); - sys_stat->commRssi = mavlink_msg_sys_stat_get_commRssi(msg); -#else - memcpy(sys_stat, _MAV_PAYLOAD(msg), 4); -#endif -} diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/sensesoar.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/sensesoar.h deleted file mode 100644 index 5a6022d59f..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/sensesoar.h +++ /dev/null @@ -1,77 +0,0 @@ -/** @file - * @brief MAVLink comm protocol generated from sensesoar.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef SENSESOAR_H -#define SENSESOAR_H - -#ifdef __cplusplus -extern "C" { -#endif - -// MESSAGE LENGTHS AND CRCS - -#ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 0, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 0, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 19, 17, 15, 15, 27, 25, 18, 18, 20, 20, 9, 54, 26, 0, 36, 0, 6, 4, 0, 21, 18, 0, 0, 0, 20, 0, 33, 3, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 28, 56, 42, 33, 0, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 12, 0, 12, 0, 32, 0, 12, 0, 12, 0, 24, 0, 4, 4, 12, 0, 12, 0, 20, 0, 4, 0, 5, 0, 5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 30, 18, 18, 51, 9, 0} -#endif - -#ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 0, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 0, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 214, 223, 141, 33, 15, 3, 100, 24, 239, 238, 30, 200, 183, 0, 130, 0, 148, 21, 0, 52, 124, 0, 0, 0, 20, 0, 152, 143, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 231, 183, 63, 54, 0, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 15, 0, 108, 0, 146, 0, 16, 0, 32, 0, 159, 0, 24, 248, 79, 0, 5, 0, 170, 0, 157, 0, 209, 0, 243, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 204, 49, 170, 44, 83, 46, 0} -#endif - -#ifndef MAVLINK_MESSAGE_INFO -#define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_SET_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_SETPOINT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SET_GLOBAL_POSITION_SETPOINT_INT, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_SET_ROLL_PITCH_YAW_SPEED_THRUST, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_ROLL_PITCH_YAW_SPEED_THRUST_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_MOTORS_SETPOINT, MAVLINK_MESSAGE_INFO_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_STATE_CORRECTION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VFR_HUD, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_POSITION, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_ATTITUDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_WIND, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_AIR_VELOCITY, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_BIAS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OBS_QFF, MAVLINK_MESSAGE_INFO_OBS_AIR_TEMP, MAVLINK_MESSAGE_INFO_FILT_ROT_VEL, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LLC_OUT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PM_ELEC, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SYS_Stat, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_CHNG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CMD_AIRSPEED_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -#endif - -#include "../protocol.h" - -#define MAVLINK_ENABLED_SENSESOAR - -#include "../common/common.h" - -// MAVLINK VERSION - -#ifndef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -#if (MAVLINK_VERSION == 0) -#undef MAVLINK_VERSION -#define MAVLINK_VERSION 2 -#endif - -// ENUM DEFINITIONS - - -/** @brief Different flight modes */ -#ifndef HAVE_ENUM_SENSESOAR_MODE -#define HAVE_ENUM_SENSESOAR_MODE -enum SENSESOAR_MODE -{ - SENSESOAR_MODE_GLIDING=1, /* | */ - SENSESOAR_MODE_AUTONOMOUS=2, /* | */ - SENSESOAR_MODE_MANUAL=3, /* | */ - SENSESOAR_MODE_ENUM_END=4, /* | */ -}; -#endif - -// MESSAGE DEFINITIONS -#include "./mavlink_msg_obs_position.h" -#include "./mavlink_msg_obs_velocity.h" -#include "./mavlink_msg_obs_attitude.h" -#include "./mavlink_msg_obs_wind.h" -#include "./mavlink_msg_obs_air_velocity.h" -#include "./mavlink_msg_obs_bias.h" -#include "./mavlink_msg_obs_qff.h" -#include "./mavlink_msg_obs_air_temp.h" -#include "./mavlink_msg_filt_rot_vel.h" -#include "./mavlink_msg_llc_out.h" -#include "./mavlink_msg_pm_elec.h" -#include "./mavlink_msg_sys_stat.h" -#include "./mavlink_msg_cmd_airspeed_chng.h" -#include "./mavlink_msg_cmd_airspeed_ack.h" - -#ifdef __cplusplus -} -#endif // __cplusplus -#endif // SENSESOAR_H diff --git a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/testsuite.h b/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/testsuite.h deleted file mode 100644 index 4c8f73c5b7..0000000000 --- a/libraries/GCS_MAVLink/include/mavlink/v1.0/sensesoar/testsuite.h +++ /dev/null @@ -1,676 +0,0 @@ -/** @file - * @brief MAVLink comm protocol testsuite generated from sensesoar.xml - * @see http://qgroundcontrol.org/mavlink/ - */ -#ifndef SENSESOAR_TESTSUITE_H -#define SENSESOAR_TESTSUITE_H - -#ifdef __cplusplus -extern "C" { -#endif - -#ifndef MAVLINK_TEST_ALL -#define MAVLINK_TEST_ALL -static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); -static void mavlink_test_sensesoar(uint8_t, uint8_t, mavlink_message_t *last_msg); - -static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_test_common(system_id, component_id, last_msg); - mavlink_test_sensesoar(system_id, component_id, last_msg); -} -#endif - -#include "../common/testsuite.h" - - -static void mavlink_test_obs_position(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_obs_position_t packet_in = { - 963497464, - 963497672, - 963497880, - }; - mavlink_obs_position_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.lon = packet_in.lon; - packet1.lat = packet_in.lat; - packet1.alt = packet_in.alt; - - - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_obs_position_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_obs_position_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_obs_position_pack(system_id, component_id, &msg , packet1.lon , packet1.lat , packet1.alt ); - mavlink_msg_obs_position_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_obs_position_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.lon , packet1.lat , packet1.alt ); - mavlink_msg_obs_position_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; iBreached fence boundary + + + + pre-initialization + disabled + checking limits + a limit has been breached + taking action eg. RTL + we're no longer in breach of a limit + + + + + pre-initialization + disabled + checking limits + + + @@ -266,5 +285,21 @@ receive errors count of error corrected packets + + + + Status of AP_Limits. Sent in extended + status stream when AP_Limits is enabled + state of AP_Limits, (see enum LimitState, LIMITS_STATE) + time of last breach in milliseconds since boot + time of last recovery action in milliseconds since boot + time of last successful recovery in milliseconds since boot + time of last all-clear in milliseconds since boot + number of fence breaches + AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + + diff --git a/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml b/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml index 673353165f..ab6f670fbe 100644 --- a/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml +++ b/libraries/GCS_MAVLink/message_definitions_v1.0/ardupilotmega.xml @@ -9,7 +9,6 @@ is general enough to move into common.xml later --> - @@ -93,6 +92,23 @@ Breached fence boundary + + + + pre-initialization + disabled + checking limits + a limit has been breached + taking action eg. RTL + we're no longer in breach of a limit + + + + + pre-initialization + disabled + checking limits + @@ -266,5 +282,22 @@ receive errors count of error corrected packets + + + + + Status of AP_Limits. Sent in extended + status stream when AP_Limits is enabled + state of AP_Limits, (see enum LimitState, LIMITS_STATE) + time of last breach in milliseconds since boot + time of last recovery action in milliseconds since boot + time of last successful recovery in milliseconds since boot + time of last all-clear in milliseconds since boot + number of fence breaches + AP_Limit_Module bitfield of enabled modules, (see enum moduleid or LIMIT_MODULE) + AP_Limit_Module bitfield of required modules, (see enum moduleid or LIMIT_MODULE) + AP_Limit_Module bitfield of triggered modules, (see enum moduleid or LIMIT_MODULE) + +