diff options
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 213 |
1 files changed, 180 insertions, 33 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 4433377c6..74089f780 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -120,50 +120,77 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ - if (status->main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + switch (status->nav_state) { + + case NAVIGATION_STATE_MANUAL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + + case NAVIGATION_STATE_ACRO: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; + break; - } else if (status->main_state == MAIN_STATE_ALTCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + case NAVIGATION_STATE_ALTCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; - } else if (status->main_state == MAIN_STATE_POSCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + case NAVIGATION_STATE_POSCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + break; - } else if (status->main_state == MAIN_STATE_AUTO) { - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + case NAVIGATION_STATE_AUTO_MISSION: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - - } else if (status->main_state == MAIN_STATE_ACRO) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - } - - } else { - /* use navigation state when navigator is active */ - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - - if (pos_sp_triplet->nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { + case NAVIGATION_STATE_AUTO_LOITER: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - - } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + case NAVIGATION_STATE_AUTO_RTL: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + case NAVIGATION_STATE_LAND: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } + break; + + case NAVIGATION_STATE_AUTO_RTGS: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; + break; + + case NAVIGATION_STATE_TERMINATION: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + } *mavlink_custom_mode = custom_mode.data; @@ -205,6 +232,11 @@ public: return "HEARTBEAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HEARTBEAT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHeartbeat(); @@ -256,6 +288,11 @@ public: return "SYS_STATUS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SYS_STATUS; + } + static MavlinkStream *new_instance() { return new MavlinkStreamSysStatus(); @@ -307,6 +344,11 @@ public: return "HIGHRES_IMU"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIGHRES_IMU; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHighresIMU(); @@ -392,6 +434,11 @@ public: return "ATTITUDE"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitude(); @@ -438,6 +485,11 @@ public: return "ATTITUDE_QUATERNION"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeQuaternion(); @@ -490,6 +542,11 @@ public: return "VFR_HUD"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_VFR_HUD; + } + static MavlinkStream *new_instance() { return new MavlinkStreamVFRHUD(); @@ -573,6 +630,11 @@ public: return "GPS_RAW_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_RAW_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGPSRawInt(); @@ -603,8 +665,8 @@ protected: gps.lat, gps.lon, gps.alt, - cm_uint16_from_m_float(gps.eph_m), - cm_uint16_from_m_float(gps.epv_m), + cm_uint16_from_m_float(gps.eph), + cm_uint16_from_m_float(gps.epv), gps.vel_m_s * 100.0f, _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, gps.satellites_visible); @@ -626,6 +688,11 @@ public: return "GLOBAL_POSITION_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionInt(); @@ -687,6 +754,11 @@ public: return "LOCAL_POSITION_NED"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED; + } + static MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionNED(); @@ -738,6 +810,11 @@ public: return "VICON_POSITION_ESTIMATE"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + } + static MavlinkStream *new_instance() { return new MavlinkStreamViconPositionEstimate(); @@ -788,6 +865,11 @@ public: return "GPS_GLOBAL_ORIGIN"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGPSGlobalOrigin(); @@ -828,6 +910,11 @@ public: return MavlinkStreamServoOutputRaw<N>::get_name_static(); } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + } + static const char *get_name_static() { switch (N) { @@ -905,6 +992,11 @@ public: return "HIL_CONTROLS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIL_CONTROLS; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHILControls(); @@ -1042,6 +1134,11 @@ public: return "GLOBAL_POSITION_SETPOINT_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionSetpointInt(); @@ -1085,6 +1182,11 @@ public: return "LOCAL_POSITION_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionSetpoint(); @@ -1133,6 +1235,11 @@ public: return "ROLL_PITCH_YAW_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawThrustSetpoint(); @@ -1181,6 +1288,11 @@ public: return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); @@ -1229,6 +1341,11 @@ public: return "RC_CHANNELS_RAW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_RC_CHANNELS_RAW; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRCChannelsRaw(); @@ -1313,6 +1430,11 @@ public: return "MANUAL_CONTROL"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL; + } + static MavlinkStream *new_instance() { return new MavlinkStreamManualControl(); @@ -1362,6 +1484,11 @@ public: return "OPTICAL_FLOW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_OPTICAL_FLOW; + } + static MavlinkStream *new_instance() { return new MavlinkStreamOpticalFlow(); @@ -1410,6 +1537,11 @@ public: return "ATTITUDE_CONTROLS"; } + uint8_t get_id() + { + return 0; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeControls(); @@ -1468,6 +1600,11 @@ public: return "NAMED_VALUE_FLOAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamNamedValueFloat(); @@ -1516,6 +1653,11 @@ public: return "CAMERA_CAPTURE"; } + uint8_t get_id() + { + return 0; + } + static MavlinkStream *new_instance() { return new MavlinkStreamCameraCapture(); @@ -1561,6 +1703,11 @@ public: return "DISTANCE_SENSOR"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_DISTANCE_SENSOR; + } + static MavlinkStream *new_instance() { return new MavlinkStreamDistanceSensor(); |