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

Add velocity control as a fw state

Browse Source 
F
F
This commit is contained in:
Jaeyoung Lim 2023-03-30 14:10:07 +02:00
parent 3a104a425c
commit ec3c3c0030
2 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/modules/fw_path_navigation/FixedwingPositionControl.cpp
View File

@ -706,6 +706,12 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now, const
_skipping_takeoff_detection = false;
if (_control_mode.flag_control_offboard_enabled && !_control_mode.flag_control_position_enabled
&& _control_mode.flag_control_velocity_enabled) {
return FW_POSCTRL_MODE_AUTO_VELOCITY;
}
if (((_control_mode.flag_control_auto_enabled && _control_mode.flag_control_position_enabled) ||
_control_mode.flag_control_offboard_enabled) && _position_setpoint_current_valid) {
@ -889,10 +895,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
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:
control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
break;
@ -976,10 +978,6 @@ FixedwingPositionControl::handle_setpoint_type(const position_setpoint_s &pos_sp
{
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};
/* current waypoint (the one currently heading for) */
@ -2347,6 +2345,11 @@ FixedwingPositionControl::Run()
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: {
control_manual_position(control_interval, curr_pos, ground_speed);
break;

1
src/modules/fw_path_navigation/FixedwingPositionControl.hpp
View File

@ -174,6 +174,7 @@ enum FW_POSCTRL_MODE {
FW_POSCTRL_MODE_AUTO_TAKEOFF,
FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
FW_POSCTRL_MODE_AUTO_VELOCITY,
FW_POSCTRL_MODE_MANUAL_POSITION,
FW_POSCTRL_MODE_MANUAL_ALTITUDE,
FW_POSCTRL_MODE_OTHER