mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-17 04:41:36 +08:00
Add fw pos control states
F
This commit is contained in:
@@ -867,57 +867,6 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
|
||||||
FixedwingPositionControl::control_auto(const float control_interval, const Vector2d &curr_pos,
|
|
||||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
|
|
||||||
const position_setpoint_s &pos_sp_next)
|
|
||||||
{
|
|
||||||
position_setpoint_s current_sp = pos_sp_curr;
|
|
||||||
move_position_setpoint_for_vtol_transition(current_sp);
|
|
||||||
|
|
||||||
const uint8_t position_sp_type = handle_setpoint_type(current_sp);
|
|
||||||
|
|
||||||
_position_sp_type = position_sp_type;
|
|
||||||
|
|
||||||
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_LOITER
|
|
||||||
|| current_sp.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {
|
|
||||||
publishOrbitStatus(current_sp);
|
|
||||||
}
|
|
||||||
|
|
||||||
switch (position_sp_type) {
|
|
||||||
case position_setpoint_s::SETPOINT_TYPE_IDLE:
|
|
||||||
_att_sp.thrust_body[0] = 0.0f;
|
|
||||||
_att_sp.roll_body = 0.0f;
|
|
||||||
_att_sp.pitch_body = radians(_param_fw_psp_off.get());
|
|
||||||
break;
|
|
||||||
|
|
||||||
case position_setpoint_s::SETPOINT_TYPE_POSITION:
|
|
||||||
control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, 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;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Copy thrust output for publication, handle special cases */
|
|
||||||
if (position_sp_type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
|
|
||||||
|
|
||||||
_att_sp.thrust_body[0] = 0.0f;
|
|
||||||
|
|
||||||
} else {
|
|
||||||
// when we are landed state we want the motor to spin at idle speed
|
|
||||||
_att_sp.thrust_body[0] = (_landed) ? min(_param_fw_thr_idle.get(), 1.f) : get_tecs_thrust();
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Copy thrust and pitch values from tecs */
|
|
||||||
_att_sp.pitch_body = get_tecs_pitch();
|
|
||||||
|
|
||||||
if (!_vehicle_status.in_transition_to_fw) {
|
|
||||||
publishLocalPositionSetpoint(current_sp);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void
|
void
|
||||||
FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
|
FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
|
||||||
{
|
{
|
||||||
@@ -1017,6 +966,8 @@ void
|
|||||||
FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos,
|
FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos,
|
||||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||||
{
|
{
|
||||||
|
move_position_setpoint_for_vtol_transition(pos_sp_curr);
|
||||||
|
|
||||||
const float acc_rad = _npfg.switchDistance(500.0f);
|
const float acc_rad = _npfg.switchDistance(500.0f);
|
||||||
Vector2d curr_wp{0, 0};
|
Vector2d curr_wp{0, 0};
|
||||||
Vector2d prev_wp{0, 0};
|
Vector2d prev_wp{0, 0};
|
||||||
@@ -1118,6 +1069,19 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
|
|||||||
tecs_fw_thr_max,
|
tecs_fw_thr_max,
|
||||||
_param_sinkrate_target.get(),
|
_param_sinkrate_target.get(),
|
||||||
_param_climbrate_target.get());
|
_param_climbrate_target.get());
|
||||||
|
|
||||||
|
if (_landed) {
|
||||||
|
_att_sp.thrust_body[0] = _param_fw_thr_min.get();
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
|
||||||
|
}
|
||||||
|
|
||||||
|
_att_sp.pitch_body = get_tecs_pitch();
|
||||||
|
|
||||||
|
if (!_vehicle_status.in_transition_to_fw) {
|
||||||
|
publishLocalPositionSetpoint(pos_sp_curr);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
@@ -1176,6 +1140,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
|||||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
|
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
|
||||||
const position_setpoint_s &pos_sp_next)
|
const position_setpoint_s &pos_sp_next)
|
||||||
{
|
{
|
||||||
|
move_position_setpoint_for_vtol_transition(pos_sp_curr);
|
||||||
|
|
||||||
Vector2d curr_wp{0, 0};
|
Vector2d curr_wp{0, 0};
|
||||||
Vector2d prev_wp{0, 0};
|
Vector2d prev_wp{0, 0};
|
||||||
|
|
||||||
@@ -1275,6 +1241,20 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
|||||||
tecs_fw_thr_max,
|
tecs_fw_thr_max,
|
||||||
_param_sinkrate_target.get(),
|
_param_sinkrate_target.get(),
|
||||||
_param_climbrate_target.get());
|
_param_climbrate_target.get());
|
||||||
|
|
||||||
|
|
||||||
|
if (_landed) {
|
||||||
|
_att_sp.thrust_body[0] = _param_fw_thr_min.get();
|
||||||
|
|
||||||
|
} else {
|
||||||
|
_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
|
||||||
|
}
|
||||||
|
|
||||||
|
_att_sp.pitch_body = get_tecs_pitch();
|
||||||
|
|
||||||
|
if (!_vehicle_status.in_transition_to_fw) {
|
||||||
|
publishLocalPositionSetpoint(pos_sp_curr);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void
|
||||||
@@ -2313,12 +2293,6 @@ FixedwingPositionControl::Run()
|
|||||||
_new_landing_gear_position = landing_gear_s::GEAR_KEEP; // is overwritten in Takeoff and Land
|
_new_landing_gear_position = landing_gear_s::GEAR_KEEP; // is overwritten in Takeoff and Land
|
||||||
|
|
||||||
switch (_control_mode_current) {
|
switch (_control_mode_current) {
|
||||||
case FW_POSCTRL_MODE_AUTO: {
|
|
||||||
control_auto(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
|
|
||||||
_pos_sp_triplet.next);
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
|
case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
|
||||||
control_auto_fixed_bank_alt_hold(control_interval);
|
control_auto_fixed_bank_alt_hold(control_interval);
|
||||||
break;
|
break;
|
||||||
@@ -2340,6 +2314,17 @@ FixedwingPositionControl::Run()
|
|||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
case FW_POSCTRL_MODE_AUTO_LOITER: {
|
||||||
|
control_auto_loiter(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
|
||||||
|
_pos_sp_triplet.next);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
case FW_POSCTRL_MODE_AUTO_POSITION: {
|
||||||
|
control_auto_position(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
|
case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
|
||||||
control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
|
control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.current);
|
||||||
break;
|
break;
|
||||||
|
|||||||
@@ -168,12 +168,13 @@ static constexpr float MANUAL_TOUCHDOWN_NUDGE_INPUT_DEADZONE = 0.15f;
|
|||||||
static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f;
|
static constexpr float POST_TOUCHDOWN_CLAMP_TIME = 0.5f;
|
||||||
|
|
||||||
enum FW_POSCTRL_MODE {
|
enum FW_POSCTRL_MODE {
|
||||||
FW_POSCTRL_MODE_AUTO,
|
|
||||||
FW_POSCTRL_MODE_AUTO_ALTITUDE,
|
FW_POSCTRL_MODE_AUTO_ALTITUDE,
|
||||||
FW_POSCTRL_MODE_AUTO_CLIMBRATE,
|
FW_POSCTRL_MODE_AUTO_CLIMBRATE,
|
||||||
FW_POSCTRL_MODE_AUTO_TAKEOFF,
|
FW_POSCTRL_MODE_AUTO_TAKEOFF,
|
||||||
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_LOITER,
|
||||||
|
FW_POSCTRL_MODE_AUTO_POSITION,
|
||||||
FW_POSCTRL_MODE_AUTO_VELOCITY,
|
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,
|
||||||
@@ -532,19 +533,6 @@ private:
|
|||||||
|
|
||||||
/* automatic control methods */
|
/* automatic control methods */
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Automatic position control for waypoints, orbits, and velocity control
|
|
||||||
*
|
|
||||||
* @param control_interval Time since last position control call [s]
|
|
||||||
* @param curr_pos Current 2D local position vector of vehicle [m]
|
|
||||||
* @param ground_speed Local 2D ground speed of vehicle [m/s]
|
|
||||||
* @param pos_sp_prev previous position setpoint
|
|
||||||
* @param pos_sp_curr current position setpoint
|
|
||||||
* @param pos_sp_next next position setpoint
|
|
||||||
*/
|
|
||||||
void control_auto(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed,
|
|
||||||
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next);
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief Controls altitude and airspeed for a fixed-bank loiter.
|
* @brief Controls altitude and airspeed for a fixed-bank loiter.
|
||||||
*
|
*
|
||||||
|
|||||||
Reference in New Issue
Block a user