From 2ad12d7977f77228ae52aba113b031184e342a1b Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 6 Aug 2019 12:55:25 -0400 Subject: [PATCH] sensors: create vehicle_angular_velocity module (#12596) * split out filtered sensor_gyro aggregation from mc_att_control and move to wq:rate_ctrl --- msg/CMakeLists.txt | 1 + msg/rate_ctrl_status.msg | 5 - msg/tools/uorb_rtps_message_ids.yaml | 5 + msg/vehicle_angular_velocity.msg | 8 + msg/vehicle_attitude.msg | 4 - src/examples/segway/BlockSegwayController.cpp | 2 +- src/examples/segway/blocks.cpp | 1 + src/examples/segway/blocks.hpp | 2 + .../attitude_estimator_q_main.cpp | 3 - src/modules/ekf2/ekf2_main.cpp | 7 - .../FixedwingAttitudeControl.cpp | 21 +- .../FixedwingAttitudeControl.hpp | 2 + .../FixedwingPositionControl.cpp | 3 +- .../FixedwingPositionControl.hpp | 2 + .../land_detector/MulticopterLandDetector.cpp | 7 +- .../land_detector/MulticopterLandDetector.h | 3 + .../BlockLocalPositionEstimator.cpp | 7 +- .../BlockLocalPositionEstimator.hpp | 2 + .../local_position_estimator/sensors/flow.cpp | 7 +- src/modules/logger/logger.cpp | 15 +- src/modules/mavlink/mavlink_messages.cpp | 104 +++-- src/modules/mavlink/mavlink_receiver.cpp | 25 +- src/modules/mc_att_control/CMakeLists.txt | 1 + src/modules/mc_att_control/mc_att_control.hpp | 56 ++- .../mc_att_control/mc_att_control_main.cpp | 379 +++++++----------- src/modules/sensors/CMakeLists.txt | 3 + src/modules/sensors/sensors.cpp | 16 +- .../vehicle_angular_velocity/CMakeLists.txt | 37 ++ .../VehicleAngularVelocity.cpp | 226 +++++++++++ .../VehicleAngularVelocity.hpp | 110 +++++ src/modules/sih/sih.cpp | 44 +- src/modules/sih/sih.hpp | 21 +- src/modules/simulator/simulator.h | 2 + src/modules/simulator/simulator_mavlink.cpp | 23 +- 34 files changed, 778 insertions(+), 376 deletions(-) create mode 100644 msg/vehicle_angular_velocity.msg create mode 100644 src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt create mode 100644 src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp create mode 100644 src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index eef98cd893..24f94f4578 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -123,6 +123,7 @@ set(msg_files ulog_stream.msg ulog_stream_ack.msg vehicle_air_data.msg + vehicle_angular_velocity.msg vehicle_attitude.msg vehicle_attitude_setpoint.msg vehicle_command.msg diff --git a/msg/rate_ctrl_status.msg b/msg/rate_ctrl_status.msg index 01008d54c5..145bf25a5c 100644 --- a/msg/rate_ctrl_status.msg +++ b/msg/rate_ctrl_status.msg @@ -1,10 +1,5 @@ uint64 timestamp # time since system start (microseconds) -# rates used by the controller -float32 rollspeed # Bias corrected angular velocity about X body axis in rad/s -float32 pitchspeed # Bias corrected angular velocity about Y body axis in rad/s -float32 yawspeed # Bias corrected angular velocity about Z body axis in rad/s - # rate controller integrator status float32 rollspeed_integ float32 pitchspeed_integ diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 944a925ca1..29bdb45084 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -198,6 +198,8 @@ rtps: id: 85 - msg: vehicle_air_data id: 86 + - msg: vehicle_angular_velocity + id: 134 - msg: vehicle_attitude id: 87 send: true @@ -279,6 +281,9 @@ rtps: - msg: fw_virtual_attitude_setpoint id: 127 alias: vehicle_attitude_setpoint + - msg: vehicle_angular_velocity_groundtruth + id: 135 + alias: vehicle_angular_velocity - msg: vehicle_attitude_groundtruth id: 128 alias: vehicle_attitude diff --git a/msg/vehicle_angular_velocity.msg b/msg/vehicle_angular_velocity.msg new file mode 100644 index 0000000000..3f1b0a7720 --- /dev/null +++ b/msg/vehicle_angular_velocity.msg @@ -0,0 +1,8 @@ + +uint64 timestamp # time since system start (microseconds) + +uint64 timestamp_sample # the timestamp of the raw data (microseconds) + +float32[3] xyz # Bias corrected angular velocity about X, Y, Z body axis in rad/s + +# TOPICS vehicle_angular_velocity vehicle_angular_velocity_groundtruth diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index ae42454c71..86a3c9094f 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -2,10 +2,6 @@ uint64 timestamp # time since system start (microseconds) -float32 rollspeed # Bias corrected angular velocity about X body axis in rad/s -float32 pitchspeed # Bias corrected angular velocity about Y body axis in rad/s -float32 yawspeed # Bias corrected angular velocity about Z body axis in rad/s - float32[4] q # Quaternion rotation from NED earth frame to XYZ body frame float32[4] delta_q_reset # Amount by which quaternion has changed during last reset uint8 quat_reset_counter # Quaternion reset counter diff --git a/src/examples/segway/BlockSegwayController.cpp b/src/examples/segway/BlockSegwayController.cpp index e3fefe171c..bacb7f7c1a 100644 --- a/src/examples/segway/BlockSegwayController.cpp +++ b/src/examples/segway/BlockSegwayController.cpp @@ -42,7 +42,7 @@ void BlockSegwayController::update() Eulerf euler = Eulerf(Quatf(_att.get().q)); // compute speed command - float spdCmd = -th2v.update(euler.theta()) - q2v.update(_att.get().pitchspeed); + float spdCmd = -th2v.update(euler.theta()) - q2v.update(_angular_velocity.get().xyz[1]); // handle autopilot modes if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION || diff --git a/src/examples/segway/blocks.cpp b/src/examples/segway/blocks.cpp index 2112ddb0db..a6348805e5 100644 --- a/src/examples/segway/blocks.cpp +++ b/src/examples/segway/blocks.cpp @@ -89,6 +89,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c _param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz _missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()), _att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()), + _angular_velocity(ORB_ID(vehicle_angular_velocity), 20, 0, &getSubscriptions()), _attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()), _pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()), _ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()), diff --git a/src/examples/segway/blocks.hpp b/src/examples/segway/blocks.hpp index 4557a76479..db521b9754 100644 --- a/src/examples/segway/blocks.hpp +++ b/src/examples/segway/blocks.hpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -96,6 +97,7 @@ protected: uORB::SubscriptionPollable _param_update; uORB::SubscriptionPollable _missionCmd; uORB::SubscriptionPollable _att; + uORB::SubscriptionPollable _angular_velocity; uORB::SubscriptionPollable _attCmd; uORB::SubscriptionPollable _pos; uORB::SubscriptionPollable _ratesCmd; diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a8f5165a8d..39b8644164 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -425,9 +425,6 @@ void AttitudeEstimatorQ::task_main() if (update(dt)) { vehicle_attitude_s att = {}; att.timestamp = sensors.timestamp; - att.rollspeed = _rates(0); - att.pitchspeed = _rates(1); - att.yawspeed = _rates(2); _q.copyTo(att.q); /* the instance count is not used here */ diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index ae99af6cdf..cca26abb6b 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1754,13 +1754,6 @@ bool Ekf2::publish_attitude(const sensor_combined_s &sensors, const hrt_abstime _ekf.get_quat_reset(&att.delta_q_reset[0], &att.quat_reset_counter); - // In-run bias estimates - float gyro_bias[3]; - _ekf.get_gyro_bias(gyro_bias); - att.rollspeed = sensors.gyro_rad[0] - gyro_bias[0]; - att.pitchspeed = sensors.gyro_rad[1] - gyro_bias[1]; - att.yawspeed = sensors.gyro_rad[2] - gyro_bias[2]; - _att_pub.publish(att); return true; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index c1b59ded73..c75588aa1a 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -504,6 +504,12 @@ void FixedwingAttitudeControl::run() /* get current rotation matrix and euler angles from control state quaternions */ matrix::Dcmf R = matrix::Quatf(_att.q); + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_rates_sub.copy(&angular_velocity); + float rollspeed = angular_velocity.xyz[0]; + float pitchspeed = angular_velocity.xyz[1]; + float yawspeed = angular_velocity.xyz[2]; + if (_is_tailsitter) { /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * @@ -542,9 +548,9 @@ void FixedwingAttitudeControl::run() R = R_adapted; /* lastly, roll- and yawspeed have to be swaped */ - float helper = _att.rollspeed; - _att.rollspeed = -_att.yawspeed; - _att.yawspeed = helper; + float helper = rollspeed; + rollspeed = -yawspeed; + yawspeed = helper; } const matrix::Eulerf euler_angles(R); @@ -624,9 +630,9 @@ void FixedwingAttitudeControl::run() control_input.roll = euler_angles.phi(); control_input.pitch = euler_angles.theta(); control_input.yaw = euler_angles.psi(); - control_input.body_x_rate = _att.rollspeed; - control_input.body_y_rate = _att.pitchspeed; - control_input.body_z_rate = _att.yawspeed; + control_input.body_x_rate = rollspeed; + control_input.body_y_rate = pitchspeed; + control_input.body_z_rate = yawspeed; control_input.roll_setpoint = _att_sp.roll_body; control_input.pitch_setpoint = _att_sp.pitch_body; control_input.yaw_setpoint = _att_sp.yaw_body; @@ -801,9 +807,6 @@ void FixedwingAttitudeControl::run() rate_ctrl_status_s rate_ctrl_status; rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.rollspeed = _att.rollspeed; - rate_ctrl_status.pitchspeed = _att.pitchspeed; - rate_ctrl_status.yawspeed = _att.yawspeed; rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator(); rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator(); rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index a93a94d809..acb7b4280b 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -103,6 +104,7 @@ private: uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; /**< vehicle status subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ + uORB::Subscription _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; uORB::SubscriptionData _airspeed_sub{ORB_ID(airspeed)}; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index c8ff1209bd..22a567f7f5 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -1016,7 +1016,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ - if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + if (fabsf(_vehicle_rates_sub.get().xyz[2]) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { // little yaw movement, lock to current heading _yaw_lock_engaged = true; @@ -1759,6 +1759,7 @@ FixedwingPositionControl::run() vehicle_control_mode_poll(); _vehicle_land_detected_sub.update(&_vehicle_land_detected); vehicle_status_poll(); + _vehicle_rates_sub.update(); Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon); Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e); diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 4f45957456..5c2eaff6d1 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include #include @@ -163,6 +164,7 @@ private: uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; ///< vehicle command subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; ///< vehicle land detected subscription */ uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; ///< vehicle status subscription */ + uORB::SubscriptionData _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; orb_advert_t _attitude_sp_pub{nullptr}; ///< attitude setpoint */ orb_advert_t _pos_ctrl_status_pub{nullptr}; ///< navigation capabilities publication */ diff --git a/src/modules/land_detector/MulticopterLandDetector.cpp b/src/modules/land_detector/MulticopterLandDetector.cpp index e5228ce294..eb06bc955d 100644 --- a/src/modules/land_detector/MulticopterLandDetector.cpp +++ b/src/modules/land_detector/MulticopterLandDetector.cpp @@ -95,6 +95,7 @@ void MulticopterLandDetector::_update_topics() _actuator_controls_sub.update(&_actuator_controls); _battery_sub.update(&_battery_status); _sensor_bias_sub.update(&_sensor_bias); + _vehicle_angular_velocity_sub.update(&_vehicle_angular_velocity); _vehicle_attitude_sub.update(&_vehicle_attitude); _vehicle_control_mode_sub.update(&_control_mode); _vehicle_local_position_sub.update(&_vehicle_local_position); @@ -215,9 +216,9 @@ bool MulticopterLandDetector::_get_maybe_landed_state() // Next look if all rotation angles are not moving. float maxRotationScaled = _params.maxRotation_rad_s * landThresholdFactor; - bool rotating = (fabsf(_vehicle_attitude.rollspeed) > maxRotationScaled) || - (fabsf(_vehicle_attitude.pitchspeed) > maxRotationScaled) || - (fabsf(_vehicle_attitude.yawspeed) > maxRotationScaled); + bool rotating = (fabsf(_vehicle_angular_velocity.xyz[0]) > maxRotationScaled) || + (fabsf(_vehicle_angular_velocity.xyz[1]) > maxRotationScaled) || + (fabsf(_vehicle_angular_velocity.xyz[2]) > maxRotationScaled); // Return status based on armed state and throttle if no position lock is available. if (!_has_altitude_lock() && !rotating) { diff --git a/src/modules/land_detector/MulticopterLandDetector.h b/src/modules/land_detector/MulticopterLandDetector.h index 19ec57dab0..24b76d083f 100644 --- a/src/modules/land_detector/MulticopterLandDetector.h +++ b/src/modules/land_detector/MulticopterLandDetector.h @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -126,6 +127,7 @@ private: uORB::Subscription _actuator_controls_sub{ORB_ID(actuator_controls_0)}; uORB::Subscription _battery_sub{ORB_ID(battery_status)}; uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; @@ -136,6 +138,7 @@ private: vehicle_control_mode_s _control_mode {}; sensor_bias_s _sensor_bias {}; vehicle_attitude_s _vehicle_attitude {}; + vehicle_angular_velocity_s _vehicle_angular_velocity{}; vehicle_local_position_s _vehicle_local_position {}; vehicle_local_position_setpoint_s _vehicle_local_position_setpoint {}; diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp index 356dd120ca..0451119b17 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.cpp @@ -25,6 +25,7 @@ BlockLocalPositionEstimator::BlockLocalPositionEstimator() : _sub_armed(ORB_ID(actuator_armed), 1000 / 2, 0, &getSubscriptions()), _sub_land(ORB_ID(vehicle_land_detected), 1000 / 2, 0, &getSubscriptions()), _sub_att(ORB_ID(vehicle_attitude), 1000 / 100, 0, &getSubscriptions()), + _sub_angular_velocity(ORB_ID(vehicle_angular_velocity), 1000 / 100, 0, &getSubscriptions()), // set flow max update rate higher than expected to we don't lose packets _sub_flow(ORB_ID(optical_flow), 1000 / 100, 0, &getSubscriptions()), // main prediction loop, 100 hz @@ -663,9 +664,9 @@ void BlockLocalPositionEstimator::publishOdom() _pub_odom.get().vz = xLP(X_vz); // vel down // angular velocity - _pub_odom.get().rollspeed = _sub_att.get().rollspeed; // roll rate - _pub_odom.get().pitchspeed = _sub_att.get().pitchspeed; // pitch rate - _pub_odom.get().yawspeed = _sub_att.get().yawspeed; // yaw rate + _pub_odom.get().rollspeed = _sub_angular_velocity.get().xyz[0]; // roll rate + _pub_odom.get().pitchspeed = _sub_angular_velocity.get().xyz[1]; // pitch rate + _pub_odom.get().yawspeed = _sub_angular_velocity.get().xyz[2]; // yaw rate // get the covariance matrix size const size_t POS_URT_SIZE = sizeof(_pub_odom.get().pose_covariance) / sizeof(_pub_odom.get().pose_covariance[0]); diff --git a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp index c38e073fa3..ef6126e2f7 100644 --- a/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp +++ b/src/modules/local_position_estimator/BlockLocalPositionEstimator.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -249,6 +250,7 @@ private: uORB::SubscriptionPollable _sub_armed; uORB::SubscriptionPollable _sub_land; uORB::SubscriptionPollable _sub_att; + uORB::SubscriptionPollable _sub_angular_velocity; uORB::SubscriptionPollable _sub_flow; uORB::SubscriptionPollable _sub_sensor; uORB::SubscriptionPollable _sub_param_update; diff --git a/src/modules/local_position_estimator/sensors/flow.cpp b/src/modules/local_position_estimator/sensors/flow.cpp index c6fd629605..60b0c03ff9 100644 --- a/src/modules/local_position_estimator/sensors/flow.cpp +++ b/src/modules/local_position_estimator/sensors/flow.cpp @@ -152,9 +152,10 @@ void BlockLocalPositionEstimator::flowCorrect() // compute polynomial value float flow_vxy_stddev = p[0] * h + p[1] * h * h + p[2] * v + p[3] * v * h + p[4] * v * h * h; - float rotrate_sq = _sub_att.get().rollspeed * _sub_att.get().rollspeed - + _sub_att.get().pitchspeed * _sub_att.get().pitchspeed - + _sub_att.get().yawspeed * _sub_att.get().yawspeed; + const Vector3f rates{_sub_angular_velocity.get().xyz}; + float rotrate_sq = rates(0) * rates(0) + + rates(1) * rates(1) + + rates(2) * rates(2); matrix::Eulerf euler(matrix::Quatf(_sub_att.get().q)); float rot_sq = euler.phi() * euler.phi() + euler.theta() * euler.theta(); diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index ebd4beb1c1..b5935ffe86 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -594,15 +594,16 @@ void Logger::add_default_topics() add_topic("optical_flow", 50); add_topic("position_setpoint_triplet", 200); //add_topic("radio_status"); - add_topic("rate_ctrl_status", 30); + add_topic("rate_ctrl_status", 200); add_topic("sensor_combined", 100); add_topic("sensor_preflight", 200); add_topic("system_power", 500); add_topic("tecs_status", 200); - add_topic("trajectory_setpoint", 200); add_topic("telemetry_status"); + add_topic("trajectory_setpoint", 200); add_topic("vehicle_air_data", 200); - add_topic("vehicle_attitude", 30); + add_topic("vehicle_angular_velocity", 20); + add_topic("vehicle_attitude", 50); add_topic("vehicle_attitude_setpoint", 100); add_topic("vehicle_command"); add_topic("vehicle_global_position", 200); @@ -611,7 +612,7 @@ void Logger::add_default_topics() add_topic("vehicle_local_position", 100); add_topic("vehicle_local_position_setpoint", 100); add_topic("vehicle_magnetometer", 200); - add_topic("vehicle_rates_setpoint", 30); + add_topic("vehicle_rates_setpoint", 20); add_topic("vehicle_status", 200); add_topic("vehicle_status_flags"); add_topic("vtol_vehicle_status", 200); @@ -623,9 +624,10 @@ void Logger::add_default_topics() add_topic("fw_virtual_attitude_setpoint"); add_topic("mc_virtual_attitude_setpoint"); add_topic("multirotor_motor_limits"); - add_topic("position_controller_status"); add_topic("offboard_control_mode"); + add_topic("position_controller_status"); add_topic("time_offset"); + add_topic("vehicle_angular_velocity", 10); add_topic("vehicle_attitude_groundtruth", 10); add_topic("vehicle_global_position_groundtruth", 100); add_topic("vehicle_local_position_groundtruth", 100); @@ -641,6 +643,7 @@ void Logger::add_high_rate_topics() add_topic("manual_control_setpoint"); add_topic("rate_ctrl_status"); add_topic("sensor_combined"); + add_topic("vehicle_angular_velocity"); add_topic("vehicle_attitude"); add_topic("vehicle_attitude_setpoint"); add_topic("vehicle_rates_setpoint"); @@ -648,10 +651,10 @@ void Logger::add_high_rate_topics() void Logger::add_debug_topics() { + add_topic("debug_array"); add_topic("debug_key_value"); add_topic("debug_value"); add_topic("debug_vect"); - add_topic("debug_array"); } void Logger::add_estimator_replay_topics() diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 73a67ef90a..e3b8df3bcf 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -85,6 +85,7 @@ #include #include #include +#include #include #include #include @@ -1182,7 +1183,9 @@ public: private: MavlinkOrbSubscription *_att_sub; - uint64_t _att_time; + MavlinkOrbSubscription *_angular_velocity_sub; + uint64_t _att_time{0}; + uint64_t _angular_velocity_time{0}; /* do not allow top copying this class */ MavlinkStreamAttitude(MavlinkStreamAttitude &) = delete; @@ -1192,23 +1195,30 @@ private: protected: explicit MavlinkStreamAttitude(Mavlink *mavlink) : MavlinkStream(mavlink), _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), - _att_time(0) + _angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity))) {} bool send(const hrt_abstime t) { - vehicle_attitude_s att; + bool updated = false; + + vehicle_attitude_s att{}; + vehicle_angular_velocity_s angular_velocity{}; + updated |= _att_sub->update(&_att_time, &att); + updated |= _angular_velocity_sub->update(&_angular_velocity_time, &angular_velocity); - if (_att_sub->update(&_att_time, &att)) { - mavlink_attitude_t msg = {}; - matrix::Eulerf euler = matrix::Quatf(att.q); - msg.time_boot_ms = att.timestamp / 1000; + if (updated) { + mavlink_attitude_t msg{}; + + const matrix::Eulerf euler = matrix::Quatf(att.q); + msg.time_boot_ms = math::max(angular_velocity.timestamp, att.timestamp) / 1000; msg.roll = euler.phi(); msg.pitch = euler.theta(); msg.yaw = euler.psi(); - msg.rollspeed = att.rollspeed; - msg.pitchspeed = att.pitchspeed; - msg.yawspeed = att.yawspeed; + + msg.rollspeed = angular_velocity.xyz[0]; + msg.pitchspeed = angular_velocity.xyz[1]; + msg.yawspeed = angular_velocity.xyz[2]; mavlink_msg_attitude_send_struct(_mavlink->get_channel(), &msg); @@ -1255,7 +1265,9 @@ public: private: MavlinkOrbSubscription *_att_sub; - uint64_t _att_time; + MavlinkOrbSubscription *_angular_velocity_sub; + uint64_t _att_time{0}; + uint64_t _angular_velocity_time{0}; /* do not allow top copying this class */ MavlinkStreamAttitudeQuaternion(MavlinkStreamAttitudeQuaternion &) = delete; @@ -1264,24 +1276,29 @@ private: protected: explicit MavlinkStreamAttitudeQuaternion(Mavlink *mavlink) : MavlinkStream(mavlink), _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude))), - _att_time(0) + _angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity))) {} bool send(const hrt_abstime t) { - vehicle_attitude_s att; + bool updated = false; + + vehicle_attitude_s att{}; + vehicle_angular_velocity_s angular_velocity{}; + updated |= _att_sub->update(&_att_time, &att); + updated |= _angular_velocity_sub->update(&_angular_velocity_time, &angular_velocity); - if (_att_sub->update(&_att_time, &att)) { - mavlink_attitude_quaternion_t msg = {}; + if (updated) { + mavlink_attitude_quaternion_t msg{}; - msg.time_boot_ms = att.timestamp / 1000; + msg.time_boot_ms = math::max(angular_velocity.timestamp, att.timestamp) / 1000; msg.q1 = att.q[0]; msg.q2 = att.q[1]; msg.q3 = att.q[2]; msg.q4 = att.q[3]; - msg.rollspeed = att.rollspeed; - msg.pitchspeed = att.pitchspeed; - msg.yawspeed = att.yawspeed; + msg.rollspeed = angular_velocity.xyz[0]; + msg.pitchspeed = angular_velocity.xyz[1]; + msg.yawspeed = angular_velocity.xyz[2]; mavlink_msg_attitude_quaternion_send_struct(_mavlink->get_channel(), &msg); @@ -1292,7 +1309,6 @@ protected: } }; - class MavlinkStreamVFRHUD : public MavlinkStream { public: @@ -4749,10 +4765,15 @@ public: } private: + MavlinkOrbSubscription *_angular_velocity_sub; MavlinkOrbSubscription *_att_sub; MavlinkOrbSubscription *_gpos_sub; - uint64_t _att_time; - uint64_t _gpos_time; + MavlinkOrbSubscription *_lpos_sub; + + uint64_t _angular_velocity_time{0}; + uint64_t _att_time{0}; + uint64_t _gpos_time{0}; + uint64_t _lpos_time{0}; /* do not allow top copying this class */ MavlinkStreamGroundTruth(MavlinkStreamGroundTruth &) = delete; @@ -4760,20 +4781,27 @@ private: protected: explicit MavlinkStreamGroundTruth(Mavlink *mavlink) : MavlinkStream(mavlink), + _angular_velocity_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_angular_velocity_groundtruth))), _att_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_groundtruth))), _gpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_global_position_groundtruth))), - _att_time(0), - _gpos_time(0) + _lpos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position_groundtruth))) {} bool send(const hrt_abstime t) { - vehicle_attitude_s att = {}; - vehicle_global_position_s gpos = {}; - bool att_updated = _att_sub->update(&_att_time, &att); - bool gpos_updated = _gpos_sub->update(&_gpos_time, &gpos); + bool updated = false; + + vehicle_angular_velocity_s angular_velocity{}; + vehicle_attitude_s att{}; + vehicle_global_position_s gpos{}; + vehicle_local_position_s lpos{}; - if (att_updated || gpos_updated) { + updated |= _angular_velocity_sub->update(&_angular_velocity_time, &angular_velocity); + updated |= _att_sub->update(&_att_time, &att); + updated |= _gpos_sub->update(&_gpos_time, &gpos); + updated |= _lpos_sub->update(&_lpos_time, &lpos); + + if (updated) { mavlink_hil_state_quaternion_t msg = {}; // vehicle_attitude -> hil_state_quaternion @@ -4781,23 +4809,23 @@ protected: msg.attitude_quaternion[1] = att.q[1]; msg.attitude_quaternion[2] = att.q[2]; msg.attitude_quaternion[3] = att.q[3]; - msg.rollspeed = att.rollspeed; - msg.pitchspeed = att.pitchspeed; - msg.yawspeed = att.yawspeed; + msg.rollspeed = angular_velocity.xyz[0]; + msg.pitchspeed = angular_velocity.xyz[1]; + msg.yawspeed = angular_velocity.xyz[2]; // vehicle_global_position -> hil_state_quaternion // same units as defined in mavlink/common.xml msg.lat = gpos.lat * 1e7; msg.lon = gpos.lon * 1e7; msg.alt = gpos.alt * 1e3f; - msg.vx = gpos.vel_n * 1e2f; - msg.vy = gpos.vel_e * 1e2f; - msg.vz = gpos.vel_d * 1e2f; + msg.vx = lpos.vx * 1e2f; + msg.vy = lpos.vy * 1e2f; + msg.vz = lpos.vz * 1e2f; msg.ind_airspeed = 0; msg.true_airspeed = 0; - msg.xacc = 0; - msg.yacc = 0; - msg.zacc = 0; + msg.xacc = lpos.ax; + msg.yacc = lpos.ay; + msg.zacc = lpos.az; mavlink_msg_hil_state_quaternion_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c57d460869..8c0383dde9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2396,10 +2396,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) matrix::Quatf q(hil_state.attitude_quaternion); q.copyTo(hil_attitude.q); - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; - if (_attitude_pub == nullptr) { _attitude_pub = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); @@ -2514,6 +2510,27 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) } } + /* gyroscope */ + { + sensor_gyro_s gyro = {}; + + gyro.timestamp = timestamp; + gyro.x_raw = hil_state.rollspeed * 1e3f; + gyro.y_raw = hil_state.pitchspeed * 1e3f; + gyro.z_raw = hil_state.yawspeed * 1e3f; + gyro.x = hil_state.rollspeed; + gyro.y = hil_state.pitchspeed; + gyro.z = hil_state.yawspeed; + gyro.temperature = 25.0f; + + if (_gyro_pub == nullptr) { + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); + + } else { + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); + } + } + /* battery status */ { struct battery_status_s hil_battery_status = {}; diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt index 3734e79b29..1d0c445fdd 100644 --- a/src/modules/mc_att_control/CMakeLists.txt +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -46,4 +46,5 @@ px4_add_module( conversion mathlib AttitudeControl + px4_work_queue ) diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index d6019cb281..4965901da2 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -40,18 +40,17 @@ #include #include #include -#include +#include #include #include +#include #include #include #include #include #include #include -#include -#include -#include +#include #include #include #include @@ -68,37 +67,36 @@ */ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); -#define MAX_GYRO_COUNT 3 - - -class MulticopterAttitudeControl : public ModuleBase, public ModuleParams +class MulticopterAttitudeControl : public ModuleBase, public ModuleParams, + public px4::WorkItem { public: MulticopterAttitudeControl(); - virtual ~MulticopterAttitudeControl() = default; + virtual ~MulticopterAttitudeControl(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); - /** @see ModuleBase */ - static MulticopterAttitudeControl *instantiate(int argc, char *argv[]); - /** @see ModuleBase */ static int custom_command(int argc, char *argv[]); /** @see ModuleBase */ static int print_usage(const char *reason = nullptr); - /** @see ModuleBase::run() */ - void run() override; + /** @see ModuleBase::print_status() */ + int print_status() override; + + void Run() override; + + bool init(); private: /** * initialize some vectors/matrices from parameters */ - void parameters_updated(); + void parameters_updated(); /** * Check for parameter update and handle it. @@ -134,7 +132,7 @@ private: /** * Attitude rates controller. */ - void control_attitude_rates(float dt); + void control_attitude_rates(float dt, const matrix::Vector3f &rates); /** * Throttle PID attenuation. @@ -152,15 +150,10 @@ private: uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)}; /**< motor limits subscription */ uORB::Subscription _battery_status_sub{ORB_ID(battery_status)}; /**< battery status subscription */ - uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; /**< vehicle land detected subscription */ uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)}; - int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */ - - unsigned _gyro_count{1}; - int _selected_gyro{0}; + uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)}; uORB::Publication _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */ uORB::Publication _landing_gear_pub{ORB_ID(landing_gear)}; @@ -182,9 +175,6 @@ private: struct actuator_controls_s _actuators {}; /**< actuator controls */ struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ struct battery_status_s _battery_status {}; /**< battery status */ - struct sensor_gyro_s _sensor_gyro {}; /**< gyro data before thermal correctons and ekf bias estimates are applied */ - struct sensor_correction_s _sensor_correction {}; /**< sensor thermal corrections */ - struct sensor_bias_s _sensor_bias {}; /**< sensor in-run bias corrections */ struct vehicle_land_detected_s _vehicle_land_detected {}; struct landing_gear_s _landing_gear {}; @@ -204,11 +194,17 @@ private: matrix::Vector3f _att_control; /**< attitude control vector */ float _thrust_sp{0.0f}; /**< thrust setpoint */ - matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - float _man_yaw_sp{0.f}; /**< current yaw setpoint in manual mode */ bool _gear_state_initialized{false}; /**< true if the gear state has been initialized */ + hrt_abstime _task_start{hrt_absolute_time()}; + hrt_abstime _last_run{0}; + float _dt_accumulator{0.0f}; + int _loop_counter{0}; + + bool _reset_yaw_sp{true}; + float _attitude_dt{0.0f}; + DEFINE_PARAMETERS( (ParamFloat) _param_mc_roll_p, (ParamFloat) _param_mc_rollrate_p, @@ -260,12 +256,6 @@ private: (ParamBool) _param_mc_bat_scale_en, - (ParamInt) _param_sens_board_rot, - - (ParamFloat) _param_sens_board_x_off, - (ParamFloat) _param_sens_board_y_off, - (ParamFloat) _param_sens_board_z_off, - /* Stabilized mode params */ (ParamFloat) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */ (ParamFloat) _param_mpc_manthr_min, /**< minimum throttle for stabilized */ 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 e1e79c887a..659b662a2e 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -63,12 +63,9 @@ using namespace matrix; MulticopterAttitudeControl::MulticopterAttitudeControl() : ModuleParams(nullptr), + WorkItem(px4::wq_configurations::rate_ctrl), _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { - for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) { - _sensor_gyro_sub[i] = -1; - } - _vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING; /* initialize quaternions in messages to be valid */ @@ -82,15 +79,23 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _thrust_sp = 0.0f; _att_control.zero(); - /* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */ - for (unsigned i = 0; i < 3; i++) { - // used scale factors to unity - _sensor_correction.gyro_scale_0[i] = 1.0f; - _sensor_correction.gyro_scale_1[i] = 1.0f; - _sensor_correction.gyro_scale_2[i] = 1.0f; + parameters_updated(); +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ + perf_free(_loop_perf); +} + +bool +MulticopterAttitudeControl::init() +{ + if (!_vehicle_angular_velocity_sub.register_callback()) { + PX4_ERR("vehicle_angular_velocity callback registration failed!"); + return false; } - parameters_updated(); + return true; } void @@ -127,16 +132,6 @@ MulticopterAttitudeControl::parameters_updated() _man_tilt_max = math::radians(_param_mpc_man_tilt_max.get()); _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); - - /* get transformation matrix from sensor/board to body frame */ - _board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); - - /* fine tune the rotation */ - Dcmf board_rotation_offset(Eulerf( - M_DEG_TO_RAD_F * _param_sens_board_x_off.get(), - M_DEG_TO_RAD_F * _param_sens_board_y_off.get(), - M_DEG_TO_RAD_F * _param_sens_board_z_off.get())); - _board_rotation = board_rotation_offset * _board_rotation; } void @@ -403,45 +398,13 @@ MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rat * Output: '_att_control' vector */ void -MulticopterAttitudeControl::control_attitude_rates(float dt) +MulticopterAttitudeControl::control_attitude_rates(float dt, const Vector3f &rates) { /* reset integral if disarmed */ if (!_v_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { _rates_int.zero(); } - // get the raw gyro data and correct for thermal errors - Vector3f rates; - - if (_selected_gyro == 0) { - rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0]; - rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1]; - rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2]; - - } else if (_selected_gyro == 1) { - rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0]; - rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1]; - rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2]; - - } else if (_selected_gyro == 2) { - rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0]; - rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1]; - rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2]; - - } else { - rates(0) = _sensor_gyro.x; - rates(1) = _sensor_gyro.y; - rates(2) = _sensor_gyro.z; - } - - // rotate corrected measurements from sensor to body frame - rates = _board_rotation * rates; - - // correct for in-run bias errors - rates(0) -= _sensor_bias.gyro_x_bias; - rates(1) -= _sensor_bias.gyro_y_bias; - rates(2) -= _sensor_bias.gyro_z_bias; - Vector3f rates_p_scaled = _rate_p.emult(pid_attenuations(_param_mc_tpa_break_p.get(), _param_mc_tpa_rate_p.get())); Vector3f rates_i_scaled = _rate_i.emult(pid_attenuations(_param_mc_tpa_break_i.get(), _param_mc_tpa_rate_i.get())); Vector3f rates_d_scaled = _rate_d.emult(pid_attenuations(_param_mc_tpa_break_d.get(), _param_mc_tpa_rate_d.get())); @@ -509,7 +472,6 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* explicitly limit the integrator state */ for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { _rates_int(i) = math::constrain(_rates_int(i), -_rate_int_lim(i), _rate_int_lim(i)); - } } @@ -532,9 +494,6 @@ MulticopterAttitudeControl::publish_rate_controller_status() { rate_ctrl_status_s rate_ctrl_status = {}; rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.rollspeed = _rates_prev(0); - rate_ctrl_status.pitchspeed = _rates_prev(1); - rate_ctrl_status.yawspeed = _rates_prev(2); rate_ctrl_status.rollspeed_integ = _rates_int(0); rate_ctrl_status.pitchspeed_integ = _rates_int(1); rate_ctrl_status.yawspeed_integ = _rates_int(2); @@ -550,8 +509,8 @@ MulticopterAttitudeControl::publish_actuator_controls() _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.control[7] = (float)_landing_gear.landing_gear; + // note: _actuators.timestamp_sample is set in MulticopterAttitudeControl::Run() _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _sensor_gyro.timestamp; /* scale effort by battery status */ if (_param_mc_bat_scale_en.get() && _battery_status.scale > 0.0f) { @@ -566,222 +525,190 @@ MulticopterAttitudeControl::publish_actuator_controls() } void -MulticopterAttitudeControl::run() +MulticopterAttitudeControl::Run() { - _gyro_count = math::constrain(orb_group_count(ORB_ID(sensor_gyro)), 1, MAX_GYRO_COUNT); - - for (unsigned s = 0; s < _gyro_count; s++) { - _sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + if (should_exit()) { + _vehicle_angular_velocity_sub.unregister_callback(); + exit_and_cleanup(); + return; } - /* wakeup source: gyro data from sensor selected by the sensor app */ - px4_pollfd_struct_t poll_fds = {}; - poll_fds.events = POLLIN; + perf_begin(_loop_perf); - const hrt_abstime task_start = hrt_absolute_time(); - hrt_abstime last_run = task_start; - float dt_accumulator = 0.f; - int loop_counter = 0; + /* run controller on gyro changes */ + vehicle_angular_velocity_s angular_velocity; - bool reset_yaw_sp = true; - float attitude_dt = 0.f; + if (_vehicle_angular_velocity_sub.update(&angular_velocity)) { + const hrt_abstime now = hrt_absolute_time(); - while (!should_exit()) { + // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. + const float dt = math::constrain(((now - _last_run) / 1e6f), 0.0002f, 0.02f); + _last_run = now; - // check if the selected gyro has updated first - _sensor_correction_sub.update(&_sensor_correction); + const Vector3f rates{angular_velocity.xyz}; - /* update the latest gyro selection */ - if (_sensor_correction.selected_gyro_instance < _gyro_count) { - _selected_gyro = _sensor_correction.selected_gyro_instance; - } - - poll_fds.fd = _sensor_gyro_sub[_selected_gyro]; + _actuators.timestamp_sample = angular_velocity.timestamp_sample; - /* wait for up to 100ms for data */ - int pret = px4_poll(&poll_fds, 1, 100); + /* run the rate controller immediately after a gyro update */ + if (_v_control_mode.flag_control_rates_enabled) { + control_attitude_rates(dt, rates); - /* timed out - periodic check for should_exit() */ - if (pret == 0) { - continue; + publish_actuator_controls(); + publish_rate_controller_status(); } - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - PX4_ERR("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - px4_usleep(100000); - continue; + /* check for updates in other topics */ + _v_control_mode_sub.update(&_v_control_mode); + _battery_status_sub.update(&_battery_status); + _vehicle_land_detected_sub.update(&_vehicle_land_detected); + _landing_gear_sub.update(&_landing_gear); + vehicle_status_poll(); + vehicle_motor_limits_poll(); + const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); + const bool attitude_updated = vehicle_attitude_poll(); + + _attitude_dt += dt; + + /* Check if we are in rattitude mode and the pilot is above the threshold on pitch + * or roll (yaw can rotate 360 in normal att control). If both are true don't + * even bother running the attitude controllers */ + if (_v_control_mode.flag_control_rattitude_enabled) { + _v_control_mode.flag_control_attitude_enabled = + fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() && + fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get(); } - perf_begin(_loop_perf); + bool attitude_setpoint_generated = false; - /* run controller on gyro changes */ - if (poll_fds.revents & POLLIN) { - const hrt_abstime now = hrt_absolute_time(); + const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING + && !_vehicle_status.in_transition_mode; - // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((now - last_run) / 1e6f), 0.0002f, 0.02f); - last_run = now; + // vehicle is a tailsitter in transition mode + const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter; - /* copy gyro data */ - orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro); + bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); - /* run the rate controller immediately after a gyro update */ - if (_v_control_mode.flag_control_rates_enabled) { - control_attitude_rates(dt); - - publish_actuator_controls(); - publish_rate_controller_status(); - } - - /* check for updates in other topics */ - _v_control_mode_sub.update(&_v_control_mode); - _battery_status_sub.update(&_battery_status); - _sensor_bias_sub.update(&_sensor_bias); - _vehicle_land_detected_sub.update(&_vehicle_land_detected); - _landing_gear_sub.update(&_landing_gear); - vehicle_status_poll(); - vehicle_motor_limits_poll(); - const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp); - const bool attitude_updated = vehicle_attitude_poll(); - - attitude_dt += dt; - - /* Check if we are in rattitude mode and the pilot is above the threshold on pitch - * or roll (yaw can rotate 360 in normal att control). If both are true don't - * even bother running the attitude controllers */ - if (_v_control_mode.flag_control_rattitude_enabled) { - _v_control_mode.flag_control_attitude_enabled = - fabsf(_manual_control_sp.y) <= _param_mc_ratt_th.get() && - fabsf(_manual_control_sp.x) <= _param_mc_ratt_th.get(); - } - - bool attitude_setpoint_generated = false; - - const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING - && !_vehicle_status.in_transition_mode; - - // vehicle is a tailsitter in transition mode - const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter; - - bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition); + if (run_att_ctrl) { + if (attitude_updated) { + // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode + if (_v_control_mode.flag_control_manual_enabled && + !_v_control_mode.flag_control_altitude_enabled && + !_v_control_mode.flag_control_velocity_enabled && + !_v_control_mode.flag_control_position_enabled) { + generate_attitude_setpoint(_attitude_dt, _reset_yaw_sp); + attitude_setpoint_generated = true; + } - if (run_att_ctrl) { - if (attitude_updated) { - // Generate the attitude setpoint from stick inputs if we are in Manual/Stabilized mode - if (_v_control_mode.flag_control_manual_enabled && - !_v_control_mode.flag_control_altitude_enabled && - !_v_control_mode.flag_control_velocity_enabled && - !_v_control_mode.flag_control_position_enabled) { - generate_attitude_setpoint(attitude_dt, reset_yaw_sp); - attitude_setpoint_generated = true; - } + control_attitude(); - control_attitude(); + if (_v_control_mode.flag_control_yawrate_override_enabled) { + /* Yaw rate override enabled, overwrite the yaw setpoint */ + _v_rates_sp_sub.update(&_v_rates_sp); + const auto yawrate_reference = _v_rates_sp.yaw; + _rates_sp(2) = yawrate_reference; + } - if (_v_control_mode.flag_control_yawrate_override_enabled) { - /* Yaw rate override enabled, overwrite the yaw setpoint */ - _v_rates_sp_sub.update(&_v_rates_sp); - const auto yawrate_reference = _v_rates_sp.yaw; - _rates_sp(2) = yawrate_reference; - } + publish_rates_setpoint(); + } + } else { + /* attitude controller disabled, poll rates setpoint topic */ + if (_v_control_mode.flag_control_manual_enabled && is_hovering) { + if (manual_control_updated) { + /* manual rates control - ACRO mode */ + Vector3f man_rate_sp( + math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), + math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())); + _rates_sp = man_rate_sp.emult(_acro_rate_max); + _thrust_sp = _manual_control_sp.z; publish_rates_setpoint(); } } else { /* attitude controller disabled, poll rates setpoint topic */ - if (_v_control_mode.flag_control_manual_enabled && is_hovering) { - - if (manual_control_updated) { - /* manual rates control - ACRO mode */ - Vector3f man_rate_sp( - math::superexpo(_manual_control_sp.y, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(-_manual_control_sp.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()), - math::superexpo(_manual_control_sp.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())); - _rates_sp = man_rate_sp.emult(_acro_rate_max); - _thrust_sp = _manual_control_sp.z; - publish_rates_setpoint(); - } - - } else { - /* attitude controller disabled, poll rates setpoint topic */ - if (_v_rates_sp_sub.update(&_v_rates_sp)) { - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = -_v_rates_sp.thrust_body[2]; - } + if (_v_rates_sp_sub.update(&_v_rates_sp)) { + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = -_v_rates_sp.thrust_body[2]; } } + } - if (_v_control_mode.flag_control_termination_enabled) { - if (!_vehicle_status.is_vtol) { - _rates_sp.zero(); - _rates_int.zero(); - _thrust_sp = 0.0f; - _att_control.zero(); - publish_actuator_controls(); - } + if (_v_control_mode.flag_control_termination_enabled) { + if (!_vehicle_status.is_vtol) { + _rates_sp.zero(); + _rates_int.zero(); + _thrust_sp = 0.0f; + _att_control.zero(); + publish_actuator_controls(); } + } - if (attitude_updated) { - // reset yaw setpoint during transitions, tailsitter.cpp generates - // attitude setpoint for the transition - reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) || - _vehicle_land_detected.landed || - (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode); + if (attitude_updated) { + // reset yaw setpoint during transitions, tailsitter.cpp generates + // attitude setpoint for the transition + _reset_yaw_sp = (!attitude_setpoint_generated && !_v_control_mode.flag_control_rattitude_enabled) || + _vehicle_land_detected.landed || + (_vehicle_status.is_vtol && _vehicle_status.in_transition_mode); - attitude_dt = 0.f; - } + _attitude_dt = 0.f; + } - /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ - if (!_v_control_mode.flag_armed || (now - task_start) < 3300000) { - dt_accumulator += dt; - ++loop_counter; - - if (dt_accumulator > 1.f) { - const float loop_update_rate = (float)loop_counter / dt_accumulator; - _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; - dt_accumulator = 0; - loop_counter = 0; - _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _param_mc_dterm_cutoff.get()); - } + /* calculate loop update rate while disarmed or at least a few times (updating the filter is expensive) */ + if (!_v_control_mode.flag_armed || (now - _task_start) < 3300000) { + _dt_accumulator += dt; + ++_loop_counter; + + if (_dt_accumulator > 1.f) { + const float loop_update_rate = (float)_loop_counter / _dt_accumulator; + _loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f; + _dt_accumulator = 0; + _loop_counter = 0; + _lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _param_mc_dterm_cutoff.get()); } - - parameter_update_poll(); } - perf_end(_loop_perf); + parameter_update_poll(); } - for (unsigned s = 0; s < _gyro_count; s++) { - orb_unsubscribe(_sensor_gyro_sub[s]); - } + perf_end(_loop_perf); } int MulticopterAttitudeControl::task_spawn(int argc, char *argv[]) { - _task_id = px4_task_spawn_cmd("mc_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_ATTITUDE_CONTROL, - 1700, - (px4_main_t)&run_trampoline, - (char *const *)argv); - - if (_task_id < 0) { - _task_id = -1; - return -errno; + MulticopterAttitudeControl *instance = new MulticopterAttitudeControl(); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); } - return 0; + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; } -MulticopterAttitudeControl *MulticopterAttitudeControl::instantiate(int argc, char *argv[]) +int MulticopterAttitudeControl::print_status() { - return new MulticopterAttitudeControl(); + PX4_INFO("Running"); + + perf_print_counter(_loop_perf); + + print_message(_actuators); + + return 0; } int MulticopterAttitudeControl::custom_command(int argc, char *argv[]) diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index c4e4dd25a0..611201aa4e 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -31,6 +31,8 @@ # ############################################################################ +add_subdirectory(vehicle_angular_velocity) + px4_add_module( MODULE modules__sensors MAIN sensors @@ -51,4 +53,5 @@ px4_add_module( git_ecl ecl_validation mathlib + sensors__vehicle_angular_velocity ) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4f838af851..b362f5b40f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -92,6 +92,8 @@ #include "rc_update.h" #include "voted_sensors_update.h" +#include "vehicle_angular_velocity/VehicleAngularVelocity.hpp" + using namespace DriverFramework; using namespace sensors; using namespace time_literals; @@ -135,7 +137,7 @@ class Sensors : public ModuleBase, public ModuleParams { public: Sensors(bool hil_enabled); - ~Sensors() = default; + ~Sensors() override; /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -202,6 +204,9 @@ private: VotedSensorsUpdate _voted_sensors_update; + VehicleAngularVelocity _angular_velocity; + + /** * Update our local parameter cache. */ @@ -253,6 +258,13 @@ Sensors::Sensors(bool hil_enabled) : } #endif /* BOARD_NUMBER_BRICKS > 0 */ + + _angular_velocity.Start(); +} + +Sensors::~Sensors() +{ + _angular_velocity.Stop(); } int @@ -724,6 +736,8 @@ int Sensors::print_status() PX4_INFO("Airspeed status:"); _airspeed_validator.print(); + _angular_velocity.PrintStatus(); + return 0; } diff --git a/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt new file mode 100644 index 0000000000..58aa0d92e3 --- /dev/null +++ b/src/modules/sensors/vehicle_angular_velocity/CMakeLists.txt @@ -0,0 +1,37 @@ +############################################################################ +# +# Copyright (c) 2019 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. +# +############################################################################ + +px4_add_library(sensors__vehicle_angular_velocity + VehicleAngularVelocity.cpp +) +target_link_libraries(sensors__vehicle_angular_velocity PRIVATE px4_work_queue) diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp new file mode 100644 index 0000000000..9f7d312bcc --- /dev/null +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#include "VehicleAngularVelocity.hpp" + +#include + +using namespace time_literals; +using namespace matrix; + +VehicleAngularVelocity::VehicleAngularVelocity() : + ModuleParams(nullptr), + WorkItem(px4::wq_configurations::rate_ctrl), + _cycle_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: cycle time")), + _interval_perf(perf_alloc(PC_INTERVAL, "vehicle_angular_velocity: interval")), + _sensor_gyro_latency_perf(perf_alloc(PC_ELAPSED, "vehicle_angular_velocity: sensor gyro latency")) +{ +} + +VehicleAngularVelocity::~VehicleAngularVelocity() +{ + Stop(); + + perf_free(_cycle_perf); + perf_free(_interval_perf); + perf_free(_sensor_gyro_latency_perf); +} + +bool +VehicleAngularVelocity::Start() +{ + // initialize thermal corrections as we might not immediately get a topic update (only non-zero values) + _scale = Vector3f{1.0f, 1.0f, 1.0f}; + _offset.zero(); + _bias.zero(); + + // force initial updates + ParametersUpdate(true); + SensorBiasUpdate(true); + + _sensor_correction_sub.register_callback(); + + return SensorCorrectionsUpdate(true); +} + +void +VehicleAngularVelocity::Stop() +{ + Deinit(); + + // clear all registered callbacks + for (auto sub : _sensor_gyro_sub) { + sub.unregister_callback(); + } + + _sensor_correction_sub.unregister_callback(); +} + +void +VehicleAngularVelocity::SensorBiasUpdate(bool force) +{ + if (_sensor_bias_sub.updated() || force) { + sensor_bias_s bias; + + if (_sensor_bias_sub.copy(&bias)) { + // TODO: should be checking device ID + _bias(0) = bias.gyro_x_bias; + _bias(1) = bias.gyro_y_bias; + _bias(2) = bias.gyro_z_bias; + } + } +} + +bool +VehicleAngularVelocity::SensorCorrectionsUpdate(bool force) +{ + // check if the selected gyro has updated + if (_sensor_correction_sub.updated() || force) { + + sensor_correction_s corrections{}; + _sensor_correction_sub.copy(&corrections); + + // TODO: should be checking device ID + if (_selected_gyro == 0) { + _offset = Vector3f{corrections.gyro_offset_0}; + _scale = Vector3f{corrections.gyro_scale_0}; + + } else if (_selected_gyro == 1) { + _offset = Vector3f{corrections.gyro_offset_1}; + _scale = Vector3f{corrections.gyro_scale_1}; + + } else if (_selected_gyro == 2) { + _offset = Vector3f{corrections.gyro_offset_2}; + _scale = Vector3f{corrections.gyro_scale_2}; + + } else { + _offset = Vector3f{0.0f, 0.0f, 0.0f}; + _scale = Vector3f{1.0f, 1.0f, 1.0f}; + } + + // update the latest gyro selection + if (_selected_gyro != corrections.selected_gyro_instance) { + if (corrections.selected_gyro_instance < MAX_GYRO_COUNT) { + // clear all registered callbacks + for (auto sub : _sensor_gyro_sub) { + sub.unregister_callback(); + } + + const int gyro_new = corrections.selected_gyro_instance; + + if (_sensor_gyro_sub[gyro_new].register_callback()) { + PX4_DEBUG("selected gyro changed %d -> %d", _selected_gyro, gyro_new); + _selected_gyro = gyro_new; + + return true; + } + } + } + } + + return false; +} + +void +VehicleAngularVelocity::ParametersUpdate(bool force) +{ + // Check if parameters have changed + if (_params_sub.updated() || force) { + // clear update + parameter_update_s param_update; + _params_sub.copy(¶m_update); + + updateParams(); + + // get transformation matrix from sensor/board to body frame + const matrix::Dcmf board_rotation = get_rot_matrix((enum Rotation)_param_sens_board_rot.get()); + + // fine tune the rotation + const Dcmf board_rotation_offset(Eulerf( + math::radians(_param_sens_board_x_off.get()), + math::radians(_param_sens_board_y_off.get()), + math::radians(_param_sens_board_z_off.get()))); + + _board_rotation = board_rotation_offset * board_rotation; + } +} + +void +VehicleAngularVelocity::Run() +{ + perf_begin(_cycle_perf); + perf_count(_interval_perf); + + // update corrections first to set _selected_gyro + SensorCorrectionsUpdate(); + + sensor_gyro_s sensor_gyro; + + if (_sensor_gyro_sub[_selected_gyro].update(&sensor_gyro)) { + perf_set_elapsed(_sensor_gyro_latency_perf, hrt_elapsed_time(&sensor_gyro.timestamp)); + + ParametersUpdate(); + SensorBiasUpdate(); + + // get the raw gyro data and correct for thermal errors + const Vector3f gyro{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z}; + + // apply offsets and scale + Vector3f rates{(gyro - _offset).emult(_scale)}; + + // rotate corrected measurements from sensor to body frame + rates = _board_rotation * rates; + + // correct for in-run bias errors + rates -= _bias; + + vehicle_angular_velocity_s angular_velocity; + angular_velocity.timestamp_sample = sensor_gyro.timestamp; + rates.copyTo(angular_velocity.xyz); + angular_velocity.timestamp = hrt_absolute_time(); + + _vehicle_angular_velocity_pub.publish(angular_velocity); + } + + perf_end(_cycle_perf); +} + +void +VehicleAngularVelocity::PrintStatus() +{ + PX4_INFO("selected gyro: %d", _selected_gyro); + + perf_print_counter(_cycle_perf); + perf_print_counter(_interval_perf); + perf_print_counter(_sensor_gyro_latency_perf); +} diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp new file mode 100644 index 0000000000..b3831fe146 --- /dev/null +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2019 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. + * + ****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define MAX_GYRO_COUNT 3 + +class VehicleAngularVelocity : public ModuleParams, public px4::WorkItem +{ +public: + + VehicleAngularVelocity(); + virtual ~VehicleAngularVelocity(); + + void Run() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + +private: + + void ParametersUpdate(bool force = false); + void SensorBiasUpdate(bool force = false); + bool SensorCorrectionsUpdate(bool force = false); + + + DEFINE_PARAMETERS( + (ParamInt) _param_sens_board_rot, + + (ParamFloat) _param_sens_board_x_off, + (ParamFloat) _param_sens_board_y_off, + (ParamFloat) _param_sens_board_z_off + ) + + uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; + + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ + + uORB::SubscriptionCallbackWorkItem _sensor_correction_sub{this, ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ + uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub[MAX_GYRO_COUNT] { /**< gyro data subscription */ + {this, ORB_ID(sensor_gyro), 0}, + {this, ORB_ID(sensor_gyro), 1}, + {this, ORB_ID(sensor_gyro), 2} + }; + + matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + + matrix::Vector3f _offset; + matrix::Vector3f _scale; + matrix::Vector3f _bias; + + perf_counter_t _cycle_perf; + perf_counter_t _interval_perf; + perf_counter_t _sensor_gyro_latency_perf; + + int _selected_gyro{-1}; + +}; diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index d7e4502b23..ca93824212 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -408,19 +408,19 @@ void Sih::send_gps() void Sih::publish_sih() { - _gpos_gt.timestamp = hrt_absolute_time(); - _gpos_gt.lat = _gps_lat_noiseless; - _gpos_gt.lon = _gps_lon_noiseless; - _gpos_gt.alt = _gps_alt_noiseless; - _gpos_gt.vel_n = _v_I(0); - _gpos_gt.vel_e = _v_I(1); - _gpos_gt.vel_d = _v_I(2); + // publish angular velocity groundtruth + _vehicle_angular_velocity_gt.timestamp = hrt_absolute_time(); + _vehicle_angular_velocity_gt.xyz[0] = _w_B(0); // rollspeed; + _vehicle_angular_velocity_gt.xyz[1] = _w_B(1); // pitchspeed; + _vehicle_angular_velocity_gt.xyz[2] = _w_B(2); // yawspeed; - if (_gpos_gt_sub != nullptr) { - orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_sub, &_gpos_gt); + if (_vehicle_angular_velocity_gt_pub != nullptr) { + orb_publish(ORB_ID(vehicle_angular_velocity_groundtruth), _vehicle_angular_velocity_gt_pub, + &_vehicle_angular_velocity_gt); } else { - _gpos_gt_sub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt); + _vehicle_angular_velocity_gt_pub = orb_advertise(ORB_ID(vehicle_angular_velocity_groundtruth), + &_vehicle_angular_velocity_gt); } // publish attitude groundtruth @@ -429,15 +429,27 @@ void Sih::publish_sih() _att_gt.q[1] = _q(1); _att_gt.q[2] = _q(2); _att_gt.q[3] = _q(3); - _att_gt.rollspeed = _w_B(0); - _att_gt.pitchspeed = _w_B(1); - _att_gt.yawspeed = _w_B(2); - if (_att_gt_sub != nullptr) { - orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_sub, &_att_gt); + if (_att_gt_pub != nullptr) { + orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_pub, &_att_gt); + + } else { + _att_gt_pub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt); + } + + _gpos_gt.timestamp = hrt_absolute_time(); + _gpos_gt.lat = _gps_lat_noiseless; + _gpos_gt.lon = _gps_lon_noiseless; + _gpos_gt.alt = _gps_alt_noiseless; + _gpos_gt.vel_n = _v_I(0); + _gpos_gt.vel_e = _v_I(1); + _gpos_gt.vel_d = _v_I(2); + + if (_gpos_gt_pub != nullptr) { + orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_pub, &_gpos_gt); } else { - _att_gt_sub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt); + _gpos_gt_pub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt); } } diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index a185668bb2..64553e6584 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -47,8 +47,9 @@ #include #include #include -#include // to publish groundtruth +#include // to publish groundtruth #include // to publish groundtruth +#include // to publish groundtruth #include extern "C" __EXPORT int sih_main(int argc, char *argv[]); @@ -103,14 +104,20 @@ private: PX4Barometer _px4_baro{ 6620172, ORB_PRIO_DEFAULT }; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION // to publish the gps position - struct vehicle_gps_position_s _vehicle_gps_pos {}; - orb_advert_t _vehicle_gps_pos_pub{nullptr}; + vehicle_gps_position_s _vehicle_gps_pos{}; + orb_advert_t _vehicle_gps_pos_pub{nullptr}; + + // angular velocity groundtruth + vehicle_angular_velocity_s _vehicle_angular_velocity_gt{}; + orb_advert_t _vehicle_angular_velocity_gt_pub{nullptr}; + // attitude groundtruth - struct vehicle_global_position_s _gpos_gt {}; - orb_advert_t _gpos_gt_sub{nullptr}; + vehicle_attitude_s _att_gt{}; + orb_advert_t _att_gt_pub{nullptr}; + // global position groundtruth - struct vehicle_attitude_s _att_gt {}; - orb_advert_t _att_gt_sub{nullptr}; + vehicle_global_position_s _gpos_gt{}; + orb_advert_t _gpos_gt_pub{nullptr}; int _parameter_update_sub {-1}; int _actuator_out_sub {-1}; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 79519cb654..2518e3ba45 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -273,6 +274,7 @@ private: static void *sending_trampoline(void *); // uORB publisher handlers + orb_advert_t _vehicle_angular_velocity_pub{nullptr}; orb_advert_t _attitude_pub{nullptr}; orb_advert_t _gpos_pub{nullptr}; orb_advert_t _lpos_pub{nullptr}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a829512d00..422c31a1ac 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -408,25 +408,36 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg uint64_t timestamp = hrt_absolute_time(); + /* angular velocity */ + vehicle_angular_velocity_s hil_angular_velocity{}; + { + hil_angular_velocity.timestamp = timestamp; + + hil_angular_velocity.xyz[0] = hil_state.rollspeed; + hil_angular_velocity.xyz[1] = hil_state.pitchspeed; + hil_angular_velocity.xyz[2] = hil_state.yawspeed; + + // always publish ground truth attitude message + int hilstate_multi; + orb_publish_auto(ORB_ID(vehicle_angular_velocity_groundtruth), &_vehicle_angular_velocity_pub, &hil_angular_velocity, + &hilstate_multi, ORB_PRIO_HIGH); + } + /* attitude */ - struct vehicle_attitude_s hil_attitude = {}; + vehicle_attitude_s hil_attitude{}; { hil_attitude.timestamp = timestamp; matrix::Quatf q(hil_state.attitude_quaternion); q.copyTo(hil_attitude.q); - hil_attitude.rollspeed = hil_state.rollspeed; - hil_attitude.pitchspeed = hil_state.pitchspeed; - hil_attitude.yawspeed = hil_state.yawspeed; - // always publish ground truth attitude message int hilstate_multi; orb_publish_auto(ORB_ID(vehicle_attitude_groundtruth), &_attitude_pub, &hil_attitude, &hilstate_multi, ORB_PRIO_HIGH); } /* global position */ - struct vehicle_global_position_s hil_gpos = {}; + vehicle_global_position_s hil_gpos{}; { hil_gpos.timestamp = timestamp;