fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 01:53:33 +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/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
+13 -16
View File
@@ -76,22 +76,15 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command)
// save current yaw estimate for ORBIT_YAW_BEHAVIOUR_HOLD_INITIAL_HEADING
_initial_heading = _yaw;
// TODO: apply x,y / z independently in geo library
// commanded center coordinates
// if(PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
// map_projection_global_project(command.param5, command.param6, &_center(0), &_center(1));
// }
if (map_projection_initialized(&_global_local_proj_ref)) {
if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6)) {
map_projection_global_project(command.param5, command.param6, &_center(0), &_center(1));
}
// commanded altitude
// if(PX4_ISFINITE(command.param7)) {
// _position_setpoint(2) = gl_ref.alt - command.param7;
// }
if (PX4_ISFINITE(command.param5) && PX4_ISFINITE(command.param6) && PX4_ISFINITE(command.param7)) {
if (globallocalconverter_tolocal(command.param5, command.param6, command.param7, &_center(0), &_center(1),
&_position_setpoint(2))) {
// global to local conversion failed
ret = false;
// commanded altitude
if (PX4_ISFINITE(command.param7)) {
_position_setpoint(2) = _global_local_alt0 - command.param7;
}
}
@@ -109,8 +102,12 @@ bool FlightTaskOrbit::sendTelemetry()
orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL
orbit_status.yaw_behaviour = _yaw_behaviour;
if (globallocalconverter_toglobal(_center(0), _center(1), _position_setpoint(2), &orbit_status.x, &orbit_status.y,
&orbit_status.z)) {
if (map_projection_initialized(&_global_local_proj_ref)) {
// local -> global
map_projection_reproject(&_global_local_proj_ref, _center(0), _center(1), &orbit_status.x, &orbit_status.y);
orbit_status.z = _global_local_alt0 - _position_setpoint(2);
} else {
return false; // don't send the message if the transformation failed
}