fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Cut case MAVLINK_MSG_ID_HIL_STATE_QUATERNION content and paste into handle_message_hil_state_quaternion() method.

Browse Source
This commit is contained in:
mcsauder 2019-02-28 10:25:32 -07:00 committed by Beat Küng
parent 8b42045546
commit b0a79996ae
2 changed files with 96 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/simulator/simulator.h
View File

@ -332,6 +332,7 @@ private:
void handle_message(mavlink_message_t *msg, bool publish);
void handle_message_distance_sensor(const mavlink_message_t *msg);
void handle_message_hil_state_quaternion(const mavlink_message_t *msg);
void handle_message_landing_target(const mavlink_message_t *msg);
void handle_message_optical_flow(const mavlink_message_t *msg);

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

@ -399,97 +399,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
}
case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
mavlink_hil_state_quaternion_t hil_state;
mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
uint64_t timestamp = hrt_absolute_time();
/* attitude */
struct 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 = {};
{
hil_gpos.timestamp = timestamp;
hil_gpos.lat = hil_state.lat / 1E7;//1E7
hil_gpos.lon = hil_state.lon / 1E7;//1E7
hil_gpos.alt = hil_state.alt / 1E3;//1E3
hil_gpos.vel_n = hil_state.vx / 100.0f;
hil_gpos.vel_e = hil_state.vy / 100.0f;
hil_gpos.vel_d = hil_state.vz / 100.0f;
// always publish ground truth attitude message
int hil_gpos_multi;
orb_publish_auto(ORB_ID(vehicle_global_position_groundtruth), &_gpos_pub, &hil_gpos, &hil_gpos_multi,
ORB_PRIO_HIGH);
}
/* local position */
struct vehicle_local_position_s hil_lpos = {};
{
hil_lpos.timestamp = timestamp;
double lat = hil_state.lat * 1e-7;
double lon = hil_state.lon * 1e-7;
if (!_hil_local_proj_inited) {
_hil_local_proj_inited = true;
map_projection_init(&_hil_local_proj_ref, lat, lon);
_hil_ref_timestamp = timestamp;
_hil_ref_lat = lat;
_hil_ref_lon = lon;
_hil_ref_alt = hil_state.alt / 1000.0f;
}
float x;
float y;
map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
hil_lpos.timestamp = timestamp;
hil_lpos.xy_valid = true;
hil_lpos.z_valid = true;
hil_lpos.v_xy_valid = true;
hil_lpos.v_z_valid = true;
hil_lpos.x = x;
hil_lpos.y = y;
hil_lpos.z = _hil_ref_alt - hil_state.alt / 1000.0f;
hil_lpos.vx = hil_state.vx / 100.0f;
hil_lpos.vy = hil_state.vy / 100.0f;
hil_lpos.vz = hil_state.vz / 100.0f;
matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
hil_lpos.yaw = euler.psi();
hil_lpos.xy_global = true;
hil_lpos.z_global = true;
hil_lpos.ref_lat = _hil_ref_lat;
hil_lpos.ref_lon = _hil_ref_lon;
hil_lpos.ref_alt = _hil_ref_alt;
hil_lpos.ref_timestamp = _hil_ref_timestamp;
hil_lpos.vxy_max = std::numeric_limits<float>::infinity();
hil_lpos.vz_max = std::numeric_limits<float>::infinity();
hil_lpos.hagl_min = std::numeric_limits<float>::infinity();
hil_lpos.hagl_max = std::numeric_limits<float>::infinity();
// always publish ground truth attitude message
int hil_lpos_multi;
orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi,
ORB_PRIO_HIGH);
}
handle_message_hil_state_quaternion(msg);
break;
}
@ -502,6 +412,100 @@ void Simulator::handle_message_distance_sensor(const mavlink_message_t *msg)
publish_distance_topic(&dist);
}
void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg)
{
mavlink_hil_state_quaternion_t hil_state;
mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
uint64_t timestamp = hrt_absolute_time();
/* attitude */
struct 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 = {};
{
hil_gpos.timestamp = timestamp;
hil_gpos.lat = hil_state.lat / 1E7;//1E7
hil_gpos.lon = hil_state.lon / 1E7;//1E7
hil_gpos.alt = hil_state.alt / 1E3;//1E3
hil_gpos.vel_n = hil_state.vx / 100.0f;
hil_gpos.vel_e = hil_state.vy / 100.0f;
hil_gpos.vel_d = hil_state.vz / 100.0f;
// always publish ground truth attitude message
int hil_gpos_multi;
orb_publish_auto(ORB_ID(vehicle_global_position_groundtruth), &_gpos_pub, &hil_gpos, &hil_gpos_multi,
ORB_PRIO_HIGH);
}
/* local position */
struct vehicle_local_position_s hil_lpos = {};
{
hil_lpos.timestamp = timestamp;
double lat = hil_state.lat * 1e-7;
double lon = hil_state.lon * 1e-7;
if (!_hil_local_proj_inited) {
_hil_local_proj_inited = true;
map_projection_init(&_hil_local_proj_ref, lat, lon);
_hil_ref_timestamp = timestamp;
_hil_ref_lat = lat;
_hil_ref_lon = lon;
_hil_ref_alt = hil_state.alt / 1000.0f;
}
float x;
float y;
map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
hil_lpos.timestamp = timestamp;
hil_lpos.xy_valid = true;
hil_lpos.z_valid = true;
hil_lpos.v_xy_valid = true;
hil_lpos.v_z_valid = true;
hil_lpos.x = x;
hil_lpos.y = y;
hil_lpos.z = _hil_ref_alt - hil_state.alt / 1000.0f;
hil_lpos.vx = hil_state.vx / 100.0f;
hil_lpos.vy = hil_state.vy / 100.0f;
hil_lpos.vz = hil_state.vz / 100.0f;
matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
hil_lpos.yaw = euler.psi();
hil_lpos.xy_global = true;
hil_lpos.z_global = true;
hil_lpos.ref_lat = _hil_ref_lat;
hil_lpos.ref_lon = _hil_ref_lon;
hil_lpos.ref_alt = _hil_ref_alt;
hil_lpos.ref_timestamp = _hil_ref_timestamp;
hil_lpos.vxy_max = std::numeric_limits<float>::infinity();
hil_lpos.vz_max = std::numeric_limits<float>::infinity();
hil_lpos.hagl_min = std::numeric_limits<float>::infinity();
hil_lpos.hagl_max = std::numeric_limits<float>::infinity();
// always publish ground truth attitude message
int hil_lpos_multi;
orb_publish_auto(ORB_ID(vehicle_local_position_groundtruth), &_lpos_pub, &hil_lpos, &hil_lpos_multi,
ORB_PRIO_HIGH);
}
}
void Simulator::handle_message_landing_target(const mavlink_message_t *msg)
{
mavlink_landing_target_t landing_target_mavlink;