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

OFFBOARD mode architecture overhaul (#16739)

Browse Source 
- handle SET_POSITION_TARGET_LOCAL_NED and SET_POSITION_TARGET_GLOBAL_INT with ORB_ID(trajectory_setpoint)
 - FlightTaskOffboard not needed at all
 - bypass position_setpoint_triplet entirely (start removing extraneous fields)
 - simplify offboard_control_mode to map to supported control modes
This commit is contained in:
Daniel Agar
2021-03-05 09:39:46 -05:00
committed by GitHub
parent 5233737a86
commit d0c9a5fc93
25 changed files with 392 additions and 1074 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+18 -10
View File
@@ -1710,15 +1710,25 @@ FixedwingPositionControl::Run()
Vector2d curr_pos(_current_latitude, _current_longitude);
Vector2f ground_speed(_local_pos.vx, _local_pos.vy);
//Convert Local setpoints to global setpoints
// Convert Local setpoints to global setpoints
if (_control_mode.flag_control_offboard_enabled) {
if (!globallocalconverter_initialized()) {
globallocalconverter_init(_local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_alt, _local_pos.ref_timestamp);
if (!map_projection_initialized(&_global_local_proj_ref)
|| (_global_local_proj_ref.timestamp != _local_pos.ref_timestamp)) {
} else {
globallocalconverter_toglobal(_pos_sp_triplet.current.x, _pos_sp_triplet.current.y, _pos_sp_triplet.current.z,
&_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon, &_pos_sp_triplet.current.alt);
map_projection_init_timestamped(&_global_local_proj_ref, _local_pos.ref_lat, _local_pos.ref_lon,
_local_pos.ref_timestamp);
_global_local_alt0 = _local_pos.ref_alt;
}
vehicle_local_position_setpoint_s trajectory_setpoint;
if (_trajectory_setpoint_sub.update(&trajectory_setpoint)) {
map_projection_reproject(&_global_local_proj_ref,
trajectory_setpoint.x, trajectory_setpoint.y,
&_pos_sp_triplet.current.lat, &_pos_sp_triplet.current.lon);
_pos_sp_triplet.current.alt = _global_local_alt0 - trajectory_setpoint.z;
_pos_sp_triplet.current.valid = true;
}
}
@@ -1736,8 +1746,7 @@ FixedwingPositionControl::Run()
radians(_param_fw_man_p_max.get()));
}
if (_control_mode.flag_control_offboard_enabled ||
_control_mode.flag_control_position_enabled ||
if (_control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled ||
_control_mode.flag_control_altitude_enabled) {
@@ -1874,7 +1883,6 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
/* tell TECS to update its state, but let it know when it cannot actually control the plane */
bool in_air_alt_control = (!_landed &&
(_control_mode.flag_control_auto_enabled ||
_control_mode.flag_control_offboard_enabled ||
_control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_altitude_enabled));