Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 02:55:07 +08:00
Code Issues Actions 19 Packages Projects Releases Wiki Activity

Enable offboard local position setpoints for fixed wing position control

Browse Source
This commit is contained in:
Jaeyoung-Lim
2019-08-11 20:35:11 +02:00
committed by Mathieu Bresciani
parent 536f073d02
commit 914417f580
1 changed files with 15 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
+15 -2
View File
@@ -803,7 +803,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
_tecs.reset_state(); _tecs.reset_state();
} }
if (_control_mode.flag_control_auto_enabled && pos_sp_curr.valid) { if ((_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled) && pos_sp_curr.valid) {
/* AUTONOMOUS FLIGHT */ /* AUTONOMOUS FLIGHT */
_control_mode_current = FW_POSCTRL_MODE_AUTO; _control_mode_current = FW_POSCTRL_MODE_AUTO;
@@ -1726,6 +1726,18 @@ FixedwingPositionControl::Run()
Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon); Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e); Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e);
//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);
} 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);
}
}
/* /*
* Attempt to control position, on success (= sensors present and not in manual mode), * Attempt to control position, on success (= sensors present and not in manual mode),
* publish setpoint. * publish setpoint.
@@ -1746,7 +1758,7 @@ FixedwingPositionControl::Run()
q.copyTo(_att_sp.q_d); q.copyTo(_att_sp.q_d);
_att_sp.q_d_valid = true; _att_sp.q_d_valid = true;
if (!_control_mode.flag_control_offboard_enabled || if (_control_mode.flag_control_offboard_enabled ||
_control_mode.flag_control_position_enabled || _control_mode.flag_control_position_enabled ||
_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_acceleration_enabled) { _control_mode.flag_control_acceleration_enabled) {
@@ -1905,6 +1917,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
/* tell TECS to update its state, but let it know when it cannot actually control the plane */ /* tell TECS to update its state, but let it know when it cannot actually control the plane */
bool in_air_alt_control = (!_vehicle_land_detected.landed && bool in_air_alt_control = (!_vehicle_land_detected.landed &&
(_control_mode.flag_control_auto_enabled || (_control_mode.flag_control_auto_enabled ||
_control_mode.flag_control_offboard_enabled ||
_control_mode.flag_control_velocity_enabled || _control_mode.flag_control_velocity_enabled ||
_control_mode.flag_control_altitude_enabled)); _control_mode.flag_control_altitude_enabled));