From e1bdc4a0fbb5e89dc5f3911dbf9a9a21ed5b459b Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Mon, 15 Dec 2014 14:23:27 +0530 Subject: New timesync interface import. Not working yet. --- src/modules/mavlink/mavlink_main.cpp | 2 + src/modules/mavlink/mavlink_messages.cpp | 818 +++++++++++++++---------------- src/modules/mavlink/mavlink_receiver.cpp | 289 ++++++++--- src/modules/mavlink/mavlink_receiver.h | 14 + 4 files changed, 608 insertions(+), 515 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 29b7ec7b7..76b2ad217 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1416,6 +1416,8 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); configure_stream("VFR_HUD", 10.0f); + configure_stream("SYSTEM_TIME", 1.0f); + configure_stream("TIMESYNC", 10.0f); break; default: diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 5908279d5..85643270f 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -121,91 +121,93 @@ 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; } @@ -238,28 +240,23 @@ 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; } @@ -273,7 +270,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), @@ -281,8 +278,7 @@ 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; @@ -313,34 +309,31 @@ 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: @@ -353,8 +346,7 @@ 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); @@ -373,6 +365,7 @@ 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] = ""; @@ -399,23 +392,19 @@ 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); } @@ -429,7 +418,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), @@ -437,8 +426,7 @@ 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)) { @@ -467,28 +455,23 @@ 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; } @@ -497,15 +480,14 @@ 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)) { @@ -534,28 +516,23 @@ 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; } @@ -570,7 +547,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), @@ -582,8 +559,7 @@ 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)) { @@ -640,28 +616,23 @@ 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; } @@ -671,7 +642,7 @@ private: /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &); - MavlinkStreamAttitude& operator = (const MavlinkStreamAttitude &); + MavlinkStreamAttitude &operator = (const MavlinkStreamAttitude &); protected: @@ -680,8 +651,7 @@ 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)) { @@ -704,28 +674,23 @@ 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; } @@ -735,7 +700,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), @@ -743,8 +708,7 @@ 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)) { @@ -769,28 +733,23 @@ 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; } @@ -812,7 +771,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), @@ -828,8 +787,7 @@ 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; @@ -861,28 +819,23 @@ 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; } @@ -892,7 +845,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), @@ -900,8 +853,7 @@ 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)) { @@ -915,40 +867,121 @@ 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); } } }; +class MavlinkStreamSystemTime : public MavlinkStream +{ +public: + const char *get_name() const { + return MavlinkStreamSystemTime::get_name_static(); + } + + static const char *get_name_static() { + return "SYSTEM_TIME"; + } + + uint8_t get_id() { + return MAVLINK_MSG_ID_SYSTEM_TIME; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) { + return new MavlinkStreamSystemTime(mavlink); + } + + unsigned get_size() { + return MAVLINK_MSG_ID_SYSTEM_TIME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + /* do not allow top copying this class */ + MavlinkStreamSystemTime(MavlinkStreamSystemTime &); + MavlinkStreamSystemTime &operator = (const MavlinkStreamSystemTime &); + +protected: + explicit MavlinkStreamSystemTime(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) { + mavlink_system_time_t msg; + timespec tv; + + clock_gettime(CLOCK_REALTIME, &tv); + + msg.time_boot_ms = hrt_absolute_time() / 1000; + msg.time_unix_usec = (uint64_t)tv.tv_sec * 1000000 + tv.tv_nsec / 1000; + + _mavlink->send_message(MAVLINK_MSG_ID_SYSTEM_TIME, &msg); + } +}; + +class MavlinkStreamTimesync : public MavlinkStream +{ +public: + const char *get_name() const { + return MavlinkStreamTimesync::get_name_static(); + } + + static const char *get_name_static() { + return "TIMESYNC"; + } + + uint8_t get_id() { + return MAVLINK_MSG_ID_TIMESYNC; + } + + static MavlinkStream *new_instance(Mavlink *mavlink) { + return new MavlinkStreamTimesync(mavlink); + } + + unsigned get_size() { + return MAVLINK_MSG_ID_TIMESYNC_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; + } + +private: + /* do not allow top copying this class */ + MavlinkStreamTimesync(MavlinkStreamTimesync &); + MavlinkStreamTimesync &operator = (const MavlinkStreamTimesync &); + +protected: + explicit MavlinkStreamTimesync(Mavlink *mavlink) : MavlinkStream(mavlink) + {} + + void send(const hrt_abstime t) { + mavlink_timesync_t msg; + + msg.tc1 = -1; + msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds + + _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg); + } +}; 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; } @@ -961,7 +994,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), @@ -971,8 +1004,7 @@ 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; @@ -1001,28 +1033,23 @@ 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; } @@ -1032,7 +1059,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), @@ -1040,8 +1067,7 @@ 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)) { @@ -1064,28 +1090,23 @@ 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; } @@ -1095,7 +1116,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), @@ -1103,8 +1124,7 @@ 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)) { @@ -1127,28 +1147,23 @@ 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; } @@ -1157,15 +1172,14 @@ 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()) { @@ -1189,40 +1203,35 @@ template class MavlinkStreamServoOutputRaw : public MavlinkStream { public: - const char *get_name() const - { + const char *get_name() const { return MavlinkStreamServoOutputRaw::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(mavlink); } - unsigned get_size() - { + unsigned get_size() { return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES; } @@ -1232,13 +1241,12 @@ 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), @@ -1249,8 +1257,7 @@ 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)) { @@ -1276,28 +1283,23 @@ 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; } @@ -1313,7 +1315,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), @@ -1325,8 +1327,7 @@ 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; @@ -1350,8 +1351,8 @@ protected: /* scale outputs depending on system type */ if (system_type == MAV_TYPE_QUADROTOR || - system_type == MAV_TYPE_HEXAROTOR || - system_type == MAV_TYPE_OCTOROTOR) { + system_type == MAV_TYPE_HEXAROTOR || + system_type == MAV_TYPE_OCTOROTOR) { /* multirotors: set number of rotor outputs depending on type */ unsigned n; @@ -1431,28 +1432,23 @@ 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; } @@ -1461,19 +1457,18 @@ 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; @@ -1489,28 +1484,23 @@ 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; } @@ -1520,7 +1510,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), @@ -1528,12 +1518,11 @@ 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,28 +1539,23 @@ 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; } @@ -1583,7 +1567,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), @@ -1593,8 +1577,7 @@ 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)) { @@ -1602,7 +1585,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); @@ -1622,30 +1605,26 @@ 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: @@ -1654,7 +1633,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), @@ -1662,8 +1641,7 @@ 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)) { @@ -1739,20 +1717,24 @@ 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) { @@ -1769,28 +1751,23 @@ 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; } @@ -1800,7 +1777,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), @@ -1808,8 +1785,7 @@ 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)) { @@ -1830,28 +1806,23 @@ 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; } @@ -1861,7 +1832,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), @@ -1869,8 +1840,7 @@ 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)) { @@ -1898,28 +1868,23 @@ 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); } @@ -1929,7 +1894,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), @@ -1937,8 +1902,7 @@ 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)) { @@ -1974,28 +1938,23 @@ 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; } @@ -2005,7 +1964,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), @@ -2013,8 +1972,7 @@ 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)) { @@ -2035,28 +1993,23 @@ 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; } @@ -2065,15 +2018,14 @@ 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); @@ -2100,28 +2052,23 @@ 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; } @@ -2131,7 +2078,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), @@ -2139,8 +2086,7 @@ 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)) { @@ -2182,6 +2128,8 @@ StreamListItem *streams_list[] = { 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(&MavlinkStreamSystemTime::new_instance, &MavlinkStreamSystemTime::get_name_static), + new StreamListItem(&MavlinkStreamTimesync::new_instance, &MavlinkStreamTimesync::get_name_static), new StreamListItem(&MavlinkStreamGlobalPositionInt::new_instance, &MavlinkStreamGlobalPositionInt::get_name_static), new StreamListItem(&MavlinkStreamLocalPositionNED::new_instance, &MavlinkStreamLocalPositionNED::get_name_static), new StreamListItem(&MavlinkStreamViconPositionEstimate::new_instance, &MavlinkStreamViconPositionEstimate::get_name_static), diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe..13e17ab64 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -88,40 +88,42 @@ 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{} + 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.8f), + _time_offset(0) { // make sure the FTP server is started @@ -188,6 +190,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) MavlinkFTP::get_server()->handle_message(_mavlink, msg); break; + case MAVLINK_MSG_ID_SYSTEM_TIME: + handle_message_system_time(msg); + break; + + case MAVLINK_MSG_ID_TIMESYNC: + handle_message_timesync(msg); + break; + default: break; } @@ -222,7 +232,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); @@ -234,7 +244,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); } @@ -267,22 +277,35 @@ 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) { @@ -323,22 +346,34 @@ 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) { @@ -426,6 +461,7 @@ 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); } @@ -501,28 +537,33 @@ 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; @@ -544,17 +585,19 @@ 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(); @@ -571,25 +614,30 @@ 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; @@ -601,11 +649,12 @@ 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; } @@ -613,11 +662,12 @@ 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; } @@ -625,13 +675,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; @@ -640,7 +690,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; @@ -651,22 +701,24 @@ 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); } } @@ -727,12 +779,13 @@ 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; @@ -740,8 +793,9 @@ 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); @@ -751,7 +805,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); @@ -765,6 +819,7 @@ 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); } @@ -773,18 +828,20 @@ 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); } @@ -793,7 +850,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; @@ -803,6 +860,7 @@ 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); } @@ -861,7 +919,8 @@ 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); @@ -918,6 +977,61 @@ MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) } } +void +MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) +{ + mavlink_system_time_t time; + mavlink_msg_system_time_decode(msg, &time); + + timespec tv; + clock_gettime(CLOCK_REALTIME, &tv); + + // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 + bool onb_unix_valid = tv.tv_sec > 1234567890L; + bool ofb_unix_valid = time.time_unix_usec > 1234567890L * 1000; + + if (!onb_unix_valid && ofb_unix_valid) { + tv.tv_sec = time.time_unix_usec / 1000000; + tv.tv_nsec = (time.time_unix_usec % 1000000) * 1000; + clock_settime(CLOCK_REALTIME, &tv); + warnx("[timesync] Set system time from SYSTEM_TIME message"); + } + +} + +void +MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) +{ + mavlink_timesync_t tsync; + mavlink_msg_timesync_decode(msg, &tsync); + + uint64_t now_ns = hrt_absolute_time() * 1000 ; + + if (tsync.tc1 == -1) { + + mavlink_timesync_t rsync; // return sync message + + rsync.tc1 = now_ns; + rsync.ts1 = tsync.ts1; + + _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); + + } else if (tsync.tc1 > -1) { + + int64_t offset_ns = ((tsync.ts1 + now_ns) - (tsync.tc1 * 2)) / 2; + int64_t dt = _time_offset - offset_ns; + + if (dt > 1000000 || dt < -100000) { // 1 millisecond skew XXX Make this + _time_offset = offset_ns; // hard-set it. + warnx("[timesync] Timesync offset is off. Hard-setting offset"); + + } else { + average_time_offset(offset_ns); + } + } + +} + void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { @@ -1385,6 +1499,21 @@ void MavlinkReceiver::print_status() } +uint64_t MavlinkReceiver::to_hrt(uint64_t usec) +{ + return usec - (_time_offset / 1000) ; +} + +void MavlinkReceiver::average_time_offset(uint64_t offset_ns) +{ + /* alpha = 0.8 fixed for now. The closer alpha is to 1.0, + * the faster the moving average updates in response to + * new offset samples. + */ + + _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; +} + void *MavlinkReceiver::start_helper(void *context) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a057074a7..71273786d 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -124,12 +124,24 @@ private: void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_request_data_stream(mavlink_message_t *msg); + void handle_message_system_time(mavlink_message_t *msg); + void handle_message_timesync(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); void *receive_thread(void *arg); + /** + * Convert remote nsec timestamp to local hrt time (usec) + */ + uint64_t to_hrt(uint64_t nsec); + + /** + * Exponential moving average filter to smooth time offset + */ + void average_time_offset(uint64_t offset_ns); + mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; struct vehicle_control_mode_s _control_mode; @@ -164,6 +176,8 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; + double _time_offset_avg_alpha; + uint64_t _time_offset; /* do not allow copying this class */ MavlinkReceiver(const MavlinkReceiver&); -- cgit v1.2.3 From 8c0d7047b209c23cb14b3a09bf20599af0f8cc6e Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Tue, 16 Dec 2014 13:43:48 +0530 Subject: Working now. --- src/modules/mavlink/mavlink_messages.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 16 +++++++++------- 2 files changed, 10 insertions(+), 8 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 85643270f..dcf9aeb64 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -955,7 +955,7 @@ protected: void send(const hrt_abstime t) { mavlink_timesync_t msg; - msg.tc1 = -1; + msg.tc1 = 0; msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 13e17ab64..cd2ef1ffd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -122,7 +122,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref {}, - _time_offset_avg_alpha(0.8f), + _time_offset_avg_alpha(0.75), _time_offset(0) { @@ -1007,22 +1007,24 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) uint64_t now_ns = hrt_absolute_time() * 1000 ; - if (tsync.tc1 == -1) { + if (tsync.tc1 == 0) { - mavlink_timesync_t rsync; // return sync message + mavlink_timesync_t rsync; // return timestamped sync message rsync.tc1 = now_ns; rsync.ts1 = tsync.ts1; _mavlink->send_message(MAVLINK_MSG_ID_TIMESYNC, &rsync); - } else if (tsync.tc1 > -1) { + return; + + } else if (tsync.tc1 > 0) { int64_t offset_ns = ((tsync.ts1 + now_ns) - (tsync.tc1 * 2)) / 2; int64_t dt = _time_offset - offset_ns; - if (dt > 1000000 || dt < -100000) { // 1 millisecond skew XXX Make this - _time_offset = offset_ns; // hard-set it. + if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew + _time_offset = offset_ns; warnx("[timesync] Timesync offset is off. Hard-setting offset"); } else { @@ -1506,7 +1508,7 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) void MavlinkReceiver::average_time_offset(uint64_t offset_ns) { - /* alpha = 0.8 fixed for now. The closer alpha is to 1.0, + /* alpha = 0.75 fixed for now. The closer alpha is to 1.0, * the faster the moving average updates in response to * new offset samples. */ -- cgit v1.2.3 From f7da65f819db2e5d6602cd494119d8dccfd5ed56 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Tue, 16 Dec 2014 14:08:28 +0530 Subject: Fix formatting --- src/modules/mavlink/mavlink_messages.cpp | 726 ++++++++++++++++++------------- src/modules/mavlink/mavlink_receiver.cpp | 211 ++++----- 2 files changed, 513 insertions(+), 424 deletions(-) (limited to 'src/modules/mavlink') 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 class MavlinkStreamServoOutputRaw : public MavlinkStream { public: - const char *get_name() const { + const char *get_name() const + { return MavlinkStreamServoOutputRaw::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(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); -- cgit v1.2.3 From 5c6155c94d3a27cdc426f7d3e18a679f0bc589e7 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Tue, 16 Dec 2014 14:22:35 +0530 Subject: minor fix --- src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 72c0d47bd..88d12e775 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -121,7 +121,9 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _old_timestamp(0), _hil_local_proj_inited(0), _hil_local_alt0(0.0f), - _hil_local_proj_ref{} + _hil_local_proj_ref{}, + _time_offset_avg_alpha(0.75), + _time_offset(0) { // make sure the FTP server is started -- cgit v1.2.3 From 0e41624f7902b13bbf830742481174e08e0f97c4 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Tue, 16 Dec 2014 20:39:28 +0530 Subject: removed exponential filter. simple weighted average works fine. --- src/modules/mavlink/mavlink_receiver.cpp | 19 ++++--------------- src/modules/mavlink/mavlink_receiver.h | 6 ------ 2 files changed, 4 insertions(+), 21 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 88d12e775..cd385ffac 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -122,7 +122,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref{}, - _time_offset_avg_alpha(0.75), _time_offset(0) { @@ -971,15 +970,15 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = ((tsync.ts1 + now_ns) - (tsync.tc1 * 2)) / 2; + int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset int64_t dt = _time_offset - offset_ns; if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew - _time_offset = offset_ns; - warnx("[timesync] Timesync offset is off. Hard-setting offset"); + _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; + warnx("[timesync] Companion clock offset is skewed. Hard-setting offset"); } else { - average_time_offset(offset_ns); + _time_offset = offset_ns; } } @@ -1457,16 +1456,6 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) return usec - (_time_offset / 1000) ; } -void MavlinkReceiver::average_time_offset(uint64_t offset_ns) -{ - /* alpha = 0.75 fixed for now. The closer alpha is to 1.0, - * the faster the moving average updates in response to - * new offset samples. - */ - - _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; -} - void *MavlinkReceiver::start_helper(void *context) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 71273786d..7c79f661b 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -137,11 +137,6 @@ private: */ uint64_t to_hrt(uint64_t nsec); - /** - * Exponential moving average filter to smooth time offset - */ - void average_time_offset(uint64_t offset_ns); - mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; struct vehicle_control_mode_s _control_mode; @@ -176,7 +171,6 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; - double _time_offset_avg_alpha; uint64_t _time_offset; /* do not allow copying this class */ -- cgit v1.2.3 From 4a42f6ca6a95c9b05ed2c1e66cc251ea0e001c59 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Wed, 17 Dec 2014 12:12:09 +0530 Subject: Restore EMA. Works better for low rates --- src/modules/mavlink/mavlink_receiver.cpp | 15 ++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 7 ++++++- 2 files changed, 20 insertions(+), 2 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index cd385ffac..5aa41e5d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -122,6 +122,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _hil_local_proj_inited(0), _hil_local_alt0(0.0f), _hil_local_proj_ref{}, + _time_offset_avg_alpha(0.6), _time_offset(0) { @@ -978,7 +979,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) warnx("[timesync] Companion clock offset is skewed. Hard-setting offset"); } else { - _time_offset = offset_ns; + smooth_time_offset(offset_ns); } } @@ -1456,6 +1457,18 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) return usec - (_time_offset / 1000) ; } + +void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) +{ + /* alpha = 0.75 fixed for now. The closer alpha is to 1.0, + * the faster the moving average updates in response to + * new offset samples. + */ + + _time_offset = (_time_offset_avg_alpha * offset_ns) + (1.0 - _time_offset_avg_alpha) * _time_offset; +} + + void *MavlinkReceiver::start_helper(void *context) { MavlinkReceiver *rcv = new MavlinkReceiver((Mavlink *)context); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 7c79f661b..a677e75cd 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -136,7 +136,11 @@ private: * Convert remote nsec timestamp to local hrt time (usec) */ uint64_t to_hrt(uint64_t nsec); - + /** + * Exponential moving average filter to smooth time offset + */ + void smooth_time_offset(uint64_t offset_ns); + mavlink_status_t status; struct vehicle_local_position_s hil_local_pos; struct vehicle_control_mode_s _control_mode; @@ -171,6 +175,7 @@ private: bool _hil_local_proj_inited; float _hil_local_alt0; struct map_projection_reference_s _hil_local_proj_ref; + double _time_offset_avg_alpha; uint64_t _time_offset; /* do not allow copying this class */ -- cgit v1.2.3 From c59f20706f93908ff9137aa315e8c77cb470e298 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Wed, 17 Dec 2014 12:16:00 +0530 Subject: Add use example --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5aa41e5d8..3a7fcd0e2 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -699,7 +699,7 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) // Use the component ID to identify the vision sensor vision_position.id = msg->compid; - vision_position.timestamp_boot = hrt_absolute_time(); + vision_position.timestamp_boot = to_hrt(pos.usec); // Synced time vision_position.timestamp_computer = pos.usec; vision_position.x = pos.x; vision_position.y = pos.y; -- cgit v1.2.3 From 11a14c2c3d6ffeff5a896496c0d673845b734d86 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sat, 27 Dec 2014 23:16:09 +0530 Subject: Add rotation switching to flow from mavlink --- src/drivers/px4flow/px4flow.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 7 +++++++ 2 files changed, 8 insertions(+), 1 deletion(-) (limited to 'src/modules/mavlink') diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 99a9cc02a..9895f780a 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -546,7 +546,7 @@ PX4FLOW::collect() report.sensor_id = 0; /* rotate measurements to flight controller frame */ - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m); + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m); // XXX Check this if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e98d72afe..f91ae3c37 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -68,6 +68,8 @@ #include +#include + #include #include #include @@ -357,6 +359,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) /* optical flow */ mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); + + enum Rotation flow_rot; + param_get(param_find("SENS_FLOW_ROT"),&flow_rot); struct optical_flow_s f; memset(&f, 0, sizeof(f)); @@ -374,6 +379,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) f.sensor_id = flow.sensor_id; f.gyro_temperature = flow.temperature; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, f.ground_distance_m); // XXX Check this + if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); -- cgit v1.2.3 From 33653b25c6569013d0a1a5a4885457c0bdc23e06 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sat, 27 Dec 2014 23:25:08 +0530 Subject: fix Z rotation --- src/drivers/px4flow/px4flow.cpp | 5 +++-- src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 2 files changed, 6 insertions(+), 3 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 9895f780a..682cbf241 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -545,8 +545,9 @@ PX4FLOW::collect() report.sensor_id = 0; - /* rotate measurements to flight controller frame */ - rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m); // XXX Check this + /* rotate measurements according to parameter */ + float zeroval = 0.0f; + rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, zeroval); if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f91ae3c37..6f5adb5fe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -379,7 +379,9 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) f.sensor_id = flow.sensor_id; f.gyro_temperature = flow.temperature; - rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, f.ground_distance_m); // XXX Check this + /* rotate measurements according to parameter */ + float zeroval = 0.0f; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval); if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); -- cgit v1.2.3 From e9b41528dc39ac8a8c565a4c16fc5aa512e37b1a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 26 Dec 2014 19:21:50 +0100 Subject: prototype for changing params by rc --- mavlink/include/mavlink/v1.0 | 2 +- src/modules/mavlink/mavlink_parameters.cpp | 39 +++++++++- src/modules/mavlink/mavlink_parameters.h | 5 ++ src/modules/sensors/sensor_params.c | 35 +++++++++ src/modules/sensors/sensors.cpp | 114 ++++++++++++++++++++++++++++- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/rc_channels.h | 7 +- src/modules/uORB/topics/rc_parameter_map.h | 74 +++++++++++++++++++ 8 files changed, 274 insertions(+), 5 deletions(-) create mode 100644 src/modules/uORB/topics/rc_parameter_map.h (limited to 'src/modules/mavlink') diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index ad5e5a645..87350ba3d 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit ad5e5a645dec152419264ad32221f7c113ea5c30 +Subproject commit 87350ba3d2e42d15c5934fe3c3387167e9f8769f diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index cd5f53d88..6e0813b38 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -44,7 +44,9 @@ #include "mavlink_main.h" MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink), - _send_all_index(-1) + _send_all_index(-1), + _rc_param_map_pub(-1), + _rc_param_map() { } @@ -135,6 +137,41 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) break; } + case MAVLINK_MSG_ID_PARAM_MAP_RC: { + /* map a rc channel to a parameter */ + mavlink_param_map_rc_t map_rc; + mavlink_msg_param_map_rc_decode(msg, &map_rc); + + if (map_rc.target_system == mavlink_system.sysid && + (map_rc.target_component == mavlink_system.compid || + map_rc.target_component == MAV_COMP_ID_ALL)) { + + /* Copy values from msg to uorb using the parameter_rc_channel_index as index */ + size_t i = map_rc.parameter_rc_channel_index; + _rc_param_map.param_index[i] = map_rc.param_index; + strncpy(&(_rc_param_map.param_id[i][0]), map_rc.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); + /* enforce null termination */ + _rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0'; + _rc_param_map.scale[i] = map_rc.scale; + _rc_param_map.value0[i] = map_rc.param_value0; + if (map_rc.param_index == -2) { // -2 means unset map + _rc_param_map.valid[i] = false; + } else { + _rc_param_map.valid[i] = true; + } + _rc_param_map.timestamp = hrt_absolute_time(); + + if (_rc_param_map_pub < 0) { + _rc_param_map_pub = orb_advertise(ORB_ID(rc_parameter_map), &_rc_param_map); + + } else { + orb_publish(ORB_ID(rc_parameter_map), _rc_param_map_pub, &_rc_param_map); + } + + } + break; + } + default: break; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index 5576e6b84..b6736f212 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -44,6 +44,8 @@ #include "mavlink_bridge_header.h" #include "mavlink_stream.h" +#include +#include class MavlinkParametersManager : public MavlinkStream { @@ -112,4 +114,7 @@ protected: void send(const hrt_abstime t); void send_param(param_t param); + + orb_advert_t _rc_param_map_pub; + struct rc_parameter_map_s _rc_param_map; }; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 229bfe3ce..fca148ec5 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -747,6 +747,41 @@ PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */ */ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); +/** + * Channel which changes a parameter + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 1st parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM1, 0); + +/** + * Channel which changes a parameter + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 2nd parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM2, 0); + +/** + * Channel which changes a parameter + * + * Can be used for parameter tuning with the RC. This one is further referenced as the 3th parameter channel. + * Set to 0 to deactivate * + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_PARAM3, 0); /** * Failsafe channel PWM threshold. diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cdcb428dd..d404d8a4c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -37,7 +37,7 @@ * * @author Lorenz Meier * @author Julian Oes - * @author Thomas Gubler + * @author Thomas Gubler */ #include @@ -82,6 +82,7 @@ #include #include #include +#include #define GYRO_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ #define ACC_HEALTH_COUNTER_LIMIT_ERROR 20 /* 40 ms downtime at 500 Hz update rate */ @@ -190,6 +191,14 @@ private: switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv); switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv); + /** + * Update paramters from RC channels if the functionality is activated and the + * input has changed since the last update + * + * @param + */ + void set_params_from_rc(); + /** * Gather and publish RC input data. */ @@ -220,6 +229,7 @@ private: int _diff_pres_sub; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ + int _rc_parameter_map_sub; /**< rc parameter map subscription */ int _manual_control_sub; /**< notification of manual control updates */ orb_advert_t _sensor_pub; /**< combined sensor data topic */ @@ -237,6 +247,7 @@ private: struct baro_report _barometer; /**< barometer data */ struct differential_pressure_s _diff_pres; struct airspeed_s _airspeed; + struct rc_parameter_map_s _rc_parameter_map; math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ @@ -288,6 +299,8 @@ private: int rc_map_aux4; int rc_map_aux5; + int rc_map_param[RC_PARAM_MAP_NCHAN]; + int32_t rc_fails_thr; float rc_assist_th; float rc_auto_th; @@ -348,6 +361,12 @@ private: param_t rc_map_aux4; param_t rc_map_aux5; + param_t rc_map_param[RC_PARAM_MAP_NCHAN]; + param_t rc_param[RC_PARAM_MAP_NCHAN]; /**< param handles for the paramters which are bound + to a RC channel, equivalent float values in the + _parameters struct are not existing + because these parameters are never read. */ + param_t rc_fails_thr; param_t rc_assist_th; param_t rc_auto_th; @@ -450,6 +469,11 @@ private: */ void parameter_update_poll(bool forced = false); + /** + * Check for changes in rc_parameter_map + */ + void rc_parameter_map_poll(bool forced = false); + /** * Poll the ADC and update readings to suit. * @@ -498,6 +522,7 @@ Sensors::Sensors() : _baro_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), + _rc_parameter_map_sub(-1), _manual_control_sub(-1), /* publications */ @@ -518,6 +543,7 @@ Sensors::Sensors() : { memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); + memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); /* basic r/c parameters */ for (unsigned i = 0; i < _rc_max_chan_count; i++) { @@ -570,6 +596,13 @@ Sensors::Sensors() : _parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4"); _parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5"); + /* RC to parameter mapping for changing parameters with RC */ + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + char name[PARAM_ID_LEN]; + snprintf(name, PARAM_ID_LEN, "RC_MAP_PARAM%d", i + 1); // shifted by 1 because param name starts at 1 + _parameter_handles.rc_map_param[i] = param_find(name); + } + /* RC thresholds */ _parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR"); _parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH"); @@ -747,6 +780,9 @@ Sensors::parameters_update() param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4)); param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5)); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + param_get(_parameter_handles.rc_map_param[i], &(_parameters.rc_map_param[i])); + } param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr)); param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th)); _parameters.rc_assist_inv = (_parameters.rc_assist_th < 0); @@ -791,6 +827,10 @@ Sensors::parameters_update() _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + _rc.function[PARAM_1 + i] = _parameters.rc_map_param[i] - 1; + } + /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1])); @@ -1373,6 +1413,43 @@ Sensors::parameter_update_poll(bool forced) } } +void +Sensors::rc_parameter_map_poll(bool forced) +{ + bool map_updated; + orb_check(_rc_parameter_map_sub, &map_updated); + + if (map_updated) { + orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map); + /* update paramter handles to which the RC channels are mapped */ + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + /* Set the handle by index if the index is set, otherwise use the id */ + if (_rc_parameter_map.param_index[i] >= 0) { + _parameter_handles.rc_param[i] = param_for_index((unsigned)_rc_parameter_map.param_index[i]); + } else { + _parameter_handles.rc_param[i] = param_find(_rc_parameter_map.param_id[i]); + } + + } + warnx("rc to parameter map updated"); + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + warnx("\ti %d param_id %s scale %.3f value0 %.3f", + i, + _rc_parameter_map.param_id[i], + (double)_rc_parameter_map.scale[i], + (double)_rc_parameter_map.value0[i] + ); + } + } +} + void Sensors::adc_poll(struct sensor_combined_s &raw) { @@ -1551,6 +1628,29 @@ Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, boo } } +void +Sensors::set_params_from_rc() +{ + static float param_rc_values[RC_PARAM_MAP_NCHAN] = {}; + for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { + if (_rc.function[PARAM_1 + i] < 0 || !_rc_parameter_map.valid[i]) { + /* This RC channel is not mapped to a RC-Parameter Channel (e.g. RC_MAP_PARAM1 == 0) + * or no request to map this channel to a param has been sent via mavlink + */ + continue; + } + + float rc_val = get_rc_value((enum RC_CHANNELS_FUNCTION)(PARAM_1 + i), -1.0, 1.0); + /* Check if the value has changed, + * maybe we need to introduce a more aggressive limit here */ + if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { + param_rc_values[i] = rc_val; + float param_val = _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val; + param_set(_parameter_handles.rc_param[i], ¶m_val); + } + } +} + void Sensors::rc_poll() { @@ -1717,6 +1817,13 @@ Sensors::rc_poll() } else { _actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3); } + + /* Update parameters from RC Channels (tuning with RC) if activated */ + static hrt_abstime last_rc_to_param_map_time = 0; + if (hrt_elapsed_time(&last_rc_to_param_map_time) > 1e6) { + set_params_from_rc(); + last_rc_to_param_map_time = hrt_absolute_time(); + } } } } @@ -1755,6 +1862,7 @@ Sensors::task_main() _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ @@ -1788,6 +1896,7 @@ Sensors::task_main() diff_pres_poll(raw); parameter_update_poll(true /* forced */); + rc_parameter_map_poll(true /* forced */); /* advertise the sensor_combined topic and make the initial publication */ _sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); @@ -1820,6 +1929,9 @@ Sensors::task_main() /* check parameters for updates */ parameter_update_poll(); + /* check rc parameter map for updates */ + rc_parameter_map_poll(); + /* the timestamp of the raw struct is updated by the gyro_poll() method */ /* copy most recent sensor data */ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 3d5755a46..d834c3574 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -246,3 +246,6 @@ ORB_DEFINE(tecs_status, struct tecs_status_s); #include "topics/wind_estimate.h" ORB_DEFINE(wind_estimate, struct wind_estimate_s); + +#include "topics/rc_parameter_map.h" +ORB_DEFINE(rc_parameter_map, struct rc_parameter_map_s); diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 16916cc4d..2fde5d424 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -65,12 +65,15 @@ enum RC_CHANNELS_FUNCTION { AUX_2, AUX_3, AUX_4, - AUX_5 + AUX_5, + PARAM_1, + PARAM_2, + PARAM_3 }; // MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS -#define RC_CHANNELS_FUNCTION_MAX 18 +#define RC_CHANNELS_FUNCTION_MAX 19 /** * @addtogroup topics diff --git a/src/modules/uORB/topics/rc_parameter_map.h b/src/modules/uORB/topics/rc_parameter_map.h new file mode 100644 index 000000000..47672c5d5 --- /dev/null +++ b/src/modules/uORB/topics/rc_parameter_map.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT , + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file rc_parameter_map.h + * Maps RC channels to parameters + * + * @author Thomas Gubler + */ + +#ifndef TOPIC_RC_PARAMETER_MAP_H +#define TOPIC_RC_PARAMETER_MAP_H + +#include +#include "../uORB.h" + +#define RC_PARAM_MAP_NCHAN 3 // This limit is also hardcoded in the enum RC_CHANNELS_FUNCTION in rc_channels.h +#define PARAM_ID_LEN 16 // corresponds to MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + +/** + * @addtogroup topics + * @{ + */ + +struct rc_parameter_map_s { + uint64_t timestamp; /**< time at which the map was updated */ + + bool valid[RC_PARAM_MAP_NCHAN]; /**< true for RC-Param channels which are mapped to a param */ + + int param_index[RC_PARAM_MAP_NCHAN]; /**< corresponding param index, this + this field is ignored if set to -1, in this case param_id will + be used*/ + char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1]; /**< corresponding param id, null terminated */ + float scale[RC_PARAM_MAP_NCHAN]; /** scale to map the RC input [-1, 1] to a parameter value */ + float value0[RC_PARAM_MAP_NCHAN]; /** inital value around which the parameter value is changed */ +}; + +/** + * @} + */ + +ORB_DECLARE(rc_parameter_map); + +#endif -- cgit v1.2.3 From ef8abfbf148ac6fd7a405acdd4ee8eeb156867b6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Dec 2014 21:57:30 +0100 Subject: rc2param: min and max values --- mavlink/include/mavlink/v1.0 | 2 +- src/modules/mavlink/mavlink_parameters.cpp | 2 ++ src/modules/sensors/sensors.cpp | 10 +++++++--- src/modules/uORB/topics/rc_parameter_map.h | 2 ++ 4 files changed, 12 insertions(+), 4 deletions(-) (limited to 'src/modules/mavlink') diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 87350ba3d..b5732d07e 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 87350ba3d2e42d15c5934fe3c3387167e9f8769f +Subproject commit b5732d07ec89025a9d321a923fe027d7df7b30a6 diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 6e0813b38..e9858b73c 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -154,6 +154,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) _rc_param_map.param_id[i][MAVLINK_MSG_PARAM_MAP_RC_FIELD_PARAM_ID_LEN] = '\0'; _rc_param_map.scale[i] = map_rc.scale; _rc_param_map.value0[i] = map_rc.param_value0; + _rc_param_map.value_min[i] = map_rc.param_value_min; + _rc_param_map.value_max[i] = map_rc.param_value_max; if (map_rc.param_index == -2) { // -2 means unset map _rc_param_map.valid[i] = false; } else { diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d404d8a4c..3fa1575f0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1440,11 +1440,13 @@ Sensors::rc_parameter_map_poll(bool forced) } warnx("rc to parameter map updated"); for (int i = 0; i < RC_PARAM_MAP_NCHAN; i++) { - warnx("\ti %d param_id %s scale %.3f value0 %.3f", + warnx("\ti %d param_id %s scale %.3f value0 %.3f, min %.3f, max %.3f", i, _rc_parameter_map.param_id[i], (double)_rc_parameter_map.scale[i], - (double)_rc_parameter_map.value0[i] + (double)_rc_parameter_map.value0[i], + (double)_rc_parameter_map.value_min[i], + (double)_rc_parameter_map.value_max[i] ); } } @@ -1645,7 +1647,9 @@ Sensors::set_params_from_rc() * maybe we need to introduce a more aggressive limit here */ if (rc_val > param_rc_values[i] + FLT_EPSILON || rc_val < param_rc_values[i] - FLT_EPSILON) { param_rc_values[i] = rc_val; - float param_val = _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val; + float param_val = math::constrain( + _rc_parameter_map.value0[i] + _rc_parameter_map.scale[i] * rc_val, + _rc_parameter_map.value_min[i], _rc_parameter_map.value_max[i]); param_set(_parameter_handles.rc_param[i], ¶m_val); } } diff --git a/src/modules/uORB/topics/rc_parameter_map.h b/src/modules/uORB/topics/rc_parameter_map.h index 47672c5d5..6e68dc4b6 100644 --- a/src/modules/uORB/topics/rc_parameter_map.h +++ b/src/modules/uORB/topics/rc_parameter_map.h @@ -63,6 +63,8 @@ struct rc_parameter_map_s { char param_id[RC_PARAM_MAP_NCHAN][PARAM_ID_LEN + 1]; /**< corresponding param id, null terminated */ float scale[RC_PARAM_MAP_NCHAN]; /** scale to map the RC input [-1, 1] to a parameter value */ float value0[RC_PARAM_MAP_NCHAN]; /** inital value around which the parameter value is changed */ + float value_min[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ + float value_max[RC_PARAM_MAP_NCHAN]; /** minimal parameter value */ }; /** -- cgit v1.2.3 From fc780a172948822886845aa7ab0a16d10a09e9e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Dec 2014 23:22:34 +0100 Subject: -Wno-attributes -Wno-packed for mavlink --- src/modules/mavlink/module.mk | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index 91fdd6154..f9d30dcbe 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -53,4 +53,6 @@ MAXOPTIMIZATION = -Os MODULE_STACKSIZE = 1024 -EXTRACXXFLAGS = -Weffc++ +EXTRACXXFLAGS = -Weffc++ -Wno-attributes -Wno-packed + +EXTRACFLAGS = -Wno-packed -- cgit v1.2.3 From e0396ffab7df6fdc89b95db2d964dd4e6e911d18 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 31 Dec 2014 08:31:07 +0100 Subject: mavlink tests module: -Wno-attributes -Wno-packed This was introduced in ca97d0156c07ad6cc09e4623140a7f47214946f5 / pull 1542 for the mavlink module --- src/modules/mavlink/mavlink_tests/module.mk | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index 1cc28cce1..b46d2bd35 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -47,4 +47,6 @@ MODULE_STACKSIZE = 5000 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST +EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST -Wno-attributes -Wno-packed + +EXTRACFLAGS = -Wno-packed -- cgit v1.2.3 From 25fc9d791a1faf5d37a6b9a13bd16fafd6a5cf5b Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 10:43:28 +0000 Subject: renaming of gps time to UTC time --- src/drivers/frsky_telemetry/frsky_data.c | 4 ++-- src/drivers/gps/ashtech.cpp | 4 ++-- src/drivers/gps/mtk.cpp | 4 ++-- src/drivers/gps/ubx.cpp | 8 ++++---- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- .../position_estimator_inav/position_estimator_inav_main.c | 10 +++++----- src/modules/sdlog2/sdlog2.c | 6 +++--- src/modules/uORB/topics/vehicle_global_position.h | 2 +- src/modules/uORB/topics/vehicle_gps_position.h | 2 +- src/modules/uavcan/sensors/gnss.cpp | 2 +- 11 files changed, 26 insertions(+), 26 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c index dd9b90fb3..890fada16 100644 --- a/src/drivers/frsky_telemetry/frsky_data.c +++ b/src/drivers/frsky_telemetry/frsky_data.c @@ -223,7 +223,7 @@ void frsky_send_frame2(int uart) char lat_ns = 0, lon_ew = 0; int sec = 0; if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { - time_t time_gps = global_pos.time_gps_usec / 1000000; + time_t time_gps = global_pos.time_utc_usec / 1000000ULL; struct tm *tm_gps = gmtime(&time_gps); course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; @@ -274,7 +274,7 @@ void frsky_send_frame3(int uart) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); /* send formatted frame */ - time_t time_gps = global_pos.time_gps_usec / 1000000; + time_t time_gps = global_pos.time_utc_usec / 1000000ULL; struct tm *tm_gps = gmtime(&time_gps); uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); frsky_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index f1c5ca549..99dbe5984 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -112,8 +112,8 @@ int ASHTECH::handle_message(int len) warn("failed setting clock"); } - _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_gps_usec += usecs; + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += usecs; _gps_position->timestamp_time = hrt_absolute_time(); } diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index c4f4f7bec..d808b59a9 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -282,8 +282,8 @@ MTK::handle_message(gps_mtk_packet_t &packet) timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; time_t epoch = mktime(&timeinfo); - _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this - _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; + _gps_position->time_utc_usec = epoch * 1e6; //TODO: test this + _gps_position->time_utc_usec += timeinfo_conversion_temp * 1e3; _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); // Position and velocity update always at the same time diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index c67965487..6b4e1630b 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -759,8 +759,8 @@ UBX::payload_rx_done(void) warn("failed setting clock"); } - _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } _gps_position->timestamp_time = hrt_absolute_time(); @@ -831,8 +831,8 @@ UBX::payload_rx_done(void) warn("failed setting clock"); } - _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } _gps_position->timestamp_time = hrt_absolute_time(); diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index e7805daa9..c41777968 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1470,7 +1470,7 @@ FixedwingEstimator::task_main() map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); _global_pos.lat = est_lat; _global_pos.lon = est_lon; - _global_pos.time_gps_usec = _gps.time_gps_usec; + _global_pos.time_utc_usec = _gps.time_utc_usec; _global_pos.eph = _gps.eph; _global_pos.epv = _gps.epv; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 6f5adb5fe..f109ceaf1 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -359,7 +359,7 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) /* optical flow */ mavlink_optical_flow_rad_t flow; mavlink_msg_optical_flow_rad_decode(msg, &flow); - + enum Rotation flow_rot; param_get(param_find("SENS_FLOW_ROT"),&flow_rot); @@ -380,8 +380,8 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg) f.gyro_temperature = flow.temperature; /* rotate measurements according to parameter */ - float zeroval = 0.0f; - rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval); + float zeroval = 0.0f; + rotate_3f(flow_rot, f.pixel_flow_x_integral, f.pixel_flow_y_integral, zeroval); if (_flow_pub < 0) { _flow_pub = orb_advertise(ORB_ID(optical_flow), &f); @@ -1137,7 +1137,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) memset(&hil_gps, 0, sizeof(hil_gps)); hil_gps.timestamp_time = timestamp; - hil_gps.time_gps_usec = gps.time_usec; + hil_gps.time_utc_usec = gps.time_usec; hil_gps.timestamp_position = timestamp; hil_gps.lat = gps.lat; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 1962151fa..2f972fc9f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -282,13 +282,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) }; float w_gps_xy = 1.0f; float w_gps_z = 1.0f; - + float corr_vision[3][2] = { { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) { 0.0f, 0.0f }, // D (pos, vel) }; - + float corr_sonar = 0.0f; float corr_sonar_filtered = 0.0f; @@ -650,13 +650,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) x_est[1] = vision.vx; y_est[0] = vision.y; y_est[1] = vision.vy; - /* only reset the z estimate if the z weight parameter is not zero */ + /* only reset the z estimate if the z weight parameter is not zero */ if (params.w_z_vision_p > MIN_VALID_W) { z_est[0] = vision.z; z_est[1] = vision.vz; } - + vision_valid = true; last_vision_x = vision.x; @@ -1166,7 +1166,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.xy_global && local_pos.z_global) { /* publish global position */ global_pos.timestamp = t; - global_pos.time_gps_usec = gps.time_gps_usec; + global_pos.time_utc_usec = gps.time_utc_usec; double est_lat, est_lon; map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2daf73bf9..2ce3d0097 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1128,7 +1128,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* check GPS topic to get GPS time */ if (log_name_timestamp) { if (!orb_copy(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos)) { - gps_time = buf_gps_pos.time_gps_usec; + gps_time = buf_gps_pos.time_utc_usec; } } @@ -1156,7 +1156,7 @@ int sdlog2_thread_main(int argc, char *argv[]) bool gps_pos_updated = copy_if_updated(ORB_ID(vehicle_gps_position), subs.gps_pos_sub, &buf_gps_pos); if (gps_pos_updated && log_name_timestamp) { - gps_time = buf_gps_pos.time_gps_usec; + gps_time = buf_gps_pos.time_utc_usec; } if (!logging_enabled) { @@ -1186,7 +1186,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (gps_pos_updated) { log_msg.msg_type = LOG_GPS_MSG; - log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; + log_msg.body.log_GPS.gps_time = buf_gps_pos.time_utc_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; log_msg.body.log_GPS.eph = buf_gps_pos.eph; log_msg.body.log_GPS.epv = buf_gps_pos.epv; diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index c3bb3b893..bc7046690 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -62,7 +62,7 @@ */ struct vehicle_global_position_s { uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t time_utc_usec; /**< GPS UTC timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude AMSL in meters */ diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 7888a50f4..102914bbb 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -79,7 +79,7 @@ struct vehicle_gps_position_s { bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ uint64_t timestamp_time; /**< Timestamp for time information */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */ + uint64_t time_utc_usec; /**< Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */ uint8_t satellites_used; /**< Number of satellites used */ }; diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index a375db37f..571a6f1cd 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -159,7 +159,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Sun, 4 Jan 2015 11:52:32 +0100 Subject: Fix integer constants in mavlink app --- src/modules/mavlink/mavlink_receiver.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 24f139176..1b24b9c52 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -947,12 +947,12 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) clock_gettime(CLOCK_REALTIME, &tv); // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - bool onb_unix_valid = tv.tv_sec > 1234567890L; - bool ofb_unix_valid = time.time_unix_usec > 1234567890L * 1000; + bool onb_unix_valid = tv.tv_sec > 1234567890ULL; + bool ofb_unix_valid = time.time_unix_usec > 1234567890ULL * 1000ULL; if (!onb_unix_valid && ofb_unix_valid) { - tv.tv_sec = time.time_unix_usec / 1000000; - tv.tv_nsec = (time.time_unix_usec % 1000000) * 1000; + tv.tv_sec = time.time_unix_usec / 1000000ULL; + tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; clock_settime(CLOCK_REALTIME, &tv); warnx("[timesync] Set system time from SYSTEM_TIME message"); } -- cgit v1.2.3 From 313af1760baf35a73201d3526405d81c4123e625 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 12:06:21 +0100 Subject: mavlink: reduce verbosity --- src/modules/mavlink/mavlink_receiver.cpp | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 090b62975..93a297285 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -263,7 +263,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ - warnx("terminated by remote command"); + warnx("terminated by remote"); fflush(stdout); usleep(50000); @@ -273,7 +273,7 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + warnx("ignoring CMD with same SYS/COMP (%d/%d) ID", mavlink_system.sysid, mavlink_system.compid); return; } @@ -319,7 +319,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ - warnx("terminated by remote command"); + warnx("terminated by remote"); fflush(stdout); usleep(50000); @@ -329,7 +329,7 @@ MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { - warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + warnx("ignoring CMD with same SYS/COMP (%d/%d) ID", mavlink_system.sysid, mavlink_system.compid); return; } @@ -880,7 +880,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); @@ -954,7 +954,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) tv.tv_sec = time.time_unix_usec / 1000000ULL; tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; clock_settime(CLOCK_REALTIME, &tv); - warnx("[timesync] Set system time from SYSTEM_TIME message"); + warnx("[timesync] synced.."); } } @@ -985,7 +985,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; - warnx("[timesync] Companion clock offset is skewed. Hard-setting offset"); + warnx("[timesync] Resetting."); } else { smooth_time_offset(offset_ns); @@ -1186,7 +1186,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* print HIL sensors rate */ if ((timestamp - _old_timestamp) > 10000000) { - printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); + // printf("receiving HIL sensors at %d hz\n", _hil_frames / 10); _old_timestamp = timestamp; _hil_frames = 0; } -- cgit v1.2.3 From 3616c83f88078ecf827fb844ffdbacdd442f9bad Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 4 Jan 2015 17:38:19 +0530 Subject: missing zero --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 93a297285..9b3274431 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -983,7 +983,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset int64_t dt = _time_offset - offset_ns; - if (dt > 10000000 || dt < -1000000) { // 10 millisecond skew + if (dt > 10000000 || dt < -10000000) { // 10 millisecond skew _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; warnx("[timesync] Resetting."); -- cgit v1.2.3 From 7d0c89ded78161d622d40c9dd7cd7b4ac7a8c8d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 15:53:07 +0100 Subject: mavlink app: Abort writing to text log on microSD once SD is not writeabele any more after 5 tries. Fix to first message write. --- src/modules/mavlink/mavlink_messages.cpp | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 89a40d032..6765100c7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -342,6 +342,8 @@ private: MavlinkStreamStatustext(MavlinkStreamStatustext &); MavlinkStreamStatustext& operator = (const MavlinkStreamStatustext &); FILE *fp = nullptr; + unsigned write_err_count = 0; + static const unsigned write_err_threshold = 5; protected: explicit MavlinkStreamStatustext(Mavlink *mavlink) : MavlinkStream(mavlink) @@ -370,10 +372,21 @@ protected: /* write log messages in first instance to disk */ if (_mavlink->get_instance_id() == 0) { if (fp) { - fputs(msg.text, fp); - fputs("\n", fp); - fsync(fileno(fp)); - } else { + if (EOF == fputs(msg.text, fp)) { + write_err_count++; + } else { + write_err_count = 0; + } + + if (write_err_count >= write_err_threshold) { + (void)fclose(fp); + fp = nullptr; + } else { + (void)fputs("\n", fp); + (void)fsync(fileno(fp)); + } + + } else if (write_err_count < write_err_threshold) { /* string to hold the path to the log */ char log_file_name[32] = ""; char log_file_path[64] = ""; @@ -389,6 +402,10 @@ protected: strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name); fp = fopen(log_file_path, "ab"); + + /* write first message */ + fputs(msg.text, fp); + fputs("\n", fp); } } } -- cgit v1.2.3 From c47e8c22688ad3ac358c71337ef8cb0feeeba409 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 4 Jan 2015 21:29:05 +0530 Subject: Add suffixes, constant --- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++++------ src/modules/mavlink/mavlink_receiver.h | 2 ++ 2 files changed, 12 insertions(+), 6 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9b3274431..d84b70e46 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -947,14 +947,18 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg) clock_gettime(CLOCK_REALTIME, &tv); // date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009 - bool onb_unix_valid = tv.tv_sec > 1234567890ULL; - bool ofb_unix_valid = time.time_unix_usec > 1234567890ULL * 1000ULL; + bool onb_unix_valid = tv.tv_sec > PX4_EPOCH_SECS; + bool ofb_unix_valid = time.time_unix_usec > PX4_EPOCH_SECS * 1000ULL; if (!onb_unix_valid && ofb_unix_valid) { tv.tv_sec = time.time_unix_usec / 1000000ULL; tv.tv_nsec = (time.time_unix_usec % 1000000ULL) * 1000ULL; - clock_settime(CLOCK_REALTIME, &tv); - warnx("[timesync] synced.."); + if(clock_settime(CLOCK_REALTIME, &tv)) { + warn("failed setting clock"); + } + else { + warnx("[timesync] UTC time synced."); + } } } @@ -983,9 +987,9 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset int64_t dt = _time_offset - offset_ns; - if (dt > 10000000 || dt < -10000000) { // 10 millisecond skew + if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; - warnx("[timesync] Resetting."); + warnx("[timesync] Resetting offset sync."); } else { smooth_time_offset(offset_ns); diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a677e75cd..4afc9b372 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -75,6 +75,8 @@ #include "mavlink_ftp.h" +#define PX4_EPOCH_SECS 1234567890ULL + class Mavlink; class MavlinkReceiver -- cgit v1.2.3 From 3df73cde3e3bd092cdf28990c0736001681a9289 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sun, 4 Jan 2015 22:38:47 +0530 Subject: Fix offset calculation. --- src/modules/mavlink/mavlink_receiver.cpp | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) (limited to 'src/modules/mavlink') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d84b70e46..97108270c 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -969,7 +969,7 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) mavlink_timesync_t tsync; mavlink_msg_timesync_decode(msg, &tsync); - uint64_t now_ns = hrt_absolute_time() * 1000 ; + uint64_t now_ns = hrt_absolute_time() * 1000LL ; if (tsync.tc1 == 0) { @@ -984,13 +984,12 @@ MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg) } else if (tsync.tc1 > 0) { - int64_t offset_ns = (9*_time_offset + (tsync.ts1 + now_ns - tsync.tc1*2)/2 )/10; // average offset + int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1*2)/2 ; int64_t dt = _time_offset - offset_ns; if (dt > 10000000LL || dt < -10000000LL) { // 10 millisecond skew - _time_offset = (tsync.ts1 + now_ns - tsync.tc1*2)/2; - warnx("[timesync] Resetting offset sync."); - + _time_offset = offset_ns; + warnx("[timesync] Hard setting offset."); } else { smooth_time_offset(offset_ns); } @@ -1473,7 +1472,7 @@ uint64_t MavlinkReceiver::to_hrt(uint64_t usec) void MavlinkReceiver::smooth_time_offset(uint64_t offset_ns) { - /* alpha = 0.75 fixed for now. The closer alpha is to 1.0, + /* alpha = 0.6 fixed for now. The closer alpha is to 1.0, * the faster the moving average updates in response to * new offset samples. */ -- cgit v1.2.3