forked from Archive/PX4-Autopilot
sensors: create vehicle_angular_velocity module (#12596)
* split out filtered sensor_gyro aggregation from mc_att_control and move to wq:rate_ctrl
This commit is contained in:
Daniel Agar
GitHub
parent
bf0eaf4d54
commit
2ad12d7977
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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 ||
|
||||
|
|
|
|||
|
|
@ -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()),
|
||||
|
|
|
|||
|
|
@ -52,6 +52,7 @@
|
|||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
|
|
@ -96,6 +97,7 @@ protected:
|
|||
uORB::SubscriptionPollable<parameter_update_s> _param_update;
|
||||
uORB::SubscriptionPollable<position_setpoint_triplet_s> _missionCmd;
|
||||
uORB::SubscriptionPollable<vehicle_attitude_s> _att;
|
||||
uORB::SubscriptionPollable<vehicle_angular_velocity_s> _angular_velocity;
|
||||
uORB::SubscriptionPollable<vehicle_attitude_setpoint_s> _attCmd;
|
||||
uORB::SubscriptionPollable<vehicle_global_position_s> _pos;
|
||||
uORB::SubscriptionPollable<vehicle_rates_setpoint_s> _ratesCmd;
|
||||
|
|
|
|||
|
|
@ -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 */
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -53,6 +53,7 @@
|
|||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/rate_ctrl_status.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
|
|
@ -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_s> _airspeed_sub{ORB_ID(airspeed)};
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -76,6 +76,7 @@
|
|||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/sensor_bias.h>
|
||||
#include <uORB/topics/tecs_status.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
|
|
@ -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_angular_velocity_s> _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 */
|
||||
|
|
|
|||
|
|
@ -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) {
|
||||
|
|
|
|||
|
|
@ -53,6 +53,7 @@
|
|||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_bias.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
|
|
@ -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 {};
|
||||
|
||||
|
|
|
|||
|
|
@ -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]);
|
||||
|
|
|
|||
|
|
@ -12,6 +12,7 @@
|
|||
#include <uORB/SubscriptionPollable.hpp>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
|
|
@ -249,6 +250,7 @@ private:
|
|||
uORB::SubscriptionPollable<actuator_armed_s> _sub_armed;
|
||||
uORB::SubscriptionPollable<vehicle_land_detected_s> _sub_land;
|
||||
uORB::SubscriptionPollable<vehicle_attitude_s> _sub_att;
|
||||
uORB::SubscriptionPollable<vehicle_angular_velocity_s> _sub_angular_velocity;
|
||||
uORB::SubscriptionPollable<optical_flow_s> _sub_flow;
|
||||
uORB::SubscriptionPollable<sensor_combined_s> _sub_sensor;
|
||||
uORB::SubscriptionPollable<parameter_update_s> _sub_param_update;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -85,6 +85,7 @@
|
|||
#include <uORB/topics/tecs_status.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/transponder_report.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
|
|
@ -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;
|
||||
|
||||
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;
|
||||
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 (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;
|
||||
|
||||
if (_att_sub->update(&_att_time, &att)) {
|
||||
mavlink_attitude_quaternion_t msg = {};
|
||||
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);
|
||||
|
||||
msg.time_boot_ms = att.timestamp / 1000;
|
||||
if (updated) {
|
||||
mavlink_attitude_quaternion_t msg{};
|
||||
|
||||
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;
|
||||
|
||||
if (att_updated || gpos_updated) {
|
||||
vehicle_angular_velocity_s angular_velocity{};
|
||||
vehicle_attitude_s att{};
|
||||
vehicle_global_position_s gpos{};
|
||||
vehicle_local_position_s lpos{};
|
||||
|
||||
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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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 = {};
|
||||
|
|
|
|||
|
|
@ -46,4 +46,5 @@ px4_add_module(
|
|||
conversion
|
||||
mathlib
|
||||
AttitudeControl
|
||||
px4_work_queue
|
||||
)
|
||||
|
|
|
|||
|
|
@ -40,18 +40,17 @@
|
|||
#include <px4_module.h>
|
||||
#include <px4_module_params.h>
|
||||
#include <px4_posix.h>
|
||||
#include <px4_tasks.h>
|
||||
#include <px4_work_queue/WorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/multirotor_motor_limits.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/rate_ctrl_status.h>
|
||||
#include <uORB/topics/sensor_bias.h>
|
||||
#include <uORB/topics/sensor_correction.h>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_attitude_setpoint.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
|
|
@ -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<MulticopterAttitudeControl>, public ModuleParams
|
||||
class MulticopterAttitudeControl : public ModuleBase<MulticopterAttitudeControl>, 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<rate_ctrl_status_s> _controller_status_pub{ORB_ID(rate_ctrl_status), ORB_PRIO_DEFAULT}; /**< controller status publication */
|
||||
uORB::Publication<landing_gear_s> _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<px4::params::MC_ROLL_P>) _param_mc_roll_p,
|
||||
(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
|
||||
|
|
@ -260,12 +256,6 @@ private:
|
|||
|
||||
(ParamBool<px4::params::MC_BAT_SCALE_EN>) _param_mc_bat_scale_en,
|
||||
|
||||
(ParamInt<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
|
||||
|
||||
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
|
||||
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
|
||||
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off,
|
||||
|
||||
/* Stabilized mode params */
|
||||
(ParamFloat<px4::params::MPC_MAN_TILT_MAX>) _param_mpc_man_tilt_max, /**< maximum tilt allowed for manual flight */
|
||||
(ParamFloat<px4::params::MPC_MANTHR_MIN>) _param_mpc_manthr_min, /**< minimum throttle for stabilized */
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
_actuators.timestamp_sample = angular_velocity.timestamp_sample;
|
||||
|
||||
/* run the rate controller immediately after a gyro update */
|
||||
if (_v_control_mode.flag_control_rates_enabled) {
|
||||
control_attitude_rates(dt, rates);
|
||||
|
||||
publish_actuator_controls();
|
||||
publish_rate_controller_status();
|
||||
}
|
||||
|
||||
poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
|
||||
/* 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();
|
||||
|
||||
/* wait for up to 100ms for data */
|
||||
int pret = px4_poll(&poll_fds, 1, 100);
|
||||
_attitude_dt += dt;
|
||||
|
||||
/* timed out - periodic check for should_exit() */
|
||||
if (pret == 0) {
|
||||
continue;
|
||||
/* 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();
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
bool attitude_setpoint_generated = false;
|
||||
|
||||
perf_begin(_loop_perf);
|
||||
const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|
||||
&& !_vehicle_status.in_transition_mode;
|
||||
|
||||
/* run controller on gyro changes */
|
||||
if (poll_fds.revents & POLLIN) {
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
// vehicle is a tailsitter in transition mode
|
||||
const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter;
|
||||
|
||||
// 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;
|
||||
bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
|
||||
|
||||
/* copy gyro data */
|
||||
orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro);
|
||||
|
||||
/* run the rate controller immediately after a gyro update */
|
||||
if (_v_control_mode.flag_control_rates_enabled) {
|
||||
control_attitude_rates(dt);
|
||||
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;
|
||||
}
|
||||
|
||||
publish_actuator_controls();
|
||||
publish_rate_controller_status();
|
||||
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;
|
||||
}
|
||||
|
||||
publish_rates_setpoint();
|
||||
}
|
||||
|
||||
/* 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;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
} 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 (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;
|
||||
}
|
||||
|
||||
/* 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);
|
||||
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);
|
||||
|
||||
_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());
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
MulticopterAttitudeControl *instance = new MulticopterAttitudeControl();
|
||||
|
||||
if (_task_id < 0) {
|
||||
_task_id = -1;
|
||||
return -errno;
|
||||
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[])
|
||||
|
|
|
|||
|
|
@ -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
|
||||
)
|
||||
|
|
|
|||
|
|
@ -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<Sensors>, 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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
@ -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 <px4_log.h>
|
||||
|
||||
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);
|
||||
}
|
||||
|
|
@ -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 <lib/perf/perf_counter.h>
|
||||
#include <px4_config.h>
|
||||
#include <px4_getopt.h>
|
||||
#include <px4_log.h>
|
||||
#include <px4_module_params.h>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/mathlib/math/Limits.hpp>
|
||||
#include <lib/mathlib/math/Functions.hpp>
|
||||
#include <lib/conversion/rotation.h>
|
||||
#include <px4_work_queue/WorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_bias.h>
|
||||
#include <uORB/topics/sensor_correction.h>
|
||||
#include <uORB/topics/sensor_gyro.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
|
||||
#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<px4::params::SENS_BOARD_ROT>) _param_sens_board_rot,
|
||||
|
||||
(ParamFloat<px4::params::SENS_BOARD_X_OFF>) _param_sens_board_x_off,
|
||||
(ParamFloat<px4::params::SENS_BOARD_Y_OFF>) _param_sens_board_y_off,
|
||||
(ParamFloat<px4::params::SENS_BOARD_Z_OFF>) _param_sens_board_z_off
|
||||
)
|
||||
|
||||
uORB::Publication<vehicle_angular_velocity_s> _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};
|
||||
|
||||
};
|
||||
|
|
@ -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_sub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt);
|
||||
_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 {
|
||||
_gpos_gt_pub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -47,8 +47,9 @@
|
|||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
#include <perf/perf_counter.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_global_position.h> // to publish groundtruth
|
||||
#include <uORB/topics/vehicle_angular_velocity.h> // to publish groundtruth
|
||||
#include <uORB/topics/vehicle_attitude.h> // to publish groundtruth
|
||||
#include <uORB/topics/vehicle_global_position.h> // to publish groundtruth
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
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};
|
||||
|
|
|
|||
|
|
@ -61,6 +61,7 @@
|
|||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <uORB/topics/optical_flow.h>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
|
|
@ -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};
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue