dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Take velocity magnitude as airspeed setpoint for offboard 

Rebase fix
Do not use altitude setpoint for velocity control
This commit is contained in:
JaeyoungLim 2023-06-04 08:18:15 +02:00
parent 2722d35231
commit 16f0bb1c38
2 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
src/modules/fw_pos_control/FixedwingPositionControl.cpp
View File

@ -724,6 +724,10 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
_control_mode_current = FW_POSCTRL_MODE_AUTO;
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);
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;
@ -997,10 +997,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) */
@ -1159,7 +1155,7 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
_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);
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;
tecs_update_pitch_throttle(control_interval,
position_sp_alt,
NAN,
target_airspeed,
radians(_param_fw_p_lim_min.get()),
radians(_param_fw_p_lim_max.get()),
@ -2414,6 +2410,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_pos_control/FixedwingPositionControl.hpp
View File

@ -243,6 +243,7 @@ private:
FW_POSCTRL_MODE_AUTO_LANDING_STRAIGHT,
FW_POSCTRL_MODE_AUTO_LANDING_CIRCULAR,
FW_POSCTRL_MODE_AUTO_PATH,
FW_POSCTRL_MODE_AUTO_VELOCITY,
FW_POSCTRL_MODE_MANUAL_POSITION,
FW_POSCTRL_MODE_MANUAL_ALTITUDE,
FW_POSCTRL_MODE_OTHER