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') 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') 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') 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') 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') 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') 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') 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 4d0d6f09ce9aef8875e33ee80aefcc8ed00d4769 Mon Sep 17 00:00:00 2001 From: Rowland O'Flaherty Date: Thu, 18 Dec 2014 16:56:28 -0800 Subject: Fixed loop limit errors in Matrix.h In few of the overloaded operators the loop limits (i.e. M and N) were swapped. --- src/lib/mathlib/math/Matrix.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp index ca931e2da..ac1f1538f 100644 --- a/src/lib/mathlib/math/Matrix.hpp +++ b/src/lib/mathlib/math/Matrix.hpp @@ -180,8 +180,8 @@ public: Matrix operator -(void) const { Matrix res; - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) res.data[i][j] = -data[i][j]; return res; @@ -193,16 +193,16 @@ public: Matrix operator +(const Matrix &m) const { Matrix res; - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) res.data[i][j] = data[i][j] + m.data[i][j]; return res; } Matrix &operator +=(const Matrix &m) { - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) data[i][j] += m.data[i][j]; return *static_cast*>(this); @@ -222,8 +222,8 @@ public: } Matrix &operator -=(const Matrix &m) { - for (unsigned int i = 0; i < N; i++) - for (unsigned int j = 0; j < M; j++) + for (unsigned int i = 0; i < M; i++) + for (unsigned int j = 0; j < N; j++) data[i][j] -= m.data[i][j]; return *static_cast*>(this); -- cgit v1.2.3 From d40168dc4b46f922b1be76dcf8a6a37a675788ae Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sat, 27 Dec 2014 23:01:31 +0530 Subject: Add support for rotations of PX4flow --- src/drivers/drv_px4flow.h | 2 +- src/drivers/drv_sensor.h | 15 +++++++++++++-- src/drivers/px4flow/px4flow.cpp | 22 +++++++++++++++++----- src/modules/sensors/sensor_params.c | 36 ++++++++++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 32 +++++++++++++++++++++++++------- 5 files changed, 92 insertions(+), 15 deletions(-) (limited to 'src') diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index ab640837b..5aed3f02b 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file Rangefinder driver interface. + * @file PX4Flow driver interface. */ #ifndef _DRV_PX4FLOW_H diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 8e8b2c1b9..5e4598de8 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -82,8 +82,19 @@ #define SENSORIOCGQUEUEDEPTH _SENSORIOC(3) /** - * Reset the sensor to its default configuration. + * Reset the sensor to its default configuration */ #define SENSORIOCRESET _SENSORIOC(4) -#endif /* _DRV_SENSOR_H */ \ No newline at end of file +/** + * Set the sensor orientation + */ +#define SENSORIOCSROTATION _SENSORIOC(5) + +/** + * Get the sensor orientation + */ +#define SENSORIOCGROTATION _SENSORIOC(6) + +#endif /* _DRV_SENSOR_H */ + diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 62308fc65..99a9cc02a 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -62,6 +62,8 @@ #include #include +#include + #include #include #include @@ -125,7 +127,7 @@ struct i2c_integral_frame f_integral; class PX4FLOW: public device::I2C { public: - PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS); + PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS, enum Rotation rotation = (enum Rotation)0); virtual ~PX4FLOW(); virtual int init(); @@ -154,6 +156,8 @@ private: perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + + enum Rotation _sensor_rotation; /** * Test whether the device supported by the driver is present at a @@ -199,7 +203,7 @@ private: */ extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); -PX4FLOW::PX4FLOW(int bus, int address) : +PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) : I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz _reports(nullptr), _sensor_ok(false), @@ -208,7 +212,8 @@ PX4FLOW::PX4FLOW(int bus, int address) : _px4flow_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")), _comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")), + _sensor_rotation(rotation) { // enable debug() calls _debug_enabled = false; @@ -358,8 +363,12 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } - case SENSORIOCGQUEUEDEPTH: - return _reports->size(); + case SENSORIOCSROTATION: + _sensor_rotation = (enum Rotation)arg; + return OK; + + case SENSORIOCGROTATION: + return _sensor_rotation; case SENSORIOCRESET: /* XXX implement this */ @@ -535,6 +544,9 @@ PX4FLOW::collect() report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius 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); if (_px4flow_topic < 0) { _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 229bfe3ce..f5f6094d1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -261,6 +261,42 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); */ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); +/** + * PX4Flow board rotation + * + * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * Possible values are: + * 0 = No rotation + * 1 = Yaw 45° + * 2 = Yaw 90° + * 3 = Yaw 135° + * 4 = Yaw 180° + * 5 = Yaw 225° + * 6 = Yaw 270° + * 7 = Yaw 315° + * 8 = Roll 180° + * 9 = Roll 180°, Yaw 45° + * 10 = Roll 180°, Yaw 90° + * 11 = Roll 180°, Yaw 135° + * 12 = Pitch 180° + * 13 = Roll 180°, Yaw 225° + * 14 = Roll 180°, Yaw 270° + * 15 = Roll 180°, Yaw 315° + * 16 = Roll 90° + * 17 = Roll 90°, Yaw 45° + * 18 = Roll 90°, Yaw 90° + * 19 = Roll 90°, Yaw 135° + * 20 = Roll 270° + * 21 = Roll 270°, Yaw 45° + * 22 = Roll 270°, Yaw 90° + * 23 = Roll 270°, Yaw 135° + * 24 = Pitch 90° + * 25 = Pitch 270° + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0); + /** * Board rotation Y (Pitch) offset * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index cdcb428dd..d8df2c8ff 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -63,6 +63,7 @@ #include #include #include +#include #include #include @@ -263,6 +264,7 @@ private: float diff_pres_analog_scale; int board_rotation; + int flow_rotation; int external_mag_rotation; float board_offset[3]; @@ -361,6 +363,7 @@ private: param_t battery_current_scaling; param_t board_rotation; + param_t flow_rotation; param_t external_mag_rotation; param_t board_offset[3]; @@ -614,6 +617,7 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); + _parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); /* rotation offsets */ @@ -831,8 +835,22 @@ Sensors::parameters_update() } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); + param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation)); param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation)); + /* set px4flow rotation */ + int flowfd; + flowfd = open(PX4FLOW_DEVICE_PATH, 0); + if (flowfd >= 0) { + int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation); + if (flowret) { + warnx("flow rotation could not be set"); + close(flowfd); + return ERROR; + } + close(flowfd); + } + get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); @@ -850,20 +868,20 @@ Sensors::parameters_update() /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); - int fd; - fd = open(BARO_DEVICE_PATH, 0); - if (fd < 0) { + int barofd; + barofd = open(BARO_DEVICE_PATH, 0); + if (barofd < 0) { warn("%s", BARO_DEVICE_PATH); errx(1, "FATAL: no barometer found"); } else { - int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); - if (ret) { + int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (baroret) { warnx("qnh could not be set"); - close(fd); + close(barofd); return ERROR; } - close(fd); + close(barofd); } return OK; -- 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') 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') 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') 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') 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') 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 ee7e008008caa04f905654cb18e6d68fd980f8cd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Dec 2014 23:22:02 +0100 Subject: increase commander framesize --- src/modules/commander/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index dac2ce59a..0e2a5356b 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -52,5 +52,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wframe-larger-than=1900 +EXTRACXXFLAGS = -Wframe-larger-than=2000 -- cgit v1.2.3 From a0b767f4676a9204260442c56ab309108d47638d Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 24 Dec 2014 13:45:04 -0700 Subject: add yaw_manual override variable, effective only in ALTCTL and do not publish it in attitude rates --- src/modules/fw_att_control/fw_att_control_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) (limited to 'src') diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 8d18ae68c..d8377899c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -824,6 +824,7 @@ FixedwingAttitudeControl::task_main() float roll_sp = _parameters.rollsp_offset_rad; float pitch_sp = _parameters.pitchsp_offset_rad; + float yaw_manual = 0.0f; float throttle_sp = 0.0f; /* Read attitude setpoint from uorb if @@ -884,6 +885,8 @@ FixedwingAttitudeControl::task_main() + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; + // allow manual control of rudder deflection + yaw_manual = _manual.r; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; @@ -983,6 +986,8 @@ FixedwingAttitudeControl::task_main() _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; + // add in manual rudder control + _actuators.control[2] += yaw_manual; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); -- cgit v1.2.3 From f05aa01e693dab6b16aee136f162a614c5931421 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 29 Dec 2014 16:55:22 +0100 Subject: fw att: fix comment style --- src/modules/fw_att_control/fw_att_control_main.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index d8377899c..45dcddb9c 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -400,7 +400,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* fetch initial parameter values */ parameters_update(); - // set correct uORB ID, depending on if vehicle is VTOL or not + + /* set correct uORB ID, depending on if vehicle is VTOL or not */ if (_parameters.autostart_id >= 13000 && _parameters.autostart_id <= 13999) { /* VTOL airframe?*/ _rates_sp_id = ORB_ID(fw_virtual_rates_setpoint); _actuators_id = ORB_ID(actuator_controls_virtual_fw); @@ -724,22 +725,24 @@ FixedwingAttitudeControl::task_main() R.set(_att.R); R_adapted.set(_att.R); - //move z to x + /* move z to x */ R_adapted(0, 0) = R(0, 2); R_adapted(1, 0) = R(1, 2); R_adapted(2, 0) = R(2, 2); - //move x to z + + /* move x to z */ R_adapted(0, 2) = R(0, 0); R_adapted(1, 2) = R(1, 0); R_adapted(2, 2) = R(2, 0); - //change direction of pitch (convert to right handed system) + /* change direction of pitch (convert to right handed system) */ R_adapted(0, 0) = -R_adapted(0, 0); R_adapted(1, 0) = -R_adapted(1, 0); R_adapted(2, 0) = -R_adapted(2, 0); math::Vector<3> euler_angles; //adapted euler angles for fixed wing operation euler_angles = R_adapted.to_euler(); - //fill in new attitude data + + /* fill in new attitude data */ _att.roll = euler_angles(0); _att.pitch = euler_angles(1); _att.yaw = euler_angles(2); @@ -753,7 +756,7 @@ FixedwingAttitudeControl::task_main() _att.R[2][1] = R_adapted(2, 1); _att.R[2][2] = R_adapted(2, 2); - // lastly, roll- and yawspeed have to be swaped + /* lastly, roll- and yawspeed have to be swaped */ float helper = _att.rollspeed; _att.rollspeed = -_att.yawspeed; _att.yawspeed = helper; @@ -850,7 +853,7 @@ FixedwingAttitudeControl::task_main() _yaw_ctrl.reset_integrator(); } } else if (_vcontrol_mode.flag_control_velocity_enabled) { - /* + /* * Velocity should be controlled and manual is enabled. */ roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll) @@ -885,7 +888,7 @@ FixedwingAttitudeControl::task_main() + _parameters.rollsp_offset_rad; pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad; - // allow manual control of rudder deflection + /* allow manual control of rudder deflection */ yaw_manual = _manual.r; throttle_sp = _manual.z; _actuators.control[4] = _manual.flaps; @@ -986,7 +989,8 @@ FixedwingAttitudeControl::task_main() _pitch_ctrl.get_desired_rate(), _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; - // add in manual rudder control + + /* add in manual rudder control */ _actuators.control[2] += yaw_manual; if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); -- cgit v1.2.3 From ac9e9835ac14284f704fda57593f16ba37159905 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 30 Dec 2014 11:13:05 +0900 Subject: i2c: const get_address --- src/drivers/device/i2c.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 705b398b0..206e71ddc 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -58,7 +58,7 @@ public: /** * Get the address */ - int16_t get_address() { return _address; } + int16_t get_address() const { return _address; } protected: /** -- cgit v1.2.3 From df9547e9d36b4fd167f746dff1b809d2f4bfac3d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 25 Nov 2014 15:03:44 +0900 Subject: batt_smbus: driver for smart battery --- src/drivers/batt_smbus/batt_smbus.cpp | 438 ++++++++++++++++++++++++++++++++++ src/drivers/batt_smbus/module.mk | 8 + src/drivers/drv_batt_smbus.h | 50 ++++ 3 files changed, 496 insertions(+) create mode 100644 src/drivers/batt_smbus/batt_smbus.cpp create mode 100644 src/drivers/batt_smbus/module.mk create mode 100644 src/drivers/drv_batt_smbus.h (limited to 'src') diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp new file mode 100644 index 000000000..89abcf838 --- /dev/null +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -0,0 +1,438 @@ +/**************************************************************************** + * + * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Randy Mackay + * + * 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 batt_smbus.cpp + * + * Driver for a battery monitor connected via SMBus (I2C). + * + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include + +#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION +#define BATT_SMBUS_ADDR 0x0B /* I2C address */ +#define BATT_SMBUS_TEMP 0x08 /* temperature register */ +#define BATT_SMBUS_VOLTAGE 0x09 /* voltage register */ +#define BATT_SMBUS_DESIGN_CAPACITY 0x18 /* design capacity register */ +#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 /* design voltage register */ +#define BATT_SMBUS_SERIALNUM 0x1c /* serial number register */ +#define BATT_SMBUS_MANUFACTURE_INFO 0x25 /* cell voltage register */ +#define BATT_SMBUS_CURRENT 0x2a /* current register */ +#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) /* time in microseconds, measure at 10hz */ + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class BATT_SMBUS : public device::I2C +{ +public: + BATT_SMBUS(int bus = PX4_I2C_BUS_EXPANSION, uint16_t batt_smbus_addr = BATT_SMBUS_ADDR); + virtual ~BATT_SMBUS(); + + virtual int init(); + virtual int test(); + +protected: + virtual int probe(); // check if the device can be contacted + +private: + + // start periodic reads from the battery + void start(); + + // stop periodic reads from the battery + void stop(); + + // static function that is called by worker queue + static void cycle_trampoline(void *arg); + + // perform a read from the battery + void cycle(); + + // read_reg - read a word from specified register + int read_reg(uint8_t reg, uint16_t &val); + + // read_block - returns number of characters read if successful, zero if unsuccessful + uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); + + // internal variables + work_s _work; // work queue for scheduling reads + RingBuffer *_reports; // buffer of recorded voltages, currents + struct battery_status_s _last_report; // last published report, used for test() + orb_advert_t _batt_topic; + orb_id_t _batt_orb_id; +}; + +/* for now, we only support one BATT_SMBUS */ +namespace +{ +BATT_SMBUS *g_batt_smbus; +} + +void batt_smbus_usage(); + +extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); + +// constructor +BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : + I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), + _work{}, + _reports(nullptr), + _batt_topic(-1), + _batt_orb_id(nullptr) +{ + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +// destructor +BATT_SMBUS::~BATT_SMBUS() +{ + /* make sure we are truly inactive */ + stop(); + + if (_reports != nullptr) { + delete _reports; + } +} + +int +BATT_SMBUS::init() +{ + int ret = ENOTTY; + + // attempt to initialise I2C bus + ret = I2C::init(); + + if (ret != OK) { + errx(1,"failed to init I2C"); + return ret; + } else { + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(struct battery_status_s)); + if (_reports == nullptr) { + ret = ENOTTY; + } else { + // start work queue + start(); + } + } + + // init orb id + _batt_orb_id = ORB_ID(battery_status); + + return ret; +} + +int +BATT_SMBUS::test() +{ + int sub = orb_subscribe(ORB_ID(battery_status)); + bool updated = false; + struct battery_status_s status; + uint64_t start_time = hrt_absolute_time(); + + // loop for 5 seconds + while ((hrt_absolute_time() - start_time) < 5000000) { + + // display new info that has arrived from the orb + orb_check(sub, &updated); + if (updated) { + if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { + warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); + } + } + + // sleep for 0.05 seconds + usleep(50000); + } + + return OK; +} + +int +BATT_SMBUS::probe() +{ + // always return OK to ensure device starts + return OK; +} + +// start periodic reads from the battery +void +BATT_SMBUS::start() +{ + /* reset the report ring and state machine */ + _reports->flush(); + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1); +} + +// stop periodic reads from the battery +void +BATT_SMBUS::stop() +{ + work_cancel(HPWORK, &_work); +} + +// static function that is called by worker queue +void +BATT_SMBUS::cycle_trampoline(void *arg) +{ + BATT_SMBUS *dev = (BATT_SMBUS *)arg; + + dev->cycle(); +} + +// perform a read from the battery +void +BATT_SMBUS::cycle() +{ + // read data from sensor + struct battery_status_s new_report; + + // set time of reading + new_report.timestamp = hrt_absolute_time(); + + // read voltage + uint16_t tmp; + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { + // initialise new_report + memset(&new_report, 0, sizeof(new_report)); + + // convert millivolts to volts + new_report.voltage_v = ((float)tmp) / 1000.0f; + + // To-Do: read current as block from BATT_SMBUS_CURRENT register + + // publish to orb + if (_batt_topic != -1) { + orb_publish(_batt_orb_id, _batt_topic, &new_report); + } else { + _batt_topic = orb_advertise(_batt_orb_id, &new_report); + if (_batt_topic < 0) { + errx(1, "ADVERT FAIL"); + } + } + + // copy report for test() + _last_report = new_report; + + /* post a report to the ring */ + _reports->force(&new_report); + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + } + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); +} + +int +BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) +{ + uint8_t buff[2]; + + // short sleep to improve reliability in cases of two consecutive reads + usleep(1); + + // read from register + int ret = transfer(®, 1, buff, 2); + if (ret == OK) { + val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + } + + // return success or failure + return ret; +} + +// read_block - returns number of characters read if successful, zero if unsuccessful +uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) +{ + uint8_t buff[max_len+1]; // buffer to hold results + + usleep(1); + + // read bytes + int ret = transfer(®, 1,buff, max_len+1); + + // return zero on failure + if (ret != OK) { + return 0; + } + + // get length + uint8_t bufflen = buff[0]; + + // sanity check length returned by smbus + if (bufflen == 0 || bufflen > max_len) { + return 0; + } + + // copy data + memcpy(data, &buff[1], bufflen); + + // optionally add zero to end + if (append_zero) { + data[bufflen] = '\0'; + } + + // return success + return bufflen; +} + +///////////////////////// shell functions /////////////////////// + +void +batt_smbus_usage() +{ + warnx("missing command: try 'start', 'test', 'stop'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS); + warnx(" -a addr (0x%x)", BATT_SMBUS_ADDR); +} + +int +batt_smbus_main(int argc, char *argv[]) +{ + int i2cdevice = BATT_SMBUS_I2C_BUS; + int batt_smbusadr = BATT_SMBUS_ADDR; /* 7bit */ + + int ch; + + /* jump over start/off/etc and look at options first */ + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + batt_smbusadr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + batt_smbus_usage(); + exit(0); + } + } + + if (optind >= argc) { + batt_smbus_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + if (!strcmp(verb, "start")) { + if (g_batt_smbus != nullptr) { + errx(1, "already started"); + } else { + // create new global object + g_batt_smbus = new BATT_SMBUS(i2cdevice, batt_smbusadr); + + if (g_batt_smbus == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_batt_smbus->init()) { + delete g_batt_smbus; + g_batt_smbus = nullptr; + errx(1, "init failed"); + } + } + + exit(0); + } + + /* need the driver past this point */ + if (g_batt_smbus == nullptr) { + warnx("not started"); + batt_smbus_usage(); + exit(1); + } + + if (!strcmp(verb, "test")) { + g_batt_smbus->test(); + exit(0); + } + + if (!strcmp(verb, "stop")) { + delete g_batt_smbus; + g_batt_smbus = nullptr; + exit(0); + } + + batt_smbus_usage(); + exit(0); +} diff --git a/src/drivers/batt_smbus/module.mk b/src/drivers/batt_smbus/module.mk new file mode 100644 index 000000000..b32ea6e55 --- /dev/null +++ b/src/drivers/batt_smbus/module.mk @@ -0,0 +1,8 @@ +# +# driver for SMBus smart batteries +# + +MODULE_COMMAND = batt_smbus +SRCS = batt_smbus.cpp + +MAXOPTIMIZATION = -Os diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h new file mode 100644 index 000000000..6bba5fe16 --- /dev/null +++ b/src/drivers/drv_batt_smbus.h @@ -0,0 +1,50 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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 drv_batt_smbus.h + * + * SMBus battery monitor device API + */ + +#pragma once + +#include +#include +#include "drv_orb_dev.h" + +/* device path */ +#define BATT_SMBUS_DEVICE_PATH "/dev/batt_smbus" + +/* ObjDev tag for battery data */ +ORB_DECLARE(battery_status); -- cgit v1.2.3 From b8e818b38715b24889858c22ab0396a35df624c3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 9 Dec 2014 19:29:55 +0900 Subject: batt_smbus: remove sleep before I2C transfer --- src/drivers/batt_smbus/batt_smbus.cpp | 3 --- 1 file changed, 3 deletions(-) (limited to 'src') diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 89abcf838..494187ac0 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -303,9 +303,6 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) { uint8_t buff[2]; - // short sleep to improve reliability in cases of two consecutive reads - usleep(1); - // read from register int ret = transfer(®, 1, buff, 2); if (ret == OK) { -- cgit v1.2.3 From 78b9e06a154cf7ef1549c0c01f8611244d5c3b2c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Dec 2014 16:42:47 +0900 Subject: batt_smbus: remove redundant ORB_DECLARE --- src/drivers/drv_batt_smbus.h | 3 --- 1 file changed, 3 deletions(-) (limited to 'src') diff --git a/src/drivers/drv_batt_smbus.h b/src/drivers/drv_batt_smbus.h index 6bba5fe16..ca130c84e 100644 --- a/src/drivers/drv_batt_smbus.h +++ b/src/drivers/drv_batt_smbus.h @@ -45,6 +45,3 @@ /* device path */ #define BATT_SMBUS_DEVICE_PATH "/dev/batt_smbus" - -/* ObjDev tag for battery data */ -ORB_DECLARE(battery_status); -- cgit v1.2.3 From 65bcd0e1226f5f5d6db28dafe5faea771ccbde1d Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 29 Dec 2014 21:09:32 +0900 Subject: batt_smbus: minor format fix --- src/drivers/batt_smbus/batt_smbus.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 494187ac0..882b38f7e 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -304,7 +304,7 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) uint8_t buff[2]; // read from register - int ret = transfer(®, 1, buff, 2); + int ret = transfer(®, 1, buff, 2); if (ret == OK) { val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; } -- cgit v1.2.3 From a952a18b42d486e6f3ba1d52506fbd2ba2fc3425 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Dec 2014 16:42:29 +0900 Subject: batt_smbus: add get_PEC --- src/drivers/batt_smbus/batt_smbus.cpp | 84 ++++++++++++++++++++++++++++++++--- 1 file changed, 77 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 882b38f7e..4d4a84049 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -84,10 +84,13 @@ #define BATT_SMBUS_DESIGN_CAPACITY 0x18 /* design capacity register */ #define BATT_SMBUS_DESIGN_VOLTAGE 0x19 /* design voltage register */ #define BATT_SMBUS_SERIALNUM 0x1c /* serial number register */ +#define BATT_SMBUS_MANUFACTURE_NAME 0x20 /* manufacturer name */ #define BATT_SMBUS_MANUFACTURE_INFO 0x25 /* cell voltage register */ #define BATT_SMBUS_CURRENT 0x2a /* current register */ #define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) /* time in microseconds, measure at 10hz */ +#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 /* Polynomial for calculating PEC */ + #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. #endif @@ -124,6 +127,10 @@ private: // read_block - returns number of characters read if successful, zero if unsuccessful uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); + // get_PEC - calculate PEC for a read or write from the battery + // buff is the data that was read or will be written + uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; + // internal variables work_s _work; // work queue for scheduling reads RingBuffer *_reports; // buffer of recorded voltages, currents @@ -272,7 +279,12 @@ BATT_SMBUS::cycle() // convert millivolts to volts new_report.voltage_v = ((float)tmp) / 1000.0f; - // To-Do: read current as block from BATT_SMBUS_CURRENT register + // read current + usleep(1); + uint8_t buff[4]; + if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { + new_report.current_a = (float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; + } // publish to orb if (_batt_topic != -1) { @@ -301,12 +313,20 @@ BATT_SMBUS::cycle() int BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) { - uint8_t buff[2]; + uint8_t buff[3]; // 2 bytes of data + PEC // read from register - int ret = transfer(®, 1, buff, 2); + int ret = transfer(®, 1, buff, 3); if (ret == OK) { - val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + // check PEC + uint8_t pec = get_PEC(reg, true, buff, 2); + if (pec == buff[2]) { + val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + warnx("PEC ok: %x",(int)pec); + } else { + ret = ENOTTY; + warnx("PEC:%x vs MyPec:%x",(int)buff[2],(int)pec); + } } // return success or failure @@ -316,12 +336,12 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) // read_block - returns number of characters read if successful, zero if unsuccessful uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) { - uint8_t buff[max_len+1]; // buffer to hold results + uint8_t buff[max_len+2]; // buffer to hold results usleep(1); - // read bytes - int ret = transfer(®, 1,buff, max_len+1); + // read bytes including PEC + int ret = transfer(®, 1, buff, max_len+2); // return zero on failure if (ret != OK) { @@ -336,6 +356,16 @@ uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool return 0; } + // check PEC + uint8_t pec = get_PEC(reg, true, buff, bufflen+1); + if (pec != buff[bufflen+1]) { + // debug + warnx("CurrPEC:%x vs MyPec:%x",(int)buff[bufflen+1],(int)pec); + return 0; + } else { + warnx("CurPEC ok: %x",(int)pec); + } + // copy data memcpy(data, &buff[1], bufflen); @@ -348,6 +378,46 @@ uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool return bufflen; } +// get_PEC - calculate PEC for a read or write from the battery +// buff is the data that was read or will be written +uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const +{ + // exit immediately if no data + if (len <= 0) { + return 0; + } + + // prepare temp buffer for calcing crc + uint8_t tmp_buff[len+3]; + tmp_buff[0] = (uint8_t)get_address() << 1; + tmp_buff[1] = cmd; + tmp_buff[2] = tmp_buff[0] | (uint8_t)reading; + memcpy(&tmp_buff[3],buff,len); + + // initialise crc to zero + uint8_t crc = 0; + uint8_t shift_reg = 0; + bool do_invert; + + // for each byte in the stream + for (uint8_t i=0; i Date: Tue, 30 Dec 2014 12:14:54 +0900 Subject: batt_smbus: add search --- src/drivers/batt_smbus/batt_smbus.cpp | 41 ++++++++++++++++++++++++++++++++--- 1 file changed, 38 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 4d4a84049..dd83dacaf 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -77,6 +77,9 @@ #include #include +#define BATT_SMBUS_ADDR_MIN 0x08 /* lowest possible address */ +#define BATT_SMBUS_ADDR_MAX 0x7F /* highest possible address */ + #define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION #define BATT_SMBUS_ADDR 0x0B /* I2C address */ #define BATT_SMBUS_TEMP 0x08 /* temperature register */ @@ -103,6 +106,7 @@ public: virtual int init(); virtual int test(); + int search(); /* search all possible slave addresses for a smart battery */ protected: virtual int probe(); // check if the device can be contacted @@ -226,6 +230,34 @@ BATT_SMBUS::test() return OK; } +/* search all possible slave addresses for a smart battery */ +int +BATT_SMBUS::search() +{ + bool found_slave = false; + uint16_t tmp; + + // search through all valid SMBus addresses + for (uint8_t i=BATT_SMBUS_ADDR_MIN; i<=BATT_SMBUS_ADDR_MAX; i++) { + set_address(i); + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { + warnx("battery found at 0x%x",(int)i); + found_slave = true; + } + // short sleep + usleep(1); + } + + // display completion message + if (found_slave) { + warnx("Done."); + } else { + warnx("No smart batteries found."); + } + + return OK; +} + int BATT_SMBUS::probe() { @@ -322,10 +354,8 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) uint8_t pec = get_PEC(reg, true, buff, 2); if (pec == buff[2]) { val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; - warnx("PEC ok: %x",(int)pec); } else { ret = ENOTTY; - warnx("PEC:%x vs MyPec:%x",(int)buff[2],(int)pec); } } @@ -423,7 +453,7 @@ uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uin void batt_smbus_usage() { - warnx("missing command: try 'start', 'test', 'stop'"); + warnx("missing command: try 'start', 'test', 'stop', 'search'"); warnx("options:"); warnx(" -b i2cbus (%d)", BATT_SMBUS_I2C_BUS); warnx(" -a addr (0x%x)", BATT_SMBUS_ADDR); @@ -500,6 +530,11 @@ batt_smbus_main(int argc, char *argv[]) exit(0); } + if (!strcmp(verb, "search")) { + g_batt_smbus->search(); + exit(0); + } + batt_smbus_usage(); exit(0); } -- cgit v1.2.3 From 7bed5b382f8c89a061970f94b61d610899475160 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Dec 2014 10:28:35 +0100 Subject: Clean up docs in PX4FLOW driver --- src/drivers/px4flow/px4flow.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 62308fc65..7e3461ba1 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -73,13 +73,14 @@ #include /* Configuration Constants */ -#define I2C_FLOW_ADDRESS 0x42 //* 7-bit address. 8-bit address is 0x84 -//range 0x42 - 0x49 +#define I2C_FLOW_ADDRESS 0x42 ///< 7-bit address. 8-bit address is 0x84, range 0x42 - 0x49 /* PX4FLOW Registers addresses */ -#define PX4FLOW_REG 0x16 /* Measure Register 22*/ +#define PX4FLOW_REG 0x16 ///< Measure Register 22 + +#define PX4FLOW_CONVERSION_INTERVAL 20000 ///< in microseconds! 20000 = 50 Hz 100000 = 10Hz +#define PX4FLOW_I2C_MAX_BUS_SPEED 400000 ///< 400 KHz maximum speed -#define PX4FLOW_CONVERSION_INTERVAL 20000 //in microseconds! 20000 = 50 Hz 100000 = 10Hz /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -200,7 +201,7 @@ private: extern "C" __EXPORT int px4flow_main(int argc, char *argv[]); PX4FLOW::PX4FLOW(int bus, int address) : - I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz + I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, PX4FLOW_I2C_MAX_BUS_SPEED), /* 100-400 KHz */ _reports(nullptr), _sensor_ok(false), _measure_ticks(0), -- cgit v1.2.3 From 4942883ddcb5d1a09e96335b1edbbf2d937937b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Dec 2014 10:45:09 +0100 Subject: RTL params: Set safer initial value for RTL altitude for all vehicles, improve dparam documentation --- src/modules/navigator/navigator_params.c | 8 +++++--- src/modules/navigator/rtl_params.c | 11 ++++++----- 2 files changed, 11 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 1f40e634e..5f8f8d02f 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -50,7 +50,8 @@ * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). * * @unit meters - * @min 0.0 + * @min 20 + * @max 200 * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); @@ -61,10 +62,11 @@ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); * Default acceptance radius, overridden by acceptance radius of waypoint if set. * * @unit meters - * @min 1.0 + * @min 0.05 + * @max 200 * @group Mission */ -PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); +PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 10.0f); /** * Set OBC mode for data link loss diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c index bfe6ce7e1..1568233b0 100644 --- a/src/modules/navigator/rtl_params.c +++ b/src/modules/navigator/rtl_params.c @@ -53,7 +53,8 @@ * Default value of loiter radius after RTL (fixedwing only). * * @unit meters - * @min 0.0 + * @min 20 + * @max 200 * @group RTL */ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); @@ -65,10 +66,10 @@ PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); * * @unit meters * @min 0 - * @max 1 + * @max 150 * @group RTL */ -PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 60); /** @@ -78,7 +79,7 @@ PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); * Land (i.e. slowly descend) from this altitude if autolanding allowed. * * @unit meters - * @min 0 + * @min 2 * @max 100 * @group RTL */ @@ -92,7 +93,7 @@ PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); * * @unit seconds * @min -1 - * @max + * @max 300 * @group RTL */ PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.0f); -- cgit v1.2.3 From 5de80bbaf1e2d494304167fc7317abec919d9c41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Dec 2014 11:05:28 +0100 Subject: Clean up MC controller usage of VTOL topics --- src/modules/mc_att_control/mc_att_control_main.cpp | 28 ++++++++++++---------- 1 file changed, 15 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 25b085b7b..82d2ff23a 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -126,8 +126,8 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ - orb_id_t _rates_sp_id; // pointer to correct rates setpoint uORB metadata structure - orb_id_t _actuators_id; // pointer to correct actuator controls0 uORB metadata structure + orb_id_t _rates_sp_id; /**< pointer to correct rates setpoint uORB metadata structure */ + orb_id_t _actuators_id; /**< pointer to correct actuator controls0 uORB metadata structure */ bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ @@ -283,8 +283,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _att_sp_pub(-1), _v_rates_sp_pub(-1), _actuators_0_pub(-1), - _rates_sp_id(ORB_ID(vehicle_rates_setpoint)), - _actuators_id(ORB_ID(actuator_controls_0)), + _rates_sp_id(0), + _actuators_id(0), _actuators_0_circuit_breaker_enabled(false), @@ -517,12 +517,14 @@ MulticopterAttitudeControl::vehicle_status_poll() if (vehicle_status_updated) { orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status); /* set correct uORB ID, depending on if vehicle is VTOL or not */ - if (_vehicle_status.is_vtol) { - _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_virtual_mc); - } else { - _rates_sp_id = ORB_ID(vehicle_rates_setpoint); - _actuators_id = ORB_ID(actuator_controls_0); + if (!_rates_sp_id) { + if (_vehicle_status.is_vtol) { + _rates_sp_id = ORB_ID(mc_virtual_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_virtual_mc); + } else { + _rates_sp_id = ORB_ID(vehicle_rates_setpoint); + _actuators_id = ORB_ID(actuator_controls_0); + } } } } @@ -837,7 +839,7 @@ MulticopterAttitudeControl::task_main() if (_v_rates_sp_pub > 0) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - } else { + } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } @@ -861,7 +863,7 @@ MulticopterAttitudeControl::task_main() if (_v_rates_sp_pub > 0) { orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - } else { + } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } @@ -889,7 +891,7 @@ MulticopterAttitudeControl::task_main() if (_actuators_0_pub > 0) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); - } else { + } else if (_actuators_id) { _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } -- cgit v1.2.3 From 32bfc6cdb8aef807259b1cd051c48d4ac90298e3 Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Tue, 30 Dec 2014 16:32:16 +0530 Subject: Minor re-addition --- src/drivers/px4flow/px4flow.cpp | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src') diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 036798543..9c9c1b0f8 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -364,6 +364,9 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } + case SENSORIOCGQUEUEDEPTH: + return _reports->size(); + case SENSORIOCSROTATION: _sensor_rotation = (enum Rotation)arg; return OK; -- cgit v1.2.3 From 84d724503f5fe687cad526cb46a142314bed02eb Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Tue, 30 Dec 2014 18:06:48 +0530 Subject: Remove invalid params --- src/modules/sensors/sensor_params.c | 21 ++------------------- 1 file changed, 2 insertions(+), 19 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index f5f6094d1..3c78b7b17 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -264,7 +264,8 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** * PX4Flow board rotation * - * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * Zero rotation is defined as Y on flow board pointing towards front of vehicle * Possible values are: * 0 = No rotation * 1 = Yaw 45° @@ -274,24 +275,6 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); * 5 = Yaw 225° * 6 = Yaw 270° * 7 = Yaw 315° - * 8 = Roll 180° - * 9 = Roll 180°, Yaw 45° - * 10 = Roll 180°, Yaw 90° - * 11 = Roll 180°, Yaw 135° - * 12 = Pitch 180° - * 13 = Roll 180°, Yaw 225° - * 14 = Roll 180°, Yaw 270° - * 15 = Roll 180°, Yaw 315° - * 16 = Roll 90° - * 17 = Roll 90°, Yaw 45° - * 18 = Roll 90°, Yaw 90° - * 19 = Roll 90°, Yaw 135° - * 20 = Roll 270° - * 21 = Roll 270°, Yaw 45° - * 22 = Roll 270°, Yaw 90° - * 23 = Roll 270°, Yaw 135° - * 24 = Pitch 90° - * 25 = Pitch 270° * * @group Sensor Calibration */ -- cgit v1.2.3 From 3e06a65516697c6bdf840d8104328ad09c0ccfbd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 24 Dec 2014 12:56:10 -0800 Subject: mpu6000: monitor some key registers for correct values this will catch both bad SPI bus comms and a sensor that has been reset causing incorrect configuration. --- src/drivers/mpu6000/mpu6000.cpp | 142 +++++++++++++++++++++++++++++++++++++--- 1 file changed, 132 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index c9c27892f..3d720cd94 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -231,8 +231,11 @@ private: perf_counter_t _gyro_reads; perf_counter_t _sample_perf; perf_counter_t _bad_transfers; + perf_counter_t _bad_registers; perf_counter_t _good_transfers; + uint8_t _register_wait; + math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; @@ -242,6 +245,14 @@ private: enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define MPU6000_NUM_CHECKED_REGISTERS 4 + static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -281,7 +292,7 @@ private: * @param The register to read. * @return The value that was read. */ - uint8_t read_reg(unsigned reg); + uint8_t read_reg(unsigned reg, uint32_t speed=MPU6000_LOW_BUS_SPEED); uint16_t read_reg16(unsigned reg); /** @@ -303,6 +314,14 @@ private: */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + /** + * Write a register in the MPU6000, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + /** * Set the MPU6000 measurement range. * @@ -347,11 +366,26 @@ private: */ void _set_sample_rate(uint16_t desired_sample_rate_hz); + /* + check that key registers still have the right value + */ + void check_registers(void); + /* do not allow to copy this class due to pointer data members */ MPU6000(const MPU6000&); MPU6000 operator=(const MPU6000&); }; +/* + list of registers that will be checked in check_registers(). Note + that MPUREG_PRODUCT_ID must be first in the list. + */ +const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, + MPUREG_GYRO_CONFIG, + MPUREG_ACCEL_CONFIG, + MPUREG_USER_CTRL }; + + /** * Helper class implementing the gyro driver node. */ @@ -407,14 +441,17 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_reads(perf_alloc(PC_COUNT, "mpu6000_gyro_read")), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")), _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), + _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), + _register_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { // disable debug() calls _debug_enabled = false; @@ -460,6 +497,7 @@ MPU6000::~MPU6000() perf_free(_accel_reads); perf_free(_gyro_reads); perf_free(_bad_transfers); + perf_free(_bad_registers); perf_free(_good_transfers); } @@ -590,7 +628,7 @@ void MPU6000::reset() up_udelay(1000); // Disable I2C bus (recommended on datasheet) - write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); irqrestore(state); usleep(1000); @@ -605,7 +643,7 @@ void MPU6000::reset() _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ); usleep(1000); // Gyro scale 2000 deg/s () - write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); + write_checked_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); usleep(1000); // correct gyro scale factors @@ -624,7 +662,7 @@ void MPU6000::reset() case MPU6000_REV_C5: // Accel scale 8g (4096 LSB/g) // Rev C has different scaling than rev D - write_reg(MPUREG_ACCEL_CONFIG, 1 << 3); + write_checked_reg(MPUREG_ACCEL_CONFIG, 1 << 3); break; case MPU6000ES_REV_D6: @@ -639,7 +677,7 @@ void MPU6000::reset() // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) - write_reg(MPUREG_ACCEL_CONFIG, 2 << 3); + write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); break; } @@ -684,6 +722,7 @@ MPU6000::probe() case MPU6000_REV_D9: case MPU6000_REV_D10: debug("ID 0x%02x", _product); + _checked_values[0] = _product; return OK; } @@ -734,7 +773,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) } else { filter = BITS_DLPF_CFG_2100HZ_NOLPF; } - write_reg(MPUREG_CONFIG, filter); + write_checked_reg(MPUREG_CONFIG, filter); } ssize_t @@ -1094,12 +1133,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) } uint8_t -MPU6000::read_reg(unsigned reg) +MPU6000::read_reg(unsigned reg, uint32_t speed) { uint8_t cmd[2] = { (uint8_t)(reg | DIR_READ), 0}; // general register transfer at low clock speed - set_frequency(MPU6000_LOW_BUS_SPEED); + set_frequency(speed); transfer(cmd, cmd, sizeof(cmd)); @@ -1144,6 +1183,17 @@ MPU6000::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) write_reg(reg, val); } +void +MPU6000::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; imeasure(); } +void +MPU6000::check_registers(void) +{ + /* + we read the register at full speed, even though it isn't + listed as a high speed register. The low speed requirement + for some registers seems to be a propgation delay + requirement for changing sensor configuration, which should + not apply to reading a single register. It is also a better + test of SPI bus health to read at the same speed as we read + the data registers. + */ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. We skip zero as that is the PRODUCT_ID, which + is not writeable + */ + if (_checked_next != 0) { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + } +#if 0 + if (_register_wait == 0) { + ::printf("MPU6000: %02x:%02x should be %02x\n", + (unsigned)_checked_registers[_checked_next], + (unsigned)v, + (unsigned)_checked_values[_checked_next]); + } +#endif + _register_wait = 20; + } else { + _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; + } +} + void MPU6000::measure() { @@ -1254,6 +1350,8 @@ MPU6000::measure() */ mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS; + check_registers(); + // sensor transfer at high clock speed set_frequency(MPU6000_HIGH_BUS_SPEED); @@ -1292,6 +1390,14 @@ MPU6000::measure() } perf_count(_good_transfers); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. We still increment + // _good_transfers, but don't return any data yet + _register_wait--; + return; + } /* @@ -1321,7 +1427,12 @@ MPU6000::measure() * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); - grb.error_count = arb.error_count = 0; // not reported + + // report the error count as the sum of the number of bad + // transfers and bad register reads. This allows the higher + // level code to decide if it should use this sensor based on + // whether it has had failures + grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers); /* * 1) Scale raw value to SI units using scaling from datasheet. @@ -1411,9 +1522,20 @@ MPU6000::print_info() perf_print_counter(_accel_reads); perf_print_counter(_gyro_reads); perf_print_counter(_bad_transfers); + perf_print_counter(_bad_registers); perf_print_counter(_good_transfers); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i Date: Mon, 29 Dec 2014 11:03:08 +1100 Subject: lsm303d: replace old register checking with new check_registers() method this uses the same method as is now used in the MPU6000 to check that the sensor retains its correct values Conflicts: src/drivers/lsm303d/lsm303d.cpp --- src/drivers/lsm303d/lsm303d.cpp | 347 +++++++++++++++------------------------- 1 file changed, 125 insertions(+), 222 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 01c89192a..dbd5c1f4c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -77,10 +77,6 @@ #endif static const int ERROR = -1; -// enable this to debug the buggy lsm303d sensor in very early -// prototype pixhawk boards -#define CHECK_EXTREMES 0 - /* SPI protocol address bits */ #define DIR_READ (1<<7) #define DIR_WRITE (0<<7) @@ -241,16 +237,6 @@ public: */ void print_registers(); - /** - * toggle logging - */ - void toggle_logging(); - - /** - * check for extreme accel values - */ - void check_extremes(const accel_report *arb); - protected: virtual int probe(); @@ -292,30 +278,25 @@ private: perf_counter_t _accel_sample_perf; perf_counter_t _mag_sample_perf; - perf_counter_t _reg1_resets; - perf_counter_t _reg7_resets; - perf_counter_t _extreme_values; perf_counter_t _accel_reschedules; + perf_counter_t _bad_registers; + + uint8_t _register_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; math::LowPassFilter2p _accel_filter_z; - // expceted values of reg1 and reg7 to catch in-flight - // brownouts of the sensor - uint8_t _reg1_expected; - uint8_t _reg7_expected; - - // accel logging - int _accel_log_fd; - bool _accel_logging_enabled; - uint64_t _last_extreme_us; - uint64_t _last_log_us; - uint64_t _last_log_sync_us; - uint64_t _last_log_reg_us; - uint64_t _last_log_alarm_us; enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define LSM303D_NUM_CHECKED_REGISTERS 6 + static const uint8_t _checked_registers[LSM303D_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -356,6 +337,11 @@ private: */ static void mag_measure_trampoline(void *arg); + /** + * check key registers for correct values + */ + void check_registers(void); + /** * Fetch accel measurements from the sensor and update the report ring. */ @@ -407,6 +393,14 @@ private: */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + /** + * Write a register in the LSM303D, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + /** * Set the LSM303D accel measurement range. * @@ -468,6 +462,17 @@ private: LSM303D operator=(const LSM303D&); }; +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG5, + ADDR_CTRL_REG6, + ADDR_CTRL_REG7 }; + /** * Helper class implementing the mag driver node. */ @@ -528,23 +533,14 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_read(0), _accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")), _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), - _reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")), - _reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")), - _extreme_values(perf_alloc(PC_COUNT, "lsm303d_extremes")), _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), + _bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")), + _register_wait(0), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), - _reg1_expected(0), - _reg7_expected(0), - _accel_log_fd(-1), - _accel_logging_enabled(false), - _last_extreme_us(0), - _last_log_us(0), - _last_log_sync_us(0), - _last_log_reg_us(0), - _last_log_alarm_us(0), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -586,9 +582,7 @@ LSM303D::~LSM303D() /* delete the perf counter */ perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); - perf_free(_reg1_resets); - perf_free(_reg7_resets); - perf_free(_extreme_values); + perf_free(_bad_registers); perf_free(_accel_reschedules); } @@ -703,13 +697,12 @@ LSM303D::reset() disable_i2c(); /* enable accel*/ - _reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A; - write_reg(ADDR_CTRL_REG1, _reg1_expected); + write_checked_reg(ADDR_CTRL_REG1, + REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); /* enable mag */ - _reg7_expected = REG7_CONT_MODE_M; - write_reg(ADDR_CTRL_REG7, _reg7_expected); - write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); + write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); + write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 @@ -739,126 +732,12 @@ LSM303D::probe() /* verify that the device is attached and functioning */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - if (success) + if (success) { + _checked_values[0] = WHO_I_AM; return OK; - - return -EIO; -} - -#define ACCEL_LOGFILE "/fs/microsd/lsm303d.log" - -/** - check for extreme accelerometer values and log to a file on the SD card - */ -void -LSM303D::check_extremes(const accel_report *arb) -{ - const float extreme_threshold = 30; - static bool boot_ok = false; - bool is_extreme = (fabsf(arb->x) > extreme_threshold && - fabsf(arb->y) > extreme_threshold && - fabsf(arb->z) > extreme_threshold); - if (is_extreme) { - perf_count(_extreme_values); - // force accel logging on if we see extreme values - _accel_logging_enabled = true; - } else { - boot_ok = true; - } - - if (! _accel_logging_enabled) { - // logging has been disabled by user, close - if (_accel_log_fd != -1) { - ::close(_accel_log_fd); - _accel_log_fd = -1; - } - return; - } - if (_accel_log_fd == -1) { - // keep last 10 logs - ::unlink(ACCEL_LOGFILE ".9"); - for (uint8_t i=8; i>0; i--) { - uint8_t len = strlen(ACCEL_LOGFILE)+3; - char log1[len], log2[len]; - snprintf(log1, sizeof(log1), "%s.%u", ACCEL_LOGFILE, (unsigned)i); - snprintf(log2, sizeof(log2), "%s.%u", ACCEL_LOGFILE, (unsigned)(i+1)); - ::rename(log1, log2); - } - ::rename(ACCEL_LOGFILE, ACCEL_LOGFILE ".1"); - - // open the new logfile - _accel_log_fd = ::open(ACCEL_LOGFILE, O_WRONLY|O_CREAT|O_TRUNC, 0666); - if (_accel_log_fd == -1) { - return; - } - } - - uint64_t now = hrt_absolute_time(); - // log accels at 1Hz - if (_last_log_us == 0 || - now - _last_log_us > 1000*1000) { - _last_log_us = now; - ::dprintf(_accel_log_fd, "ARB %llu %.3f %.3f %.3f %d %d %d boot_ok=%u\r\n", - (unsigned long long)arb->timestamp, - (double)arb->x, (double)arb->y, (double)arb->z, - (int)arb->x_raw, - (int)arb->y_raw, - (int)arb->z_raw, - (unsigned)boot_ok); - } - - const uint8_t reglist[] = { ADDR_WHO_AM_I, 0x02, 0x15, ADDR_STATUS_A, ADDR_STATUS_M, ADDR_CTRL_REG0, ADDR_CTRL_REG1, - ADDR_CTRL_REG2, ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, - ADDR_CTRL_REG7, ADDR_OUT_TEMP_L, ADDR_OUT_TEMP_H, ADDR_INT_CTRL_M, ADDR_INT_SRC_M, - ADDR_REFERENCE_X, ADDR_REFERENCE_Y, ADDR_REFERENCE_Z, ADDR_OUT_X_L_A, ADDR_OUT_X_H_A, - ADDR_OUT_Y_L_A, ADDR_OUT_Y_H_A, ADDR_OUT_Z_L_A, ADDR_OUT_Z_H_A, ADDR_FIFO_CTRL, - ADDR_FIFO_SRC, ADDR_IG_CFG1, ADDR_IG_SRC1, ADDR_IG_THS1, ADDR_IG_DUR1, ADDR_IG_CFG2, - ADDR_IG_SRC2, ADDR_IG_THS2, ADDR_IG_DUR2, ADDR_CLICK_CFG, ADDR_CLICK_SRC, - ADDR_CLICK_THS, ADDR_TIME_LIMIT, ADDR_TIME_LATENCY, ADDR_TIME_WINDOW, - ADDR_ACT_THS, ADDR_ACT_DUR, - ADDR_OUT_X_L_M, ADDR_OUT_X_H_M, - ADDR_OUT_Y_L_M, ADDR_OUT_Y_H_M, ADDR_OUT_Z_L_M, ADDR_OUT_Z_H_M, 0x02, 0x15, ADDR_WHO_AM_I}; - uint8_t regval[sizeof(reglist)]; - for (uint8_t i=0; i 250*1000)) || - (now - _last_log_reg_us > 10*1000*1000)) { - _last_log_reg_us = now; - ::dprintf(_accel_log_fd, "XREG %llu", (unsigned long long)hrt_absolute_time()); - for (uint8_t i=0; i 10*1000*1000) { - _last_log_sync_us = now; - ::fsync(_accel_log_fd); - } - - // play alarm every 10s if we have had an extreme value - if (perf_event_count(_extreme_values) != 0 && - (now - _last_log_alarm_us > 10*1000*1000)) { - _last_log_alarm_us = now; - int tfd = ::open(TONEALARM_DEVICE_PATH, 0); - if (tfd != -1) { - uint8_t tone = 3; - if (!is_extreme) { - tone = 3; - } else if (boot_ok) { - tone = 4; - } else { - tone = 5; - } - ::ioctl(tfd, TONE_SET_ALARM, tone); - ::close(tfd); - } - } + return -EIO; } ssize_t @@ -879,9 +758,6 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen) */ while (count--) { if (_accel_reports->get(arb)) { -#if CHECK_EXTREMES - check_extremes(arb); -#endif ret += sizeof(*arb); arb++; } @@ -1262,6 +1138,17 @@ LSM303D::write_reg(unsigned reg, uint8_t value) transfer(cmd, nullptr, sizeof(cmd)); } +void +LSM303D::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; imag_measure(); } +void +LSM303D::check_registers(void) +{ + uint8_t v; + if ((v=read_reg(_checked_registers[_checked_next])) != _checked_values[_checked_next]) { + /* + if we get the wrong value then we know the SPI bus + or sensor is very sick. We set _register_wait to 20 + and wait until we have seen 20 good values in a row + before we consider the sensor to be OK again. + */ + perf_count(_bad_registers); + + /* + try to fix the bad register value. We only try to + fix one per loop to prevent a bad sensor hogging the + bus. We skip zero as that is the WHO_AM_I, which + is not writeable + */ + if (_checked_next != 0) { + write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + } +#if 1 + if (_register_wait == 0) { + ::printf("LSM303D: %02x:%02x should be %02x\n", + (unsigned)_checked_registers[_checked_next], + (unsigned)v, + (unsigned)_checked_values[_checked_next]); + } +#endif + _register_wait = 20; + } + _checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS; +} + void LSM303D::measure() { @@ -1522,16 +1443,13 @@ LSM303D::measure() // read a value and then miss the next value. // Note that DRDY is not available when the lsm303d is // connected on the external bus +#ifdef GPIO_EXTI_ACCEL_DRDY if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { perf_count(_accel_reschedules); hrt_call_delay(&_accel_call, 100); return; } - if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) { - perf_count(_reg1_resets); - reset(); - return; - } +#endif /* status register and data as read back from the device */ @@ -1550,6 +1468,16 @@ LSM303D::measure() /* start the performance counter */ perf_begin(_accel_sample_perf); + check_registers(); + + if (_register_wait != 0) { + // we are waiting for some good transfers before using + // the sensor again. + _register_wait--; + perf_end(_accel_sample_perf); + return; + } + /* fetch data from the sensor */ memset(&raw_accel_report, 0, sizeof(raw_accel_report)); raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; @@ -1572,7 +1500,12 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); - accel_report.error_count = 0; // not reported + + // report the error count as the sum of the number of bad bad + // register reads. This allows the higher level code to decide + // if it should use this sensor based on whether it has had + // failures + accel_report.error_count = perf_event_count(_bad_registers); accel_report.x_raw = raw_accel_report.x; accel_report.y_raw = raw_accel_report.y; @@ -1611,12 +1544,6 @@ LSM303D::measure() void LSM303D::mag_measure() { - if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) { - perf_count(_reg7_resets); - reset(); - return; - } - /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -1691,8 +1618,19 @@ LSM303D::print_info() printf("accel reads: %u\n", _accel_read); printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); + perf_print_counter(_bad_registers); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; itoggle_logging(); - - exit(0); -} - void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'logging'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2126,11 +2035,5 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) lsm303d::regdump(); - /* - * dump device registers - */ - if (!strcmp(verb, "logging")) - lsm303d::logging(); - - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'logging' or 'regdump'"); + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', or 'regdump'"); } -- cgit v1.2.3 From ae3a92569d3b10a5f8c125ff54910a686594ea68 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 14:00:09 +1100 Subject: mpu6000: try resetting the mpu6000 up to 5 times this mirrors the ardupilot driver. We have seen situations where the mpu6000 on the Pixhawk comes up in SLEEP mode, despite a reset --- src/drivers/mpu6000/mpu6000.cpp | 62 ++++++++++++++++++++++++++--------------- 1 file changed, 39 insertions(+), 23 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 3d720cd94..eb9666dc5 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -233,6 +233,7 @@ private: perf_counter_t _bad_transfers; perf_counter_t _bad_registers; perf_counter_t _good_transfers; + perf_counter_t _reset_retries; uint8_t _register_wait; @@ -248,7 +249,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define MPU6000_NUM_CHECKED_REGISTERS 4 +#define MPU6000_NUM_CHECKED_REGISTERS 5 static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -268,7 +269,7 @@ private: * * Resets the chip and measurements ranges, but not scale and offset. */ - void reset(); + int reset(); /** * Static trampoline from the hrt_call context; because we don't have a @@ -383,7 +384,8 @@ private: const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, - MPUREG_USER_CTRL }; + MPUREG_USER_CTRL, + MPUREG_PWR_MGMT_1}; /** @@ -443,6 +445,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _bad_transfers(perf_alloc(PC_COUNT, "mpu6000_bad_transfers")), _bad_registers(perf_alloc(PC_COUNT, "mpu6000_bad_registers")), _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), + _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -524,7 +527,8 @@ MPU6000::init() if (_gyro_reports == nullptr) goto out; - reset(); + if (reset() != OK) + goto out; /* Initialize offsets and scales */ _accel_scale.x_offset = 0; @@ -609,27 +613,39 @@ out: return ret; } -void MPU6000::reset() +int MPU6000::reset() { // if the mpu6000 is initialised after the l3gd20 and lsm303d // then if we don't do an irqsave/irqrestore here the mpu6000 // frequenctly comes up in a bad state where all transfers // come as zero - irqstate_t state; - state = irqsave(); - - write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); - up_udelay(10000); - - // Wake up device and select GyroZ clock. Note that the - // MPU6000 starts up in sleep mode, and it can take some time - // for it to come out of sleep - write_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); - up_udelay(1000); + uint8_t tries = 5; + while (--tries != 0) { + irqstate_t state; + state = irqsave(); + + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + up_udelay(10000); + + // Wake up device and select GyroZ clock. Note that the + // MPU6000 starts up in sleep mode, and it can take some time + // for it to come out of sleep + write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); + up_udelay(1000); + + // Disable I2C bus (recommended on datasheet) + write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); + irqrestore(state); - // Disable I2C bus (recommended on datasheet) - write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); - irqrestore(state); + if (read_reg(MPUREG_PWR_MGMT_1) == MPU_CLK_SEL_PLLGYROZ) { + break; + } + perf_count(_reset_retries); + usleep(2000); + } + if (read_reg(MPUREG_PWR_MGMT_1) != MPU_CLK_SEL_PLLGYROZ) { + return -EIO; + } usleep(1000); @@ -698,6 +714,7 @@ void MPU6000::reset() // write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ); usleep(1000); + return OK; } int @@ -913,8 +930,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case SENSORIOCRESET: - reset(); - return OK; + return reset(); case SENSORIOCSPOLLRATE: { switch (arg) { @@ -1306,9 +1322,8 @@ MPU6000::check_registers(void) } #endif _register_wait = 20; - } else { - _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; } + _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; } void @@ -1524,6 +1539,7 @@ MPU6000::print_info() perf_print_counter(_bad_transfers); perf_print_counter(_bad_registers); perf_print_counter(_good_transfers); + perf_print_counter(_reset_retries); _accel_reports->print_info("accel queue"); _gyro_reports->print_info("gyro queue"); ::printf("checked_next: %u\n", _checked_next); -- cgit v1.2.3 From ca47952281cfe66732b08d3878eb6c8b1613abeb Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 14:18:10 +1100 Subject: l3gd20: added register checking this checks at runtime that key registers have correct values --- src/drivers/l3gd20/l3gd20.cpp | 146 +++++++++++++++++++++++++++++++++++------- 1 file changed, 122 insertions(+), 24 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 82fa5cc6e..de9d9140f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -225,6 +225,9 @@ private: perf_counter_t _sample_perf; perf_counter_t _reschedules; perf_counter_t _errors; + perf_counter_t _bad_registers; + + uint8_t _register_wait; math::LowPassFilter2p _gyro_filter_x; math::LowPassFilter2p _gyro_filter_y; @@ -235,6 +238,14 @@ private: enum Rotation _rotation; + // this is used to support runtime checking of key + // configuration registers to detect SPI bus errors and sensor + // reset +#define L3GD20_NUM_CHECKED_REGISTERS 7 + static const uint8_t _checked_registers[L3GD20_NUM_CHECKED_REGISTERS]; + uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS]; + uint8_t _checked_next; + /** * Start automatic measurement. */ @@ -266,6 +277,11 @@ private: */ static void measure_trampoline(void *arg); + /** + * check key registers for correct values + */ + void check_registers(void); + /** * Fetch measurements from the sensor and update the report ring. */ @@ -298,6 +314,14 @@ private: */ void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); + /** + * Write a register in the L3GD20, updating _checked_values + * + * @param reg The register to write. + * @param value The new value to write. + */ + void write_checked_reg(unsigned reg, uint8_t value); + /** * Set the L3GD20 measurement range. * @@ -338,6 +362,18 @@ private: L3GD20 operator=(const L3GD20&); }; +/* + list of registers that will be checked in check_registers(). Note + that ADDR_WHO_AM_I must be first in the list. + */ +const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, + ADDR_CTRL_REG1, + ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, + ADDR_CTRL_REG5, + ADDR_FIFO_CTRL_REG }; + L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), _call{}, @@ -355,11 +391,14 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")), _reschedules(perf_alloc(PC_COUNT, "l3gd20_reschedules")), _errors(perf_alloc(PC_COUNT, "l3gd20_errors")), + _bad_registers(perf_alloc(PC_COUNT, "l3gd20_bad_registers")), + _register_wait(0), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _is_l3g4200d(false), - _rotation(rotation) + _rotation(rotation), + _checked_next(0) { // enable debug() calls _debug_enabled = true; @@ -389,6 +428,7 @@ L3GD20::~L3GD20() perf_free(_sample_perf); perf_free(_reschedules); perf_free(_errors); + perf_free(_bad_registers); } int @@ -448,29 +488,27 @@ L3GD20::probe() (void)read_reg(ADDR_WHO_AM_I); bool success = false; + uint8_t v = 0; - /* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) { - + /* verify that the device is attached and functioning, accept + * L3GD20, L3GD20H and L3G4200D */ + if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM) { _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; - } - - - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) { + } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_H) { _orientation = SENSOR_BOARD_ROTATION_180_DEG; success = true; - } - - /* Detect the L3G4200D used on AeroCore */ - if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_L3G4200D) { + } else if ((v=read_reg(ADDR_WHO_AM_I)) == WHO_I_AM_L3G4200D) { + /* Detect the L3G4200D used on AeroCore */ _is_l3g4200d = true; _orientation = SENSOR_BOARD_ROTATION_DEFAULT; success = true; } - if (success) + if (success) { + _checked_values[0] = v; return OK; + } return -EIO; } @@ -672,6 +710,18 @@ L3GD20::write_reg(unsigned reg, uint8_t value) transfer(cmd, nullptr, sizeof(cmd)); } +void +L3GD20::write_checked_reg(unsigned reg, uint8_t value) +{ + write_reg(reg, value); + for (uint8_t i=0; iprint_info("report queue"); + ::printf("checked_next: %u\n", _checked_next); + for (uint8_t i=0; i Date: Mon, 29 Dec 2014 16:15:41 +1100 Subject: mpu6000: added factory self-test function this follows the factory calibration self-test method in the datasheet to see if the sensor still has the same calibration it had in the factory --- src/drivers/mpu6000/mpu6000.cpp | 226 ++++++++++++++++++++++++++++++++++++---- 1 file changed, 208 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index eb9666dc5..2559c73e0 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -113,6 +113,10 @@ #define MPUREG_FIFO_COUNTL 0x73 #define MPUREG_FIFO_R_W 0x74 #define MPUREG_PRODUCT_ID 0x0C +#define MPUREG_TRIM1 0x0D +#define MPUREG_TRIM2 0x0E +#define MPUREG_TRIM3 0x0F +#define MPUREG_TRIM4 0x10 // Configuration bits MPU 3000 and MPU 6000 (not revised)? #define BIT_SLEEP 0x40 @@ -121,6 +125,9 @@ #define MPU_CLK_SEL_PLLGYROX 0x01 #define MPU_CLK_SEL_PLLGYROZ 0x03 #define MPU_EXT_SYNC_GYROX 0x02 +#define BITS_GYRO_ST_X 0x80 +#define BITS_GYRO_ST_Y 0x40 +#define BITS_GYRO_ST_Z 0x20 #define BITS_FS_250DPS 0x00 #define BITS_FS_500DPS 0x08 #define BITS_FS_1000DPS 0x10 @@ -196,6 +203,13 @@ public: void print_registers(); + /** + * Test behaviour against factory offsets + * + * @return 0 on success, 1 on failure + */ + int factory_self_test(); + protected: virtual int probe(); @@ -254,6 +268,10 @@ private: uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; + // use this to avoid processing measurements when in factory + // self test + volatile bool _in_factory_test; + /** * Start automatic measurement. */ @@ -375,6 +393,24 @@ private: /* do not allow to copy this class due to pointer data members */ MPU6000(const MPU6000&); MPU6000 operator=(const MPU6000&); + +#pragma pack(push, 1) + /** + * Report conversation within the MPU6000, including command byte and + * interrupt status. + */ + struct MPUReport { + uint8_t cmd; + uint8_t status; + uint8_t accel_x[2]; + uint8_t accel_y[2]; + uint8_t accel_z[2]; + uint8_t temp[2]; + uint8_t gyro_x[2]; + uint8_t gyro_y[2]; + uint8_t gyro_z[2]; + }; +#pragma pack(pop) }; /* @@ -454,7 +490,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), - _checked_next(0) + _checked_next(0), + _in_factory_test(false) { // disable debug() calls _debug_enabled = false; @@ -889,6 +926,152 @@ MPU6000::gyro_self_test() return 0; } + +/* + perform a self-test comparison to factory trim values. This takes + about 200ms and will return OK if the current values are within 14% + of the expected values + */ +int +MPU6000::factory_self_test() +{ + _in_factory_test = true; + uint8_t saved_gyro_config = read_reg(MPUREG_GYRO_CONFIG); + uint8_t saved_accel_config = read_reg(MPUREG_ACCEL_CONFIG); + const uint16_t repeats = 100; + int ret = OK; + + // gyro self test has to be done at 250DPS + write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); + + struct MPUReport mpu_report; + float accel_baseline[3]; + float gyro_baseline[3]; + float accel[3]; + float gyro[3]; + float accel_ftrim[3]; + float gyro_ftrim[3]; + + // get baseline values without self-test enabled + set_frequency(MPU6000_HIGH_BUS_SPEED); + + memset(accel_baseline, 0, sizeof(accel_baseline)); + memset(gyro_baseline, 0, sizeof(gyro_baseline)); + memset(accel, 0, sizeof(accel)); + memset(gyro, 0, sizeof(gyro)); + + for (uint8_t i=0; i>3)&0x1C) | ((trims[3]>>4)&0x03); + atrim[1] = ((trims[1]>>3)&0x1C) | ((trims[3]>>2)&0x03); + atrim[2] = ((trims[2]>>3)&0x1C) | ((trims[3]>>0)&0x03); + gtrim[0] = trims[0] & 0x1F; + gtrim[1] = trims[1] & 0x1F; + gtrim[2] = trims[2] & 0x1F; + + // convert factory trims to right units + for (uint8_t i=0; i<3; i++) { + accel_ftrim[i] = 4096 * 0.34f * powf(0.92f/0.34f, (atrim[i]-1)/30.0f); + gyro_ftrim[i] = 25 * 131.0f * powf(1.046f, gtrim[i]-1); + } + // Y gyro trim is negative + gyro_ftrim[1] *= -1; + + for (uint8_t i=0; i<3; i++) { + float diff = accel[i]-accel_baseline[i]; + float err = 100*(diff - accel_ftrim[i]) / accel_ftrim[i]; + ::printf("ACCEL[%u] baseline=%d accel=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*accel_baseline[i]), + (int)(1000*accel[i]), + (int)(1000*diff), + (int)(1000*accel_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + for (uint8_t i=0; i<3; i++) { + float diff = gyro[i]-gyro_baseline[i]; + float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; + ::printf("GYRO[%u] baseline=%d gyro=%d diff=%d ftrim=%d err=%d\n", + (unsigned)i, + (int)(1000*gyro_baseline[i]), + (int)(1000*gyro[i]), + (int)(1000*(gyro[i]-gyro_baseline[i])), + (int)(1000*gyro_ftrim[i]), + (int)err); + if (fabsf(err) > 14) { + ::printf("FAIL\n"); + ret = -EIO; + } + } + + write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); + write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); + + _in_factory_test = false; + + return ret; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -1329,24 +1512,12 @@ MPU6000::check_registers(void) void MPU6000::measure() { -#pragma pack(push, 1) - /** - * Report conversation within the MPU6000, including command byte and - * interrupt status. - */ - struct MPUReport { - uint8_t cmd; - uint8_t status; - uint8_t accel_x[2]; - uint8_t accel_y[2]; - uint8_t accel_z[2]; - uint8_t temp[2]; - uint8_t gyro_x[2]; - uint8_t gyro_y[2]; - uint8_t gyro_z[2]; - } mpu_report; -#pragma pack(pop) + if (_in_factory_test) { + // don't publish any data while in factory test mode + return; + } + struct MPUReport mpu_report; struct Report { int16_t accel_x; int16_t accel_y; @@ -1635,6 +1806,7 @@ void test(bool); void reset(bool); void info(bool); void regdump(bool); +void factorytest(bool); void usage(); /** @@ -1826,6 +1998,21 @@ regdump(bool external_bus) exit(0); } +/** + * Dump the register information + */ +void +factorytest(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + (*g_dev_ptr)->factory_self_test(); + + exit(0); +} + void usage() { @@ -1892,5 +2079,8 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) mpu6000::regdump(external_bus); + if (!strcmp(verb, "factorytest")) + mpu6000::factorytest(external_bus); + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } -- cgit v1.2.3 From b1574666084b64b43b241a56ac01596035ebccdf Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 20:44:05 +1100 Subject: mpu6000: monitor some more registers --- src/drivers/mpu6000/mpu6000.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 2559c73e0..91eb40b96 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -263,7 +263,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define MPU6000_NUM_CHECKED_REGISTERS 5 +#define MPU6000_NUM_CHECKED_REGISTERS 9 static const uint8_t _checked_registers[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[MPU6000_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -418,10 +418,14 @@ private: that MPUREG_PRODUCT_ID must be first in the list. */ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, + MPUREG_PWR_MGMT_1, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, MPUREG_USER_CTRL, - MPUREG_PWR_MGMT_1}; + MPUREG_INT_ENABLE, + MPUREG_INT_PIN_CFG, + MPUREG_SMPLRT_DIV }; /** -- cgit v1.2.3 From 333039d3db7b1f3b4bfb06c27874bf3372b7d750 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 20:44:46 +1100 Subject: mpu6000: added "mpu6000 testerror" command used to generate a error case for reset testing --- src/drivers/mpu6000/mpu6000.cpp | 47 +++++++++++++++++++++++++++++++++++++++-- 1 file changed, 45 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 91eb40b96..8dfac9859 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -210,6 +210,9 @@ public: */ int factory_self_test(); + // deliberately cause a sensor error + void test_error(); + protected: virtual int probe(); @@ -1072,10 +1075,31 @@ MPU6000::factory_self_test() write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); _in_factory_test = false; + if (ret == OK) { + ::printf("PASSED\n"); + } return ret; } + +/* + deliberately trigger an error in the sensor to trigger recovery + */ +void +MPU6000::test_error() +{ + _in_factory_test = true; + // deliberately trigger an error. This was noticed during + // development as a handy way to test the reset logic + uint8_t data[16]; + memset(data, 0, sizeof(data)); + transfer(data, data, sizeof(data)); + ::printf("error triggered\n"); + print_registers(); + _in_factory_test = false; +} + ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { @@ -1810,6 +1834,7 @@ void test(bool); void reset(bool); void info(bool); void regdump(bool); +void testerror(bool); void factorytest(bool); void usage(); @@ -2002,6 +2027,21 @@ regdump(bool external_bus) exit(0); } +/** + * deliberately produce an error to test recovery + */ +void +testerror(bool external_bus) +{ + MPU6000 **g_dev_ptr = external_bus?&g_dev_ext:&g_dev_int; + if (*g_dev_ptr == nullptr) + errx(1, "driver not running"); + + (*g_dev_ptr)->test_error(); + + exit(0); +} + /** * Dump the register information */ @@ -2020,7 +2060,7 @@ factorytest(bool external_bus) void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump', 'factorytest', 'testerror'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2086,5 +2126,8 @@ mpu6000_main(int argc, char *argv[]) if (!strcmp(verb, "factorytest")) mpu6000::factorytest(external_bus); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + if (!strcmp(verb, "testerror")) + mpu6000::testerror(external_bus); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'regdump', 'factorytest' or 'testerror'"); } -- cgit v1.2.3 From e537de20e3f06525ce9900109a0574142d3e6833 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 29 Dec 2014 20:45:31 +1100 Subject: mpu6000: wait for 10ms after a full reset this prevents the mpu6000 getting in a really weird state! --- src/drivers/mpu6000/mpu6000.cpp | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 8dfac9859..558006d0d 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -253,6 +253,7 @@ private: perf_counter_t _reset_retries; uint8_t _register_wait; + uint64_t _reset_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -490,6 +491,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _good_transfers(perf_alloc(PC_COUNT, "mpu6000_good_transfers")), _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), + _reset_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -749,9 +751,9 @@ int MPU6000::reset() usleep(1000); // INT CFG => Interrupt on Data Ready - write_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready + write_checked_reg(MPUREG_INT_ENABLE, BIT_RAW_RDY_EN); // INT: Raw data ready usleep(1000); - write_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read + write_checked_reg(MPUREG_INT_PIN_CFG, BIT_INT_ANYRD_2CLEAR); // INT: Clear on any read usleep(1000); // Oscillator set @@ -800,7 +802,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) uint8_t div = 1000 / desired_sample_rate_hz; if(div>200) div=200; if(div<1) div=1; - write_reg(MPUREG_SMPLRT_DIV, div-1); + write_checked_reg(MPUREG_SMPLRT_DIV, div-1); _sample_rate = 1000 / div; } @@ -937,7 +939,7 @@ MPU6000::gyro_self_test() /* perform a self-test comparison to factory trim values. This takes about 200ms and will return OK if the current values are within 14% - of the expected values + of the expected values (as per datasheet) */ int MPU6000::factory_self_test() @@ -1518,10 +1520,20 @@ MPU6000::check_registers(void) /* try to fix the bad register value. We only try to fix one per loop to prevent a bad sensor hogging the - bus. We skip zero as that is the PRODUCT_ID, which - is not writeable + bus. */ - if (_checked_next != 0) { + if (_checked_next == 0) { + // if the product_id is wrong then reset the + // sensor completely + write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); + // after doing a reset we need to wait a long + // time before we do any other register writes + // or we will end up with the mpu6000 in a + // bizarre state where it has all correct + // register values but large offsets on the + // accel axes + _reset_wait = hrt_absolute_time() + 10000; + } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); } #if 0 @@ -1545,6 +1557,11 @@ MPU6000::measure() return; } + if (hrt_absolute_time() < _reset_wait) { + // we're waiting for a reset to complete + return; + } + struct MPUReport mpu_report; struct Report { int16_t accel_x; -- cgit v1.2.3 From 4a81384b2d001e5a777a05b6f4c995ea35da2451 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Dec 2014 08:41:00 +1100 Subject: mpu6000: make register fixup much closer to a reset() this may help automatic reset on the faulty boards --- src/drivers/mpu6000/mpu6000.cpp | 42 ++++++++++++++++++++++++++++++++++++----- 1 file changed, 37 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 558006d0d..e7e88bb82 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -254,6 +254,7 @@ private: uint8_t _register_wait; uint64_t _reset_wait; + uint64_t _printf_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -423,13 +424,14 @@ private: */ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPUREG_PRODUCT_ID, MPUREG_PWR_MGMT_1, - MPUREG_CONFIG, + MPUREG_USER_CTRL, + MPUREG_SMPLRT_DIV, + MPUREG_CONFIG, MPUREG_GYRO_CONFIG, MPUREG_ACCEL_CONFIG, - MPUREG_USER_CTRL, MPUREG_INT_ENABLE, - MPUREG_INT_PIN_CFG, - MPUREG_SMPLRT_DIV }; + MPUREG_INT_PIN_CFG }; + /** @@ -492,6 +494,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), _reset_wait(0), + _printf_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -1522,7 +1525,7 @@ MPU6000::check_registers(void) fix one per loop to prevent a bad sensor hogging the bus. */ - if (_checked_next == 0) { + if (_register_wait == 0 || _checked_next == 0) { // if the product_id is wrong then reset the // sensor completely write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); @@ -1533,8 +1536,37 @@ MPU6000::check_registers(void) // register values but large offsets on the // accel axes _reset_wait = hrt_absolute_time() + 10000; + _checked_next = 0; } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); + _reset_wait = hrt_absolute_time() + 1000; + if (_checked_next == 1 && hrt_absolute_time() > _printf_wait) { + /* + rather bizarrely this printf() seems + to be critical for successfully + resetting the sensor. If you take + this printf out then all the + registers do get successfully reset, + but the sensor ends up with a large + fixed offset on the + accelerometers. Try a up_udelay() + of various sizes instead of a + printf() doesn't work. That makes no + sense at all, and investigating why + this is would be worthwhile. + + The rate limit on the printf to 5Hz + prevents this from causing enough + delays in interrupt context to cause + the vehicle to not be able to fly + correctly. + */ + _printf_wait = hrt_absolute_time() + 200*1000UL; + ::printf("Setting %u %02x to %02x\n", + (unsigned)_checked_next, + (unsigned)_checked_registers[_checked_next], + (unsigned)_checked_values[_checked_next]); + } } #if 0 if (_register_wait == 0) { -- cgit v1.2.3 From b455d647b264ebe3390bbc1a3077de3117c1a1dd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Dec 2014 13:14:29 +1100 Subject: l3gd20: added "l3gd20 regdump" command --- src/drivers/l3gd20/l3gd20.cpp | 43 +++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 41 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index de9d9140f..63974a7df 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -200,6 +200,9 @@ public: */ void print_info(); + // print register dump + void print_registers(); + protected: virtual int probe(); @@ -1074,6 +1077,20 @@ L3GD20::print_info() } } +void +L3GD20::print_registers() +{ + printf("L3GD20 registers\n"); + for (uint8_t reg=0; reg<=0x40; reg++) { + uint8_t v = read_reg(reg); + printf("%02x:%02x ",(unsigned)reg, (unsigned)v); + if ((reg+1) % 16 == 0) { + printf("\n"); + } + } + printf("\n"); +} + int L3GD20::self_test() { @@ -1109,6 +1126,7 @@ void start(bool external_bus, enum Rotation rotation); void test(); void reset(); void info(); +void regdump(); /** * Start the driver. @@ -1247,10 +1265,25 @@ info() exit(0); } +/** + * Dump the register information + */ +void +regdump(void) +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("regdump @ %p\n", g_dev); + g_dev->print_registers(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset'"); + warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1307,5 +1340,11 @@ l3gd20_main(int argc, char *argv[]) if (!strcmp(verb, "info")) l3gd20::info(); - errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); + /* + * Print register information. + */ + if (!strcmp(verb, "regdump")) + l3gd20::regdump(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); } -- cgit v1.2.3 From a8cea3a4da89457b3a8fc0fbb424a6af055453f0 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 30 Dec 2014 16:07:47 +1100 Subject: l3gd20: use the I2C disable bit on l3gd20H this seems to prevent a mpu6000 reset from causing an issue on the l3gd20H --- src/drivers/l3gd20/l3gd20.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 63974a7df..3a0c05c9e 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -142,6 +142,7 @@ static const int ERROR = -1; #define ADDR_INT1_TSH_ZH 0x36 #define ADDR_INT1_TSH_ZL 0x37 #define ADDR_INT1_DURATION 0x38 +#define ADDR_LOW_ODR 0x39 /* Internal configuration values */ @@ -244,7 +245,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define L3GD20_NUM_CHECKED_REGISTERS 7 +#define L3GD20_NUM_CHECKED_REGISTERS 8 static const uint8_t _checked_registers[L3GD20_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[L3GD20_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -375,7 +376,8 @@ const uint8_t L3GD20::_checked_registers[L3GD20_NUM_CHECKED_REGISTERS] = { ADDR_ ADDR_CTRL_REG3, ADDR_CTRL_REG4, ADDR_CTRL_REG5, - ADDR_FIFO_CTRL_REG }; + ADDR_FIFO_CTRL_REG, + ADDR_LOW_ODR }; L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) : SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */), @@ -844,6 +846,11 @@ L3GD20::disable_i2c(void) uint8_t a = read_reg(0x05); write_reg(0x05, (0x20 | a)); if (read_reg(0x05) == (a | 0x20)) { + // this sets the I2C_DIS bit on the + // L3GD20H. The l3gd20 datasheet doesn't + // mention this register, but it does seem to + // accept it. + write_checked_reg(ADDR_LOW_ODR, 0x08); return; } } -- 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') 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 72eafad5104ca0919f822fe44391c69f1ca80e8c Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 31 Dec 2014 16:25:15 +0100 Subject: introduced vtol_fw_permanent stabilization: allows vtol to be attitude-stabilized in manual mode --- src/modules/commander/commander.cpp | 6 +++--- src/modules/uORB/topics/vehicle_status.h | 2 ++ src/modules/uORB/topics/vtol_vehicle_status.h | 1 + src/modules/vtol_att_control/vtol_att_control_main.cpp | 12 ++++++++++++ src/modules/vtol_att_control/vtol_att_control_params.c | 12 ++++++++++++ 5 files changed, 30 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index dc0594bf2..086f291f6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1267,7 +1267,7 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { /* vtol status changed */ orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status); - + status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab; /* Make sure that this is only adjusted if vehicle realy is of type vtol*/ if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) { status.is_rotary_wing = vtol_status.vtol_in_rw_mode; @@ -2246,8 +2246,8 @@ set_control_mode() case NAVIGATION_STATE_MANUAL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = status.is_rotary_wing; - control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_rates_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); + control_mode.flag_control_attitude_enabled = (status.is_rotary_wing || status.vtol_fw_permanent_stab); control_mode.flag_control_altitude_enabled = false; control_mode.flag_control_climb_rate_enabled = false; control_mode.flag_control_position_enabled = false; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b4d5c7b88..b56e81e04 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -189,6 +189,8 @@ struct vehicle_status_s { this is only true while flying as a multicopter */ bool is_vtol; /**< True if the system is VTOL capable */ + bool vtol_fw_permanent_stab; /**< True if vtol should stabilize attitude for fw in manual mode */ + bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; diff --git a/src/modules/uORB/topics/vtol_vehicle_status.h b/src/modules/uORB/topics/vtol_vehicle_status.h index 24ecca9fa..7b4e22bc8 100644 --- a/src/modules/uORB/topics/vtol_vehicle_status.h +++ b/src/modules/uORB/topics/vtol_vehicle_status.h @@ -54,6 +54,7 @@ struct vtol_vehicle_status_s { uint64_t timestamp; /**< Microseconds since system boot */ bool vtol_in_rw_mode; /*true: vtol vehicle is in rotating wing mode */ + bool fw_permanent_stab; /**< In fw mode stabilize attitude even if in manual mode*/ }; /** diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 9a80562f3..10a3950ac 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -134,6 +134,7 @@ private: struct { param_t idle_pwm_mc; //pwm value for idle in mc mode param_t vtol_motor_count; + param_t vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) float mc_airspeed_trim; // trim airspeed in multicopter mode float mc_airspeed_max; // max airpseed in multicopter mode @@ -142,6 +143,7 @@ private: struct { param_t idle_pwm_mc; param_t vtol_motor_count; + param_t vtol_fw_permanent_stab; param_t mc_airspeed_min; param_t mc_airspeed_trim; param_t mc_airspeed_max; @@ -234,9 +236,11 @@ VtolAttitudeControl::VtolAttitudeControl() : _params.idle_pwm_mc = PWM_LOWEST_MIN; _params.vtol_motor_count = 0; + _params.vtol_fw_permanent_stab = 0; _params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC"); _params_handles.vtol_motor_count = param_find("VT_MOT_COUNT"); + _params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB"); _params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN"); _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); @@ -411,6 +415,9 @@ VtolAttitudeControl::parameters_update() /* vtol motor count */ param_get(_params_handles.vtol_motor_count, &_params.vtol_motor_count); + /* vtol fw permanent stabilization */ + param_get(_params_handles.vtol_fw_permanent_stab, &_params.vtol_fw_permanent_stab); + /* vtol mc mode min airspeed */ param_get(_params_handles.mc_airspeed_min, &v); _params.mc_airspeed_min = v; @@ -597,6 +604,9 @@ void VtolAttitudeControl::task_main() parameters_update(); // initialize parameter cache + /* update vtol vehicle status*/ + _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; + // make sure we start with idle in mc mode set_idle_mc(); flag_idle_mc = true; @@ -647,6 +657,8 @@ void VtolAttitudeControl::task_main() parameters_update(); } + _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; + vehicle_control_mode_poll(); //Check for changes in vehicle control mode. vehicle_manual_poll(); //Check for changes in manual inputs. arming_status_poll(); //Check for arming status updates. diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index e21bccb0c..0ab446317 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -86,3 +86,15 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_MAX,30.0f); */ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); +/** + * Permanent stabilization in fw mode + * + * If set to one this parameter will cause permanent attitude stabilization in fw mode. + * This parameter has been introduced for pure convenience sake. + * + * @min 0.0 + * @max 1.0 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); + -- cgit v1.2.3 From 353ea9beacc5f670bb82a6dc234fcaf7c4c0f183 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:42:45 +1100 Subject: l3gd20: added testerror command useful for testing error handling --- src/drivers/l3gd20/l3gd20.cpp | 35 +++++++++++++++++++++++++++++++++-- 1 file changed, 33 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 3a0c05c9e..e5ace48c9 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -204,6 +204,9 @@ public: // print register dump void print_registers(); + // trigger an error + void test_error(); + protected: virtual int probe(); @@ -1098,6 +1101,13 @@ L3GD20::print_registers() printf("\n"); } +void +L3GD20::test_error() +{ + // trigger a deliberate error + write_reg(ADDR_CTRL_REG3, 0); +} + int L3GD20::self_test() { @@ -1287,10 +1297,25 @@ regdump(void) exit(0); } +/** + * trigger an error + */ +void +test_error(void) +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("regdump @ %p\n", g_dev); + g_dev->test_error(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset' or 'regdump'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -1353,5 +1378,11 @@ l3gd20_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) l3gd20::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info' or 'regdump'"); + /* + * trigger an error + */ + if (!strcmp(verb, "testerror")) + l3gd20::test_error(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } -- cgit v1.2.3 From 8db206615183a4bd8064734c78554fffab5c1c6d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:43:13 +1100 Subject: l3gd20: removed printf in interrupt context this is not safe --- src/drivers/l3gd20/l3gd20.cpp | 8 -------- 1 file changed, 8 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index e5ace48c9..71a54c0f9 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -924,14 +924,6 @@ L3GD20::check_registers(void) if (_checked_next != 0) { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); } -#if 1 - if (_register_wait == 0) { - ::printf("L3GD20: %02x:%02x should be %02x\n", - (unsigned)_checked_registers[_checked_next], - (unsigned)v, - (unsigned)_checked_values[_checked_next]); - } -#endif _register_wait = 20; } _checked_next = (_checked_next+1) % L3GD20_NUM_CHECKED_REGISTERS; -- cgit v1.2.3 From 1bb3974c8429b2e680d258d37ad42790b608564b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:43:36 +1100 Subject: l3gd20: check DRDY after check_registers() is called this allows us to recover from an error that disables data ready --- src/drivers/l3gd20/l3gd20.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 71a54c0f9..5c9235017 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -932,17 +932,6 @@ L3GD20::check_registers(void) void L3GD20::measure() { -#if L3GD20_USE_DRDY - // if the gyro doesn't have any data ready then re-schedule - // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { - perf_count(_reschedules); - hrt_call_delay(&_call, 100); - return; - } -#endif - /* status register and data as read back from the device */ #pragma pack(push, 1) struct { @@ -962,6 +951,18 @@ L3GD20::measure() check_registers(); +#if L3GD20_USE_DRDY + // if the gyro doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) { + perf_count(_reschedules); + hrt_call_delay(&_call, 100); + perf_end(_sample_perf); + return; + } +#endif + /* fetch data from the sensor */ memset(&raw_report, 0, sizeof(raw_report)); raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT; -- cgit v1.2.3 From 5c1da70d0c738c0614ea1652a510f126c31369d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:44:12 +1100 Subject: l3gd20: fixed reporting of error count --- src/drivers/l3gd20/l3gd20.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 5c9235017..4e2f198b8 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -993,7 +993,7 @@ L3GD20::measure() * 74 from all measurements centers them around zero. */ report.timestamp = hrt_absolute_time(); - report.error_count = 0; // not recorded + report.error_count = perf_event_count(_bad_registers); switch (_orientation) { -- cgit v1.2.3 From 2ac3a6fcf4ab051b53f635d696a45edac438228e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:44:50 +1100 Subject: l3m303d: added testerror command useful for testing error handling --- src/drivers/lsm303d/lsm303d.cpp | 36 ++++++++++++++++++++++++++++++++++-- 1 file changed, 34 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index dbd5c1f4c..a062b2828 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -237,6 +237,11 @@ public: */ void print_registers(); + /** + * deliberately trigger an error + */ + void test_error(); + protected: virtual int probe(); @@ -1690,6 +1695,13 @@ LSM303D::print_registers() } } +void +LSM303D::test_error() +{ + // trigger an error + write_reg(ADDR_CTRL_REG3, 0); +} + LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), _parent(parent), @@ -1969,10 +1981,24 @@ regdump() exit(0); } +/** + * trigger an error + */ +void +test_error() +{ + if (g_dev == nullptr) + errx(1, "driver not running\n"); + + g_dev->test_error(); + + exit(0); +} + void usage() { - warnx("missing command: try 'start', 'info', 'test', 'reset', 'regdump'"); + warnx("missing command: try 'start', 'info', 'test', 'reset', 'testerror' or 'regdump'"); warnx("options:"); warnx(" -X (external bus)"); warnx(" -R rotation"); @@ -2035,5 +2061,11 @@ lsm303d_main(int argc, char *argv[]) if (!strcmp(verb, "regdump")) lsm303d::regdump(); - errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', or 'regdump'"); + /* + * trigger an error + */ + if (!strcmp(verb, "testerror")) + lsm303d::test_error(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset', 'info', 'testerror' or 'regdump'"); } -- cgit v1.2.3 From e4318345f3a105ac769f93f09f6e11cfb7aa0072 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:45:25 +1100 Subject: lsm303d: added two more checked registers these are key for DRDY behaviour --- src/drivers/lsm303d/lsm303d.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index a062b2828..ace228920 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -297,7 +297,7 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define LSM303D_NUM_CHECKED_REGISTERS 6 +#define LSM303D_NUM_CHECKED_REGISTERS 8 static const uint8_t _checked_registers[LSM303D_NUM_CHECKED_REGISTERS]; uint8_t _checked_values[LSM303D_NUM_CHECKED_REGISTERS]; uint8_t _checked_next; @@ -474,6 +474,8 @@ private: const uint8_t LSM303D::_checked_registers[LSM303D_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I, ADDR_CTRL_REG1, ADDR_CTRL_REG2, + ADDR_CTRL_REG3, + ADDR_CTRL_REG4, ADDR_CTRL_REG5, ADDR_CTRL_REG6, ADDR_CTRL_REG7 }; @@ -708,8 +710,8 @@ LSM303D::reset() /* enable mag */ write_checked_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M); write_checked_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M); - write_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 - write_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 + write_checked_reg(ADDR_CTRL_REG3, 0x04); // DRDY on ACCEL on INT1 + write_checked_reg(ADDR_CTRL_REG4, 0x04); // DRDY on MAG on INT2 accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G); accel_set_samplerate(LSM303D_ACCEL_DEFAULT_RATE); -- cgit v1.2.3 From 1de7fab974b21afdc43caaeb6c8ecdf2ac88cc10 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:45:51 +1100 Subject: lsm303d: removed unsafe printf in interrupt context --- src/drivers/lsm303d/lsm303d.cpp | 8 -------- 1 file changed, 8 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index ace228920..58dcdf410 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1429,14 +1429,6 @@ LSM303D::check_registers(void) if (_checked_next != 0) { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); } -#if 1 - if (_register_wait == 0) { - ::printf("LSM303D: %02x:%02x should be %02x\n", - (unsigned)_checked_registers[_checked_next], - (unsigned)v, - (unsigned)_checked_values[_checked_next]); - } -#endif _register_wait = 20; } _checked_next = (_checked_next+1) % LSM303D_NUM_CHECKED_REGISTERS; -- cgit v1.2.3 From 083d9e3e698e794640ac216959ba34157609e6e1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:46:13 +1100 Subject: lsm303d: check DRDY after check_registers() this allows recovery from a state where DRDY is not set --- src/drivers/lsm303d/lsm303d.cpp | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 58dcdf410..3e5ff1f7c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1437,19 +1437,6 @@ LSM303D::check_registers(void) void LSM303D::measure() { - // if the accel doesn't have any data ready then re-schedule - // for 100 microseconds later. This ensures we don't double - // read a value and then miss the next value. - // Note that DRDY is not available when the lsm303d is - // connected on the external bus -#ifdef GPIO_EXTI_ACCEL_DRDY - if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { - perf_count(_accel_reschedules); - hrt_call_delay(&_accel_call, 100); - return; - } -#endif - /* status register and data as read back from the device */ #pragma pack(push, 1) @@ -1469,6 +1456,20 @@ LSM303D::measure() check_registers(); + // if the accel doesn't have any data ready then re-schedule + // for 100 microseconds later. This ensures we don't double + // read a value and then miss the next value. + // Note that DRDY is not available when the lsm303d is + // connected on the external bus +#ifdef GPIO_EXTI_ACCEL_DRDY + if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_ACCEL_DRDY) == 0) { + perf_count(_accel_reschedules); + hrt_call_delay(&_accel_call, 100); + perf_end(_accel_sample_perf); + return; + } +#endif + if (_register_wait != 0) { // we are waiting for some good transfers before using // the sensor again. -- cgit v1.2.3 From b5f6fedfa81297587c185079b803c85069704c9a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:46:40 +1100 Subject: lsm303d: show all perf counters in "info" --- src/drivers/lsm303d/lsm303d.cpp | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 3e5ff1f7c..4f4e5f39c 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1618,7 +1618,9 @@ LSM303D::print_info() printf("accel reads: %u\n", _accel_read); printf("mag reads: %u\n", _mag_read); perf_print_counter(_accel_sample_perf); + perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); + perf_print_counter(_accel_reschedules); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); ::printf("checked_next: %u\n", _checked_next); -- cgit v1.2.3 From 82b9e08e4c2053df2e01a5c6b0576ca73d9a5c0d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 08:47:07 +1100 Subject: mpu6000: removed unsafe printf in interrupt context instead delay 3ms between register writes. This seems to give a quite high probability of correctly resetting the sensor, and does still reliably detect the sensor going bad, which is the most important thing in this code --- src/drivers/mpu6000/mpu6000.cpp | 42 ++++------------------------------------- 1 file changed, 4 insertions(+), 38 deletions(-) (limited to 'src') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index e7e88bb82..6cac28a7d 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -254,7 +254,6 @@ private: uint8_t _register_wait; uint64_t _reset_wait; - uint64_t _printf_wait; math::LowPassFilter2p _accel_filter_x; math::LowPassFilter2p _accel_filter_y; @@ -494,7 +493,6 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _reset_retries(perf_alloc(PC_COUNT, "mpu6000_reset_retries")), _register_wait(0), _reset_wait(0), - _printf_wait(0), _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), @@ -1539,43 +1537,11 @@ MPU6000::check_registers(void) _checked_next = 0; } else { write_reg(_checked_registers[_checked_next], _checked_values[_checked_next]); - _reset_wait = hrt_absolute_time() + 1000; - if (_checked_next == 1 && hrt_absolute_time() > _printf_wait) { - /* - rather bizarrely this printf() seems - to be critical for successfully - resetting the sensor. If you take - this printf out then all the - registers do get successfully reset, - but the sensor ends up with a large - fixed offset on the - accelerometers. Try a up_udelay() - of various sizes instead of a - printf() doesn't work. That makes no - sense at all, and investigating why - this is would be worthwhile. - - The rate limit on the printf to 5Hz - prevents this from causing enough - delays in interrupt context to cause - the vehicle to not be able to fly - correctly. - */ - _printf_wait = hrt_absolute_time() + 200*1000UL; - ::printf("Setting %u %02x to %02x\n", - (unsigned)_checked_next, - (unsigned)_checked_registers[_checked_next], - (unsigned)_checked_values[_checked_next]); - } + // waiting 3ms between register writes seems + // to raise the chance of the sensor + // recovering considerably + _reset_wait = hrt_absolute_time() + 3000; } -#if 0 - if (_register_wait == 0) { - ::printf("MPU6000: %02x:%02x should be %02x\n", - (unsigned)_checked_registers[_checked_next], - (unsigned)v, - (unsigned)_checked_values[_checked_next]); - } -#endif _register_wait = 20; } _checked_next = (_checked_next+1) % MPU6000_NUM_CHECKED_REGISTERS; -- cgit v1.2.3 From 7864958f1c2da5b9a64b92082ee287883479768e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 10:03:34 +1100 Subject: l3gd20: fixed build warning --- src/drivers/l3gd20/l3gd20.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 4e2f198b8..08bc1fead 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1137,6 +1137,7 @@ void test(); void reset(); void info(); void regdump(); +void test_error(); /** * Start the driver. -- cgit v1.2.3 From 8470105f9efe57f6abb9f1caa20deb4355d5a7f9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 10:03:42 +1100 Subject: lsm303d: fixed build warning --- src/drivers/lsm303d/lsm303d.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 4f4e5f39c..00484db79 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1773,6 +1773,7 @@ void reset(); void info(); void regdump(); void usage(); +void test_error(); /** * Start the driver. -- cgit v1.2.3 From 7b6620b319e41abc546b9d6ab4f522a106686b28 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 1 Jan 2015 10:59:42 +1100 Subject: ll40ls: fixed exit code on external sensor startup failure --- src/drivers/ll40ls/ll40ls.cpp | 3 +++ 1 file changed, 3 insertions(+) (limited to 'src') diff --git a/src/drivers/ll40ls/ll40ls.cpp b/src/drivers/ll40ls/ll40ls.cpp index 4676c6ad7..e68f24152 100644 --- a/src/drivers/ll40ls/ll40ls.cpp +++ b/src/drivers/ll40ls/ll40ls.cpp @@ -746,6 +746,9 @@ start(int bus) if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { delete g_dev_ext; g_dev_ext = nullptr; + if (bus == PX4_I2C_BUS_EXPANSION) { + goto fail; + } } } -- cgit v1.2.3 From a7bc38869e2121b7f63e63e0fae4bfe4117a3315 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 1 Jan 2015 14:51:18 +0100 Subject: added pitch trim for vtol in fixed wing mode --- src/modules/vtol_att_control/vtol_att_control_main.cpp | 9 ++++++++- src/modules/vtol_att_control/vtol_att_control_params.c | 11 +++++++++++ 2 files changed, 19 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 10a3950ac..c72a85ecd 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -138,6 +138,7 @@ private: float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) float mc_airspeed_trim; // trim airspeed in multicopter mode float mc_airspeed_max; // max airpseed in multicopter mode + float fw_pitch_trim; // trim for neutral elevon position in fw mode } _params; struct { @@ -147,6 +148,7 @@ private: param_t mc_airspeed_min; param_t mc_airspeed_trim; param_t mc_airspeed_max; + param_t fw_pitch_trim; } _params_handles; perf_counter_t _loop_perf; /**< loop performance counter */ @@ -244,6 +246,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.mc_airspeed_min = param_find("VT_MC_ARSPD_MIN"); _params_handles.mc_airspeed_max = param_find("VT_MC_ARSPD_MAX"); _params_handles.mc_airspeed_trim = param_find("VT_MC_ARSPD_TRIM"); + _params_handles.fw_pitch_trim = param_find("VT_FW_PITCH_TRIM"); /* fetch initial parameter values */ parameters_update(); @@ -430,6 +433,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.mc_airspeed_trim, &v); _params.mc_airspeed_trim = v; + /* vtol pitch trim for fw mode */ + param_get(_params_handles.fw_pitch_trim, &v); + _params.fw_pitch_trim = v; + return OK; } @@ -459,7 +466,7 @@ void VtolAttitudeControl::fill_fw_att_control_output() _actuators_out_0.control[3] = _actuators_fw_in.control[3]; /*controls for the elevons */ _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon - _actuators_out_1.control[1] = _actuators_fw_in.control[1]; // pitch elevon + _actuators_out_1.control[1] = _actuators_fw_in.control[1] + _params.fw_pitch_trim; // pitch elevon // unused now but still logged _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw _actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 0ab446317..d1d4697f3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -98,3 +98,14 @@ PARAM_DEFINE_FLOAT(VT_MC_ARSPD_TRIM,10.0f); */ PARAM_DEFINE_INT32(VT_FW_PERM_STAB,0); +/** + * Fixed wing pitch trim + * + * This parameter allows to adjust the neutral elevon position in fixed wing mode. + * + * @min -1 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_FW_PITCH_TRIM,0.0f); + -- cgit v1.2.3 From 500ac1443bcfc9082a76b0a2a0a72b0a3d539b9b Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 2 Jan 2015 16:45:08 +1100 Subject: lsm303d: detect large fixed values and report an error we have logs where the lsm303d gets large fixed values for long periods. This will detect that error case and raise error_count so the higher level sensor integration code can choose another sensor --- src/drivers/lsm303d/lsm303d.cpp | 49 ++++++++++++++++++++++++++++++++++++----- 1 file changed, 44 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 00484db79..57754e4c0 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -285,6 +285,7 @@ private: perf_counter_t _mag_sample_perf; perf_counter_t _accel_reschedules; perf_counter_t _bad_registers; + perf_counter_t _bad_values; uint8_t _register_wait; @@ -294,6 +295,10 @@ private: enum Rotation _rotation; + // values used to + float _last_accel[3]; + uint8_t _constant_accel_count; + // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset @@ -542,11 +547,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")), _accel_reschedules(perf_alloc(PC_COUNT, "lsm303d_accel_resched")), _bad_registers(perf_alloc(PC_COUNT, "lsm303d_bad_registers")), + _bad_values(perf_alloc(PC_COUNT, "lsm303d_bad_values")), _register_wait(0), _accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), + _constant_accel_count(0), _checked_next(0) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; @@ -590,6 +597,7 @@ LSM303D::~LSM303D() perf_free(_accel_sample_perf); perf_free(_mag_sample_perf); perf_free(_bad_registers); + perf_free(_bad_values); perf_free(_accel_reschedules); } @@ -1501,11 +1509,11 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); - // report the error count as the sum of the number of bad bad - // register reads. This allows the higher level code to decide - // if it should use this sensor based on whether it has had - // failures - accel_report.error_count = perf_event_count(_bad_registers); + // report the error count as the sum of the number of bad + // register reads and bad values. This allows the higher level + // code to decide if it should use this sensor based on + // whether it has had failures + accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); accel_report.x_raw = raw_accel_report.x; accel_report.y_raw = raw_accel_report.y; @@ -1515,6 +1523,36 @@ LSM303D::measure() float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; + /* + we have logs where the accelerometers get stuck at a fixed + large value. We want to detect this and mark the sensor as + being faulty + */ + if (fabsf(_last_accel[0] - x_in_new) < 0.001f && + fabsf(_last_accel[1] - y_in_new) < 0.001f && + fabsf(_last_accel[2] - z_in_new) < 0.001f && + fabsf(x_in_new) > 20 && + fabsf(y_in_new) > 20 && + fabsf(z_in_new) > 20) { + _constant_accel_count += 1; + } else { + _constant_accel_count = 0; + } + if (_constant_accel_count > 100) { + // we've had 100 constant accel readings with large + // values. The sensor is almost certainly dead. We + // will raise the error_count so that the top level + // flight code will know to avoid this sensor, but + // we'll still give the data so that it can be logged + // and viewed + perf_count(_bad_values); + _constant_accel_count = 0; + } + + _last_accel[0] = x_in_new; + _last_accel[1] = y_in_new; + _last_accel[2] = z_in_new; + accel_report.x = _accel_filter_x.apply(x_in_new); accel_report.y = _accel_filter_y.apply(y_in_new); accel_report.z = _accel_filter_z.apply(z_in_new); @@ -1620,6 +1658,7 @@ LSM303D::print_info() perf_print_counter(_accel_sample_perf); perf_print_counter(_mag_sample_perf); perf_print_counter(_bad_registers); + perf_print_counter(_bad_values); perf_print_counter(_accel_reschedules); _accel_reports->print_info("accel reports"); _mag_reports->print_info("mag reports"); -- cgit v1.2.3 From 2effc9a23d8df3922f9f7cec9b0af9b15b2e2367 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Oct 2014 11:57:46 +0200 Subject: MS5611: Allow two instances --- src/drivers/ms5611/ms5611.cpp | 60 ++++++++++++++++++++++++++++++++----------- 1 file changed, 45 insertions(+), 15 deletions(-) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 889643d0d..340a2e660 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -787,7 +787,9 @@ MS5611::print_info() namespace ms5611 { -MS5611 *g_dev; +/* initialize explicitely for clarity */ +MS5611 *g_dev_ext = nullptr; +MS5611 *g_dev_int = nullptr; void start(bool external_bus); void test(); @@ -852,9 +854,13 @@ start(bool external_bus) int fd; prom_u prom_buf; - if (g_dev != nullptr) + if (external_bus && (g_dev_ext != nullptr)) /* if already started, the still command succeeded */ - errx(0, "already started"); + errx(0, "ext already started"); + + if (g_dev_int != nullptr) + /* if already started, the still command succeeded */ + errx(0, "int already started"); device::Device *interface = nullptr; @@ -872,13 +878,25 @@ start(bool external_bus) errx(1, "interface init failed"); } - g_dev = new MS5611(interface, prom_buf); - if (g_dev == nullptr) { - delete interface; - errx(1, "failed to allocate driver"); + if (external_bus) { + g_dev_ext = new MS5611(interface, prom_buf); + if (g_dev_ext == nullptr) { + delete interface; + errx(1, "failed to allocate driver"); + } + if (g_dev_ext->init() != OK) + goto fail; + } else { + + g_dev_int = new MS5611(interface, prom_buf); + if (g_dev_int == nullptr) { + delete interface; + errx(1, "failed to allocate driver"); + } + if (g_dev_int->init() != OK) + goto fail; + } - if (g_dev->init() != OK) - goto fail; /* set the poll rate to default, starts automatic data collection */ fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); @@ -895,9 +913,14 @@ start(bool external_bus) fail: - if (g_dev != nullptr) { - delete g_dev; - g_dev = nullptr; + if (g_dev_int != nullptr) { + delete g_dev_int; + g_dev_int = nullptr; + } + + if (g_dev_ext != nullptr) { + delete g_dev_ext; + g_dev_ext = nullptr; } errx(1, "driver start failed"); @@ -994,11 +1017,18 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev_ext == nullptr && g_dev_int == nullptr) errx(1, "driver not running"); - printf("state @ %p\n", g_dev); - g_dev->print_info(); + if (g_dev_ext) { + warnx("ext:"); + g_dev_ext->print_info(); + } + + if (g_dev_int) { + warnx("int:"); + g_dev_int->print_info(); + } exit(0); } -- cgit v1.2.3 From bf134e697971194923fa6834c9919550d418665c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Oct 2014 12:11:45 +0200 Subject: Allow to get access to dev name --- src/drivers/device/device.h | 7 +++++++ 1 file changed, 7 insertions(+) (limited to 'src') diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h index 67aaa0aff..4d4bed835 100644 --- a/src/drivers/device/device.h +++ b/src/drivers/device/device.h @@ -445,6 +445,13 @@ protected: */ virtual int unregister_class_devname(const char *class_devname, unsigned class_instance); + /** + * Get the device name. + * + * @return the file system string of the device handle + */ + const char* get_devname() { return _devname; } + bool _pub_blocked; /**< true if publishing should be blocked */ private: -- cgit v1.2.3 From a77c9225df3aa494d5db64824926174c95381b97 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Oct 2014 12:12:17 +0200 Subject: Allow access to both device handles, make external use a special handle --- src/drivers/ms5611/ms5611.cpp | 70 +++++++++++++++++++++++++++++-------------- 1 file changed, 48 insertions(+), 22 deletions(-) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 340a2e660..beca29d6d 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -91,12 +91,13 @@ static const int ERROR = -1; /* internal conversion time: 9.17 ms, so should not be read at rates higher than 100 Hz */ #define MS5611_CONVERSION_INTERVAL 10000 /* microseconds */ #define MS5611_MEASUREMENT_RATIO 3 /* pressure measurements per temperature measurement */ -#define MS5611_BARO_DEVICE_PATH "/dev/ms5611" +#define MS5611_BARO_DEVICE_PATH_EXT "/dev/ms5611_ext" +#define MS5611_BARO_DEVICE_PATH_INT "/dev/ms5611_int" class MS5611 : public device::CDev { public: - MS5611(device::Device *interface, ms5611::prom_u &prom_buf); + MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path); ~MS5611(); virtual int init(); @@ -195,8 +196,8 @@ protected: */ extern "C" __EXPORT int ms5611_main(int argc, char *argv[]); -MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf) : - CDev("MS5611", MS5611_BARO_DEVICE_PATH), +MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* path) : + CDev("MS5611", path), _interface(interface), _prom(prom_buf.s), _measure_ticks(0), @@ -224,7 +225,7 @@ MS5611::~MS5611() stop_cycle(); if (_class_instance != -1) - unregister_class_devname(MS5611_BARO_DEVICE_PATH, _class_instance); + unregister_class_devname(get_devname(), _class_instance); /* free any existing reports */ if (_reports != nullptr) @@ -792,10 +793,10 @@ MS5611 *g_dev_ext = nullptr; MS5611 *g_dev_int = nullptr; void start(bool external_bus); -void test(); -void reset(); +void test(bool external_bus); +void reset(bool external_bus); void info(); -void calibrate(unsigned altitude); +void calibrate(unsigned altitude, bool external_bus); void usage(); /** @@ -879,27 +880,34 @@ start(bool external_bus) } if (external_bus) { - g_dev_ext = new MS5611(interface, prom_buf); + g_dev_ext = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_EXT); if (g_dev_ext == nullptr) { delete interface; errx(1, "failed to allocate driver"); } + if (g_dev_ext->init() != OK) goto fail; + + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { - g_dev_int = new MS5611(interface, prom_buf); + g_dev_int = new MS5611(interface, prom_buf, MS5611_BARO_DEVICE_PATH_INT); if (g_dev_int == nullptr) { delete interface; errx(1, "failed to allocate driver"); } + if (g_dev_int->init() != OK) goto fail; + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } /* set the poll rate to default, starts automatic data collection */ - fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + if (fd < 0) { warnx("can't open baro device"); goto fail; @@ -932,16 +940,22 @@ fail: * and automatic modes. */ void -test() +test(bool external_bus) { struct baro_report report; ssize_t sz; int ret; - int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + int fd; + + if (external_bus) { + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); + err(1, "open failed (try 'ms5611 start' if the driver is not running)"); /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); @@ -995,9 +1009,15 @@ test() * Reset the driver. */ void -reset() +reset(bool external_bus) { - int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + int fd; + + if (external_bus) { + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } if (fd < 0) err(1, "failed "); @@ -1037,16 +1057,22 @@ info() * Calculate actual MSL pressure given current altitude */ void -calibrate(unsigned altitude) +calibrate(unsigned altitude, bool external_bus) { struct baro_report report; float pressure; float p1; - int fd = open(MS5611_BARO_DEVICE_PATH, O_RDONLY); + int fd; + + if (external_bus) { + fd = open(MS5611_BARO_DEVICE_PATH_EXT, O_RDONLY); + } else { + fd = open(MS5611_BARO_DEVICE_PATH_INT, O_RDONLY); + } if (fd < 0) - err(1, "%s open failed (try 'ms5611 start' if the driver is not running)", MS5611_BARO_DEVICE_PATH); + err(1, "open failed (try 'ms5611 start' if the driver is not running)"); /* start the sensor polling at max */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_MAX)) @@ -1141,13 +1167,13 @@ ms5611_main(int argc, char *argv[]) * Test the driver/device. */ if (!strcmp(verb, "test")) - ms5611::test(); + ms5611::test(external_bus); /* * Reset the driver. */ if (!strcmp(verb, "reset")) - ms5611::reset(); + ms5611::reset(external_bus); /* * Print driver information. @@ -1164,7 +1190,7 @@ ms5611_main(int argc, char *argv[]) long altitude = strtol(argv[optind+1], nullptr, 10); - ms5611::calibrate(altitude); + ms5611::calibrate(altitude, external_bus); } errx(1, "unrecognised command, try 'start', 'test', 'reset' or 'info'"); -- cgit v1.2.3 From 4c3ebee15b2d3626b328d313b1356634e70ab6d8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Oct 2014 13:29:25 +0200 Subject: Hackery on option parsing to make MS5611 comply --- src/drivers/ms5611/ms5611.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index beca29d6d..aed72de01 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -855,13 +855,13 @@ start(bool external_bus) int fd; prom_u prom_buf; - if (external_bus && (g_dev_ext != nullptr)) + if (external_bus && (g_dev_ext != nullptr)) { /* if already started, the still command succeeded */ errx(0, "ext already started"); - - if (g_dev_int != nullptr) + } else if (!external_bus && (g_dev_int != nullptr)) { /* if already started, the still command succeeded */ errx(0, "int already started"); + } device::Device *interface = nullptr; @@ -1157,6 +1157,12 @@ ms5611_main(int argc, char *argv[]) const char *verb = argv[optind]; + if (argc > optind+1) { + if (!strcmp(argv[optind+1], "-X")) { + external_bus = true; + } + } + /* * Start/load the driver. */ -- cgit v1.2.3 From d20ebd739045c3fb1fb215c549707618f0b2554b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 21:49:52 +0200 Subject: Add logging for 2nd baro Conflicts: src/modules/sdlog2/sdlog2_messages.h --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 6 +++++- 2 files changed, 23 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 0a8564da6..2daf73bf9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1112,6 +1112,7 @@ int sdlog2_thread_main(int argc, char *argv[]) hrt_abstime magnetometer_timestamp = 0; hrt_abstime barometer_timestamp = 0; hrt_abstime differential_pressure_timestamp = 0; + hrt_abstime barometer1_timestamp = 0; hrt_abstime gyro1_timestamp = 0; hrt_abstime accelerometer1_timestamp = 0; hrt_abstime magnetometer1_timestamp = 0; @@ -1257,6 +1258,7 @@ int sdlog2_thread_main(int argc, char *argv[]) bool write_IMU1 = false; bool write_IMU2 = false; bool write_SENS = false; + bool write_SENS1 = false; if (buf.sensor.timestamp != gyro_timestamp) { gyro_timestamp = buf.sensor.timestamp; @@ -1307,6 +1309,22 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(SENS); } + if (buf.sensor.baro1_timestamp != barometer1_timestamp) { + barometer1_timestamp = buf.sensor.baro1_timestamp; + write_SENS1 = true; + } + + if (write_SENS1) { + log_msg.msg_type = LOG_AIR1_MSG; + log_msg.body.log_SENS.baro_pres = buf.sensor.baro1_pres_mbar; + log_msg.body.log_SENS.baro_alt = buf.sensor.baro1_alt_meter; + log_msg.body.log_SENS.baro_temp = buf.sensor.baro1_temp_celcius; + log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure1_pa; + log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure1_filtered_pa; + // XXX moving to AIR0-AIR2 instead of SENS + LOGBUFFER_WRITE_AND_COUNT(SENS); + } + if (buf.sensor.accelerometer1_timestamp != accelerometer1_timestamp) { accelerometer1_timestamp = buf.sensor.accelerometer1_timestamp; write_IMU1 = true; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5b047f538..5941bfac0 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -424,6 +424,9 @@ struct log_ENCD_s { float vel1; }; +/* --- AIR SPEED SENSORS - DIFF. PRESSURE --- */ +#define LOG_AIR1_MSG 40 + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -457,7 +460,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT_S(IMU, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU1, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT_S(IMU2, IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT(SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), + LOG_FORMAT_S(SENS, SENS, "fffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), + LOG_FORMAT_S(AIR1, SENS, "fffff", "BaroPa,BaroAlt,BaroTmp,DiffPres,DiffPresF"), LOG_FORMAT(LPOS, "ffffffffLLfBBBff", "X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffffBHHH", "GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"), -- cgit v1.2.3 From 08297db39a909df9b024867eeb23720532f5625f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 21:50:12 +0200 Subject: Support for 2nd baro --- src/drivers/drv_baro.h | 1 + src/drivers/ms5611/ms5611.cpp | 21 ++++----------------- 2 files changed, 5 insertions(+), 17 deletions(-) (limited to 'src') diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 248b9a73d..3e28d3d3d 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -65,6 +65,7 @@ struct baro_report { */ ORB_DECLARE(sensor_baro0); ORB_DECLARE(sensor_baro1); +ORB_DECLARE(sensor_baro2); /* * ioctl() definitions diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index aed72de01..0a793cbc9 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -134,6 +134,7 @@ protected: unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; + orb_id_t _orb_id; int _class_instance; @@ -262,6 +263,7 @@ MS5611::init() /* register alternate interfaces if we have to */ _class_instance = register_class_devname(BARO_DEVICE_PATH); + _orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance); struct baro_report brp; /* do a first measurement cycle to populate reports with valid data */ @@ -301,14 +303,7 @@ MS5611::init() ret = OK; - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _baro_topic = orb_advertise(ORB_ID(sensor_baro0), &brp); - break; - case CLASS_DEVICE_SECONDARY: - _baro_topic = orb_advertise(ORB_ID(sensor_baro1), &brp); - break; - } + _baro_topic = orb_advertise(_orb_id, &brp); if (_baro_topic < 0) { warnx("failed to create sensor_baro publication"); @@ -730,15 +725,7 @@ MS5611::collect() /* publish it */ if (!(_pub_blocked)) { /* publish it */ - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - orb_publish(ORB_ID(sensor_baro0), _baro_topic, &report); - break; - - case CLASS_DEVICE_SECONDARY: - orb_publish(ORB_ID(sensor_baro1), _baro_topic, &report); - break; - } + orb_publish(_orb_id, _baro_topic, &report); } if (_reports->force(&report)) { -- cgit v1.2.3 From f5b6b831617632dfb237f8331e29242774b7c0c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 21:50:52 +0200 Subject: Added 2nd baro to sensor combined topic --- src/modules/uORB/topics/sensor_combined.h | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index 06dfcdab3..583a39ded 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -122,15 +122,25 @@ struct sensor_combined_s { float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ float baro_alt_meter; /**< Altitude, already temp. comp. */ float baro_temp_celcius; /**< Temperature in degrees celsius */ - float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ + uint64_t baro_timestamp; /**< Barometer timestamp */ + + float baro1_pres_mbar; /**< Barometric pressure, already temp. comp. */ + float baro1_alt_meter; /**< Altitude, already temp. comp. */ + float baro1_temp_celcius; /**< Temperature in degrees celsius */ + uint64_t baro1_timestamp; /**< Barometer timestamp */ + + float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ unsigned adc_mapping[10]; /**< Channel indices of each of these values */ float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - uint64_t baro_timestamp; /**< Barometer timestamp */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ float differential_pressure_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ + float differential_pressure1_pa; /**< Airspeed sensor differential pressure */ + uint64_t differential_pressure1_timestamp; /**< Last measurement timestamp */ + float differential_pressure1_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ + }; /** -- cgit v1.2.3 From a12ac452a1f8309429d50e1c2477fd15e48a04e6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 21:51:05 +0200 Subject: Objects common --- src/modules/uORB/objects_common.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index d834c3574..293412455 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -63,6 +63,7 @@ ORB_DEFINE(sensor_gyro2, struct gyro_report); #include ORB_DEFINE(sensor_baro0, struct baro_report); ORB_DEFINE(sensor_baro1, struct baro_report); +ORB_DEFINE(sensor_baro2, struct baro_report); #include ORB_DEFINE(sensor_range_finder, struct range_finder_report); -- cgit v1.2.3 From 1f81a8bd611e295161b68e188041ab79b4d017db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 15 Oct 2014 21:52:59 +0200 Subject: 2nd baro support for common topics --- src/modules/sensors/sensors.cpp | 46 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 46 insertions(+) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 952b0447d..018eadaff 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -226,6 +226,7 @@ private: int _mag2_sub; /**< raw mag2 data subscription */ int _rc_sub; /**< raw rc channels data subscription */ int _baro_sub; /**< raw baro data subscription */ + int _baro1_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ int _vcontrol_mode_sub; /**< vehicle control mode subscription */ @@ -523,6 +524,7 @@ Sensors::Sensors() : _mag2_sub(-1), _rc_sub(-1), _baro_sub(-1), + _baro1_sub(-1), _vcontrol_mode_sub(-1), _params_sub(-1), _rc_parameter_map_sub(-1), @@ -1258,6 +1260,34 @@ Sensors::mag_poll(struct sensor_combined_s &raw) raw.magnetometer_timestamp = mag_report.timestamp; } + + orb_check(_mag1_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag1), _mag1_sub, &mag_report); + + raw.magnetometer1_raw[0] = mag_report.x_raw; + raw.magnetometer1_raw[1] = mag_report.y_raw; + raw.magnetometer1_raw[2] = mag_report.z_raw; + + raw.magnetometer1_timestamp = mag_report.timestamp; + } + + orb_check(_mag2_sub, &mag_updated); + + if (mag_updated) { + struct mag_report mag_report; + + orb_copy(ORB_ID(sensor_mag2), _mag2_sub, &mag_report); + + raw.magnetometer2_raw[0] = mag_report.x_raw; + raw.magnetometer2_raw[1] = mag_report.y_raw; + raw.magnetometer2_raw[2] = mag_report.z_raw; + + raw.magnetometer2_timestamp = mag_report.timestamp; + } } void @@ -1276,6 +1306,21 @@ Sensors::baro_poll(struct sensor_combined_s &raw) raw.baro_timestamp = _barometer.timestamp; } + + orb_check(_baro1_sub, &baro_updated); + + if (baro_updated) { + + struct baro_report baro_report; + + orb_copy(ORB_ID(sensor_baro1), _baro1_sub, &baro_report); + + raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar + raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters + raw.baro1_temp_celcius = baro_report.temperature; // Temperature in degrees celcius + + raw.baro1_timestamp = baro_report.timestamp; + } } void @@ -1881,6 +1926,7 @@ Sensors::task_main() _mag2_sub = orb_subscribe(ORB_ID(sensor_mag2)); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); + _baro1_sub = orb_subscribe(ORB_ID(sensor_baro1)); _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)); -- cgit v1.2.3 From f28e8d6731014afa13bef368d8a58fe7f3579249 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Dec 2014 02:14:35 +0100 Subject: sdlog2: Adjust frame size --- src/modules/sdlog2/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index f1118000e..4fd6fd1c2 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -42,7 +42,7 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" SRCS = sdlog2.c \ logbuffer.c -MODULE_STACKSIZE = 1200 +MODULE_STACKSIZE = 1300 MAXOPTIMIZATION = -Os -- cgit v1.2.3 From ebc4cc3a7dfc6a705a4585c75e92969a6a0e798f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 31 Dec 2014 02:24:21 +0100 Subject: sdlog2: Adjust frame size warning limit, cross-checked stack size of app --- src/modules/sdlog2/module.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 4fd6fd1c2..91a9611d4 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -42,9 +42,9 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30" SRCS = sdlog2.c \ logbuffer.c -MODULE_STACKSIZE = 1300 +MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wframe-larger-than=1200 +EXTRACFLAGS = -Wframe-larger-than=1300 -- cgit v1.2.3 From dd165100fb8c0943c7518b40ae67c36b408baa0d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 1 Jan 2015 15:47:07 +0100 Subject: Support for HMC5983, which can also be attached via SPI --- src/drivers/hmc5883/hmc5883.cpp | 237 ++++++++++++++++++------------------ src/drivers/hmc5883/hmc5883.h | 52 ++++++++ src/drivers/hmc5883/hmc5883_i2c.cpp | 175 ++++++++++++++++++++++++++ src/drivers/hmc5883/hmc5883_spi.cpp | 189 ++++++++++++++++++++++++++++ src/drivers/hmc5883/module.mk | 4 +- 5 files changed, 539 insertions(+), 118 deletions(-) create mode 100644 src/drivers/hmc5883/hmc5883.h create mode 100644 src/drivers/hmc5883/hmc5883_i2c.cpp create mode 100644 src/drivers/hmc5883/hmc5883_spi.cpp (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index d4dbf3778..5989703dd 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.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 @@ -34,7 +34,7 @@ /** * @file hmc5883.cpp * - * Driver for the HMC5883 magnetometer connected via I2C. + * Driver for the HMC5883 / HMC5983 magnetometer connected via I2C or SPI. */ #include @@ -74,11 +74,12 @@ #include #include +#include "hmc5883.h" + /* * HMC5883 internal constants and data structures. */ -#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 #define HMC5883L_DEVICE_PATH_INT "/dev/hmc5883_int" #define HMC5883L_DEVICE_PATH_EXT "/dev/hmc5883_ext" @@ -95,9 +96,6 @@ #define ADDR_DATA_OUT_Y_MSB 0x07 #define ADDR_DATA_OUT_Y_LSB 0x08 #define ADDR_STATUS 0x09 -#define ADDR_ID_A 0x0a -#define ADDR_ID_B 0x0b -#define ADDR_ID_C 0x0c /* modes not changeable outside of driver */ #define HMC5883L_MODE_NORMAL (0 << 0) /* default */ @@ -115,10 +113,11 @@ #define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */ #define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */ -#define ID_A_WHO_AM_I 'H' -#define ID_B_WHO_AM_I '4' -#define ID_C_WHO_AM_I '3' - +enum HMC5883_BUS { + HMC5883_BUS_ALL, + HMC5883_BUS_INTERNAL, + HMC5883_BUS_EXTERNAL +}; /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -130,10 +129,10 @@ static const int ERROR = -1; # error This requires CONFIG_SCHED_WORKQUEUE. #endif -class HMC5883 : public device::I2C +class HMC5883 : public device::CDev { public: - HMC5883(int bus, const char *path, enum Rotation rotation); + HMC5883(device::Device *interface, const char *path, enum Rotation rotation); virtual ~HMC5883(); virtual int init(); @@ -147,7 +146,7 @@ public: void print_info(); protected: - virtual int probe(); + Device *_interface; private: work_s _work; @@ -174,7 +173,6 @@ private: bool _sensor_ok; /**< sensor was found and reports ok */ bool _calibrated; /**< the calibration is valid */ - int _bus; /**< the bus the device is connected to */ enum Rotation _rotation; struct mag_report _last_report; /**< used for info() */ @@ -182,15 +180,6 @@ private: uint8_t _range_bits; uint8_t _conf_reg; - /** - * Test whether the device supported by the driver is present at a - * specific address. - * - * @param address The I2C bus address to probe. - * @return True if the device is present. - */ - int probe_address(uint8_t address); - /** * Initialise the automatic measurement state machine and start it. * @@ -349,8 +338,9 @@ private: extern "C" __EXPORT int hmc5883_main(int argc, char *argv[]); -HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : - I2C("HMC5883", path, bus, HMC5883L_ADDRESS, 400000), +HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rotation) : + CDev("HMC5883", path), + _interface(interface), _work{}, _measure_ticks(0), _reports(nullptr), @@ -369,7 +359,6 @@ HMC5883::HMC5883(int bus, const char *path, enum Rotation rotation) : _conf_errors(perf_alloc(PC_COUNT, "hmc5883_conf_errors")), _sensor_ok(false), _calibrated(false), - _bus(bus), _rotation(rotation), _last_report{0}, _range_bits(0), @@ -416,9 +405,11 @@ HMC5883::init() { int ret = ERROR; - /* do I2C init (and probe) first */ - if (I2C::init() != OK) + ret = CDev::init(); + if (ret != OK) { + debug("CDev init failed"); goto out; + } /* allocate basic report buffers */ _reports = new RingBuffer(2, sizeof(mag_report)); @@ -429,20 +420,7 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _mag_orb_id = ORB_ID(sensor_mag0); - break; - - case CLASS_DEVICE_SECONDARY: - _mag_orb_id = ORB_ID(sensor_mag1); - break; - - case CLASS_DEVICE_TERTIARY: - _mag_orb_id = ORB_ID(sensor_mag2); - break; - } + _mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance); ret = OK; /* sensor is ok, but not calibrated */ @@ -559,30 +537,6 @@ void HMC5883::check_conf(void) } } -int -HMC5883::probe() -{ - uint8_t data[3] = {0, 0, 0}; - - _retries = 10; - - if (read_reg(ADDR_ID_A, data[0]) || - read_reg(ADDR_ID_B, data[1]) || - read_reg(ADDR_ID_C, data[2])) - debug("read_reg fail"); - - _retries = 2; - - if ((data[0] != ID_A_WHO_AM_I) || - (data[1] != ID_B_WHO_AM_I) || - (data[2] != ID_C_WHO_AM_I)) { - debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); - return -EIO; - } - - return OK; -} - ssize_t HMC5883::read(struct file *filp, char *buffer, size_t buflen) { @@ -643,6 +597,8 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen) int HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) { + unsigned dummy = arg; + switch (cmd) { case SENSORIOCSPOLLRATE: { switch (arg) { @@ -768,14 +724,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return check_calibration(); case MAGIOCGEXTERNAL: - if (_bus == PX4_I2C_BUS_EXPANSION) - return 1; - else - return 0; + debug("MAGIOCGEXTERNAL in main driver"); + return _interface->ioctl(cmd, dummy); default: /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); + return CDev::ioctl(filp, cmd, arg); } } @@ -890,7 +844,6 @@ HMC5883::collect() } report; int ret; - uint8_t cmd; uint8_t check_counter; perf_begin(_sample_perf); @@ -908,8 +861,7 @@ HMC5883::collect() */ /* get measurements from the device */ - cmd = ADDR_DATA_OUT_X_MSB; - ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); + ret = _interface->read(ADDR_DATA_OUT_X_MSB, (uint8_t *)&hmc_report, sizeof(hmc_report)); if (ret != OK) { perf_count(_comms_errors); @@ -946,14 +898,14 @@ HMC5883::collect() /* scale values for output */ -#ifdef PX4_I2C_BUS_ONBOARD - if (_bus == PX4_I2C_BUS_ONBOARD) { + // XXX revisit for SPI part, might require a bus type IOCTL + unsigned dummy; + if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; report.x = -report.x; } -#endif /* the standard external mag by 3DR has x pointing to the * right, y pointing backwards, and z down, therefore switch x @@ -1291,15 +1243,17 @@ int HMC5883::set_excitement(unsigned enable) int HMC5883::write_reg(uint8_t reg, uint8_t val) { - uint8_t cmd[] = { reg, val }; - - return transfer(&cmd[0], 2, nullptr, 0); + uint8_t buf = val; + return _interface->write(reg, &buf, 1); } int HMC5883::read_reg(uint8_t reg, uint8_t &val) { - return transfer(®, 1, &val, 1); + uint8_t buf = val; + int ret = _interface->read(reg, &buf, 1); + val = buf; + return ret; } float @@ -1351,6 +1305,7 @@ void test(int bus); void reset(int bus); int info(int bus); int calibrate(int bus); +const char* get_path(int bus); void usage(); /** @@ -1360,43 +1315,87 @@ void usage(); * is either successfully up and running or failed to start. */ void -start(int bus, enum Rotation rotation) +start(int external_bus, enum Rotation rotation) { int fd; /* create the driver, attempt expansion bus first */ - if (bus == -1 || bus == PX4_I2C_BUS_EXPANSION) { - if (g_dev_ext != nullptr) + if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { + if (g_dev_ext != nullptr) { errx(0, "already started external"); - g_dev_ext = new HMC5883(PX4_I2C_BUS_EXPANSION, HMC5883L_DEVICE_PATH_EXT, rotation); - if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { - delete g_dev_ext; - g_dev_ext = nullptr; + } + + device::Device *interface = nullptr; + + /* create the driver, only attempt I2C for the external bus */ + if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { + interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION); + } + + if (interface == nullptr) { + warnx("failed to allocate an interface"); + } + + if (interface->init() != OK) { + delete interface; + warnx("interface init failed"); + } else { + + g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation); + if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { + delete g_dev_ext; + g_dev_ext = nullptr; + } + } } -#ifdef PX4_I2C_BUS_ONBOARD /* if this failed, attempt onboard sensor */ - if (bus == -1 || bus == PX4_I2C_BUS_ONBOARD) { - if (g_dev_int != nullptr) + if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { + if (g_dev_int != nullptr) { errx(0, "already started internal"); - g_dev_int = new HMC5883(PX4_I2C_BUS_ONBOARD, HMC5883L_DEVICE_PATH_INT, rotation); - if (g_dev_int != nullptr && OK != g_dev_int->init()) { + } + + device::Device *interface = nullptr; + + /* create the driver, try SPI first, fall back to I2C if unsuccessful */ +#ifdef PX4_SPIDEV_HMC + if (HMC5883_SPI_interface != nullptr) { + interface = HMC5883_SPI_interface(PX4_SPI_BUS_SENSORS); + } +#endif + +#ifdef PX4_I2C_BUS_ONBOARD + if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { + interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); + } +#endif + if (interface == nullptr) { + warnx("failed to allocate an interface"); + } - /* tear down the failing onboard instance */ - delete g_dev_int; - g_dev_int = nullptr; + if (interface->init() != OK) { + delete interface; + warnx("interface init failed"); + } else { + + g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation); + if (g_dev_int != nullptr && OK != g_dev_int->init()) { - if (bus == PX4_I2C_BUS_ONBOARD) { + /* tear down the failing onboard instance */ + delete g_dev_int; + g_dev_int = nullptr; + + if (external_bus == HMC5883_BUS_INTERNAL) { + goto fail; + } + } + if (g_dev_int == nullptr && external_bus == HMC5883_BUS_INTERNAL) { goto fail; } } - if (g_dev_int == nullptr && bus == PX4_I2C_BUS_ONBOARD) { - goto fail; - } } -#endif if (g_dev_int == nullptr && g_dev_ext == nullptr) goto fail; @@ -1425,11 +1424,11 @@ start(int bus, enum Rotation rotation) exit(0); fail: - if (g_dev_int != nullptr && (bus == -1 || bus == PX4_I2C_BUS_ONBOARD)) { + if (g_dev_int != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL)) { delete g_dev_int; g_dev_int = nullptr; } - if (g_dev_ext != nullptr && (bus == -1 || bus == PX4_I2C_BUS_EXPANSION)) { + if (g_dev_ext != nullptr && (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL)) { delete g_dev_ext; g_dev_ext = nullptr; } @@ -1448,7 +1447,7 @@ test(int bus) struct mag_report report; ssize_t sz; int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1549,7 +1548,7 @@ test(int bus) int calibrate(int bus) { int ret; - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1576,7 +1575,7 @@ int calibrate(int bus) void reset(int bus) { - const char *path = (bus==PX4_I2C_BUS_ONBOARD?HMC5883L_DEVICE_PATH_INT:HMC5883L_DEVICE_PATH_EXT); + const char *path = get_path(bus); int fd = open(path, O_RDONLY); @@ -1600,12 +1599,12 @@ info(int bus) { int ret = 1; - HMC5883 *g_dev = (bus == PX4_I2C_BUS_ONBOARD ? g_dev_int : g_dev_ext); + HMC5883 *g_dev = (bus == HMC5883_BUS_INTERNAL ? g_dev_int : g_dev_ext); if (g_dev == nullptr) { warnx("not running on bus %d", bus); } else { - warnx("running on bus: %d (%s)\n", bus, ((PX4_I2C_BUS_ONBOARD) ? "onboard" : "offboard")); + warnx("running on bus: %d (%s)\n", bus, ((HMC5883_BUS_INTERNAL) ? "onboard" : "offboard")); g_dev->print_info(); ret = 0; @@ -1614,6 +1613,12 @@ info(int bus) return ret; } +const char* +get_path(int bus) +{ + return ((bus == HMC5883_BUS_INTERNAL) ? HMC5883L_DEVICE_PATH_INT : HMC5883L_DEVICE_PATH_EXT); +} + void usage() { @@ -1622,7 +1627,7 @@ usage() warnx(" -R rotation"); warnx(" -C calibrate on start"); warnx(" -X only external bus"); -#ifdef PX4_I2C_BUS_ONBOARD +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) warnx(" -I only internal bus"); #endif } @@ -1633,7 +1638,7 @@ int hmc5883_main(int argc, char *argv[]) { int ch; - int bus = -1; + int bus = HMC5883_BUS_ALL; enum Rotation rotation = ROTATION_NONE; bool calibrate = false; @@ -1642,13 +1647,13 @@ hmc5883_main(int argc, char *argv[]) case 'R': rotation = (enum Rotation)atoi(optarg); break; -#ifdef PX4_I2C_BUS_ONBOARD +#if (PX4_I2C_BUS_ONBOARD || PX4_SPIDEV_HMC) case 'I': - bus = PX4_I2C_BUS_ONBOARD; + bus = HMC5883_BUS_INTERNAL; break; #endif case 'X': - bus = PX4_I2C_BUS_EXPANSION; + bus = HMC5883_BUS_EXTERNAL; break; case 'C': calibrate = true; @@ -1692,13 +1697,13 @@ hmc5883_main(int argc, char *argv[]) * Print driver information. */ if (!strcmp(verb, "info") || !strcmp(verb, "status")) { - if (bus == -1) { + if (bus == HMC5883_BUS_ALL) { int ret = 0; - if (hmc5883::info(PX4_I2C_BUS_ONBOARD)) { + if (hmc5883::info(HMC5883_BUS_INTERNAL)) { ret = 1; } - if (hmc5883::info(PX4_I2C_BUS_EXPANSION)) { + if (hmc5883::info(HMC5883_BUS_EXTERNAL)) { ret = 1; } exit(ret); diff --git a/src/drivers/hmc5883/hmc5883.h b/src/drivers/hmc5883/hmc5883.h new file mode 100644 index 000000000..0eb773629 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883.h @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 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 + * 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 hmc5883.h + * + * Shared defines for the hmc5883 driver. + */ + +#pragma once + +#define ADDR_ID_A 0x0a +#define ADDR_ID_B 0x0b +#define ADDR_ID_C 0x0c + +#define ID_A_WHO_AM_I 'H' +#define ID_B_WHO_AM_I '4' +#define ID_C_WHO_AM_I '3' + +/* interface factories */ +extern device::Device *HMC5883_SPI_interface(int bus) weak_function; +extern device::Device *HMC5883_I2C_interface(int bus) weak_function; diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp new file mode 100644 index 000000000..2d3e8fa08 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -0,0 +1,175 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 + * 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 HMC5883_I2C.cpp + * + * I2C interface for HMC5883 / HMC 5983 + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "hmc5883.h" + +#include "board_config.h" + +#ifdef PX4_I2C_OBDEV_HMC5883 + +#define HMC5883L_ADDRESS PX4_I2C_OBDEV_HMC5883 + +device::Device *HMC5883_I2C_interface(int bus); + +class HMC5883_I2C : public device::I2C +{ +public: + HMC5883_I2C(int bus); + virtual ~HMC5883_I2C(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +protected: + virtual int probe(); + +}; + +device::Device * +HMC5883_I2C_interface(int bus) +{ + return new HMC5883_I2C(bus); +} + +HMC5883_I2C::HMC5883_I2C(int bus) : + I2C("HMC5883_I2C", nullptr, bus, 0, 400000) +{ +} + +HMC5883_I2C::~HMC5883_I2C() +{ +} + +int +HMC5883_I2C::init() +{ + /* this will call probe() */ + return I2C::init(); +} + +int +HMC5883_I2C::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + if (_bus == PX4_I2C_BUS_EXPANSION) { + return 1; + } else { + return 0; + } + + default: + ret = -EINVAL; + } + + return ret; +} + +int +HMC5883_I2C::probe() +{ + uint8_t data[3] = {0, 0, 0}; + + _retries = 10; + + if (read(ADDR_ID_A, &data[0], 1) || + read(ADDR_ID_B, &data[1], 1) || + read(ADDR_ID_C, &data[2], 1)) { + debug("read_reg fail"); + return -EIO; + } + + _retries = 2; + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I) || + (data[2] != ID_C_WHO_AM_I)) { + debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + return -EIO; + } + + return OK; +} + +int +HMC5883_I2C::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], count + 1, nullptr, 0); +} + +int +HMC5883_I2C::read(unsigned address, void *data, unsigned count) +{ + uint8_t cmd = address; + return transfer(&cmd, 1, (uint8_t *)data, count); +} + +#endif /* PX4_I2C_OBDEV_HMC5883 */ diff --git a/src/drivers/hmc5883/hmc5883_spi.cpp b/src/drivers/hmc5883/hmc5883_spi.cpp new file mode 100644 index 000000000..25a2f2b40 --- /dev/null +++ b/src/drivers/hmc5883/hmc5883_spi.cpp @@ -0,0 +1,189 @@ +/**************************************************************************** + * + * Copyright (c) 2013-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 + * 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 HMC5883_SPI.cpp + * + * SPI interface for HMC5983 + */ + +/* XXX trim includes */ +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include "hmc5883.h" +#include + +#ifdef PX4_SPIDEV_HMC + +/* SPI protocol address bits */ +#define DIR_READ (1<<7) +#define DIR_WRITE (0<<7) +#define ADDR_INCREMENT (1<<6) + +#define HMC_MAX_SEND_LEN 4 +#define HMC_MAX_RCV_LEN 8 + +device::Device *HMC5883_SPI_interface(int bus); + +class HMC5883_SPI : public device::SPI +{ +public: + HMC5883_SPI(int bus, spi_dev_e device); + virtual ~HMC5883_SPI(); + + virtual int init(); + virtual int read(unsigned address, void *data, unsigned count); + virtual int write(unsigned address, void *data, unsigned count); + + virtual int ioctl(unsigned operation, unsigned &arg); + +}; + +device::Device * +HMC5883_SPI_interface(int bus) +{ + return new HMC5883_SPI(bus, (spi_dev_e)PX4_SPIDEV_HMC); +} + +HMC5883_SPI::HMC5883_SPI(int bus, spi_dev_e device) : + SPI("HMC5883_SPI", nullptr, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz */) +{ +} + +HMC5883_SPI::~HMC5883_SPI() +{ +} + +int +HMC5883_SPI::init() +{ + int ret; + + ret = SPI::init(); + if (ret != OK) { + debug("SPI init failed"); + return -EIO; + } + + // read WHO_AM_I value + uint8_t data[3] = {0, 0, 0}; + + if (read(ADDR_ID_A, &data[0], 1) || + read(ADDR_ID_B, &data[1], 1) || + read(ADDR_ID_C, &data[2], 1)) { + debug("read_reg fail"); + } + + if ((data[0] != ID_A_WHO_AM_I) || + (data[1] != ID_B_WHO_AM_I) || + (data[2] != ID_C_WHO_AM_I)) { + debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); + return -EIO; + } + + return OK; +} + +int +HMC5883_SPI::ioctl(unsigned operation, unsigned &arg) +{ + int ret; + + switch (operation) { + + case MAGIOCGEXTERNAL: + +#ifdef PX4_SPI_BUS_EXT + if (_bus == PX4_SPI_BUS_EXT) { + return 1; + } else +#endif + { + return 0; + } + + default: + { + ret = -EINVAL; + } + } + + return ret; +} + +int +HMC5883_SPI::write(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_WRITE; + memcpy(&buf[1], data, count); + + return transfer(&buf[0], &buf[0], count + 1); +} + +int +HMC5883_SPI::read(unsigned address, void *data, unsigned count) +{ + uint8_t buf[32]; + + if (sizeof(buf) < (count + 1)) { + return -EIO; + } + + buf[0] = address | DIR_READ | ADDR_INCREMENT; + + int ret = transfer(&buf[0], &buf[0], count + 1); + memcpy(data, &buf[1], count); + return ret; +} + +#endif /* PX4_SPIDEV_HMC */ diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk index be2ee7276..d4028b511 100644 --- a/src/drivers/hmc5883/module.mk +++ b/src/drivers/hmc5883/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 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 @@ -37,7 +37,7 @@ MODULE_COMMAND = hmc5883 -SRCS = hmc5883.cpp +SRCS = hmc5883_i2c.cpp hmc5883_spi.cpp hmc5883.cpp MODULE_STACKSIZE = 1200 -- cgit v1.2.3 From f3e3565d1ba827f20c8cc724a56c8e429b0cc7cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 2 Jan 2015 15:23:40 +0100 Subject: sensors app: Decent error handling / reporting --- src/modules/sensors/sensors.cpp | 101 +++++++++++++++++++++++++++------------- 1 file changed, 68 insertions(+), 33 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 018eadaff..2c798af3b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -401,27 +401,27 @@ private: /** * Do accel-related initialisation. */ - void accel_init(); + int accel_init(); /** * Do gyro-related initialisation. */ - void gyro_init(); + int gyro_init(); /** * Do mag-related initialisation. */ - void mag_init(); + int mag_init(); /** * Do baro-related initialisation. */ - void baro_init(); + int baro_init(); /** * Do adc-related initialisation. */ - void adc_init(); + int adc_init(); /** * Poll the accelerometer for updated data. @@ -507,7 +507,7 @@ Sensors::Sensors() : _fd_adc(-1), _last_adc(0), - _task_should_exit(false), + _task_should_exit(true), _sensors_task(-1), _hil_enabled(false), _publishing(true), @@ -913,8 +913,8 @@ Sensors::parameters_update() int barofd; barofd = open(BARO_DEVICE_PATH, 0); if (barofd < 0) { - warn("%s", BARO_DEVICE_PATH); - errx(1, "FATAL: no barometer found"); + warnx("ERROR: no barometer foundon %s", BARO_DEVICE_PATH); + return ERROR; } else { int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); @@ -929,7 +929,7 @@ Sensors::parameters_update() return OK; } -void +int Sensors::accel_init() { int fd; @@ -937,8 +937,8 @@ Sensors::accel_init() fd = open(ACCEL_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", ACCEL_DEVICE_PATH); - errx(1, "FATAL: no accelerometer found"); + warnx("FATAL: no accelerometer found: %s", ACCEL_DEVICE_PATH); + return ERROR; } else { @@ -967,9 +967,11 @@ Sensors::accel_init() close(fd); } + + return OK; } -void +int Sensors::gyro_init() { int fd; @@ -977,8 +979,8 @@ Sensors::gyro_init() fd = open(GYRO_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", GYRO_DEVICE_PATH); - errx(1, "FATAL: no gyro found"); + warnx("FATAL: no gyro found: %s", GYRO_DEVICE_PATH); + return ERROR; } else { @@ -1008,9 +1010,11 @@ Sensors::gyro_init() close(fd); } + + return OK; } -void +int Sensors::mag_init() { int fd; @@ -1019,8 +1023,8 @@ Sensors::mag_init() fd = open(MAG_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", MAG_DEVICE_PATH); - errx(1, "FATAL: no magnetometer found"); + warnx("FATAL: no magnetometer found: %s", MAG_DEVICE_PATH); + return ERROR; } /* try different mag sampling rates */ @@ -1041,7 +1045,8 @@ Sensors::mag_init() ioctl(fd, SENSORIOCSPOLLRATE, 100); } else { - errx(1, "FATAL: mag sampling rate could not be set"); + warnx("FATAL: mag sampling rate could not be set"); + return ERROR; } } @@ -1050,7 +1055,8 @@ Sensors::mag_init() ret = ioctl(fd, MAGIOCGEXTERNAL, 0); if (ret < 0) { - errx(1, "FATAL: unknown if magnetometer is external or onboard"); + warnx("FATAL: unknown if magnetometer is external or onboard"); + return ERROR; } else if (ret == 1) { _mag_is_external = true; @@ -1060,9 +1066,11 @@ Sensors::mag_init() } close(fd); + + return OK; } -void +int Sensors::baro_init() { int fd; @@ -1070,26 +1078,30 @@ Sensors::baro_init() fd = open(BARO_DEVICE_PATH, 0); if (fd < 0) { - warn("%s", BARO_DEVICE_PATH); - errx(1, "FATAL: No barometer found"); + warnx("FATAL: No barometer found: %s", BARO_DEVICE_PATH); + return ERROR; } /* set the driver to poll at 150Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 150); close(fd); + + return OK; } -void +int Sensors::adc_init() { _fd_adc = open(ADC_DEVICE_PATH, O_RDONLY | O_NONBLOCK); if (_fd_adc < 0) { - warn(ADC_DEVICE_PATH); - warnx("FATAL: no ADC found"); + warnx("FATAL: no ADC found: %s", ADC_DEVICE_PATH); + return ERROR; } + + return OK; } void @@ -1906,11 +1918,27 @@ Sensors::task_main() { /* start individual sensors */ - accel_init(); - gyro_init(); - mag_init(); - baro_init(); - adc_init(); + int ret; + ret = accel_init(); + if (ret) { + goto exit_immediate; + } + ret = gyro_init(); + if (ret) { + goto exit_immediate; + } + ret = mag_init(); + if (ret) { + goto exit_immediate; + } + ret = baro_init(); + if (ret) { + goto exit_immediate; + } + ret = adc_init(); + if (ret) { + goto exit_immediate; + } /* * do subscriptions @@ -1976,6 +2004,8 @@ Sensors::task_main() fds[0].fd = _gyro_sub; fds[0].events = POLLIN; + _task_should_exit = false; + while (!_task_should_exit) { /* wait for up to 50ms for data */ @@ -2026,8 +2056,9 @@ Sensors::task_main() warnx("[sensors] exiting."); +exit_immediate: _sensors_task = -1; - _exit(0); + _exit(ret); } int @@ -2043,9 +2074,13 @@ Sensors::start() (main_t)&Sensors::task_main_trampoline, nullptr); + /* wait until the task is up and running or has failed */ + while(_sensors_task > 0 && _task_should_exit) { + usleep(100); + } + if (_sensors_task < 0) { - warn("task start failed"); - return -errno; + return -ERROR; } return OK; -- cgit v1.2.3 From 268cf7efc59057119d261838157e17d6c2d318f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jan 2015 17:45:25 +0100 Subject: HMC5883: Fix I2C operation --- src/drivers/hmc5883/hmc5883.cpp | 30 ++++++++++++++++++++++-------- src/drivers/hmc5883/hmc5883_i2c.cpp | 2 +- 2 files changed, 23 insertions(+), 9 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5989703dd..80b2c5f13 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1330,16 +1330,28 @@ start(int external_bus, enum Rotation rotation) /* create the driver, only attempt I2C for the external bus */ if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { interface = HMC5883_I2C_interface(PX4_I2C_BUS_EXPANSION); + + if (interface->init() != OK) { + delete interface; + interface = nullptr; + warnx("no device on I2C bus #%u", PX4_I2C_BUS_EXPANSION); + } } - if (interface == nullptr) { - warnx("failed to allocate an interface"); +#ifdef PX4_I2C_BUS_ONBOARD + if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { + interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); + + if (interface->init() != OK) { + delete interface; + interface = nullptr; + warnx("no device on I2C bus #%u", PX4_I2C_BUS_ONBOARD); + } } +#endif - if (interface->init() != OK) { - delete interface; - warnx("interface init failed"); - } else { + /* interface will be null if init failed */ + if (interface != nullptr) { g_dev_ext = new HMC5883(interface, HMC5883L_DEVICE_PATH_EXT, rotation); if (g_dev_ext != nullptr && OK != g_dev_ext->init()) { @@ -1367,17 +1379,19 @@ start(int external_bus, enum Rotation rotation) #endif #ifdef PX4_I2C_BUS_ONBOARD + /* this device is already connected as external if present above */ if (interface == nullptr && (HMC5883_I2C_interface != nullptr)) { interface = HMC5883_I2C_interface(PX4_I2C_BUS_ONBOARD); } #endif if (interface == nullptr) { - warnx("failed to allocate an interface"); + warnx("no internal bus scanned"); + goto fail; } if (interface->init() != OK) { delete interface; - warnx("interface init failed"); + warnx("no device on internal bus"); } else { g_dev_int = new HMC5883(interface, HMC5883L_DEVICE_PATH_INT, rotation); diff --git a/src/drivers/hmc5883/hmc5883_i2c.cpp b/src/drivers/hmc5883/hmc5883_i2c.cpp index 2d3e8fa08..782ea62fe 100644 --- a/src/drivers/hmc5883/hmc5883_i2c.cpp +++ b/src/drivers/hmc5883/hmc5883_i2c.cpp @@ -88,7 +88,7 @@ HMC5883_I2C_interface(int bus) } HMC5883_I2C::HMC5883_I2C(int bus) : - I2C("HMC5883_I2C", nullptr, bus, 0, 400000) + I2C("HMC5883_I2C", nullptr, bus, HMC5883L_ADDRESS, 400000) { } -- cgit v1.2.3 From f99c29c50cbf4ed73b8fb05624726598b8ffec5b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jan 2015 17:47:41 +0100 Subject: Dev info: Reflect address changes --- src/drivers/device/i2c.h | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/drivers/device/i2c.h b/src/drivers/device/i2c.h index 206e71ddc..8518596ea 100644 --- a/src/drivers/device/i2c.h +++ b/src/drivers/device/i2c.h @@ -132,6 +132,7 @@ protected: */ void set_address(uint16_t address) { _address = address; + _device_id.devid_s.address = _address; } private: -- cgit v1.2.3 From dc7ee4247f3cfee6647876ba1873e0508e29ae00 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jan 2015 17:54:22 +0100 Subject: HMC5883 startup: Ensure sensor configuration is always performed --- src/drivers/hmc5883/hmc5883.cpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 80b2c5f13..fe70bd37f 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1320,10 +1320,9 @@ start(int external_bus, enum Rotation rotation) int fd; /* create the driver, attempt expansion bus first */ - if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { - if (g_dev_ext != nullptr) { - errx(0, "already started external"); - } + if (g_dev_ext != nullptr) { + warnx("already started external"); + } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_EXTERNAL) { device::Device *interface = nullptr; @@ -1364,10 +1363,9 @@ start(int external_bus, enum Rotation rotation) /* if this failed, attempt onboard sensor */ - if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { - if (g_dev_int != nullptr) { - errx(0, "already started internal"); - } + if (g_dev_int != nullptr) { + warnx("already started internal"); + } else if (external_bus == HMC5883_BUS_ALL || external_bus == HMC5883_BUS_INTERNAL) { device::Device *interface = nullptr; -- cgit v1.2.3 From 2a95b4a9b89324354967c7a45974a534142efec0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Jan 2015 19:05:42 +0100 Subject: GPS: Update the RTC even when RTC is enabled --- src/drivers/gps/ubx.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b0eb4ab66..91ecdb474 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -808,7 +808,7 @@ UBX::payload_rx_done(void) UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); { - /* convert to unix timestamp */ + // convert to unix timestamp struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; timeinfo.tm_mon = _buf.payload_rx_nav_timeutc.month - 1; @@ -818,14 +818,15 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; time_t epoch = mktime(&timeinfo); -#ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? + // FMUv2+ have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + // TODO generalize this by moving into gps.cpp? timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; clock_settime(CLOCK_REALTIME, &ts); -#endif _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f); -- cgit v1.2.3 From 48a8ea7f199d44f02621767cdde1f50ae228b31e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 00:07:42 +0100 Subject: GPS driver: Fixed implicit cast causing time logic errors. Found by @Zefz --- src/drivers/gps/ubx.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 91ecdb474..5db3ced7c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -828,8 +828,8 @@ UBX::payload_rx_done(void) ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; clock_settime(CLOCK_REALTIME, &ts); - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_timeutc.nano * 1e-3f); + _gps_position->time_gps_usec = ((uint64_t)epoch) * 1000000ULL; + _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } _gps_position->timestamp_time = hrt_absolute_time(); -- cgit v1.2.3 From c1f89dbd5c9de5f1bbb1bc0f858911a9f06d6f9d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 00:08:04 +0100 Subject: GPS driver: Check return value of settime and notify shell if call fails --- src/drivers/gps/ubx.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 5db3ced7c..4563fb65c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -826,7 +826,9 @@ UBX::payload_rx_done(void) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; - clock_settime(CLOCK_REALTIME, &ts); + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } _gps_position->time_gps_usec = ((uint64_t)epoch) * 1000000ULL; _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; -- cgit v1.2.3 From a62baf2cb210535e1e7682a860adb5156233f5f0 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 3 Jan 2015 18:48:43 -0500 Subject: add simple nonsymmetric Matrix testing to test_mathlib --- src/systemcmds/tests/module.mk | 2 +- src/systemcmds/tests/test_mathlib.cpp | 51 ++++++++++++++++++++++++++++++++++- 2 files changed, 51 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 6eed3922c..0dc333f0a 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -35,5 +35,5 @@ SRCS = test_adc.c \ test_mount.c \ test_mtd.c -EXTRACXXFLAGS = -Wframe-larger-than=2500 +EXTRACXXFLAGS = -Wframe-larger-than=2500 -Wno-float-equal -Wno-double-promotion diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 70d173fc9..9fa52aaaa 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -54,6 +54,7 @@ using namespace math; int test_mathlib(int argc, char *argv[]) { + int rc = 0; warnx("testing mathlib"); { @@ -156,5 +157,53 @@ int test_mathlib(int argc, char *argv[]) TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2); } - return 0; + { + // test nonsymmetric +, -, +=, -= + + float data1[2][3] = {{1,2,3},{4,5,6}}; + float data2[2][3] = {{2,4,6},{8,10,12}}; + float data3[2][3] = {{3,6,9},{12,15,18}}; + + Matrix<2, 3> m1(data1); + Matrix<2, 3> m2(data2); + Matrix<2, 3> m3(data3); + + if (m1 + m2 != m3) { + warnx("Matrix<2, 3> + Matrix<2, 3> failed!"); + (m1 + m2).print(); + printf("!=\n"); + m3.print(); + rc = 1; + } + + if (m3 - m2 != m1) { + warnx("Matrix<2, 3> - Matrix<2, 3> failed!"); + (m3 - m2).print(); + printf("!=\n"); + m1.print(); + rc = 1; + } + + m1 += m2; + if (m1 != m3) { + warnx("Matrix<2, 3> += Matrix<2, 3> failed!"); + m1.print(); + printf("!=\n"); + m3.print(); + rc = 1; + } + + m1 -= m2; + Matrix<2, 3> m1_orig(data1); + if (m1 != m1_orig) { + warnx("Matrix<2, 3> -= Matrix<2, 3> failed!"); + m1.print(); + printf("!=\n"); + m1_orig.print(); + rc = 1; + } + + } + + return rc; } -- cgit v1.2.3 From 50a00bee8ed56d41d1c3e74eaaa7997191793595 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 10:44:40 +0100 Subject: GPS driver: UBX time handling for all protocol revisions --- src/drivers/gps/ubx.cpp | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 4563fb65c..c67965487 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -748,17 +748,19 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; time_t epoch = mktime(&timeinfo); -#ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; - clock_settime(CLOCK_REALTIME, &ts); -#endif + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(_buf.payload_rx_nav_pvt.nano * 1e-3f); + _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } _gps_position->timestamp_time = hrt_absolute_time(); @@ -818,11 +820,10 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; time_t epoch = mktime(&timeinfo); - // FMUv2+ have a hardware RTC, but GPS helps us to configure it + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it // and control its drift. Since we rely on the HRT for our monotonic // clock, updating it from time to time is safe. - // TODO generalize this by moving into gps.cpp? timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; @@ -830,7 +831,7 @@ UBX::payload_rx_done(void) warn("failed setting clock"); } - _gps_position->time_gps_usec = ((uint64_t)epoch) * 1000000ULL; + _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_gps_usec += _buf.payload_rx_nav_timeutc.nano / 1000; } -- cgit v1.2.3 From f48abafbc9b9b5f3683fe0c86792f70c0f32bd8e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 10:45:00 +0100 Subject: GPS driver: Set RTC from Ashtech receivers as well --- src/drivers/gps/ashtech.cpp | 17 +++++++++++++++-- 1 file changed, 15 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index a2e292de2..c2381b8dc 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -99,8 +99,21 @@ int ASHTECH::handle_message(int len) timeinfo.tm_sec = int(ashtech_sec); time_t epoch = mktime(&timeinfo); - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)((ashtech_sec - int(ashtech_sec)) * 1e6); + uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1e6; + + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_gps_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_gps_usec += usecs; _gps_position->timestamp_time = hrt_absolute_time(); } -- cgit v1.2.3 From 17e4e283d86ad07b55c4f2ceaedf801adcfb1a53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 10:58:45 +0100 Subject: SMBus battery driver: Stylize according to style guide --- src/drivers/batt_smbus/batt_smbus.cpp | 119 +++++++++++++++++++--------------- 1 file changed, 66 insertions(+), 53 deletions(-) (limited to 'src') diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index dd83dacaf..e80c0660e 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -1,7 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Randy Mackay + * 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 @@ -37,6 +36,7 @@ * * Driver for a battery monitor connected via SMBus (I2C). * + * @author Randy Mackay */ #include @@ -77,22 +77,22 @@ #include #include -#define BATT_SMBUS_ADDR_MIN 0x08 /* lowest possible address */ -#define BATT_SMBUS_ADDR_MAX 0x7F /* highest possible address */ +#define BATT_SMBUS_ADDR_MIN 0x08 ///< lowest possible address +#define BATT_SMBUS_ADDR_MAX 0x7F ///< highest possible address -#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION -#define BATT_SMBUS_ADDR 0x0B /* I2C address */ -#define BATT_SMBUS_TEMP 0x08 /* temperature register */ -#define BATT_SMBUS_VOLTAGE 0x09 /* voltage register */ -#define BATT_SMBUS_DESIGN_CAPACITY 0x18 /* design capacity register */ -#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 /* design voltage register */ -#define BATT_SMBUS_SERIALNUM 0x1c /* serial number register */ -#define BATT_SMBUS_MANUFACTURE_NAME 0x20 /* manufacturer name */ -#define BATT_SMBUS_MANUFACTURE_INFO 0x25 /* cell voltage register */ -#define BATT_SMBUS_CURRENT 0x2a /* current register */ -#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) /* time in microseconds, measure at 10hz */ +#define BATT_SMBUS_I2C_BUS PX4_I2C_BUS_EXPANSION +#define BATT_SMBUS_ADDR 0x0B ///< I2C address +#define BATT_SMBUS_TEMP 0x08 ///< temperature register +#define BATT_SMBUS_VOLTAGE 0x09 ///< voltage register +#define BATT_SMBUS_DESIGN_CAPACITY 0x18 ///< design capacity register +#define BATT_SMBUS_DESIGN_VOLTAGE 0x19 ///< design voltage register +#define BATT_SMBUS_SERIALNUM 0x1c ///< serial number register +#define BATT_SMBUS_MANUFACTURE_NAME 0x20 ///< manufacturer name +#define BATT_SMBUS_MANUFACTURE_INFO 0x25 ///< cell voltage register +#define BATT_SMBUS_CURRENT 0x2a ///< current register +#define BATT_SMBUS_MEASUREMENT_INTERVAL_MS (1000000 / 10) ///< time in microseconds, measure at 10hz -#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 /* Polynomial for calculating PEC */ +#define BATT_SMBUS_PEC_POLYNOMIAL 0x07 ///< Polynomial for calculating PEC #ifndef CONFIG_SCHED_WORKQUEUE # error This requires CONFIG_SCHED_WORKQUEUE. @@ -106,41 +106,62 @@ public: virtual int init(); virtual int test(); - int search(); /* search all possible slave addresses for a smart battery */ + /** + * Search all possible slave addresses for a smart battery + */ + int search(); protected: - virtual int probe(); // check if the device can be contacted + /** + * Check if the device can be contacted + */ + virtual int probe(); private: - // start periodic reads from the battery + /** + * Start periodic reads from the battery + */ void start(); - // stop periodic reads from the battery + /** + * Stop periodic reads from the battery + */ void stop(); - // static function that is called by worker queue + /** + * static function that is called by worker queue + */ static void cycle_trampoline(void *arg); - // perform a read from the battery + /** + * perform a read from the battery + */ void cycle(); - // read_reg - read a word from specified register + /** + * Read a word from specified register + */ int read_reg(uint8_t reg, uint16_t &val); - // read_block - returns number of characters read if successful, zero if unsuccessful + /** + * Read block from bus + * @return returns number of characters read if successful, zero if unsuccessful + */ uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); - // get_PEC - calculate PEC for a read or write from the battery - // buff is the data that was read or will be written + /** + * Calculate PEC for a read or write from the battery + * @param buff is the data that was read or will be written + */ uint8_t get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const; // internal variables - work_s _work; // work queue for scheduling reads - RingBuffer *_reports; // buffer of recorded voltages, currents - struct battery_status_s _last_report; // last published report, used for test() - orb_advert_t _batt_topic; - orb_id_t _batt_orb_id; + work_s _work; ///< work queue for scheduling reads + RingBuffer *_reports; ///< buffer of recorded voltages, currents + struct battery_status_s _last_report; ///< last published report, used for test() + orb_advert_t _batt_topic; ///< uORB battery topic + orb_id_t _batt_orb_id; ///< uORB battery topic ID }; /* for now, we only support one BATT_SMBUS */ @@ -153,7 +174,6 @@ void batt_smbus_usage(); extern "C" __EXPORT int batt_smbus_main(int argc, char *argv[]); -// constructor BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : I2C("batt_smbus", BATT_SMBUS_DEVICE_PATH, bus, batt_smbus_addr, 400000), _work{}, @@ -165,10 +185,9 @@ BATT_SMBUS::BATT_SMBUS(int bus, uint16_t batt_smbus_addr) : memset(&_work, 0, sizeof(_work)); } -// destructor BATT_SMBUS::~BATT_SMBUS() { - /* make sure we are truly inactive */ + // make sure we are truly inactive stop(); if (_reports != nullptr) { @@ -188,7 +207,7 @@ BATT_SMBUS::init() errx(1,"failed to init I2C"); return ret; } else { - /* allocate basic report buffers */ + // allocate basic report buffers _reports = new RingBuffer(2, sizeof(struct battery_status_s)); if (_reports == nullptr) { ret = ENOTTY; @@ -230,7 +249,6 @@ BATT_SMBUS::test() return OK; } -/* search all possible slave addresses for a smart battery */ int BATT_SMBUS::search() { @@ -265,25 +283,22 @@ BATT_SMBUS::probe() return OK; } -// start periodic reads from the battery void BATT_SMBUS::start() { - /* reset the report ring and state machine */ + // reset the report ring and state machine _reports->flush(); - /* schedule a cycle to start things */ + // schedule a cycle to start things work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, 1); } -// stop periodic reads from the battery void BATT_SMBUS::stop() { work_cancel(HPWORK, &_work); } -// static function that is called by worker queue void BATT_SMBUS::cycle_trampoline(void *arg) { @@ -292,7 +307,6 @@ BATT_SMBUS::cycle_trampoline(void *arg) dev->cycle(); } -// perform a read from the battery void BATT_SMBUS::cycle() { @@ -331,14 +345,14 @@ BATT_SMBUS::cycle() // copy report for test() _last_report = new_report; - /* post a report to the ring */ + // post a report to the ring _reports->force(&new_report); - /* notify anyone waiting for data */ + // notify anyone waiting for data poll_notify(POLLIN); } - /* schedule a fresh cycle call when the measurement is done */ + // schedule a fresh cycle call when the measurement is done work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); } @@ -363,8 +377,8 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) return ret; } -// read_block - returns number of characters read if successful, zero if unsuccessful -uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) +uint8_t +BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) { uint8_t buff[max_len+2]; // buffer to hold results @@ -408,9 +422,8 @@ uint8_t BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool return bufflen; } -// get_PEC - calculate PEC for a read or write from the battery -// buff is the data that was read or will be written -uint8_t BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const +uint8_t +BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len) const { // exit immediately if no data if (len <= 0) { @@ -463,11 +476,11 @@ int batt_smbus_main(int argc, char *argv[]) { int i2cdevice = BATT_SMBUS_I2C_BUS; - int batt_smbusadr = BATT_SMBUS_ADDR; /* 7bit */ + int batt_smbusadr = BATT_SMBUS_ADDR; // 7bit address int ch; - /* jump over start/off/etc and look at options first */ + // jump over start/off/etc and look at options first while ((ch = getopt(argc, argv, "a:b:")) != EOF) { switch (ch) { case 'a': @@ -512,7 +525,7 @@ batt_smbus_main(int argc, char *argv[]) exit(0); } - /* need the driver past this point */ + // need the driver past this point if (g_batt_smbus == nullptr) { warnx("not started"); batt_smbus_usage(); -- cgit v1.2.3 From 1106e0bff3173cf480cd63e006ce7e00c962a812 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 11:02:53 +0100 Subject: SMBus battery driver: further code style fixes --- src/drivers/batt_smbus/batt_smbus.cpp | 78 +++++++++++++++++++++++++---------- 1 file changed, 56 insertions(+), 22 deletions(-) (limited to 'src') diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index e80c0660e..6f9ec6318 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -104,8 +104,22 @@ public: BATT_SMBUS(int bus = PX4_I2C_BUS_EXPANSION, uint16_t batt_smbus_addr = BATT_SMBUS_ADDR); virtual ~BATT_SMBUS(); + /** + * Initialize device + * + * Calls probe() to check for device on bus. + * + * @return 0 on success, error code on failure + */ virtual int init(); + + /** + * Test device + * + * @return 0 on success, error code on failure + */ virtual int test(); + /** * Search all possible slave addresses for a smart battery */ @@ -142,13 +156,13 @@ private: /** * Read a word from specified register */ - int read_reg(uint8_t reg, uint16_t &val); + int read_reg(uint8_t reg, uint16_t &val); /** * Read block from bus * @return returns number of characters read if successful, zero if unsuccessful */ - uint8_t read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero); + uint8_t read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero); /** * Calculate PEC for a read or write from the battery @@ -164,10 +178,9 @@ private: orb_id_t _batt_orb_id; ///< uORB battery topic ID }; -/* for now, we only support one BATT_SMBUS */ namespace { -BATT_SMBUS *g_batt_smbus; +BATT_SMBUS *g_batt_smbus; ///< device handle. For now, we only support one BATT_SMBUS device } void batt_smbus_usage(); @@ -204,13 +217,16 @@ BATT_SMBUS::init() ret = I2C::init(); if (ret != OK) { - errx(1,"failed to init I2C"); + errx(1, "failed to init I2C"); return ret; + } else { // allocate basic report buffers _reports = new RingBuffer(2, sizeof(struct battery_status_s)); + if (_reports == nullptr) { ret = ENOTTY; + } else { // start work queue start(); @@ -236,6 +252,7 @@ BATT_SMBUS::test() // display new info that has arrived from the orb orb_check(sub, &updated); + if (updated) { if (orb_copy(ORB_ID(battery_status), sub, &status) == OK) { warnx("V=%4.2f C=%4.2f", status.voltage_v, status.current_a); @@ -256,12 +273,14 @@ BATT_SMBUS::search() uint16_t tmp; // search through all valid SMBus addresses - for (uint8_t i=BATT_SMBUS_ADDR_MIN; i<=BATT_SMBUS_ADDR_MAX; i++) { + for (uint8_t i = BATT_SMBUS_ADDR_MIN; i <= BATT_SMBUS_ADDR_MAX; i++) { set_address(i); + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { - warnx("battery found at 0x%x",(int)i); + warnx("battery found at 0x%x", (int)i); found_slave = true; } + // short sleep usleep(1); } @@ -269,6 +288,7 @@ BATT_SMBUS::search() // display completion message if (found_slave) { warnx("Done."); + } else { warnx("No smart batteries found."); } @@ -318,6 +338,7 @@ BATT_SMBUS::cycle() // read voltage uint16_t tmp; + if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) { // initialise new_report memset(&new_report, 0, sizeof(new_report)); @@ -328,15 +349,19 @@ BATT_SMBUS::cycle() // read current usleep(1); uint8_t buff[4]; + if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) { - new_report.current_a = (float)((int32_t)((uint32_t)buff[3]<<24 | (uint32_t)buff[2]<<16 | (uint32_t)buff[1]<<8 | (uint32_t)buff[0])) / 1000.0f; + new_report.current_a = (float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 | + (uint32_t)buff[0])) / 1000.0f; } // publish to orb if (_batt_topic != -1) { orb_publish(_batt_orb_id, _batt_topic, &new_report); + } else { _batt_topic = orb_advertise(_batt_orb_id, &new_report); + if (_batt_topic < 0) { errx(1, "ADVERT FAIL"); } @@ -353,7 +378,8 @@ BATT_SMBUS::cycle() } // schedule a fresh cycle call when the measurement is done - work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); + work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this, + USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS)); } int @@ -363,11 +389,14 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) // read from register int ret = transfer(®, 1, buff, 3); + if (ret == OK) { // check PEC uint8_t pec = get_PEC(reg, true, buff, 2); + if (pec == buff[2]) { val = (uint16_t)buff[1] << 8 | (uint16_t)buff[0]; + } else { ret = ENOTTY; } @@ -378,14 +407,14 @@ BATT_SMBUS::read_reg(uint8_t reg, uint16_t &val) } uint8_t -BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_zero) +BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_zero) { - uint8_t buff[max_len+2]; // buffer to hold results + uint8_t buff[max_len + 2]; // buffer to hold results usleep(1); // read bytes including PEC - int ret = transfer(®, 1, buff, max_len+2); + int ret = transfer(®, 1, buff, max_len + 2); // return zero on failure if (ret != OK) { @@ -401,13 +430,15 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t* data, uint8_t max_len, bool append_ } // check PEC - uint8_t pec = get_PEC(reg, true, buff, bufflen+1); - if (pec != buff[bufflen+1]) { + uint8_t pec = get_PEC(reg, true, buff, bufflen + 1); + + if (pec != buff[bufflen + 1]) { // debug - warnx("CurrPEC:%x vs MyPec:%x",(int)buff[bufflen+1],(int)pec); + warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec); return 0; + } else { - warnx("CurPEC ok: %x",(int)pec); + warnx("CurPEC ok: %x", (int)pec); } // copy data @@ -431,11 +462,11 @@ BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len } // prepare temp buffer for calcing crc - uint8_t tmp_buff[len+3]; + uint8_t tmp_buff[len + 3]; tmp_buff[0] = (uint8_t)get_address() << 1; tmp_buff[1] = cmd; tmp_buff[2] = tmp_buff[0] | (uint8_t)reading; - memcpy(&tmp_buff[3],buff,len); + memcpy(&tmp_buff[3], buff, len); // initialise crc to zero uint8_t crc = 0; @@ -443,14 +474,16 @@ BATT_SMBUS::get_PEC(uint8_t cmd, bool reading, const uint8_t buff[], uint8_t len bool do_invert; // for each byte in the stream - for (uint8_t i=0; i Date: Sun, 4 Jan 2015 11:04:55 +0100 Subject: SMBus battery driver: Remove console print on normal block read operation --- src/drivers/batt_smbus/batt_smbus.cpp | 2 -- 1 file changed, 2 deletions(-) (limited to 'src') diff --git a/src/drivers/batt_smbus/batt_smbus.cpp b/src/drivers/batt_smbus/batt_smbus.cpp index 6f9ec6318..2b5fef4d7 100644 --- a/src/drivers/batt_smbus/batt_smbus.cpp +++ b/src/drivers/batt_smbus/batt_smbus.cpp @@ -437,8 +437,6 @@ BATT_SMBUS::read_block(uint8_t reg, uint8_t *data, uint8_t max_len, bool append_ warnx("CurrPEC:%x vs MyPec:%x", (int)buff[bufflen + 1], (int)pec); return 0; - } else { - warnx("CurPEC ok: %x", (int)pec); } // copy data -- cgit v1.2.3 From 4cc7f599d1f29607fae625e26a586d0758f67283 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 10:40:39 +0000 Subject: ashtech: whitespace --- src/drivers/gps/ashtech.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index c2381b8dc..f1c5ca549 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -38,7 +38,7 @@ ASHTECH::~ASHTECH() int ASHTECH::handle_message(int len) { char * endp; - + if (len < 7) { return 0; } int uiCalcComma = 0; @@ -624,8 +624,8 @@ void ASHTECH::decode_init(void) } -/* - * ashtech board configuration script +/* + * ashtech board configuration script */ const char comm[] = "$PASHS,POP,20\r\n"\ -- 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') 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') 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 eefad6217dbcae56e44d142815f7559b17eaa9a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 12:05:40 +0100 Subject: GPS driver feedback cleanup --- src/drivers/gps/gps.cpp | 8 ++++---- src/drivers/gps/gps_helper.cpp | 11 +++++------ src/drivers/gps/mtk.cpp | 22 ++++++++++------------ src/drivers/gps/ubx.cpp | 2 +- 4 files changed, 20 insertions(+), 23 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 47c907bd3..8d7176791 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -552,7 +552,7 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info) fd = open(GPS_DEVICE_PATH, O_RDONLY); if (fd < 0) { - errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); + errx(1, "open: %s\n", GPS_DEVICE_PATH); goto fail; } @@ -565,7 +565,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "start failed"); } /** @@ -604,7 +604,7 @@ reset() err(1, "failed "); if (ioctl(fd, SENSORIOCRESET, 0) < 0) - err(1, "driver reset failed"); + err(1, "reset failed"); exit(0); } @@ -616,7 +616,7 @@ void info() { if (g_dev == nullptr) - errx(1, "driver not running"); + errx(1, "not running"); g_dev->print_info(); diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 3b92f1bf4..05dfc99b7 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.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 @@ -91,7 +91,7 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) warnx("try baudrate: %d\n", speed); default: - warnx("ERROR: Unsupported baudrate: %d\n", baud); + warnx("ERR: baudrate: %d\n", baud); return -EINVAL; } @@ -109,20 +109,19 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) /* set baud rate */ if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + warnx("ERR: %d (cfsetispeed)\n", termios_state); return -1; } if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { - warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); + warnx("ERR: %d (cfsetospeed)\n", termios_state); return -1; } if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { - warnx("ERROR setting baudrate (tcsetattr)\n"); + warnx("ERR: %d (tcsetattr)\n", termios_state); return -1; } - /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ return 0; } diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index d808b59a9..c0c47073b 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 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 @@ -73,39 +73,38 @@ MTK::configure(unsigned &baudrate) /* Write config messages, don't wait for an answer */ if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } usleep(10000); if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { - warnx("mtk: config write failed"); - return -1; + goto errout; } return 0; + +errout: + warnx("mtk: config write failed"); + return -1; } int @@ -222,7 +221,6 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) ret = 1; } else { - warnx("MTK Checksum invalid"); ret = -1; } diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 6b4e1630b..05809b361 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013, 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 -- cgit v1.2.3 From 06aa3fceee670ca510cea3d148634c0daf3c7911 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 12:05:59 +0100 Subject: Hott driver: Feedback cleanup --- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 4b8e0c0b0..1aade450b 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -157,7 +157,7 @@ hott_sensors_thread_main(int argc, char *argv[]) /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ const int uart = open_uart(device); if (uart < 0) { - errx(1, "Failed opening HoTT UART, exiting."); + errx(1, "Open fail, exiting."); thread_running = false; } -- 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') 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 b6f6a99799514ad884f27ebc4b6d925952812633 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 12:08:25 +0100 Subject: FW attitude control: Build with space optimization --- src/modules/fw_att_control/module.mk | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk index 89c6494c5..3661a171f 100644 --- a/src/modules/fw_att_control/module.mk +++ b/src/modules/fw_att_control/module.mk @@ -41,3 +41,5 @@ SRCS = fw_att_control_main.cpp \ fw_att_control_params.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os -- 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') 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 460402e07ac84315a32cc42dae295c88a6297924 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 13:31:18 +0000 Subject: sdlog2: checks and warnings to make sure there is space left on the SD card --- src/modules/sdlog2/sdlog2.c | 50 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2ce3d0097..da181dad5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -39,12 +39,14 @@ * * @author Lorenz Meier * @author Anton Babushkin + * @author Ban Siesta */ #include #include #include #include +#include #include #include #include @@ -158,6 +160,7 @@ static const int MIN_BYTES_TO_WRITE = 512; static bool _extended_logging = false; +static const char *mountpoint = "/fs/microsd"; static const char *log_root = "/fs/microsd/log"; static int mavlink_fd = -1; struct logbuffer_s lb; @@ -185,6 +188,9 @@ static bool log_name_timestamp = false; /* helper flag to track system state changes */ static bool flag_system_armed = false; +/* flag if warning about MicroSD card being almost full has already been sent */ +static bool space_warning_sent = false; + static pthread_t logwriter_pthread = 0; static pthread_attr_t logwriter_attr; @@ -244,6 +250,11 @@ static bool file_exist(const char *filename); static int file_copy(const char *file_old, const char *file_new); +/** + * Check if there is still free space available + */ +static int check_free_space(void); + static void handle_command(struct vehicle_command_s *cmd); static void handle_status(struct vehicle_status_s *cmd); @@ -547,6 +558,7 @@ static void *logwriter_thread(void *arg) pthread_mutex_unlock(&logbuffer_mutex); if (available > 0) { + /* do heavy IO here */ if (available > MAX_WRITE_CHUNK) { n = MAX_WRITE_CHUNK; @@ -584,6 +596,12 @@ static void *logwriter_thread(void *arg) if (++poll_count == 10) { fsync(log_fd); poll_count = 0; + + /* check if space is available, if not stop everything */ + if (check_free_space() != OK) { + logwriter_should_exit = true; + main_thread_should_exit = true; + } } } @@ -607,6 +625,7 @@ void sdlog2_start_log() errx(1, "error creating log dir"); } + /* initialize statistics counter */ log_bytes_written = 0; start_time = hrt_absolute_time(); @@ -899,6 +918,12 @@ int sdlog2_thread_main(int argc, char *argv[]) } + + if (check_free_space() != OK) { + errx(1, "error MicroSD almost full"); + } + + /* create log root dir */ int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); @@ -1822,6 +1847,31 @@ int file_copy(const char *file_old, const char *file_new) return OK; } +int check_free_space() +{ + /* use statfs to determine the number of blocks left */ + FAR struct statfs statfs_buf; + if (statfs(mountpoint, &statfs_buf) != OK) { + warnx("could not determine statfs"); + return ERROR; + } + + /* use a threshold of 4 MiB */ + if (statfs_buf.f_bavail < (int)(4*1024*1024/statfs_buf.f_bsize)) { + warnx("no more space on MicroSD (less than 4 MiB)"); + mavlink_log_critical(mavlink_fd, "[sdlog2] no more space left on MicroSD"); + return ERROR; + + /* use a threshold of 100 MiB to send a warning */ + } else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100*1024*1024/statfs_buf.f_bsize)) { + warnx("space on MicroSD running out (less than 100MiB)"); + mavlink_log_critical(mavlink_fd, "[sdlog2] space on MicroSD running out"); + space_warning_sent = true; + } + + return OK; +} + void handle_command(struct vehicle_command_s *cmd) { int param; -- cgit v1.2.3 From 3b418a5a595f0e48c4365217815372d9b278f64c Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 13:48:11 +0000 Subject: sdlog2: concatenate path strings --- src/modules/sdlog2/sdlog2.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index da181dad5..3b3b5c7a5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -160,8 +160,9 @@ static const int MIN_BYTES_TO_WRITE = 512; static bool _extended_logging = false; -static const char *mountpoint = "/fs/microsd"; -static const char *log_root = "/fs/microsd/log"; +#define MOUNTPOINT "/fs/microsd" +static const char *mountpoint = MOUNTPOINT; +static const char *log_root = MOUNTPOINT "/log"; static int mavlink_fd = -1; struct logbuffer_s lb; -- cgit v1.2.3 From 04c273bca6c99f31fd04741234d9c8efa849b553 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 14:06:33 +0000 Subject: sdlog2: slow down the free space check a bit more --- src/modules/sdlog2/sdlog2.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3b3b5c7a5..8537d03b3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -594,16 +594,19 @@ static void *logwriter_thread(void *arg) should_wait = true; } - if (++poll_count == 10) { + /* slow down fsync */ + if (poll_count % 10 == 0) { fsync(log_fd); - poll_count = 0; + /* slow down the check even more, and don't do both at the same time */ + } else if (poll_count % 1000 == 5) { /* check if space is available, if not stop everything */ if (check_free_space() != OK) { logwriter_should_exit = true; main_thread_should_exit = true; } } + poll_count++; } fsync(log_fd); -- cgit v1.2.3 From 7ce0a56d75f210a947db13b61c1a3c447244850e Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 14:06:52 +0000 Subject: sdlog2: some comments --- src/modules/sdlog2/sdlog2.c | 2 ++ 1 file changed, 2 insertions(+) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8537d03b3..16ca72891 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1864,12 +1864,14 @@ int check_free_space() if (statfs_buf.f_bavail < (int)(4*1024*1024/statfs_buf.f_bsize)) { warnx("no more space on MicroSD (less than 4 MiB)"); mavlink_log_critical(mavlink_fd, "[sdlog2] no more space left on MicroSD"); + /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return ERROR; /* use a threshold of 100 MiB to send a warning */ } else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100*1024*1024/statfs_buf.f_bsize)) { warnx("space on MicroSD running out (less than 100MiB)"); mavlink_log_critical(mavlink_fd, "[sdlog2] space on MicroSD running out"); + /* we don't want to flood the user with warnings */ space_warning_sent = true; } -- cgit v1.2.3 From abc8bf2e84d566b8760896e3c2fc60a871264c71 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 14:07:52 +0000 Subject: sdlog2: adapt threshold --- src/modules/sdlog2/sdlog2.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 16ca72891..efd6594b5 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1860,9 +1860,9 @@ int check_free_space() return ERROR; } - /* use a threshold of 4 MiB */ - if (statfs_buf.f_bavail < (int)(4*1024*1024/statfs_buf.f_bsize)) { - warnx("no more space on MicroSD (less than 4 MiB)"); + /* use a threshold of 10 MiB */ + if (statfs_buf.f_bavail < (int)(10*1024*1024/statfs_buf.f_bsize)) { + warnx("no more space on MicroSD (less than 10 MiB)"); mavlink_log_critical(mavlink_fd, "[sdlog2] no more space left on MicroSD"); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return ERROR; -- cgit v1.2.3 From 0142002737637e80071487a632b151d47a11a2a0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 15:33:32 +0100 Subject: mavlink log: Macro added to log to mavlink and console in one go --- src/include/mavlink/mavlink_log.h | 36 ++++++++++++++++++++++++++++++++++-- src/modules/sdlog2/sdlog2.c | 3 +-- 2 files changed, 35 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/include/mavlink/mavlink_log.h b/src/include/mavlink/mavlink_log.h index 6d56c546a..4962d029d 100644 --- a/src/include/mavlink/mavlink_log.h +++ b/src/include/mavlink/mavlink_log.h @@ -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 @@ -35,7 +35,7 @@ * @file mavlink_log.h * MAVLink text logging. * - * @author Lorenz Meier + * @author Lorenz Meier */ #ifndef MAVLINK_LOG @@ -99,6 +99,38 @@ __EXPORT void mavlink_vasprintf(int _fd, int severity, const char *fmt, ...); */ #define mavlink_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); +/** + * Send a mavlink emergency message and print to console. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#define mavlink_and_console_log_emergency(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); + +/** + * Send a mavlink critical message and print to console. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#define mavlink_and_console_log_critical(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); + +/** + * Send a mavlink emergency message and print to console. + * + * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); + * @param _text The text to log; + */ +#define mavlink_and_console_log_info(_fd, _text, ...) mavlink_vasprintf(_fd, MAVLINK_IOC_SEND_TEXT_INFO, _text, ##__VA_ARGS__); \ + fprintf(stderr, "telem> "); \ + fprintf(stderr, _text, ##__VA_ARGS__); \ + fprintf(stderr, "\n"); struct mavlink_logmessage { char text[MAVLINK_LOG_MAXLEN + 1]; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index efd6594b5..218145809 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -396,8 +396,7 @@ int create_log_dir() } /* print logging path, important to find log file later */ - warnx("log dir: %s", log_dir); - mavlink_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log dir: %s", log_dir); return 0; } -- 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') 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 1f181fb03f5eb467d5e1f0933164363269986844 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 16:14:41 +0100 Subject: SDLOG2: Use RTC, not GPS time for file naming --- src/modules/sdlog2/sdlog2.c | 48 ++++++++++++++++++++++++++++----------------- 1 file changed, 30 insertions(+), 18 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 2ce3d0097..10b6c903f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -103,6 +103,8 @@ #include "sdlog2_format.h" #include "sdlog2_messages.h" +#define PX4_EPOCH_SECS 1234567890ULL + /** * Logging rate. * @@ -338,13 +340,16 @@ int create_log_dir() uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - if (log_name_timestamp && gps_time != 0) { - /* use GPS date for log dir naming: e.g. /fs/microsd/2014-01-19 */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { int n = snprintf(log_dir, sizeof(log_dir), "%s/", log_root); - strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &t); + strftime(log_dir + n, sizeof(log_dir) - n, "%Y-%m-%d", &tt); mkdir_ret = mkdir(log_dir, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret == OK) { @@ -395,12 +400,16 @@ int open_log_file() char log_file_name[32] = ""; char log_file_path[64] = ""; - if (log_name_timestamp && gps_time != 0) { - /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); - strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &t); + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + /* start logging if we have a valid time and the time is not in the past */ + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { @@ -446,12 +455,15 @@ int open_perf_file(const char* str) char log_file_name[32] = ""; char log_file_path[64] = ""; - if (log_name_timestamp && gps_time != 0) { - /* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ - time_t gps_time_sec = gps_time / 1000000; - struct tm t; - gmtime_r(&gps_time_sec, &t); - strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t); + timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); + struct tm tt; + struct tm *ttp = gmtime_r(&utc_time_sec, &tt); + + if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { + strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); } else { -- cgit v1.2.3 From 0d103dcb383f29d77753ee1e7ab49cc7a0f76fb3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 16:24:02 +0100 Subject: sdlog2: Use correct struct --- src/modules/sdlog2/sdlog2.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 10b6c903f..c3952b77d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -57,6 +57,7 @@ #include #include #include +#include #include @@ -340,7 +341,7 @@ int create_log_dir() uint16_t dir_number = 1; // start with dir sess001 int mkdir_ret; - timespec ts; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); @@ -400,7 +401,7 @@ int open_log_file() char log_file_name[32] = ""; char log_file_path[64] = ""; - timespec ts; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); @@ -455,7 +456,7 @@ int open_perf_file(const char* str) char log_file_name[32] = ""; char log_file_path[64] = ""; - timespec ts; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); -- cgit v1.2.3 From 1cc92c03612c504414092bc6742bb17c812ce9c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 16:24:43 +0100 Subject: GPS driver: Require valid minimum time to allow setting the wall clock. Protection against nulled time fields --- src/drivers/gps/ashtech.cpp | 29 +++++++++++++---------- src/drivers/gps/gps_helper.h | 2 ++ src/drivers/gps/ubx.cpp | 56 ++++++++++++++++++++++++++------------------ 3 files changed, 52 insertions(+), 35 deletions(-) (limited to 'src') diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index 99dbe5984..a50767c22 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -99,21 +99,26 @@ int ASHTECH::handle_message(int len) timeinfo.tm_sec = int(ashtech_sec); time_t epoch = mktime(&timeinfo); - uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1e6; - - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. + if (epoch > GPS_EPOCH_SECS) { + uint64_t usecs = static_cast((ashtech_sec - static_cast(ashtech_sec))) * 1e6; + + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = usecs * 1000; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += usecs; + } else { + _gps_position->time_utc_usec = 0; } - _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/gps_helper.h b/src/drivers/gps/gps_helper.h index 3623397b2..0ce05fcb5 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -43,6 +43,8 @@ #include #include +#define GPS_EPOCH_SECS 1234567890ULL + class GPS_Helper { public: diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 05809b361..e197059db 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -748,19 +748,23 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_pvt.sec; time_t epoch = mktime(&timeinfo); - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. - - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); - } + if (epoch > GPS_EPOCH_SECS) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. + + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_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; + } else { + _gps_position->time_utc_usec = 0; + } } _gps_position->timestamp_time = hrt_absolute_time(); @@ -820,19 +824,25 @@ UBX::payload_rx_done(void) timeinfo.tm_sec = _buf.payload_rx_nav_timeutc.sec; time_t epoch = mktime(&timeinfo); - // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it - // and control its drift. Since we rely on the HRT for our monotonic - // clock, updating it from time to time is safe. + // only set the time if it makes sense - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; - if (clock_settime(CLOCK_REALTIME, &ts)) { - warn("failed setting clock"); - } + if (epoch > GPS_EPOCH_SECS) { + // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it + // and control its drift. Since we rely on the HRT for our monotonic + // clock, updating it from time to time is safe. - _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; - _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { + warn("failed setting clock"); + } + + _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; + _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { + _gps_position->time_utc_usec = 0; + } } _gps_position->timestamp_time = hrt_absolute_time(); -- 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') 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') 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 From 55741be473e1aafddffbd621ccbbc90e88fbe669 Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Mon, 17 Nov 2014 11:29:20 +0200 Subject: Made it possible to specify yaw scale for the copter --- src/modules/systemlib/mixer/multi_tables | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 18c828578..897834501 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -102,8 +102,10 @@ foreach table $tables { foreach {angle dir} $angles { if {$dir == "CW"} { set dd 1.0 - } else { + } else if {$dir == "CCW"} { set dd -1.0 + } else { + set dd $dir } factors $angle $dd } -- cgit v1.2.3 From 8612189c65e5af73699e482d42950570dd298fd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 18:42:36 +0100 Subject: Moved text feedback to new API --- src/modules/sdlog2/sdlog2.c | 57 ++++++++++++++++++++------------------------- 1 file changed, 25 insertions(+), 32 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 218145809..22b178310 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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 @@ -432,7 +432,7 @@ int open_log_file() if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -440,12 +440,10 @@ int open_log_file() int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - warn("failed opening log: %s", log_file_name); - mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { - warnx("log file: %s", log_file_name); - mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; @@ -483,7 +481,7 @@ int open_perf_file(const char* str) if (file_number > MAX_NO_LOGFILE) { /* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */ - mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE); return -1; } } @@ -491,12 +489,10 @@ int open_perf_file(const char* str) int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - warn("failed opening log: %s", log_file_name); - mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); } else { - warnx("log file: %s", log_file_name); - mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; @@ -619,13 +615,12 @@ static void *logwriter_thread(void *arg) void sdlog2_start_log() { - warnx("start logging"); - mavlink_log_info(mavlink_fd, "[sdlog2] start logging"); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] start logging"); /* create log dir if needed */ if (create_log_dir() != 0) { - mavlink_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); - errx(1, "error creating log dir"); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] error creating log dir"); + exit(1); } @@ -666,8 +661,7 @@ void sdlog2_start_log() void sdlog2_stop_log() { - warnx("stop logging"); - mavlink_log_info(mavlink_fd, "[sdlog2] stop logging"); + mavlink_and_console_log_info(mavlink_fd, "[sdlog2] stop logging"); logging_enabled = false; @@ -793,7 +787,7 @@ int sdlog2_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("failed to open MAVLink log stream, start mavlink app first"); + warnx("ERR: log stream, start mavlink app first"); } /* delay = 1 / rate (rate defined by -r option), default log rate: 50 Hz */ @@ -923,7 +917,7 @@ int sdlog2_thread_main(int argc, char *argv[]) if (check_free_space() != OK) { - errx(1, "error MicroSD almost full"); + errx(1, "ERR: MicroSD almost full"); } @@ -931,7 +925,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO); if (mkdir_ret != 0 && errno != EEXIST) { - err(1, "failed creating log root dir: %s", log_root); + err(1, "ERR: failed creating log dir: %s", log_root); } /* copy conversion scripts */ @@ -1783,8 +1777,6 @@ int sdlog2_thread_main(int argc, char *argv[]) free(lb.data); - warnx("exiting"); - thread_running = false; return 0; @@ -1817,7 +1809,7 @@ int file_copy(const char *file_old, const char *file_new) int ret = 0; if (source == NULL) { - warnx("failed opening input file to copy"); + warnx("ERR: open in"); return 1; } @@ -1825,7 +1817,7 @@ int file_copy(const char *file_old, const char *file_new) if (target == NULL) { fclose(source); - warnx("failed to open output file to copy"); + warnx("ERR: open out"); return 1; } @@ -1855,21 +1847,22 @@ int check_free_space() /* use statfs to determine the number of blocks left */ FAR struct statfs statfs_buf; if (statfs(mountpoint, &statfs_buf) != OK) { - warnx("could not determine statfs"); - return ERROR; + errx(ERROR, "ERR: statfs"); } /* use a threshold of 10 MiB */ - if (statfs_buf.f_bavail < (int)(10*1024*1024/statfs_buf.f_bsize)) { - warnx("no more space on MicroSD (less than 10 MiB)"); - mavlink_log_critical(mavlink_fd, "[sdlog2] no more space left on MicroSD"); + if (statfs_buf.f_bavail < (int)(10 * 1024 * 1024 / statfs_buf.f_bsize)) { + mavlink_and_console_log_critical(mavlink_fd, + "[sdlog2] no space on MicroSD: %u MiB", + (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we do not need a flag to remember that we sent this warning because we will exit anyway */ return ERROR; /* use a threshold of 100 MiB to send a warning */ - } else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100*1024*1024/statfs_buf.f_bsize)) { - warnx("space on MicroSD running out (less than 100MiB)"); - mavlink_log_critical(mavlink_fd, "[sdlog2] space on MicroSD running out"); + } else if (!space_warning_sent && statfs_buf.f_bavail < (int)(100 * 1024 * 1024 / statfs_buf.f_bsize)) { + mavlink_and_console_log_critical(mavlink_fd, + "[sdlog2] space on MicroSD low: %u MiB", + (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); /* we don't want to flood the user with warnings */ space_warning_sent = true; } -- cgit v1.2.3 From c4471d77d7d6a20709240ab933c32d87ba7c07fd Mon Sep 17 00:00:00 2001 From: Anton Matosov Date: Sun, 4 Jan 2015 18:58:57 +0200 Subject: Moved quad_v and twin_engine to the multi_tables in order to make all the tables been generated automatically --- src/modules/systemlib/mixer/mixer_multirotor.cpp | 126 +++++++++++------------ src/modules/systemlib/mixer/multi_tables | 19 +++- 2 files changed, 76 insertions(+), 69 deletions(-) (limited to 'src') diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 24187c9bc..eb1aef6c1 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -78,89 +78,87 @@ float constrain(float val, float min, float max) */ const MultirotorMixer::Rotor _config_quad_x[] = { - { -0.707107, 0.707107, 1.00 }, - { 0.707107, -0.707107, 1.00 }, - { 0.707107, 0.707107, -1.00 }, - { -0.707107, -0.707107, -1.00 }, + { -0.707107, 0.707107, 1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { 0.707107, 0.707107, -1.000000 }, + { -0.707107, -0.707107, -1.000000 }, }; const MultirotorMixer::Rotor _config_quad_plus[] = { - { -1.000000, 0.000000, 1.00 }, - { 1.000000, 0.000000, 1.00 }, - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, -1.00 }, + { -1.000000, 0.000000, 1.000000 }, + { 1.000000, 0.000000, 1.000000 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, -1.000000 }, }; -//Add table for quad in V configuration, which is not generated by multi_tables! const MultirotorMixer::Rotor _config_quad_v[] = { - { -0.3223, 0.9466, 0.4242 }, - { 0.3223, -0.9466, 1.0000 }, - { 0.3223, 0.9466, -0.4242 }, - { -0.3223, -0.9466, -1.0000 }, + { -0.322266, 0.946649, 0.424200 }, + { 0.322266, 0.946649, 1.000000 }, + { 0.322266, 0.946649, -0.424200 }, + { -0.322266, 0.946649, -1.000000 }, }; const MultirotorMixer::Rotor _config_quad_wide[] = { - { -0.927184, 0.374607, 1.00 }, - { 0.777146, -0.629320, 1.00 }, - { 0.927184, 0.374607, -1.00 }, - { -0.777146, -0.629320, -1.00 }, + { -0.927184, 0.374607, 1.000000 }, + { 0.777146, -0.629320, 1.000000 }, + { 0.927184, 0.374607, -1.000000 }, + { -0.777146, -0.629320, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_x[] = { - { -1.000000, 0.000000, -1.00 }, - { 1.000000, 0.000000, 1.00 }, - { 0.500000, 0.866025, -1.00 }, - { -0.500000, -0.866025, 1.00 }, - { -0.500000, 0.866025, 1.00 }, - { 0.500000, -0.866025, -1.00 }, + { -1.000000, 0.000000, -1.000000 }, + { 1.000000, 0.000000, 1.000000 }, + { 0.500000, 0.866025, -1.000000 }, + { -0.500000, -0.866025, 1.000000 }, + { -0.500000, 0.866025, 1.000000 }, + { 0.500000, -0.866025, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_plus[] = { - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, 1.00 }, - { 0.866025, -0.500000, -1.00 }, - { -0.866025, 0.500000, 1.00 }, - { 0.866025, 0.500000, 1.00 }, - { -0.866025, -0.500000, -1.00 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, 1.000000 }, + { 0.866025, -0.500000, -1.000000 }, + { -0.866025, 0.500000, 1.000000 }, + { 0.866025, 0.500000, 1.000000 }, + { -0.866025, -0.500000, -1.000000 }, }; const MultirotorMixer::Rotor _config_hex_cox[] = { - { -0.866025, 0.500000, -1.00 }, - { -0.866025, 0.500000, 1.00 }, - { -0.000000, -1.000000, -1.00 }, - { -0.000000, -1.000000, 1.00 }, - { 0.866025, 0.500000, -1.00 }, - { 0.866025, 0.500000, 1.00 }, + { -0.866025, 0.500000, -1.000000 }, + { -0.866025, 0.500000, 1.000000 }, + { -0.000000, -1.000000, -1.000000 }, + { -0.000000, -1.000000, 1.000000 }, + { 0.866025, 0.500000, -1.000000 }, + { 0.866025, 0.500000, 1.000000 }, }; const MultirotorMixer::Rotor _config_octa_x[] = { - { -0.382683, 0.923880, -1.00 }, - { 0.382683, -0.923880, -1.00 }, - { -0.923880, 0.382683, 1.00 }, - { -0.382683, -0.923880, 1.00 }, - { 0.382683, 0.923880, 1.00 }, - { 0.923880, -0.382683, 1.00 }, - { 0.923880, 0.382683, -1.00 }, - { -0.923880, -0.382683, -1.00 }, + { -0.382683, 0.923880, -1.000000 }, + { 0.382683, -0.923880, -1.000000 }, + { -0.923880, 0.382683, 1.000000 }, + { -0.382683, -0.923880, 1.000000 }, + { 0.382683, 0.923880, 1.000000 }, + { 0.923880, -0.382683, 1.000000 }, + { 0.923880, 0.382683, -1.000000 }, + { -0.923880, -0.382683, -1.000000 }, }; const MultirotorMixer::Rotor _config_octa_plus[] = { - { 0.000000, 1.000000, -1.00 }, - { -0.000000, -1.000000, -1.00 }, - { -0.707107, 0.707107, 1.00 }, - { -0.707107, -0.707107, 1.00 }, - { 0.707107, 0.707107, 1.00 }, - { 0.707107, -0.707107, 1.00 }, - { 1.000000, 0.000000, -1.00 }, - { -1.000000, 0.000000, -1.00 }, + { 0.000000, 1.000000, -1.000000 }, + { -0.000000, -1.000000, -1.000000 }, + { -0.707107, 0.707107, 1.000000 }, + { -0.707107, -0.707107, 1.000000 }, + { 0.707107, 0.707107, 1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { 1.000000, 0.000000, -1.000000 }, + { -1.000000, 0.000000, -1.000000 }, }; const MultirotorMixer::Rotor _config_octa_cox[] = { - { -0.707107, 0.707107, 1.00 }, - { 0.707107, 0.707107, -1.00 }, - { 0.707107, -0.707107, 1.00 }, - { -0.707107, -0.707107, -1.00 }, - { 0.707107, 0.707107, 1.00 }, - { -0.707107, 0.707107, -1.00 }, - { -0.707107, -0.707107, 1.00 }, - { 0.707107, -0.707107, -1.00 }, + { -0.707107, 0.707107, 1.000000 }, + { 0.707107, 0.707107, -1.000000 }, + { 0.707107, -0.707107, 1.000000 }, + { -0.707107, -0.707107, -1.000000 }, + { 0.707107, 0.707107, 1.000000 }, + { -0.707107, 0.707107, -1.000000 }, + { -0.707107, -0.707107, 1.000000 }, + { 0.707107, -0.707107, -1.000000 }, }; -const MultirotorMixer::Rotor _config_duorotor[] = { - { -1.000000, 0.000000, 0.00 }, - { 1.000000, 0.000000, 0.00 }, +const MultirotorMixer::Rotor _config_twin_engine[] = { + { -1.000000, 0.000000, 0.000000 }, + { 1.000000, 0.000000, 0.000000 }, }; - const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_quad_x[0], &_config_quad_plus[0], @@ -172,7 +170,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = { &_config_octa_x[0], &_config_octa_plus[0], &_config_octa_cox[0], - &_config_duorotor[0], + &_config_twin_engine[0], }; const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = { 4, /* quad_x */ diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables index 897834501..bdb62f812 100755 --- a/src/modules/systemlib/mixer/multi_tables +++ b/src/modules/systemlib/mixer/multi_tables @@ -21,6 +21,12 @@ set quad_plus { 180 CW } +set quad_v { + 18.8 0.4242 + -18.8 1.0 + -18.8 -0.4242 + 18.8 -1.0 +} set quad_wide { 68 CCW @@ -89,11 +95,14 @@ set octa_cox { -135 CW } +set twin_engine { + 90 0.0 + -90 0.0 +} -set tables {quad_x quad_plus quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox} - +set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox twin_engine} -proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]} +proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %9.6f }," [rcos [expr $a + 90]] [rcos $a] [expr $d]]} foreach table $tables { puts [format "const MultirotorMixer::Rotor _config_%s\[\] = {" $table] @@ -101,9 +110,9 @@ foreach table $tables { upvar #0 $table angles foreach {angle dir} $angles { if {$dir == "CW"} { - set dd 1.0 - } else if {$dir == "CCW"} { set dd -1.0 + } elseif {$dir == "CCW"} { + set dd 1.0 } else { set dd $dir } -- cgit v1.2.3 From 552ff809693d340ba6f5fed6837b99effe8bf2c3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 18:53:34 +0100 Subject: Fix NSH timeout logic --- src/systemcmds/nshterm/nshterm.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index edeb5c624..c2ea2a1cc 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -66,20 +66,18 @@ nshterm_main(int argc, char *argv[]) int fd = -1; int armed_fd = orb_subscribe(ORB_ID(actuator_armed)); struct actuator_armed_s armed; - /* we assume the system does not provide arming status feedback */ - bool armed_updated = false; - /* try the first 30 seconds or if arming system is ready */ - while ((retries < 300) || armed_updated) { + /* try to bring up the console - stop doing so if the system gets armed */ + while (true) { /* abort if an arming topic is published and system is armed */ bool updated = false; - if (orb_check(armed_fd, &updated)) { + orb_check(armed_fd, &updated) + if (updated) { /* the system is now providing arming status feedback. * instead of timing out, we resort to abort bringing * up the terminal. */ - armed_updated = true; orb_copy(ORB_ID(actuator_armed), armed_fd, &armed); if (armed.armed) { -- cgit v1.2.3 From c451fdd1986fd36bd075e9a67387a13be8f505a0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 19:04:15 +0100 Subject: sdlog2: Output cleanup --- src/modules/sdlog2/sdlog2.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 963149779..9dd80a18e 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -502,10 +502,8 @@ int open_perf_file(const char* str) int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC); if (fd < 0) { - mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name); + mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] failed opening: %s", log_file_name); - } else { - mavlink_and_console_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name); } return fd; -- cgit v1.2.3 From 1b7bdcb07a27c1710cf6e47168db3152a0d26143 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 19:04:42 +0100 Subject: NSH term fix --- src/systemcmds/nshterm/nshterm.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index c2ea2a1cc..ceaea35b6 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -72,7 +72,7 @@ nshterm_main(int argc, char *argv[]) /* abort if an arming topic is published and system is armed */ bool updated = false; - orb_check(armed_fd, &updated) + orb_check(armed_fd, &updated); if (updated) { /* the system is now providing arming status feedback. * instead of timing out, we resort to abort bringing @@ -90,6 +90,7 @@ nshterm_main(int argc, char *argv[]) /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); if (fd != -1) { + close(armed_fd); break; } usleep(100000); @@ -114,7 +115,7 @@ nshterm_main(int argc, char *argv[]) } /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); + uart_config.c_oflag |= (ONLCR | OPOST); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERR set config %s\n", argv[1]); -- cgit v1.2.3 From d70ed8ad123b44667a636056fe5783944157d735 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 19:04:42 +0100 Subject: NSH term fix --- src/systemcmds/nshterm/nshterm.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index c2ea2a1cc..ceaea35b6 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -72,7 +72,7 @@ nshterm_main(int argc, char *argv[]) /* abort if an arming topic is published and system is armed */ bool updated = false; - orb_check(armed_fd, &updated) + orb_check(armed_fd, &updated); if (updated) { /* the system is now providing arming status feedback. * instead of timing out, we resort to abort bringing @@ -90,6 +90,7 @@ nshterm_main(int argc, char *argv[]) /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); if (fd != -1) { + close(armed_fd); break; } usleep(100000); @@ -114,7 +115,7 @@ nshterm_main(int argc, char *argv[]) } /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); + uart_config.c_oflag |= (ONLCR | OPOST); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERR set config %s\n", argv[1]); -- cgit v1.2.3 From e8eff3061f5e9c451c94d081932cac0e62e1a9b9 Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 18:59:53 +0000 Subject: Revert "sdlog2: slow down the free space check a bit more" This reverts commit 04c273bca6c99f31fd04741234d9c8efa849b553. --- src/modules/sdlog2/sdlog2.c | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 9dd80a18e..1bf7d7198 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -600,19 +600,16 @@ static void *logwriter_thread(void *arg) should_wait = true; } - /* slow down fsync */ - if (poll_count % 10 == 0) { + if (++poll_count == 10) { fsync(log_fd); + poll_count = 0; - /* slow down the check even more, and don't do both at the same time */ - } else if (poll_count % 1000 == 5) { /* check if space is available, if not stop everything */ if (check_free_space() != OK) { logwriter_should_exit = true; main_thread_should_exit = true; } } - poll_count++; } fsync(log_fd); -- cgit v1.2.3 From 0265885a39282cf9aa34cf1647cf9b260f9f41cc Mon Sep 17 00:00:00 2001 From: Ban Siesta Date: Sun, 4 Jan 2015 19:13:27 +0000 Subject: sdlog2: check every 20MiB if we're running out of space, moved the threshold to 50MiB --- src/modules/sdlog2/sdlog2.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1bf7d7198..31cdff306 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -178,6 +178,7 @@ static char log_dir[32]; /* statistics counters */ static uint64_t start_time = 0; static unsigned long log_bytes_written = 0; +static unsigned long last_checked_bytes_written = 0; static unsigned long log_msgs_written = 0; static unsigned long log_msgs_skipped = 0; @@ -604,11 +605,15 @@ static void *logwriter_thread(void *arg) fsync(log_fd); poll_count = 0; + } + + if (log_bytes_written - last_checked_bytes_written > 20*1024*1024) { /* check if space is available, if not stop everything */ if (check_free_space() != OK) { logwriter_should_exit = true; main_thread_should_exit = true; } + last_checked_bytes_written = log_bytes_written; } } @@ -1858,8 +1863,8 @@ int check_free_space() errx(ERROR, "ERR: statfs"); } - /* use a threshold of 10 MiB */ - if (statfs_buf.f_bavail < (int)(10 * 1024 * 1024 / statfs_buf.f_bsize)) { + /* use a threshold of 50 MiB */ + if (statfs_buf.f_bavail < (int)(50 * 1024 * 1024 / statfs_buf.f_bsize)) { mavlink_and_console_log_critical(mavlink_fd, "[sdlog2] no space on MicroSD: %u MiB", (unsigned int)(statfs_buf.f_bavail * statfs_buf.f_bsize) / (1024U * 1024U)); -- cgit v1.2.3 From 0e1a532720a80660b2d4a3b24c1a071a517ec82e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Jan 2015 20:57:25 +0100 Subject: sdlog2: Use .px4log as file format. Fixes #1243 --- src/modules/sdlog2/sdlog2.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index c3952b77d..a88b6045a 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -343,7 +343,7 @@ int create_log_dir() struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; struct tm *ttp = gmtime_r(&utc_time_sec, &tt); @@ -403,14 +403,14 @@ int open_log_file() struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.px4log */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; struct tm *ttp = gmtime_r(&utc_time_sec, &tt); /* start logging if we have a valid time and the time is not in the past */ if (log_name_timestamp && ttp && (utc_time_sec > PX4_EPOCH_SECS)) { - strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.bin", &tt); + strftime(log_file_name, sizeof(log_file_name), "%H_%M_%S.px4log", &tt); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); } else { @@ -418,8 +418,8 @@ int open_log_file() /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { - /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ - snprintf(log_file_name, sizeof(log_file_name), "log%03u.bin", file_number); + /* format log file path: e.g. /fs/microsd/sess001/log001.px4log */ + snprintf(log_file_name, sizeof(log_file_name), "log%03u.px4log", file_number); snprintf(log_file_path, sizeof(log_file_path), "%s/%s", log_dir, log_file_name); if (!file_exist(log_file_path)) { @@ -458,7 +458,7 @@ int open_perf_file(const char* str) struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); - /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */ + /* use RTC time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.txt */ time_t utc_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); struct tm tt; struct tm *ttp = gmtime_r(&utc_time_sec, &tt); @@ -472,7 +472,7 @@ int open_perf_file(const char* str) /* look for the next file that does not exist */ while (file_number <= MAX_NO_LOGFILE) { - /* format log file path: e.g. /fs/microsd/sess001/log001.bin */ + /* format log file path: e.g. /fs/microsd/sess001/log001.txt */ snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number); snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name); -- cgit v1.2.3