fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 06:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: create vehicle_angular_velocity module (#12596)

Browse Source 
* split out filtered sensor_gyro aggregation from mc_att_control and move to wq:rate_ctrl
This commit is contained in:
Daniel Agar
2019-08-06 12:55:25 -04:00
committed by GitHub
parent bf0eaf4d54
commit 2ad12d7977
34 changed files with 784 additions and 382 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_messages.cpp
+66 -38
View File
@@ -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);
src/modules/mavlink/mavlink_receiver.cpp
+21 -4
View File
@@ -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 = {};