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.cpp1161
1 files changed, 781 insertions, 380 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 9c552515d..667be87b7 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -45,7 +45,6 @@
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/rc_channels.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/vehicle_global_position.h>
@@ -74,6 +73,8 @@
#include <drivers/drv_pwm_output.h>
#include <drivers/drv_range_finder.h>
+#include <systemlib/err.h>
+
#include "mavlink_messages.h"
@@ -118,46 +119,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);
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
-
- } else if (status->main_state == MAIN_STATE_SEATBELT) {
- *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
- custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
-
- } else if (status->main_state == MAIN_STATE_EASY) {
- *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_EASY;
+ switch (status->nav_state) {
- } 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_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;
+
+ 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;
+
+ 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;
+
+ 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 {
- /* 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;
@@ -189,42 +221,57 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
class MavlinkStreamHeartbeat : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamHeartbeat::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "HEARTBEAT";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_HEARTBEAT;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamHeartbeat();
}
private:
MavlinkOrbSubscription *status_sub;
- struct vehicle_status_s *status;
-
MavlinkOrbSubscription *pos_sp_triplet_sub;
- struct position_setpoint_triplet_s *pos_sp_triplet;
protected:
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- status = (struct vehicle_status_s *)status_sub->get_data();
-
pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
- pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
}
void send(const hrt_abstime t)
{
- (void)status_sub->update(t);
- (void)pos_sp_triplet_sub->update(t);
+ struct vehicle_status_s status;
+ struct position_setpoint_triplet_s pos_sp_triplet;
+
+ /* always send the heartbeat, independent of the update status of the topics */
+ if (!status_sub->update(&status)) {
+ /* if topic update failed fill it with defaults */
+ memset(&status, 0, sizeof(status));
+ }
+
+ if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) {
+ /* if topic update failed fill it with defaults */
+ memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
+ }
uint8_t mavlink_state = 0;
uint8_t mavlink_base_mode = 0;
uint32_t mavlink_custom_mode = 0;
- get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+ get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
mavlink_msg_heartbeat_send(_channel,
mavlink_system.type,
@@ -232,7 +279,6 @@ protected:
mavlink_base_mode,
mavlink_custom_mode,
mavlink_state);
-
}
};
@@ -240,44 +286,55 @@ protected:
class MavlinkStreamSysStatus : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamSysStatus::get_name_static();
+ }
+
+ static const char *get_name_static ()
{
return "SYS_STATUS";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_SYS_STATUS;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamSysStatus();
}
private:
MavlinkOrbSubscription *status_sub;
- struct vehicle_status_s *status;
protected:
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- status = (struct vehicle_status_s *)status_sub->get_data();
}
void send(const hrt_abstime t)
{
- status_sub->update(t);
- mavlink_msg_sys_status_send(_channel,
- status->onboard_control_sensors_present,
- status->onboard_control_sensors_enabled,
- status->onboard_control_sensors_health,
- status->load * 1000.0f,
- status->battery_voltage * 1000.0f,
- status->battery_current * 1000.0f,
- status->battery_remaining * 100.0f,
- status->drop_rate_comm,
- status->errors_comm,
- status->errors_count1,
- status->errors_count2,
- status->errors_count3,
- status->errors_count4);
+ struct vehicle_status_s status;
+
+ if (status_sub->update(&status)) {
+ mavlink_msg_sys_status_send(_channel,
+ status.onboard_control_sensors_present,
+ status.onboard_control_sensors_enabled,
+ status.onboard_control_sensors_health,
+ status.load * 1000.0f,
+ status.battery_voltage * 1000.0f,
+ status.battery_current * 100.0f,
+ status.battery_remaining * 100.0f,
+ status.drop_rate_comm,
+ status.errors_comm,
+ status.errors_count1,
+ status.errors_count2,
+ status.errors_count3,
+ status.errors_count4);
+ }
}
};
@@ -285,23 +342,29 @@ protected:
class MavlinkStreamHighresIMU : public MavlinkStream
{
public:
- MavlinkStreamHighresIMU() : MavlinkStream(), accel_timestamp(0), gyro_timestamp(0), mag_timestamp(0), baro_timestamp(0)
+ const char *get_name() const
{
+ return MavlinkStreamHighresIMU::get_name_static();
}
- const char *get_name()
+ static const char *get_name_static()
{
return "HIGHRES_IMU";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_HIGHRES_IMU;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamHighresIMU();
}
private:
MavlinkOrbSubscription *sensor_sub;
- struct sensor_combined_s *sensor;
+ uint64_t sensor_time;
uint64_t accel_timestamp;
uint64_t gyro_timestamp;
@@ -309,48 +372,57 @@ private:
uint64_t baro_timestamp;
protected:
+ explicit MavlinkStreamHighresIMU() : MavlinkStream(),
+ sensor_time(0),
+ accel_timestamp(0),
+ gyro_timestamp(0),
+ mag_timestamp(0),
+ baro_timestamp(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
sensor_sub = mavlink->add_orb_subscription(ORB_ID(sensor_combined));
- sensor = (struct sensor_combined_s *)sensor_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (sensor_sub->update(t)) {
+ struct sensor_combined_s sensor;
+
+ if (sensor_sub->update(&sensor_time, &sensor)) {
uint16_t fields_updated = 0;
- if (accel_timestamp != sensor->accelerometer_timestamp) {
+ if (accel_timestamp != sensor.accelerometer_timestamp) {
/* mark first three dimensions as changed */
fields_updated |= (1 << 0) | (1 << 1) | (1 << 2);
- accel_timestamp = sensor->accelerometer_timestamp;
+ accel_timestamp = sensor.accelerometer_timestamp;
}
- if (gyro_timestamp != sensor->timestamp) {
+ if (gyro_timestamp != sensor.timestamp) {
/* mark second group dimensions as changed */
fields_updated |= (1 << 3) | (1 << 4) | (1 << 5);
- gyro_timestamp = sensor->timestamp;
+ gyro_timestamp = sensor.timestamp;
}
- if (mag_timestamp != sensor->magnetometer_timestamp) {
+ if (mag_timestamp != sensor.magnetometer_timestamp) {
/* mark third group dimensions as changed */
fields_updated |= (1 << 6) | (1 << 7) | (1 << 8);
- mag_timestamp = sensor->magnetometer_timestamp;
+ mag_timestamp = sensor.magnetometer_timestamp;
}
- if (baro_timestamp != sensor->baro_timestamp) {
+ if (baro_timestamp != sensor.baro_timestamp) {
/* mark last group dimensions as changed */
fields_updated |= (1 << 9) | (1 << 11) | (1 << 12);
- baro_timestamp = sensor->baro_timestamp;
+ baro_timestamp = sensor.baro_timestamp;
}
mavlink_msg_highres_imu_send(_channel,
- sensor->timestamp,
- sensor->accelerometer_m_s2[0], sensor->accelerometer_m_s2[1], sensor->accelerometer_m_s2[2],
- sensor->gyro_rad_s[0], sensor->gyro_rad_s[1], sensor->gyro_rad_s[2],
- sensor->magnetometer_ga[0], sensor->magnetometer_ga[1], sensor->magnetometer_ga[2],
- sensor->baro_pres_mbar, sensor->differential_pressure_pa,
- sensor->baro_alt_meter, sensor->baro_temp_celcius,
+ sensor.timestamp,
+ sensor.accelerometer_m_s2[0], sensor.accelerometer_m_s2[1], sensor.accelerometer_m_s2[2],
+ sensor.gyro_rad_s[0], sensor.gyro_rad_s[1], sensor.gyro_rad_s[2],
+ sensor.magnetometer_ga[0], sensor.magnetometer_ga[1], sensor.magnetometer_ga[2],
+ sensor.baro_pres_mbar, sensor.differential_pressure_pa,
+ sensor.baro_alt_meter, sensor.baro_temp_celcius,
fields_updated);
}
}
@@ -360,34 +432,49 @@ protected:
class MavlinkStreamAttitude : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamAttitude::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "ATTITUDE";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_ATTITUDE;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamAttitude();
}
private:
MavlinkOrbSubscription *att_sub;
- struct vehicle_attitude_s *att;
+ uint64_t att_time;
protected:
+ explicit MavlinkStreamAttitude() : MavlinkStream(),
+ att_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
- att = (struct vehicle_attitude_s *)att_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (att_sub->update(t)) {
+ struct vehicle_attitude_s att;
+
+ if (att_sub->update(&att_time, &att)) {
mavlink_msg_attitude_send(_channel,
- att->timestamp / 1000,
- att->roll, att->pitch, att->yaw,
- att->rollspeed, att->pitchspeed, att->yawspeed);
+ att.timestamp / 1000,
+ att.roll, att.pitch, att.yaw,
+ att.rollspeed, att.pitchspeed, att.yawspeed);
}
}
};
@@ -396,39 +483,54 @@ protected:
class MavlinkStreamAttitudeQuaternion : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamAttitudeQuaternion::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "ATTITUDE_QUATERNION";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_ATTITUDE_QUATERNION;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamAttitudeQuaternion();
}
private:
MavlinkOrbSubscription *att_sub;
- struct vehicle_attitude_s *att;
+ uint64_t att_time;
protected:
+ explicit MavlinkStreamAttitudeQuaternion() : MavlinkStream(),
+ att_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
- att = (struct vehicle_attitude_s *)att_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (att_sub->update(t)) {
+ struct vehicle_attitude_s att;
+
+ if (att_sub->update(&att_time, &att)) {
mavlink_msg_attitude_quaternion_send(_channel,
- att->timestamp / 1000,
- att->q[0],
- att->q[1],
- att->q[2],
- att->q[3],
- att->rollspeed,
- att->pitchspeed,
- att->yawspeed);
+ att.timestamp / 1000,
+ att.q[0],
+ att.q[1],
+ att.q[2],
+ att.q[3],
+ att.rollspeed,
+ att.pitchspeed,
+ att.yawspeed);
}
}
};
@@ -437,71 +539,87 @@ protected:
class MavlinkStreamVFRHUD : public MavlinkStream
{
public:
- const char *get_name()
+
+ const char *get_name() const
+ {
+ return MavlinkStreamVFRHUD::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "VFR_HUD";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_VFR_HUD;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamVFRHUD();
}
private:
MavlinkOrbSubscription *att_sub;
- struct vehicle_attitude_s *att;
+ uint64_t att_time;
MavlinkOrbSubscription *pos_sub;
- struct vehicle_global_position_s *pos;
+ uint64_t pos_time;
MavlinkOrbSubscription *armed_sub;
- struct actuator_armed_s *armed;
+ uint64_t armed_time;
MavlinkOrbSubscription *act_sub;
- struct actuator_controls_s *act;
+ uint64_t act_time;
MavlinkOrbSubscription *airspeed_sub;
- struct airspeed_s *airspeed;
+ uint64_t airspeed_time;
protected:
+ explicit MavlinkStreamVFRHUD() : MavlinkStream(),
+ att_time(0),
+ pos_time(0),
+ armed_time(0),
+ act_time(0),
+ airspeed_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude));
- att = (struct vehicle_attitude_s *)att_sub->get_data();
-
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
- pos = (struct vehicle_global_position_s *)pos_sub->get_data();
-
armed_sub = mavlink->add_orb_subscription(ORB_ID(actuator_armed));
- armed = (struct actuator_armed_s *)armed_sub->get_data();
-
act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_controls_0));
- act = (struct actuator_controls_s *)act_sub->get_data();
-
airspeed_sub = mavlink->add_orb_subscription(ORB_ID(airspeed));
- airspeed = (struct airspeed_s *)airspeed_sub->get_data();
}
void send(const hrt_abstime t)
{
- bool updated = att_sub->update(t);
- updated |= pos_sub->update(t);
- updated |= armed_sub->update(t);
- updated |= act_sub->update(t);
- updated |= airspeed_sub->update(t);
+ struct vehicle_attitude_s att;
+ struct vehicle_global_position_s pos;
+ struct actuator_armed_s armed;
+ struct actuator_controls_s act;
+ struct airspeed_s airspeed;
+
+ 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);
if (updated) {
- float groundspeed = sqrtf(pos->vel_n * pos->vel_n + pos->vel_e * pos->vel_e);
- uint16_t heading = _wrap_2pi(att->yaw) * M_RAD_TO_DEG_F;
- float throttle = armed->armed ? act->control[3] * 100.0f : 0.0f;
+ float groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
+ uint16_t heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
+ float throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
mavlink_msg_vfr_hud_send(_channel,
- airspeed->true_airspeed_m_s,
+ airspeed.true_airspeed_m_s,
groundspeed,
heading,
throttle,
- pos->alt,
- -pos->vel_d);
+ pos.alt,
+ -pos.vel_d);
}
}
};
@@ -510,41 +628,56 @@ protected:
class MavlinkStreamGPSRawInt : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamGPSRawInt::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "GPS_RAW_INT";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_GPS_RAW_INT;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamGPSRawInt();
}
private:
MavlinkOrbSubscription *gps_sub;
- struct vehicle_gps_position_s *gps;
+ uint64_t gps_time;
protected:
+ explicit MavlinkStreamGPSRawInt() : MavlinkStream(),
+ gps_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
gps_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_gps_position));
- gps = (struct vehicle_gps_position_s *)gps_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (gps_sub->update(t)) {
+ struct vehicle_gps_position_s gps;
+
+ if (gps_sub->update(&gps_time, &gps)) {
mavlink_msg_gps_raw_int_send(_channel,
- gps->timestamp_position,
- gps->fix_type,
- gps->lat,
- gps->lon,
- gps->alt,
- cm_uint16_from_m_float(gps->eph_m),
- cm_uint16_from_m_float(gps->epv_m),
- gps->vel_m_s * 100.0f,
- _wrap_2pi(gps->cog_rad) * M_RAD_TO_DEG_F * 1e2f,
- gps->satellites_visible);
+ gps.timestamp_position,
+ gps.fix_type,
+ gps.lat,
+ gps.lon,
+ gps.alt,
+ 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_used);
}
}
};
@@ -553,49 +686,64 @@ protected:
class MavlinkStreamGlobalPositionInt : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamGlobalPositionInt::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "GLOBAL_POSITION_INT";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamGlobalPositionInt();
}
private:
MavlinkOrbSubscription *pos_sub;
- struct vehicle_global_position_s *pos;
+ uint64_t pos_time;
MavlinkOrbSubscription *home_sub;
- struct home_position_s *home;
+ uint64_t home_time;
protected:
+ explicit MavlinkStreamGlobalPositionInt() : MavlinkStream(),
+ pos_time(0),
+ home_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_global_position));
- pos = (struct vehicle_global_position_s *)pos_sub->get_data();
-
home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
- home = (struct home_position_s *)home_sub->get_data();
}
void send(const hrt_abstime t)
{
- bool updated = pos_sub->update(t);
- updated |= home_sub->update(t);
+ struct vehicle_global_position_s pos;
+ struct home_position_s home;
+
+ bool updated = pos_sub->update(&pos_time, &pos);
+ updated |= home_sub->update(&home_time, &home);
if (updated) {
mavlink_msg_global_position_int_send(_channel,
- pos->timestamp / 1000,
- pos->lat * 1e7,
- pos->lon * 1e7,
- pos->alt * 1000.0f,
- (pos->alt - home->alt) * 1000.0f,
- pos->vel_n * 100.0f,
- pos->vel_e * 100.0f,
- pos->vel_d * 100.0f,
- _wrap_2pi(pos->yaw) * M_RAD_TO_DEG_F * 100.0f);
+ pos.timestamp / 1000,
+ pos.lat * 1e7,
+ pos.lon * 1e7,
+ pos.alt * 1000.0f,
+ (pos.alt - home.alt) * 1000.0f,
+ pos.vel_n * 100.0f,
+ pos.vel_e * 100.0f,
+ pos.vel_d * 100.0f,
+ _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f);
}
}
};
@@ -604,38 +752,53 @@ protected:
class MavlinkStreamLocalPositionNED : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamLocalPositionNED::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "LOCAL_POSITION_NED";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_LOCAL_POSITION_NED;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamLocalPositionNED();
}
private:
MavlinkOrbSubscription *pos_sub;
- struct vehicle_local_position_s *pos;
+ uint64_t pos_time;
protected:
+ explicit MavlinkStreamLocalPositionNED() : MavlinkStream(),
+ pos_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position));
- pos = (struct vehicle_local_position_s *)pos_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (pos_sub->update(t)) {
+ struct vehicle_local_position_s pos;
+
+ if (pos_sub->update(&pos_time, &pos)) {
mavlink_msg_local_position_ned_send(_channel,
- pos->timestamp / 1000,
- pos->x,
- pos->y,
- pos->z,
- pos->vx,
- pos->vy,
- pos->vz);
+ pos.timestamp / 1000,
+ pos.x,
+ pos.y,
+ pos.z,
+ pos.vx,
+ pos.vy,
+ pos.vz);
}
}
};
@@ -645,38 +808,53 @@ protected:
class MavlinkStreamViconPositionEstimate : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamViconPositionEstimate::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "VICON_POSITION_ESTIMATE";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamViconPositionEstimate();
}
private:
MavlinkOrbSubscription *pos_sub;
- struct vehicle_vicon_position_s *pos;
+ uint64_t pos_time;
protected:
+ explicit MavlinkStreamViconPositionEstimate() : MavlinkStream(),
+ pos_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
pos_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_vicon_position));
- pos = (struct vehicle_vicon_position_s *)pos_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (pos_sub->update(t)) {
+ struct vehicle_vicon_position_s pos;
+
+ if (pos_sub->update(&pos_time, &pos)) {
mavlink_msg_vicon_position_estimate_send(_channel,
- pos->timestamp / 1000,
- pos->x,
- pos->y,
- pos->z,
- pos->roll,
- pos->pitch,
- pos->yaw);
+ pos.timestamp / 1000,
+ pos.x,
+ pos.y,
+ pos.z,
+ pos.roll,
+ pos.pitch,
+ pos.yaw);
}
}
};
@@ -685,70 +863,97 @@ protected:
class MavlinkStreamGPSGlobalOrigin : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamGPSGlobalOrigin::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "GPS_GLOBAL_ORIGIN";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamGPSGlobalOrigin();
}
private:
MavlinkOrbSubscription *home_sub;
- struct home_position_s *home;
protected:
void subscribe(Mavlink *mavlink)
{
home_sub = mavlink->add_orb_subscription(ORB_ID(home_position));
- home = (struct home_position_s *)home_sub->get_data();
}
void send(const hrt_abstime t)
{
-
/* we're sending the GPS home periodically to ensure the
* the GCS does pick it up at one point */
if (home_sub->is_published()) {
- home_sub->update(t);
+ struct home_position_s home;
- mavlink_msg_gps_global_origin_send(_channel,
- (int32_t)(home->lat * 1e7),
- (int32_t)(home->lon * 1e7),
- (int32_t)(home->alt) * 1000.0f);
+ if (home_sub->update(&home)) {
+ mavlink_msg_gps_global_origin_send(_channel,
+ (int32_t)(home.lat * 1e7),
+ (int32_t)(home.lon * 1e7),
+ (int32_t)(home.alt) * 1000.0f);
+ }
}
}
};
-
+template <int N>
class MavlinkStreamServoOutputRaw : public MavlinkStream
{
public:
- MavlinkStreamServoOutputRaw(unsigned int n) : MavlinkStream(), _n(n)
+ const char *get_name() const
{
- sprintf(_name, "SERVO_OUTPUT_RAW_%d", _n);
+ return MavlinkStreamServoOutputRaw<N>::get_name_static();
}
- const char *get_name()
+ uint8_t get_id()
{
- return _name;
+ return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
}
- MavlinkStream *new_instance()
+ static const char *get_name_static()
{
- return new MavlinkStreamServoOutputRaw(_n);
+ switch (N) {
+ case 0:
+ return "SERVO_OUTPUT_RAW_0";
+
+ case 1:
+ return "SERVO_OUTPUT_RAW_1";
+
+ case 2:
+ return "SERVO_OUTPUT_RAW_2";
+
+ case 3:
+ return "SERVO_OUTPUT_RAW_3";
+ }
+ }
+
+ static MavlinkStream *new_instance()
+ {
+ return new MavlinkStreamServoOutputRaw<N>();
}
private:
MavlinkOrbSubscription *act_sub;
- struct actuator_outputs_s *act;
-
- char _name[20];
- unsigned int _n;
+ uint64_t act_time;
protected:
+ explicit MavlinkStreamServoOutputRaw() : MavlinkStream(),
+ act_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
orb_id_t act_topics[] = {
@@ -758,24 +963,25 @@ protected:
ORB_ID(actuator_outputs_3)
};
- act_sub = mavlink->add_orb_subscription(act_topics[_n]);
- act = (struct actuator_outputs_s *)act_sub->get_data();
+ act_sub = mavlink->add_orb_subscription(act_topics[N]);
}
void send(const hrt_abstime t)
{
- if (act_sub->update(t)) {
+ struct actuator_outputs_s act;
+
+ if (act_sub->update(&act_time, &act)) {
mavlink_msg_servo_output_raw_send(_channel,
- act->timestamp / 1000,
- _n,
- act->output[0],
- act->output[1],
- act->output[2],
- act->output[3],
- act->output[4],
- act->output[5],
- act->output[6],
- act->output[7]);
+ act.timestamp / 1000,
+ N,
+ act.output[0],
+ act.output[1],
+ act.output[2],
+ act.output[3],
+ act.output[4],
+ act.output[5],
+ act.output[6],
+ act.output[7]);
}
}
};
@@ -784,51 +990,66 @@ protected:
class MavlinkStreamHILControls : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamHILControls::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "HIL_CONTROLS";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_HIL_CONTROLS;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamHILControls();
}
private:
MavlinkOrbSubscription *status_sub;
- struct vehicle_status_s *status;
+ uint64_t status_time;
MavlinkOrbSubscription *pos_sp_triplet_sub;
- struct position_setpoint_triplet_s *pos_sp_triplet;
+ uint64_t pos_sp_triplet_time;
MavlinkOrbSubscription *act_sub;
- struct actuator_outputs_s *act;
+ uint64_t act_time;
protected:
+ explicit MavlinkStreamHILControls() : MavlinkStream(),
+ status_time(0),
+ pos_sp_triplet_time(0),
+ act_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- status = (struct vehicle_status_s *)status_sub->get_data();
-
pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
- pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
-
act_sub = mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0));
- act = (struct actuator_outputs_s *)act_sub->get_data();
}
void send(const hrt_abstime t)
{
- bool updated = act_sub->update(t);
- (void)pos_sp_triplet_sub->update(t);
- (void)status_sub->update(t);
+ struct vehicle_status_s status;
+ struct position_setpoint_triplet_s pos_sp_triplet;
+ struct actuator_outputs_s act;
+
+ bool updated = act_sub->update(&act_time, &act);
+ updated |= pos_sp_triplet_sub->update(&pos_sp_triplet_time, &pos_sp_triplet);
+ updated |= status_sub->update(&status_time, &status);
- if (updated && (status->arming_state == ARMING_STATE_ARMED)) {
+ if (updated && (status.arming_state == ARMING_STATE_ARMED)) {
/* translate the current syste state to mavlink state and mode */
uint8_t mavlink_state;
uint8_t mavlink_base_mode;
uint32_t mavlink_custom_mode;
- get_mavlink_mode_state(status, pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+ get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
mavlink_system.type == MAV_TYPE_HEXAROTOR ||
@@ -857,7 +1078,7 @@ protected:
if (i < n) {
if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
/* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */
- out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
} else {
/* send 0 when disarmed */
@@ -887,12 +1108,12 @@ protected:
for (unsigned i = 0; i < 8; i++) {
if (i != 3) {
/* scale fake 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);
+ out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2);
} else {
/* scale fake PWM out 900..2100 us to 0..1 for throttle */
- out[i] = (act->output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
+ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN);
}
}
@@ -911,36 +1132,46 @@ protected:
class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "GLOBAL_POSITION_SETPOINT_INT";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamGlobalPositionSetpointInt();
}
private:
MavlinkOrbSubscription *pos_sp_triplet_sub;
- struct position_setpoint_triplet_s *pos_sp_triplet;
protected:
void subscribe(Mavlink *mavlink)
{
pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet));
- pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (pos_sp_triplet_sub->update(t)) {
+ struct position_setpoint_triplet_s pos_sp_triplet;
+
+ if (pos_sp_triplet_sub->update(&pos_sp_triplet)) {
mavlink_msg_global_position_setpoint_int_send(_channel,
MAV_FRAME_GLOBAL,
- (int32_t)(pos_sp_triplet->current.lat * 1e7),
- (int32_t)(pos_sp_triplet->current.lon * 1e7),
- (int32_t)(pos_sp_triplet->current.alt * 1000),
- (int16_t)(pos_sp_triplet->current.yaw * M_RAD_TO_DEG_F * 100.0f));
+ (int32_t)(pos_sp_triplet.current.lat * 1e7),
+ (int32_t)(pos_sp_triplet.current.lon * 1e7),
+ (int32_t)(pos_sp_triplet.current.alt * 1000),
+ (int16_t)(pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f));
}
}
};
@@ -949,36 +1180,51 @@ protected:
class MavlinkStreamLocalPositionSetpoint : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamLocalPositionSetpoint::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "LOCAL_POSITION_SETPOINT";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamLocalPositionSetpoint();
}
private:
MavlinkOrbSubscription *pos_sp_sub;
- struct vehicle_local_position_setpoint_s *pos_sp;
+ uint64_t pos_sp_time;
protected:
+ explicit MavlinkStreamLocalPositionSetpoint() : MavlinkStream(),
+ pos_sp_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
pos_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_setpoint));
- pos_sp = (struct vehicle_local_position_setpoint_s *)pos_sp_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (pos_sp_sub->update(t)) {
+ struct vehicle_local_position_setpoint_s pos_sp;
+
+ if (pos_sp_sub->update(&pos_sp_time, &pos_sp)) {
mavlink_msg_local_position_setpoint_send(_channel,
MAV_FRAME_LOCAL_NED,
- pos_sp->x,
- pos_sp->y,
- pos_sp->z,
- pos_sp->yaw);
+ pos_sp.x,
+ pos_sp.y,
+ pos_sp.z,
+ pos_sp.yaw);
}
}
};
@@ -987,36 +1233,51 @@ protected:
class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "ROLL_PITCH_YAW_THRUST_SETPOINT";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamRollPitchYawThrustSetpoint();
}
private:
MavlinkOrbSubscription *att_sp_sub;
- struct vehicle_attitude_setpoint_s *att_sp;
+ uint64_t att_sp_time;
protected:
+ explicit MavlinkStreamRollPitchYawThrustSetpoint() : MavlinkStream(),
+ att_sp_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint));
- att_sp = (struct vehicle_attitude_setpoint_s *)att_sp_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (att_sp_sub->update(t)) {
+ struct vehicle_attitude_setpoint_s att_sp;
+
+ if (att_sp_sub->update(&att_sp_time, &att_sp)) {
mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(_channel,
- att_sp->timestamp / 1000,
- att_sp->roll_body,
- att_sp->pitch_body,
- att_sp->yaw_body,
- att_sp->thrust);
+ att_sp.timestamp / 1000,
+ att_sp.roll_body,
+ att_sp.pitch_body,
+ att_sp.yaw_body,
+ att_sp.thrust);
}
}
};
@@ -1025,36 +1286,51 @@ protected:
class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamRollPitchYawRatesThrustSetpoint();
}
private:
MavlinkOrbSubscription *att_rates_sp_sub;
- struct vehicle_rates_setpoint_s *att_rates_sp;
+ uint64_t att_rates_sp_time;
protected:
+ explicit MavlinkStreamRollPitchYawRatesThrustSetpoint() : MavlinkStream(),
+ att_rates_sp_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_rates_sp_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint));
- att_rates_sp = (struct vehicle_rates_setpoint_s *)att_rates_sp_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (att_rates_sp_sub->update(t)) {
+ struct vehicle_rates_setpoint_s att_rates_sp;
+
+ if (att_rates_sp_sub->update(&att_rates_sp_time, &att_rates_sp)) {
mavlink_msg_roll_pitch_yaw_rates_thrust_setpoint_send(_channel,
- att_rates_sp->timestamp / 1000,
- att_rates_sp->roll,
- att_rates_sp->pitch,
- att_rates_sp->yaw,
- att_rates_sp->thrust);
+ att_rates_sp.timestamp / 1000,
+ att_rates_sp.roll,
+ att_rates_sp.pitch,
+ att_rates_sp.yaw,
+ att_rates_sp.thrust);
}
}
};
@@ -1063,47 +1339,87 @@ protected:
class MavlinkStreamRCChannelsRaw : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamRCChannelsRaw::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "RC_CHANNELS_RAW";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_RC_CHANNELS_RAW;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamRCChannelsRaw();
}
private:
MavlinkOrbSubscription *rc_sub;
- struct rc_input_values *rc;
+ uint64_t rc_time;
protected:
+ explicit MavlinkStreamRCChannelsRaw() : MavlinkStream(),
+ rc_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
rc_sub = mavlink->add_orb_subscription(ORB_ID(input_rc));
- rc = (struct rc_input_values *)rc_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (rc_sub->update(t)) {
+ struct rc_input_values rc;
+
+ if (rc_sub->update(&rc_time, &rc)) {
const unsigned port_width = 8;
- for (unsigned i = 0; (i * port_width) < rc->channel_count; i++) {
+ // Deprecated message (but still needed for compatibility!)
+ for (unsigned i = 0; (i * port_width) < rc.channel_count; i++) {
/* Channels are sent in MAVLink main loop at a fixed interval */
mavlink_msg_rc_channels_raw_send(_channel,
- rc->timestamp_publication / 1000,
+ rc.timestamp_publication / 1000,
i,
- (rc->channel_count > (i * port_width) + 0) ? rc->values[(i * port_width) + 0] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 1) ? rc->values[(i * port_width) + 1] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 2) ? rc->values[(i * port_width) + 2] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 3) ? rc->values[(i * port_width) + 3] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 4) ? rc->values[(i * port_width) + 4] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 5) ? rc->values[(i * port_width) + 5] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 6) ? rc->values[(i * port_width) + 6] : UINT16_MAX,
- (rc->channel_count > (i * port_width) + 7) ? rc->values[(i * port_width) + 7] : UINT16_MAX,
- rc->rssi);
+ (rc.channel_count > (i * port_width) + 0) ? rc.values[(i * port_width) + 0] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 1) ? rc.values[(i * port_width) + 1] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 2) ? rc.values[(i * port_width) + 2] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 3) ? rc.values[(i * port_width) + 3] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 4) ? rc.values[(i * port_width) + 4] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 5) ? rc.values[(i * port_width) + 5] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 6) ? rc.values[(i * port_width) + 6] : UINT16_MAX,
+ (rc.channel_count > (i * port_width) + 7) ? rc.values[(i * port_width) + 7] : UINT16_MAX,
+ rc.rssi);
}
+
+ // New message
+ mavlink_msg_rc_channels_send(_channel,
+ rc.timestamp_publication / 1000,
+ rc.channel_count,
+ ((rc.channel_count > 0) ? rc.values[0] : UINT16_MAX),
+ ((rc.channel_count > 1) ? rc.values[1] : UINT16_MAX),
+ ((rc.channel_count > 2) ? rc.values[2] : UINT16_MAX),
+ ((rc.channel_count > 3) ? rc.values[3] : UINT16_MAX),
+ ((rc.channel_count > 4) ? rc.values[4] : UINT16_MAX),
+ ((rc.channel_count > 5) ? rc.values[5] : UINT16_MAX),
+ ((rc.channel_count > 6) ? rc.values[6] : UINT16_MAX),
+ ((rc.channel_count > 7) ? rc.values[7] : UINT16_MAX),
+ ((rc.channel_count > 8) ? rc.values[8] : UINT16_MAX),
+ ((rc.channel_count > 9) ? rc.values[9] : UINT16_MAX),
+ ((rc.channel_count > 10) ? rc.values[10] : UINT16_MAX),
+ ((rc.channel_count > 11) ? rc.values[11] : UINT16_MAX),
+ ((rc.channel_count > 12) ? rc.values[12] : UINT16_MAX),
+ ((rc.channel_count > 13) ? rc.values[13] : UINT16_MAX),
+ ((rc.channel_count > 14) ? rc.values[14] : UINT16_MAX),
+ ((rc.channel_count > 15) ? rc.values[15] : UINT16_MAX),
+ ((rc.channel_count > 16) ? rc.values[16] : UINT16_MAX),
+ ((rc.channel_count > 17) ? rc.values[17] : UINT16_MAX),
+ rc.rssi);
}
}
};
@@ -1112,36 +1428,51 @@ protected:
class MavlinkStreamManualControl : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamManualControl::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "MANUAL_CONTROL";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_MANUAL_CONTROL;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamManualControl();
}
private:
MavlinkOrbSubscription *manual_sub;
- struct manual_control_setpoint_s *manual;
+ uint64_t manual_time;
protected:
+ explicit MavlinkStreamManualControl() : MavlinkStream(),
+ manual_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
manual_sub = mavlink->add_orb_subscription(ORB_ID(manual_control_setpoint));
- manual = (struct manual_control_setpoint_s *)manual_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (manual_sub->update(t)) {
+ struct manual_control_setpoint_s manual;
+
+ if (manual_sub->update(&manual_time, &manual)) {
mavlink_msg_manual_control_send(_channel,
mavlink_system.sysid,
- manual->roll * 1000,
- manual->pitch * 1000,
- manual->yaw * 1000,
- manual->throttle * 1000,
+ manual.x * 1000,
+ manual.y * 1000,
+ manual.z * 1000,
+ manual.r * 1000,
0);
}
}
@@ -1151,37 +1482,52 @@ protected:
class MavlinkStreamOpticalFlow : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamOpticalFlow::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "OPTICAL_FLOW";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_OPTICAL_FLOW;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamOpticalFlow();
}
private:
MavlinkOrbSubscription *flow_sub;
- struct optical_flow_s *flow;
+ uint64_t flow_time;
protected:
+ explicit MavlinkStreamOpticalFlow() : MavlinkStream(),
+ flow_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
flow_sub = mavlink->add_orb_subscription(ORB_ID(optical_flow));
- flow = (struct optical_flow_s *)flow_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (flow_sub->update(t)) {
+ struct optical_flow_s flow;
+
+ if (flow_sub->update(&flow_time, &flow)) {
mavlink_msg_optical_flow_send(_channel,
- flow->timestamp,
- flow->sensor_id,
- flow->flow_raw_x, flow->flow_raw_y,
- flow->flow_comp_x_m, flow->flow_comp_y_m,
- flow->quality,
- flow->ground_distance_m);
+ flow.timestamp,
+ flow.sensor_id,
+ flow.flow_raw_x, flow.flow_raw_y,
+ flow.flow_comp_x_m, flow.flow_comp_y_m,
+ flow.quality,
+ flow.ground_distance_m);
}
}
};
@@ -1189,47 +1535,62 @@ protected:
class MavlinkStreamAttitudeControls : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamAttitudeControls::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "ATTITUDE_CONTROLS";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return 0;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamAttitudeControls();
}
private:
MavlinkOrbSubscription *att_ctrl_sub;
- struct actuator_controls_s *att_ctrl;
+ uint64_t att_ctrl_time;
protected:
+ explicit MavlinkStreamAttitudeControls() : MavlinkStream(),
+ att_ctrl_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
att_ctrl_sub = mavlink->add_orb_subscription(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
- att_ctrl = (struct actuator_controls_s *)att_ctrl_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (att_ctrl_sub->update(t)) {
+ struct actuator_controls_s att_ctrl;
+
+ if (att_ctrl_sub->update(&att_ctrl_time, &att_ctrl)) {
/* send, add spaces so that string buffer is at least 10 chars long */
mavlink_msg_named_value_float_send(_channel,
- att_ctrl->timestamp / 1000,
+ att_ctrl.timestamp / 1000,
"rll ctrl ",
- att_ctrl->control[0]);
+ att_ctrl.control[0]);
mavlink_msg_named_value_float_send(_channel,
- att_ctrl->timestamp / 1000,
+ att_ctrl.timestamp / 1000,
"ptch ctrl ",
- att_ctrl->control[1]);
+ att_ctrl.control[1]);
mavlink_msg_named_value_float_send(_channel,
- att_ctrl->timestamp / 1000,
+ att_ctrl.timestamp / 1000,
"yaw ctrl ",
- att_ctrl->control[2]);
+ att_ctrl.control[2]);
mavlink_msg_named_value_float_send(_channel,
- att_ctrl->timestamp / 1000,
+ att_ctrl.timestamp / 1000,
"thr ctrl ",
- att_ctrl->control[3]);
+ att_ctrl.control[3]);
}
}
};
@@ -1237,37 +1598,52 @@ protected:
class MavlinkStreamNamedValueFloat : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamNamedValueFloat::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "NAMED_VALUE_FLOAT";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamNamedValueFloat();
}
private:
MavlinkOrbSubscription *debug_sub;
- struct debug_key_value_s *debug;
+ uint64_t debug_time;
protected:
+ explicit MavlinkStreamNamedValueFloat() : MavlinkStream(),
+ debug_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
debug_sub = mavlink->add_orb_subscription(ORB_ID(debug_key_value));
- debug = (struct debug_key_value_s *)debug_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (debug_sub->update(t)) {
+ struct debug_key_value_s debug;
+
+ if (debug_sub->update(&debug_time, &debug)) {
/* enforce null termination */
- debug->key[sizeof(debug->key) - 1] = '\0';
+ debug.key[sizeof(debug.key) - 1] = '\0';
mavlink_msg_named_value_float_send(_channel,
- debug->timestamp_ms,
- debug->key,
- debug->value);
+ debug.timestamp_ms,
+ debug.key,
+ debug.value);
}
}
};
@@ -1275,33 +1651,42 @@ protected:
class MavlinkStreamCameraCapture : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamCameraCapture::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "CAMERA_CAPTURE";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return 0;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamCameraCapture();
}
private:
MavlinkOrbSubscription *status_sub;
- struct vehicle_status_s *status;
protected:
void subscribe(Mavlink *mavlink)
{
status_sub = mavlink->add_orb_subscription(ORB_ID(vehicle_status));
- status = (struct vehicle_status_s *)status_sub->get_data();
}
void send(const hrt_abstime t)
{
- (void)status_sub->update(t);
+ struct vehicle_status_s status;
+ (void)status_sub->update(&status);
- if (status->arming_state == ARMING_STATE_ARMED
- || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+ if (status.arming_state == ARMING_STATE_ARMED
+ || status.arming_state == ARMING_STATE_ARMED_ERROR) {
/* send camera capture on */
mavlink_msg_command_long_send(_channel, mavlink_system.sysid, 0, MAV_CMD_DO_CONTROL_VIDEO, 0, 0, 0, 0, 1, 0, 0, 0);
@@ -1316,34 +1701,49 @@ protected:
class MavlinkStreamDistanceSensor : public MavlinkStream
{
public:
- const char *get_name()
+ const char *get_name() const
+ {
+ return MavlinkStreamDistanceSensor::get_name_static();
+ }
+
+ static const char *get_name_static()
{
return "DISTANCE_SENSOR";
}
- MavlinkStream *new_instance()
+ uint8_t get_id()
+ {
+ return MAVLINK_MSG_ID_DISTANCE_SENSOR;
+ }
+
+ static MavlinkStream *new_instance()
{
return new MavlinkStreamDistanceSensor();
}
private:
MavlinkOrbSubscription *range_sub;
- struct range_finder_report *range;
+ uint64_t range_time;
protected:
+ explicit MavlinkStreamDistanceSensor() : MavlinkStream(),
+ range_time(0)
+ {}
+
void subscribe(Mavlink *mavlink)
{
range_sub = mavlink->add_orb_subscription(ORB_ID(sensor_range_finder));
- range = (struct range_finder_report *)range_sub->get_data();
}
void send(const hrt_abstime t)
{
- if (range_sub->update(t)) {
+ struct range_finder_report range;
+
+ if (range_sub->update(&range_time, &range)) {
uint8_t type;
- switch (range->type) {
+ switch (range.type) {
case RANGE_FINDER_TYPE_LASER:
type = MAV_DISTANCE_SENSOR_LASER;
break;
@@ -1353,39 +1753,40 @@ protected:
uint8_t orientation = 0;
uint8_t covariance = 20;
- mavlink_msg_distance_sensor_send(_channel, range->timestamp / 1000, type, id, orientation,
- range->minimum_distance*100, range->maximum_distance*100, range->distance*100, covariance);
+ mavlink_msg_distance_sensor_send(_channel, range.timestamp / 1000, type, id, orientation,
+ range.minimum_distance*100, range.maximum_distance*100, range.distance*100, covariance);
}
}
};
-MavlinkStream *streams_list[] = {
- new MavlinkStreamHeartbeat(),
- new MavlinkStreamSysStatus(),
- new MavlinkStreamHighresIMU(),
- new MavlinkStreamAttitude(),
- new MavlinkStreamAttitudeQuaternion(),
- new MavlinkStreamVFRHUD(),
- new MavlinkStreamGPSRawInt(),
- new MavlinkStreamGlobalPositionInt(),
- new MavlinkStreamLocalPositionNED(),
- new MavlinkStreamGPSGlobalOrigin(),
- new MavlinkStreamServoOutputRaw(0),
- new MavlinkStreamServoOutputRaw(1),
- new MavlinkStreamServoOutputRaw(2),
- new MavlinkStreamServoOutputRaw(3),
- new MavlinkStreamHILControls(),
- new MavlinkStreamGlobalPositionSetpointInt(),
- new MavlinkStreamLocalPositionSetpoint(),
- new MavlinkStreamRollPitchYawThrustSetpoint(),
- new MavlinkStreamRollPitchYawRatesThrustSetpoint(),
- new MavlinkStreamRCChannelsRaw(),
- new MavlinkStreamManualControl(),
- new MavlinkStreamOpticalFlow(),
- new MavlinkStreamAttitudeControls(),
- new MavlinkStreamNamedValueFloat(),
- new MavlinkStreamCameraCapture(),
- new MavlinkStreamDistanceSensor(),
- new MavlinkStreamViconPositionEstimate(),
+
+StreamListItem *streams_list[] = {
+ new StreamListItem(&MavlinkStreamHeartbeat::new_instance, &MavlinkStreamHeartbeat::get_name_static),
+ new StreamListItem(&MavlinkStreamSysStatus::new_instance, &MavlinkStreamSysStatus::get_name_static),
+ new StreamListItem(&MavlinkStreamHighresIMU::new_instance, &MavlinkStreamHighresIMU::get_name_static),
+ new StreamListItem(&MavlinkStreamAttitude::new_instance, &MavlinkStreamAttitude::get_name_static),
+ new StreamListItem(&MavlinkStreamAttitudeQuaternion::new_instance, &MavlinkStreamAttitudeQuaternion::get_name_static),
+ new StreamListItem(&MavlinkStreamVFRHUD::new_instance, &MavlinkStreamVFRHUD::get_name_static),
+ new StreamListItem(&MavlinkStreamGPSRawInt::new_instance, &MavlinkStreamGPSRawInt::get_name_static),
+ new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static),
+ new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static),
+ new StreamListItem(&MavlinkStreamGPSGlobalOrigin::new_instance, &MavlinkStreamGPSGlobalOrigin::get_name_static),
+ new StreamListItem(&MavlinkStreamServoOutputRaw<0>::new_instance, &MavlinkStreamServoOutputRaw<0>::get_name_static),
+ new StreamListItem(&MavlinkStreamServoOutputRaw<1>::new_instance, &MavlinkStreamServoOutputRaw<1>::get_name_static),
+ 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(&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(&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),
+ new StreamListItem(&MavlinkStreamAttitudeControls::new_instance, &MavlinkStreamAttitudeControls::get_name_static),
+ new StreamListItem(&MavlinkStreamNamedValueFloat::new_instance, &MavlinkStreamNamedValueFloat::get_name_static),
+ new StreamListItem(&MavlinkStreamCameraCapture::new_instance, &MavlinkStreamCameraCapture::get_name_static),
+ new StreamListItem(&MavlinkStreamDistanceSensor::new_instance, &MavlinkStreamDistanceSensor::get_name_static),
+ new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static),
nullptr
};
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-13 04:29:15 +0000