fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-16 05:11:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

Take velocity magnitude as airspeed setpoint for offboard

Browse Source 
Rebase fix
Do not use altitude setpoint for velocity control
This commit is contained in:
JaeyoungLim
2023-06-04 08:22:53 +02:00
parent 2722d35231
commit 16f0bb1c38
2 changed files with 12 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control/FixedwingPositionControl.cpp
+11 -10
View File
@@ -724,6 +724,10 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
_control_mode_current = FW_POSCTRL_MODE_AUTO; _control_mode_current = FW_POSCTRL_MODE_AUTO;
return; return;
} }
} else if (_control_mode.flag_control_velocity_enabled) {
_control_mode_current = FW_POSCTRL_MODE_AUTO_VELOCITY;
return;
} }
} }
@@ -910,10 +914,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp); control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
break; break;
case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
control_auto_velocity(control_interval, curr_pos, ground_speed, current_sp);
break;
case position_setpoint_s::SETPOINT_TYPE_LOITER: case position_setpoint_s::SETPOINT_TYPE_LOITER:
control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next); control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
break; break;
@@ -997,10 +997,6 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
{ {
uint8_t position_sp_type = pos_sp_curr.type; uint8_t position_sp_type = pos_sp_curr.type;
if (!_control_mode.flag_control_position_enabled && _control_mode.flag_control_velocity_enabled) {
return position_setpoint_s::SETPOINT_TYPE_VELOCITY;
}
Vector2d curr_wp{0, 0}; Vector2d curr_wp{0, 0};
/* current waypoint (the one currently heading for) */ /* current waypoint (the one currently heading for) */
@@ -1159,7 +1155,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy}; Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0))); _target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));
float target_airspeed = adapt_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, float target_airspeed = adapt_airspeed_setpoint(control_interval, target_velocity.norm(),
_param_fw_airspd_min.get(), ground_speed); _param_fw_airspd_min.get(), ground_speed);
Vector2f curr_pos_local{_local_pos.x, _local_pos.y}; Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
@@ -1172,7 +1168,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
_att_sp.yaw_body = _yaw; _att_sp.yaw_body = _yaw;
tecs_update_pitch_throttle(control_interval, tecs_update_pitch_throttle(control_interval,
position_sp_alt, NAN,
target_airspeed, target_airspeed,
radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()), radians(_param_fw_p_lim_max.get()),
@@ -2414,6 +2410,11 @@ FixedwingPositionControl::Run()
break; break;
} }
case FW_POSCTRL_MODE_AUTO_VELOCITY: {
control_auto_velocity(control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
break;
}
case FW_POSCTRL_MODE_MANUAL_POSITION: { case FW_POSCTRL_MODE_MANUAL_POSITION: {
control_manual_position(control_interval, curr_pos, ground_speed); control_manual_position(control_interval, curr_pos, ground_speed);
break; break;
src/modules/fw_pos_control/FixedwingPositionControl.hpp
+1
View File
@@ -243,6 +243,7 @@ private:
FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT, FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR, FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
FW_POSCTRL_MODE_AUTO_PATH, FW_POSCTRL_MODE_AUTO_PATH,
FW_POSCTRL_MODE_AUTO_VELOCITY,
FW_POSCTRL_MODE_MANUAL_POSITION, FW_POSCTRL_MODE_MANUAL_POSITION,
FW_POSCTRL_MODE_MANUAL_ALTITUDE, FW_POSCTRL_MODE_MANUAL_ALTITUDE,
FW_POSCTRL_MODE_OTHER FW_POSCTRL_MODE_OTHER