aboutsummaryrefslogtreecommitdiff
path: root/src/modules/mavlink/mavlink_messages.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
-rw-r--r--src/modules/mavlink/mavlink_messages.cpp222
1 files changed, 93 insertions, 129 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 32592c5f4..5dfa5e6d2 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -169,6 +169,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
break;
case NAVIGATION_STATE_AUTO_RTL:
+ /* fallthrough */
+ case NAVIGATION_STATE_AUTO_RCRECOVER:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED
| MAV_MODE_FLAG_GUIDED_ENABLED;
@@ -177,6 +179,8 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
break;
case NAVIGATION_STATE_LAND:
+ case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+ case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
/* fallthrough */
case NAVIGATION_STATE_DESCEND:
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED
@@ -781,6 +785,9 @@ private:
MavlinkOrbSubscription *_airspeed_sub;
uint64_t _airspeed_time;
+ MavlinkOrbSubscription *_sensor_combined_sub;
+ uint64_t _sensor_combined_time;
+
/* do not allow top copying this class */
MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &);
MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &);
@@ -796,7 +803,9 @@ protected:
_act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))),
_act_time(0),
_airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))),
- _airspeed_time(0)
+ _airspeed_time(0),
+ _sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))),
+ _sensor_combined_time(0)
{}
void send(const hrt_abstime t)
@@ -806,12 +815,14 @@ protected:
struct actuator_armed_s armed;
struct actuator_controls_s act;
struct airspeed_s airspeed;
+ struct sensor_combined_s sensor_combined;
bool updated = _att_sub->update(&_att_time, &att);
updated |= _pos_sub->update(&_pos_time, &pos);
updated |= _armed_sub->update(&_armed_time, &armed);
updated |= _act_sub->update(&_act_time, &act);
updated |= _airspeed_sub->update(&_airspeed_time, &airspeed);
+ updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined);
if (updated) {
mavlink_vfr_hud_t msg;
@@ -820,7 +831,7 @@ protected:
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
- msg.alt = pos.alt;
+ msg.alt = sensor_combined.baro_alt_meter;
msg.climb = -pos.vel_d;
_mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg);
@@ -886,8 +897,8 @@ protected:
msg.eph = cm_uint16_from_m_float(gps.eph);
msg.epv = cm_uint16_from_m_float(gps.epv);
msg.vel = cm_uint16_from_m_float(gps.vel_m_s),
- msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
- msg.satellites_visible = gps.satellites_used;
+ msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f,
+ msg.satellites_visible = gps.satellites_used;
_mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg);
}
@@ -957,11 +968,11 @@ protected:
msg.lat = pos.lat * 1e7;
msg.lon = pos.lon * 1e7;
msg.alt = pos.alt * 1000.0f;
- msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
- msg.vx = pos.vel_n * 100.0f;
- msg.vy = pos.vel_e * 100.0f;
- msg.vz = pos.vel_d * 100.0f;
- msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
+ msg.relative_alt = (pos.alt - home.alt) * 1000.0f;
+ msg.vx = pos.vel_n * 100.0f;
+ msg.vy = pos.vel_e * 100.0f;
+ msg.vz = pos.vel_d * 100.0f;
+ msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f;
_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg);
}
@@ -1022,9 +1033,9 @@ protected:
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
- msg.vx = pos.vx;
- msg.vy = pos.vy;
- msg.vz = pos.vz;
+ msg.vx = pos.vx;
+ msg.vy = pos.vy;
+ msg.vz = pos.vz;
_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg);
}
@@ -1085,9 +1096,9 @@ protected:
msg.x = pos.x;
msg.y = pos.y;
msg.z = pos.z;
- msg.roll = pos.roll;
- msg.pitch = pos.pitch;
- msg.yaw = pos.yaw;
+ msg.roll = pos.roll;
+ msg.pitch = pos.pitch;
+ msg.yaw = pos.yaw;
_mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg);
}
@@ -1340,7 +1351,7 @@ protected:
}
for (unsigned i = 0; i < 8; i++) {
- if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
+ if (act.output[i] > PWM_LOWEST_MIN / 2) {
if (i < n) {
/* scale PWM out 900..2100 us to 0..1 for rotors */
out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
@@ -1351,7 +1362,7 @@ protected:
}
} else {
- /* send 0 when disarmed */
+ /* send 0 when disarmed and for disabled channels */
out[i] = 0.0f;
}
}
@@ -1360,15 +1371,20 @@ protected:
/* fixed wing: scale throttle to 0..1 and other channels to -1..1 */
for (unsigned i = 0; i < 8; i++) {
- if (i != 3) {
- /* scale PWM out 900..2100 us to -1..1 for normal channels */
- out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+ if (act.output[i] > PWM_LOWEST_MIN / 2) {
+ if (i != 3) {
+ /* scale PWM out 900..2100 us to -1..1 for normal channels */
+ out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
+
+ } else {
+ /* scale PWM out 900..2100 us to 0..1 for throttle */
+ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+ }
} else {
- /* scale PWM out 900..2100 us to 0..1 for throttle */
- out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+ /* set 0 for disabled channels */
+ out[i] = 0.0f;
}
-
}
}
@@ -1392,43 +1408,43 @@ protected:
};
-class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
+class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream
{
public:
const char *get_name() const
{
- return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
+ return MavlinkStreamPositionTargetGlobalInt::get_name_static();
}
static const char *get_name_static()
{
- return "GLOBAL_POSITION_SETPOINT_INT";
+ return "POSITION_TARGET_GLOBAL_INT";
}
uint8_t get_id()
{
- return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+ return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamGlobalPositionSetpointInt(mavlink);
+ return new MavlinkStreamPositionTargetGlobalInt(mavlink);
}
unsigned get_size()
{
- return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+ return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
MavlinkOrbSubscription *_pos_sp_triplet_sub;
/* do not allow top copying this class */
- MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &);
- MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &);
+ MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &);
+ MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &);
protected:
- explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink),
+ explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink),
_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
{}
@@ -1437,15 +1453,14 @@ protected:
struct position_setpoint_triplet_s pos_sp_triplet;
if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
- mavlink_global_position_setpoint_int_t msg;
+ mavlink_position_target_global_int_t msg{};
msg.coordinate_frame = MAV_FRAME_GLOBAL;
- msg.latitude = pos_sp_triplet.current.lat * 1e7;
- msg.longitude = pos_sp_triplet.current.lon * 1e7;
- msg.altitude = pos_sp_triplet.current.alt * 1000;
- msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f;
+ msg.lat_int = pos_sp_triplet.current.lat * 1e7;
+ msg.lon_int = pos_sp_triplet.current.lon * 1e7;
+ msg.alt = pos_sp_triplet.current.alt;
- _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg);
+ _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg);
}
}
};
@@ -1461,12 +1476,12 @@ public:
static const char *get_name_static()
{
- return "LOCAL_POSITION_SETPOINT";
+ return "POSITION_TARGET_LOCAL_NED";
}
uint8_t get_id()
{
- return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+ return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
@@ -1476,7 +1491,7 @@ public:
unsigned get_size()
{
- return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+ return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
@@ -1498,137 +1513,87 @@ protected:
struct vehicle_local_position_setpoint_s pos_sp;
if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
- mavlink_local_position_setpoint_t msg;
+ mavlink_position_target_local_ned_t msg{};
+ msg.time_boot_ms = pos_sp.timestamp / 1000;
msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
msg.x = pos_sp.x;
msg.y = pos_sp.y;
msg.z = pos_sp.z;
- msg.yaw = pos_sp.yaw;
- _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg);
+ _mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg);
}
}
};
-class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
+class MavlinkStreamAttitudeTarget : public MavlinkStream
{
public:
const char *get_name() const
{
- return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
+ return MavlinkStreamAttitudeTarget::get_name_static();
}
static const char *get_name_static()
{
- return "ROLL_PITCH_YAW_THRUST_SETPOINT";
+ return "ATTITUDE_TARGET";
}
uint8_t get_id()
{
- return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+ return MAVLINK_MSG_ID_ATTITUDE_TARGET;
}
static MavlinkStream *new_instance(Mavlink *mavlink)
{
- return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink);
+ return new MavlinkStreamAttitudeTarget(mavlink);
}
unsigned get_size()
{
- return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+ return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
}
private:
- MavlinkOrbSubscription *att_sp_sub;
- uint64_t att_sp_time;
+ MavlinkOrbSubscription *_att_sp_sub;
+ MavlinkOrbSubscription *_att_rates_sp_sub;
+ uint64_t _att_sp_time;
+ uint64_t _att_rates_sp_time;
/* do not allow top copying this class */
- MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &);
- MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &);
+ MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &);
+ MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &);
protected:
- explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
- att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
- att_sp_time(0)
+ explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink),
+ _att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
+ _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
+ _att_sp_time(0),
+ _att_rates_sp_time(0)
{}
void send(const hrt_abstime t)
{
struct vehicle_attitude_setpoint_s att_sp;
- if (att_sp_sub->update(&att_sp_time, &att_sp)) {
- mavlink_roll_pitch_yaw_thrust_setpoint_t msg;
-
- msg.time_boot_ms = att_sp.timestamp / 1000;
- msg.roll = att_sp.roll_body;
- msg.pitch = att_sp.pitch_body;
- msg.yaw = att_sp.yaw_body;
- msg.thrust = att_sp.thrust;
-
- _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg);
- }
- }
-};
-
-
-class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
-{
-public:
- const char *get_name() const
- {
- return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
- }
-
- static const char *get_name_static()
- {
- 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(Mavlink *mavlink)
- {
- return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink);
- }
-
- unsigned get_size()
- {
- return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
- }
+ if (_att_sp_sub->update(&_att_sp_time, &att_sp)) {
-private:
- MavlinkOrbSubscription *_att_rates_sp_sub;
- uint64_t _att_rates_sp_time;
+ struct vehicle_rates_setpoint_s att_rates_sp;
+ (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp);
- /* do not allow top copying this class */
- MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &);
- MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &);
-
-protected:
- explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
- _att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
- _att_rates_sp_time(0)
- {}
+ mavlink_attitude_target_t msg{};
- void send(const hrt_abstime t)
- {
- struct vehicle_rates_setpoint_s att_rates_sp;
+ msg.time_boot_ms = att_sp.timestamp / 1000;
+ mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
- if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) {
- mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg;
+ msg.body_roll_rate = att_rates_sp.roll;
+ msg.body_pitch_rate = att_rates_sp.pitch;
+ msg.body_yaw_rate = att_rates_sp.yaw;
- msg.time_boot_ms = att_rates_sp.timestamp / 1000;
- msg.roll_rate = att_rates_sp.roll;
- msg.pitch_rate = att_rates_sp.pitch;
- msg.yaw_rate = att_rates_sp.yaw;
- msg.thrust = att_rates_sp.thrust;
+ msg.thrust = att_sp.thrust;
- _mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg);
+ _mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg);
}
}
};
@@ -1695,10 +1660,10 @@ protected:
msg.chan2_raw = (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX;
msg.chan3_raw = (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX;
msg.chan4_raw = (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX;
- msg.chan4_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
- msg.chan4_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
- msg.chan4_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
- msg.chan4_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
+ msg.chan5_raw = (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX;
+ msg.chan6_raw = (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX;
+ msg.chan7_raw = (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX;
+ msg.chan8_raw = (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX;
msg.rssi = rc.rssi;
_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS_RAW, &msg);
@@ -2156,10 +2121,9 @@ StreamListItem *streams_list[] = {
new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
- new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static),
+ new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static),
new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
- new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
- new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
+ new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-12 18:22:15 +0000