diff options
author | M.H.Kabir <mhkabir98@gmail.com> | 2014-12-16 14:08:28 +0530 |
---|---|---|
committer | M.H.Kabir <mhkabir98@gmail.com> | 2014-12-16 14:08:28 +0530 |
commit | f7da65f819db2e5d6602cd494119d8dccfd5ed56 (patch) | |
tree | 7f23b90f22ac3bb1c4258944823dc9c4fce70844 | |
parent | a5591b47919dbe7f61024c95d90e6333460e5b11 (diff) | |
download | px4-firmware-f7da65f819db2e5d6602cd494119d8dccfd5ed56.tar.gz px4-firmware-f7da65f819db2e5d6602cd494119d8dccfd5ed56.tar.bz2 px4-firmware-f7da65f819db2e5d6602cd494119d8dccfd5ed56.zip |
Fix formatting
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 726 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 211 |
2 files changed, 513 insertions, 424 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index bad788ad0..f674d6501 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -121,93 +121,91 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set 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; - - 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_MISSION; - break; - - 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; - - 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; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; - 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 - | 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; - - case NAVIGATION_STATE_OFFBOARD: - *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_OFFBOARD; - break; - - case NAVIGATION_STATE_MAX: - /* this is an unused case, ignore */ - break; + 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_MISSION; + break; + + 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; + + 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; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + 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 + | 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; + + case NAVIGATION_STATE_OFFBOARD: + *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_OFFBOARD; + break; + + case NAVIGATION_STATE_MAX: + /* this is an unused case, ignore */ + break; } @@ -240,23 +238,28 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set class MavlinkStreamHeartbeat : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamHeartbeat::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "HEARTBEAT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_HEARTBEAT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamHeartbeat(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_HEARTBEAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -270,7 +273,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHeartbeat(MavlinkStreamHeartbeat &); - MavlinkStreamHeartbeat &operator = (const MavlinkStreamHeartbeat &); + MavlinkStreamHeartbeat& operator = (const MavlinkStreamHeartbeat &); protected: explicit MavlinkStreamHeartbeat(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -278,7 +281,8 @@ protected: _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; @@ -309,31 +313,34 @@ protected: class MavlinkStreamStatustext : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamStatustext::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "STATUSTEXT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_STATUSTEXT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamStatustext(mavlink); } unsigned get_size() { - return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + - MAVLINK_NUM_NON_PAYLOAD_BYTES); + return mavlink_logbuffer_is_empty(_mavlink->get_logbuffer()) ? 0 : (MAVLINK_MSG_ID_STATUSTEXT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); } private: /* do not allow top copying this class */ MavlinkStreamStatustext(MavlinkStreamStatustext &); - MavlinkStreamStatustext &operator = (const MavlinkStreamStatustext &); + MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); FILE *fp = nullptr; protected: @@ -346,7 +353,8 @@ protected: } } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { if (!mavlink_logbuffer_is_empty(_mavlink->get_logbuffer())) { struct mavlink_logmessage logmsg; int lb_ret = mavlink_logbuffer_read(_mavlink->get_logbuffer(), &logmsg); @@ -365,7 +373,6 @@ protected: fputs(msg.text, fp); fputs("\n", fp); fsync(fileno(fp)); - } else { /* string to hold the path to the log */ char log_file_name[32] = ""; @@ -392,19 +399,23 @@ protected: class MavlinkStreamCommandLong : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamCommandLong::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "COMMAND_LONG"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_COMMAND_LONG; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamCommandLong(mavlink); } @@ -418,7 +429,7 @@ private: /* do not allow top copying this class */ MavlinkStreamCommandLong(MavlinkStreamCommandLong &); - MavlinkStreamCommandLong &operator = (const MavlinkStreamCommandLong &); + MavlinkStreamCommandLong& operator = (const MavlinkStreamCommandLong &); protected: explicit MavlinkStreamCommandLong(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -426,7 +437,8 @@ protected: _cmd_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_command_s cmd; if (_cmd_sub->update(&_cmd_time, &cmd)) { @@ -455,23 +467,28 @@ protected: class MavlinkStreamSysStatus : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamSysStatus::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "SYS_STATUS"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_SYS_STATUS; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamSysStatus(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_SYS_STATUS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -480,14 +497,15 @@ private: /* do not allow top copying this class */ MavlinkStreamSysStatus(MavlinkStreamSysStatus &); - MavlinkStreamSysStatus &operator = (const MavlinkStreamSysStatus &); + MavlinkStreamSysStatus& operator = (const MavlinkStreamSysStatus &); protected: explicit MavlinkStreamSysStatus(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_status_s status; if (_status_sub->update(&status)) { @@ -516,23 +534,28 @@ protected: class MavlinkStreamHighresIMU : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamHighresIMU::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "HIGHRES_IMU"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_HIGHRES_IMU; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamHighresIMU(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_HIGHRES_IMU_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -547,7 +570,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHighresIMU(MavlinkStreamHighresIMU &); - MavlinkStreamHighresIMU &operator = (const MavlinkStreamHighresIMU &); + MavlinkStreamHighresIMU& operator = (const MavlinkStreamHighresIMU &); protected: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -559,7 +582,8 @@ protected: _baro_timestamp(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct sensor_combined_s sensor; if (_sensor_sub->update(&_sensor_time, &sensor)) { @@ -616,23 +640,28 @@ protected: class MavlinkStreamAttitude : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamAttitude::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "ATTITUDE"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_ATTITUDE; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamAttitude(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_ATTITUDE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -642,7 +671,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &); - MavlinkStreamAttitude &operator = (const MavlinkStreamAttitude &); + MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); protected: @@ -651,7 +680,8 @@ protected: _att_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_attitude_s att; if (_att_sub->update(&_att_time, &att)) { @@ -674,23 +704,28 @@ protected: class MavlinkStreamAttitudeQuaternion : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamAttitudeQuaternion::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "ATTITUDE_QUATERNION"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamAttitudeQuaternion(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -700,7 +735,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &); - MavlinkStreamAttitudeQuaternion &operator = (const MavlinkStreamAttitudeQuaternion &); + MavlinkStreamAttitudeQuaternion& operator = (const MavlinkStreamAttitudeQuaternion &); protected: explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -708,7 +743,8 @@ protected: _att_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_attitude_s att; if (_att_sub->update(&_att_time, &att)) { @@ -733,23 +769,28 @@ class MavlinkStreamVFRHUD : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamVFRHUD::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "VFR_HUD"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_VFR_HUD; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamVFRHUD(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_VFR_HUD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -771,7 +812,7 @@ private: /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); - MavlinkStreamVFRHUD &operator = (const MavlinkStreamVFRHUD &); + MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); protected: explicit MavlinkStreamVFRHUD(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -787,7 +828,8 @@ protected: _airspeed_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_attitude_s att; struct vehicle_global_position_s pos; struct actuator_armed_s armed; @@ -819,23 +861,28 @@ protected: class MavlinkStreamGPSRawInt : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamGPSRawInt::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "GPS_RAW_INT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_GPS_RAW_INT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamGPSRawInt(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_GPS_RAW_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -845,7 +892,7 @@ private: /* do not allow top copying this class */ MavlinkStreamGPSRawInt(MavlinkStreamGPSRawInt &); - MavlinkStreamGPSRawInt &operator = (const MavlinkStreamGPSRawInt &); + MavlinkStreamGPSRawInt& operator = (const MavlinkStreamGPSRawInt &); protected: explicit MavlinkStreamGPSRawInt(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -853,7 +900,8 @@ protected: _gps_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_gps_position_s gps; if (_gps_sub->update(&_gps_time, &gps)) { @@ -867,8 +915,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); } @@ -965,23 +1013,28 @@ protected: class MavlinkStreamGlobalPositionInt : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamGlobalPositionInt::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "GLOBAL_POSITION_INT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamGlobalPositionInt(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -994,7 +1047,7 @@ private: /* do not allow top copying this class */ MavlinkStreamGlobalPositionInt(MavlinkStreamGlobalPositionInt &); - MavlinkStreamGlobalPositionInt &operator = (const MavlinkStreamGlobalPositionInt &); + MavlinkStreamGlobalPositionInt& operator = (const MavlinkStreamGlobalPositionInt &); protected: explicit MavlinkStreamGlobalPositionInt(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1004,7 +1057,8 @@ protected: _home_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_global_position_s pos; struct home_position_s home; @@ -1033,23 +1087,28 @@ protected: class MavlinkStreamLocalPositionNED : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamLocalPositionNED::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "LOCAL_POSITION_NED"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_LOCAL_POSITION_NED; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamLocalPositionNED(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_LOCAL_POSITION_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1059,7 +1118,7 @@ private: /* do not allow top copying this class */ MavlinkStreamLocalPositionNED(MavlinkStreamLocalPositionNED &); - MavlinkStreamLocalPositionNED &operator = (const MavlinkStreamLocalPositionNED &); + MavlinkStreamLocalPositionNED& operator = (const MavlinkStreamLocalPositionNED &); protected: explicit MavlinkStreamLocalPositionNED(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1067,7 +1126,8 @@ protected: _pos_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_local_position_s pos; if (_pos_sub->update(&_pos_time, &pos)) { @@ -1090,23 +1150,28 @@ protected: class MavlinkStreamViconPositionEstimate : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamViconPositionEstimate::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "VICON_POSITION_ESTIMATE"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamViconPositionEstimate(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1116,7 +1181,7 @@ private: /* do not allow top copying this class */ MavlinkStreamViconPositionEstimate(MavlinkStreamViconPositionEstimate &); - MavlinkStreamViconPositionEstimate &operator = (const MavlinkStreamViconPositionEstimate &); + MavlinkStreamViconPositionEstimate& operator = (const MavlinkStreamViconPositionEstimate &); protected: explicit MavlinkStreamViconPositionEstimate(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1124,7 +1189,8 @@ protected: _pos_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_vicon_position_s pos; if (_pos_sub->update(&_pos_time, &pos)) { @@ -1147,23 +1213,28 @@ protected: class MavlinkStreamGPSGlobalOrigin : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamGPSGlobalOrigin::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "GPS_GLOBAL_ORIGIN"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamGPSGlobalOrigin(mavlink); } - unsigned get_size() { + unsigned get_size() + { return _home_sub->is_published() ? (MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -1172,14 +1243,15 @@ private: /* do not allow top copying this class */ MavlinkStreamGPSGlobalOrigin(MavlinkStreamGPSGlobalOrigin &); - MavlinkStreamGPSGlobalOrigin &operator = (const MavlinkStreamGPSGlobalOrigin &); + MavlinkStreamGPSGlobalOrigin& operator = (const MavlinkStreamGPSGlobalOrigin &); protected: explicit MavlinkStreamGPSGlobalOrigin(Mavlink *mavlink) : MavlinkStream(mavlink), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))) {} - void send(const hrt_abstime t) { + 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()) { @@ -1203,35 +1275,40 @@ template <int N> class MavlinkStreamServoOutputRaw : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamServoOutputRaw<N>::get_name_static(); } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; } - static const char *get_name_static() { + static const char *get_name_static() + { switch (N) { - case 0: + case 0: return "SERVO_OUTPUT_RAW_0"; - case 1: + case 1: return "SERVO_OUTPUT_RAW_1"; - case 2: + case 2: return "SERVO_OUTPUT_RAW_2"; - case 3: + case 3: return "SERVO_OUTPUT_RAW_3"; } } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamServoOutputRaw<N>(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1241,12 +1318,13 @@ private: /* do not allow top copying this class */ MavlinkStreamServoOutputRaw(MavlinkStreamServoOutputRaw &); - MavlinkStreamServoOutputRaw &operator = (const MavlinkStreamServoOutputRaw &); + MavlinkStreamServoOutputRaw& operator = (const MavlinkStreamServoOutputRaw &); protected: explicit MavlinkStreamServoOutputRaw(Mavlink *mavlink) : MavlinkStream(mavlink), _act_sub(nullptr), - _act_time(0) { + _act_time(0) + { orb_id_t act_topics[] = { ORB_ID(actuator_outputs_0), ORB_ID(actuator_outputs_1), @@ -1257,7 +1335,8 @@ protected: _act_sub = _mavlink->add_orb_subscription(act_topics[N]); } - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct actuator_outputs_s act; if (_act_sub->update(&_act_time, &act)) { @@ -1283,23 +1362,28 @@ protected: class MavlinkStreamHILControls : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamHILControls::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "HIL_CONTROLS"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_HIL_CONTROLS; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamHILControls(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_HIL_CONTROLS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1315,7 +1399,7 @@ private: /* do not allow top copying this class */ MavlinkStreamHILControls(MavlinkStreamHILControls &); - MavlinkStreamHILControls &operator = (const MavlinkStreamHILControls &); + MavlinkStreamHILControls& operator = (const MavlinkStreamHILControls &); protected: explicit MavlinkStreamHILControls(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1327,7 +1411,8 @@ protected: _act_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_status_s status; struct position_setpoint_triplet_s pos_sp_triplet; struct actuator_outputs_s act; @@ -1443,23 +1528,28 @@ protected: class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamPositionTargetGlobalInt::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "POSITION_TARGET_GLOBAL_INT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamPositionTargetGlobalInt(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1468,18 +1558,19 @@ private: /* do not allow top copying this class */ MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &); - MavlinkStreamPositionTargetGlobalInt &operator = (const MavlinkStreamPositionTargetGlobalInt &); + MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &); protected: explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct position_setpoint_triplet_s pos_sp_triplet; if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_position_target_global_int_t msg {}; + mavlink_position_target_global_int_t msg{}; msg.coordinate_frame = MAV_FRAME_GLOBAL; msg.lat_int = pos_sp_triplet.current.lat * 1e7; @@ -1495,23 +1586,28 @@ protected: class MavlinkStreamLocalPositionSetpoint : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamLocalPositionSetpoint::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "POSITION_TARGET_LOCAL_NED"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamLocalPositionSetpoint(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1521,7 +1617,7 @@ private: /* do not allow top copying this class */ MavlinkStreamLocalPositionSetpoint(MavlinkStreamLocalPositionSetpoint &); - MavlinkStreamLocalPositionSetpoint &operator = (const MavlinkStreamLocalPositionSetpoint &); + MavlinkStreamLocalPositionSetpoint& operator = (const MavlinkStreamLocalPositionSetpoint &); protected: explicit MavlinkStreamLocalPositionSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1529,11 +1625,12 @@ protected: _pos_sp_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_local_position_setpoint_s pos_sp; if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_position_target_local_ned_t msg {}; + mavlink_position_target_local_ned_t msg{}; msg.time_boot_ms = pos_sp.timestamp / 1000; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; @@ -1550,23 +1647,28 @@ protected: class MavlinkStreamAttitudeTarget : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamAttitudeTarget::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "ATTITUDE_TARGET"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_ATTITUDE_TARGET; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamAttitudeTarget(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1578,7 +1680,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &); - MavlinkStreamAttitudeTarget &operator = (const MavlinkStreamAttitudeTarget &); + MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &); protected: explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1588,7 +1690,8 @@ protected: _att_rates_sp_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_attitude_setpoint_s att_sp; if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { @@ -1596,7 +1699,7 @@ protected: struct vehicle_rates_setpoint_s att_rates_sp; (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); - mavlink_attitude_target_t msg {}; + mavlink_attitude_target_t msg{}; 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); @@ -1616,26 +1719,30 @@ protected: class MavlinkStreamRCChannelsRaw : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamRCChannelsRaw::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "RC_CHANNELS_RAW"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_RC_CHANNELS_RAW; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamRCChannelsRaw(mavlink); } - unsigned get_size() { - return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + - MAVLINK_NUM_NON_PAYLOAD_BYTES) + - MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; + unsigned get_size() + { + return _rc_sub->is_published() ? ((RC_INPUT_MAX_CHANNELS / 8) * (MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) + + MAVLINK_MSG_ID_RC_CHANNELS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } private: @@ -1644,7 +1751,7 @@ private: /* do not allow top copying this class */ MavlinkStreamRCChannelsRaw(MavlinkStreamRCChannelsRaw &); - MavlinkStreamRCChannelsRaw &operator = (const MavlinkStreamRCChannelsRaw &); + MavlinkStreamRCChannelsRaw& operator = (const MavlinkStreamRCChannelsRaw &); protected: explicit MavlinkStreamRCChannelsRaw(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1652,7 +1759,8 @@ protected: _rc_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct rc_input_values rc; if (_rc_sub->update(&_rc_time, &rc)) { @@ -1728,24 +1836,20 @@ protected: msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15; switch (rc.input_source) { - case RC_INPUT_SOURCE_PX4FMU_PPM: - + case RC_INPUT_SOURCE_PX4FMU_PPM: /* fallthrough */ - case RC_INPUT_SOURCE_PX4IO_PPM: - msg.rssi |= (0 << 4); - break; - - case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: - msg.rssi |= (1 << 4); - break; - - case RC_INPUT_SOURCE_PX4IO_SBUS: - msg.rssi |= (2 << 4); - break; - - case RC_INPUT_SOURCE_PX4IO_ST24: - msg.rssi |= (3 << 4); - break; + case RC_INPUT_SOURCE_PX4IO_PPM: + msg.rssi |= (0 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SPEKTRUM: + msg.rssi |= (1 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_SBUS: + msg.rssi |= (2 << 4); + break; + case RC_INPUT_SOURCE_PX4IO_ST24: + msg.rssi |= (3 << 4); + break; } if (rc.rc_lost) { @@ -1762,23 +1866,28 @@ protected: class MavlinkStreamManualControl : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamManualControl::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "MANUAL_CONTROL"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_MANUAL_CONTROL; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamManualControl(mavlink); } - unsigned get_size() { + unsigned get_size() + { return _manual_sub->is_published() ? (MAVLINK_MSG_ID_MANUAL_CONTROL_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -1788,7 +1897,7 @@ private: /* do not allow top copying this class */ MavlinkStreamManualControl(MavlinkStreamManualControl &); - MavlinkStreamManualControl &operator = (const MavlinkStreamManualControl &); + MavlinkStreamManualControl& operator = (const MavlinkStreamManualControl &); protected: explicit MavlinkStreamManualControl(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1796,7 +1905,8 @@ protected: _manual_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct manual_control_setpoint_s manual; if (_manual_sub->update(&_manual_time, &manual)) { @@ -1817,23 +1927,28 @@ protected: class MavlinkStreamOpticalFlowRad : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamOpticalFlowRad::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "OPTICAL_FLOW_RAD"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_OPTICAL_FLOW_RAD; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamOpticalFlowRad(mavlink); } - unsigned get_size() { + unsigned get_size() + { return _flow_sub->is_published() ? (MAVLINK_MSG_ID_OPTICAL_FLOW_RAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -1843,7 +1958,7 @@ private: /* do not allow top copying this class */ MavlinkStreamOpticalFlowRad(MavlinkStreamOpticalFlowRad &); - MavlinkStreamOpticalFlowRad &operator = (const MavlinkStreamOpticalFlowRad &); + MavlinkStreamOpticalFlowRad& operator = (const MavlinkStreamOpticalFlowRad &); protected: explicit MavlinkStreamOpticalFlowRad(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1851,7 +1966,8 @@ protected: _flow_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct optical_flow_s flow; if (_flow_sub->update(&_flow_time, &flow)) { @@ -1879,23 +1995,28 @@ protected: class MavlinkStreamAttitudeControls : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamAttitudeControls::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "ATTITUDE_CONTROLS"; } - uint8_t get_id() { + uint8_t get_id() + { return 0; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamAttitudeControls(mavlink); } - unsigned get_size() { + unsigned get_size() + { return 4 * (MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES); } @@ -1905,7 +2026,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitudeControls(MavlinkStreamAttitudeControls &); - MavlinkStreamAttitudeControls &operator = (const MavlinkStreamAttitudeControls &); + MavlinkStreamAttitudeControls& operator = (const MavlinkStreamAttitudeControls &); protected: explicit MavlinkStreamAttitudeControls(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1913,7 +2034,8 @@ protected: _att_ctrl_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct actuator_controls_s att_ctrl; if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { @@ -1949,23 +2071,28 @@ protected: class MavlinkStreamNamedValueFloat : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamNamedValueFloat::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "NAMED_VALUE_FLOAT"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamNamedValueFloat(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1975,7 +2102,7 @@ private: /* do not allow top copying this class */ MavlinkStreamNamedValueFloat(MavlinkStreamNamedValueFloat &); - MavlinkStreamNamedValueFloat &operator = (const MavlinkStreamNamedValueFloat &); + MavlinkStreamNamedValueFloat& operator = (const MavlinkStreamNamedValueFloat &); protected: explicit MavlinkStreamNamedValueFloat(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -1983,7 +2110,8 @@ protected: _debug_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct debug_key_value_s debug; if (_debug_sub->update(&_debug_time, &debug)) { @@ -2004,23 +2132,28 @@ protected: class MavlinkStreamCameraCapture : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamCameraCapture::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "CAMERA_CAPTURE"; } - uint8_t get_id() { + uint8_t get_id() + { return 0; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamCameraCapture(mavlink); } - unsigned get_size() { + unsigned get_size() + { return MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -2029,14 +2162,15 @@ private: /* do not allow top copying this class */ MavlinkStreamCameraCapture(MavlinkStreamCameraCapture &); - MavlinkStreamCameraCapture &operator = (const MavlinkStreamCameraCapture &); + MavlinkStreamCameraCapture& operator = (const MavlinkStreamCameraCapture &); protected: explicit MavlinkStreamCameraCapture(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct vehicle_status_s status; (void)_status_sub->update(&status); @@ -2063,23 +2197,28 @@ protected: class MavlinkStreamDistanceSensor : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamDistanceSensor::get_name_static(); } - static const char *get_name_static() { + static const char *get_name_static() + { return "DISTANCE_SENSOR"; } - uint8_t get_id() { + uint8_t get_id() + { return MAVLINK_MSG_ID_DISTANCE_SENSOR; } - static MavlinkStream *new_instance(Mavlink *mavlink) { + static MavlinkStream *new_instance(Mavlink *mavlink) + { return new MavlinkStreamDistanceSensor(mavlink); } - unsigned get_size() { + unsigned get_size() + { return _range_sub->is_published() ? (MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0; } @@ -2089,7 +2228,7 @@ private: /* do not allow top copying this class */ MavlinkStreamDistanceSensor(MavlinkStreamDistanceSensor &); - MavlinkStreamDistanceSensor &operator = (const MavlinkStreamDistanceSensor &); + MavlinkStreamDistanceSensor& operator = (const MavlinkStreamDistanceSensor &); protected: explicit MavlinkStreamDistanceSensor(Mavlink *mavlink) : MavlinkStream(mavlink), @@ -2097,7 +2236,8 @@ protected: _range_time(0) {} - void send(const hrt_abstime t) { + void send(const hrt_abstime t) + { struct range_finder_report range; if (_range_sub->update(&_range_time, &range)) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cd2ef1ffd..72c0d47bd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -88,42 +88,40 @@ static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f; MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _mavlink(parent), - status {}, - hil_local_pos {}, - _control_mode {}, - _global_pos_pub(-1), - _local_pos_pub(-1), - _attitude_pub(-1), - _gps_pub(-1), - _sensors_pub(-1), - _gyro_pub(-1), - _accel_pub(-1), - _mag_pub(-1), - _baro_pub(-1), - _airspeed_pub(-1), - _battery_pub(-1), - _cmd_pub(-1), - _flow_pub(-1), - _range_pub(-1), - _offboard_control_sp_pub(-1), - _global_vel_sp_pub(-1), - _att_sp_pub(-1), - _rates_sp_pub(-1), - _force_sp_pub(-1), - _pos_sp_triplet_pub(-1), - _vicon_position_pub(-1), - _vision_position_pub(-1), - _telemetry_status_pub(-1), - _rc_pub(-1), - _manual_pub(-1), - _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), - _hil_frames(0), - _old_timestamp(0), - _hil_local_proj_inited(0), - _hil_local_alt0(0.0f), - _hil_local_proj_ref {}, - _time_offset_avg_alpha(0.75), - _time_offset(0) + status{}, + hil_local_pos{}, + _control_mode{}, + _global_pos_pub(-1), + _local_pos_pub(-1), + _attitude_pub(-1), + _gps_pub(-1), + _sensors_pub(-1), + _gyro_pub(-1), + _accel_pub(-1), + _mag_pub(-1), + _baro_pub(-1), + _airspeed_pub(-1), + _battery_pub(-1), + _cmd_pub(-1), + _flow_pub(-1), + _range_pub(-1), + _offboard_control_sp_pub(-1), + _global_vel_sp_pub(-1), + _att_sp_pub(-1), + _rates_sp_pub(-1), + _force_sp_pub(-1), + _pos_sp_triplet_pub(-1), + _vicon_position_pub(-1), + _vision_position_pub(-1), + _telemetry_status_pub(-1), + _rc_pub(-1), + _manual_pub(-1), + _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), + _hil_frames(0), + _old_timestamp(0), + _hil_local_proj_inited(0), + _hil_local_alt0(0.0f), + _hil_local_proj_ref{} { // make sure the FTP server is started @@ -232,7 +230,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } - if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { + if (_mavlink->get_hil_enabled() || (_mavlink->get_use_hil_gps() && msg->sysid == mavlink_system.sysid)) { switch (msg->msgid) { case MAVLINK_MSG_ID_HIL_GPS: handle_message_hil_gps(msg); @@ -244,7 +242,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) } - /* If we've received a valid message, mark the flag indicating so. + /* If we've received a valid message, mark the flag indicating so. This is used in the '-w' command-line flag. */ _mavlink->set_has_received_messages(true); } @@ -277,35 +275,22 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } struct vehicle_command_s vcmd; - memset(&vcmd, 0, sizeof(vcmd)); /* Copy the content of mavlink_command_long_t cmd_mavlink into command_t cmd */ vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - vcmd.param5 = cmd_mavlink.param5; - vcmd.param6 = cmd_mavlink.param6; - vcmd.param7 = cmd_mavlink.param7; - // XXX do proper translation vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; - vcmd.confirmation = cmd_mavlink.confirmation; if (_cmd_pub < 0) { @@ -346,34 +331,22 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } struct vehicle_command_s vcmd; - memset(&vcmd, 0, sizeof(vcmd)); /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ vcmd.param1 = cmd_mavlink.param1; - vcmd.param2 = cmd_mavlink.param2; - vcmd.param3 = cmd_mavlink.param3; - vcmd.param4 = cmd_mavlink.param4; - /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; - vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; - vcmd.param7 = cmd_mavlink.z; - // XXX do proper translation vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; - vcmd.target_system = cmd_mavlink.target_system; - vcmd.target_component = cmd_mavlink.target_component; - vcmd.source_system = msg->sysid; - vcmd.source_component = msg->compid; if (_cmd_pub < 0) { @@ -461,7 +434,6 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg) if (_range_pub < 0) { _range_pub = orb_advertise(ORB_ID(sensor_range_finder), &r); - } else { orb_publish(ORB_ID(sensor_range_finder), _range_pub, &r); } @@ -537,33 +509,28 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_position_target_local_ned.target_system || - set_position_target_local_ned.target_system == 0) && - (mavlink_system.compid == set_position_target_local_ned.target_component || - set_position_target_local_ned.target_component == 0)) { + set_position_target_local_ned.target_system == 0) && + (mavlink_system.compid == set_position_target_local_ned.target_component || + set_position_target_local_ned.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ switch (set_position_target_local_ned.coordinate_frame) { - case MAV_FRAME_LOCAL_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; - break; - - case MAV_FRAME_LOCAL_OFFSET_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; - break; - - case MAV_FRAME_BODY_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; - break; - - case MAV_FRAME_BODY_OFFSET_NED: - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; - break; - - default: - /* invalid setpoint, avoid publishing */ - return; + case MAV_FRAME_LOCAL_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; + break; + case MAV_FRAME_LOCAL_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; + break; + case MAV_FRAME_BODY_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; + break; + case MAV_FRAME_BODY_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; + break; + default: + /* invalid setpoint, avoid publishing */ + return; } - offboard_control_sp.position[0] = set_position_target_local_ned.x; offboard_control_sp.position[1] = set_position_target_local_ned.y; offboard_control_sp.position[2] = set_position_target_local_ned.z; @@ -585,19 +552,17 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set ignore flags */ for (int i = 0; i < 9; i++) { offboard_control_sp.ignore &= ~(1 << i); - offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); - if (set_position_target_local_ned.type_mask & (1 << 10)) { - offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAW); } offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); - if (set_position_target_local_ned.type_mask & (1 << 11)) { - offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); + offboard_control_sp.ignore |= (1 << OFB_IGN_BIT_YAWRATE); } offboard_control_sp.timestamp = hrt_absolute_time(); @@ -614,30 +579,25 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t if (_mavlink->get_forward_externalsp()) { bool updated; orb_check(_control_mode_sub, &updated); - if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } - if (_control_mode.flag_control_offboard_enabled) { if (offboard_control_sp.isForceSetpoint && - offboard_control_sp_ignore_position_all(offboard_control_sp) && - offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + offboard_control_sp_ignore_position_all(offboard_control_sp) && + offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { /* The offboard setpoint is a force setpoint only, directly writing to the force * setpoint topic and not publishing the setpoint triplet topic */ struct vehicle_force_setpoint_s force_sp; force_sp.x = offboard_control_sp.acceleration[0]; force_sp.y = offboard_control_sp.acceleration[1]; force_sp.z = offboard_control_sp.acceleration[2]; - //XXX: yaw if (_force_sp_pub < 0) { _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); - } else { orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); } - } else { /* It's not a pure force setpoint: publish to setpoint triplet topic */ struct position_setpoint_triplet_s pos_sp_triplet; @@ -649,12 +609,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position_some(offboard_control_sp)) { - pos_sp_triplet.current.position_valid = true; - pos_sp_triplet.current.x = offboard_control_sp.position[0]; - pos_sp_triplet.current.y = offboard_control_sp.position[1]; - pos_sp_triplet.current.z = offboard_control_sp.position[2]; - + !offboard_control_sp_ignore_position_some(offboard_control_sp)) { + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = offboard_control_sp.position[0]; + pos_sp_triplet.current.y = offboard_control_sp.position[1]; + pos_sp_triplet.current.z = offboard_control_sp.position[2]; } else { pos_sp_triplet.current.position_valid = false; } @@ -662,12 +621,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local vel values if the setpoint type is 'local pos' and none * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { + !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; - } else { pos_sp_triplet.current.velocity_valid = false; } @@ -675,13 +633,13 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local acceleration values if the setpoint type is 'local pos' and none * of the accelerations fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { + !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; pos_sp_triplet.current.acceleration_is_force = - offboard_control_sp.isForceSetpoint; + offboard_control_sp.isForceSetpoint; } else { pos_sp_triplet.current.acceleration_valid = false; @@ -690,7 +648,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the yaw sp value if the setpoint type is 'local pos' and the yaw * field is not set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + !offboard_control_sp_ignore_yaw(offboard_control_sp)) { pos_sp_triplet.current.yaw_valid = true; pos_sp_triplet.current.yaw = offboard_control_sp.yaw; @@ -701,24 +659,22 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate * field is not set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { pos_sp_triplet.current.yawspeed_valid = true; pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; } else { pos_sp_triplet.current.yawspeed_valid = false; } - //XXX handle global pos setpoints (different MAV frames) if (_pos_sp_triplet_pub < 0) { _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), - &pos_sp_triplet); - + &pos_sp_triplet); } else { orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, - &pos_sp_triplet); + &pos_sp_triplet); } } @@ -779,13 +735,12 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == set_attitude_target.target_system || - set_attitude_target.target_system == 0) && - (mavlink_system.compid == set_attitude_target.target_component || - set_attitude_target.target_component == 0)) { + set_attitude_target.target_system == 0) && + (mavlink_system.compid == set_attitude_target.target_component || + set_attitude_target.target_component == 0)) { for (int i = 0; i < 4; i++) { offboard_control_sp.attitude[i] = set_attitude_target.q[i]; } - offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate; offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; @@ -793,9 +748,8 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* set correct ignore flags for body rate fields: copy from mavlink message */ for (int i = 0; i < 3; i++) { offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); - offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; + offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; } - /* set correct ignore flags for thrust field: copy from mavlink message */ offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); @@ -805,7 +759,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_sp.timestamp = hrt_absolute_time(); - offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode + offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); @@ -819,7 +773,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_mavlink->get_forward_externalsp()) { bool updated; orb_check(_control_mode_sub, &updated); - if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } @@ -828,20 +781,18 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || - offboard_control_sp_ignore_thrust(offboard_control_sp))) { + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, - &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); att_sp.q_d_valid = true; - if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); - } else { orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); } @@ -850,7 +801,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || - offboard_control_sp_ignore_thrust(offboard_control_sp))) { + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_rates_setpoint_s rates_sp; rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = set_attitude_target.body_roll_rate; @@ -860,7 +811,6 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_att_sp_pub < 0) { _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); - } else { orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); } @@ -919,8 +869,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) manual.r = man.r / 1000.0f; manual.z = man.z / 1000.0f; - warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, - (double)manual.z); + warnx("pitch: %.2f, roll: %.2f, yaw: %.2f, throttle: %.2f", (double)manual.x, (double)manual.y, (double)manual.r, (double)manual.z); if (_manual_pub < 0) { _manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual); |