yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00: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

23
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -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;