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

ekf2 support SET_GPS_GLOBAL_ORIGIN and remove globallocalconverter usage

Browse Source 
- vehicle_command cmd extended from uint16 to support PX4 internal commands that don't map to mavlink
This commit is contained in:
Daniel Agar
2021-03-05 18:25:14 -05:00
committed by GitHub
parent 12a4b0334f
commit 263b00b65f
21 changed files with 212 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator.h
+2 -8
View File
@@ -261,14 +261,8 @@ private:
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
// hil map_ref data
struct map_projection_reference_s _hil_local_proj_ref {};
bool _hil_local_proj_inited{false};
double _hil_ref_lat{0};
double _hil_ref_lon{0};
float _hil_ref_alt{0.0f};
uint64_t _hil_ref_timestamp{0};
map_projection_reference_s _global_local_proj_ref{};
float _global_local_alt0{NAN};
vehicle_status_s _vehicle_status{};
src/modules/simulator/simulator_mavlink.cpp
+10 -14
View File
@@ -504,25 +504,21 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
}
/* local position */
struct vehicle_local_position_s hil_lpos = {};
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;
if (!map_projection_initialized(&_global_local_proj_ref)) {
map_projection_init(&_global_local_proj_ref, lat, lon);
_global_local_alt0 = hil_state.alt / 1000.f;
}
float x;
float y;
map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
map_projection_project(&_global_local_proj_ref, lat, lon, &x, &y);
hil_lpos.timestamp = timestamp;
hil_lpos.xy_valid = true;
hil_lpos.z_valid = true;
@@ -530,7 +526,7 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
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.z = _global_local_alt0 - 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;
@@ -538,10 +534,10 @@ void Simulator::handle_message_hil_state_quaternion(const mavlink_message_t *msg
hil_lpos.heading = 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.ref_timestamp = _global_local_proj_ref.timestamp;
hil_lpos.ref_lat = math::degrees(_global_local_proj_ref.lat_rad);
hil_lpos.ref_lon = math::degrees(_global_local_proj_ref.lon_rad);
hil_lpos.ref_alt = _global_local_alt0;
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();