mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-09 07:30:35 +08:00
Remove euler angles from attitude setpoint (#23482)
* Remove euler angles from attitude setpoint message * Remove usage of euler angles in attitude setpoint messages This commit removes the usage of euler angles in the vehicle_attitude_setpoint messages * Fix standard vtol
This commit is contained in:
Jaeyoung Lim
@@ -451,9 +451,6 @@ FixedwingPositionControl::status_publish()
|
||||
{
|
||||
position_controller_status_s pos_ctrl_status = {};
|
||||
|
||||
pos_ctrl_status.nav_roll = _att_sp.roll_body;
|
||||
pos_ctrl_status.nav_pitch = _att_sp.pitch_body;
|
||||
|
||||
npfg_status_s npfg_status = {};
|
||||
|
||||
npfg_status.wind_est_valid = _wind_valid;
|
||||
@@ -791,8 +788,11 @@ FixedwingPositionControl::set_control_mode_current(const hrt_abstime &now)
|
||||
|
||||
/* reset setpoints from other modes (auto) otherwise we won't
|
||||
* level out without new manual input */
|
||||
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
const Eulerf current_setpoint(Quatf(_att_sp.q_d));
|
||||
const Quatf setpoint(Eulerf(roll_body, current_setpoint.theta(), yaw_body));
|
||||
setpoint.copyTo(_att_sp.q_d);
|
||||
}
|
||||
|
||||
_control_mode_current = FW_POSCTRL_MODE_MANUAL_POSITION;
|
||||
@@ -877,11 +877,16 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
|
||||
}
|
||||
|
||||
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_IDLE: {
|
||||
_att_sp.thrust_body[0] = 0.0f;
|
||||
const float roll_body = 0.0f;
|
||||
const float pitch_body = radians(_param_fw_psp_off.get());
|
||||
const float yaw_body = 0.0f;
|
||||
|
||||
const Quatf setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
setpoint.copyTo(_att_sp.q_d);
|
||||
break;
|
||||
}
|
||||
|
||||
case position_setpoint_s::SETPOINT_TYPE_POSITION:
|
||||
control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
|
||||
@@ -927,9 +932,6 @@ FixedwingPositionControl::control_auto(const float control_interval, const Vecto
|
||||
_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);
|
||||
}
|
||||
@@ -950,8 +952,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
|
||||
_param_sinkrate_target.get(),
|
||||
_param_climbrate_target.get());
|
||||
|
||||
_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
|
||||
_att_sp.yaw_body = 0.f;
|
||||
const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
|
||||
const float yaw_body = 0.f;
|
||||
|
||||
if (_landed) {
|
||||
_att_sp.thrust_body[0] = _param_fw_thr_min.get();
|
||||
@@ -960,7 +962,9 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i
|
||||
_att_sp.thrust_body[0] = min(get_tecs_thrust(), _param_fw_thr_max.get());
|
||||
}
|
||||
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
|
||||
}
|
||||
|
||||
@@ -983,11 +987,13 @@ FixedwingPositionControl::control_auto_descend(const float control_interval)
|
||||
false,
|
||||
descend_rate);
|
||||
|
||||
_att_sp.roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
|
||||
_att_sp.yaw_body = 0.f;
|
||||
const float roll_body = math::radians(_param_nav_gpsf_r.get()); // open loop loiter bank angle
|
||||
const float yaw_body = 0.f;
|
||||
|
||||
_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
}
|
||||
|
||||
uint8_t
|
||||
@@ -1109,10 +1115,10 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
|
||||
navigateWaypoint(curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
|
||||
}
|
||||
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
float roll_body = getCorrectedNpfgRollSetpoint();
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
position_sp_alt,
|
||||
@@ -1123,6 +1129,9 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co
|
||||
tecs_fw_thr_max,
|
||||
_param_sinkrate_target.get(),
|
||||
_param_climbrate_target.get());
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
}
|
||||
|
||||
void
|
||||
@@ -1158,10 +1167,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
|
||||
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
||||
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
|
||||
navigateBearing(curr_pos_local, _target_bearing, ground_speed, _wind_vel);
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
float roll_body = getCorrectedNpfgRollSetpoint();
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
|
||||
_att_sp.yaw_body = _yaw;
|
||||
float yaw_body = _yaw;
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
position_sp_alt,
|
||||
@@ -1174,6 +1183,10 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co
|
||||
_param_climbrate_target.get(),
|
||||
false,
|
||||
pos_sp_curr.vz);
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
}
|
||||
|
||||
void
|
||||
@@ -1253,10 +1266,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
||||
navigateLoiter(curr_wp_local, curr_pos_local, loiter_radius, pos_sp_curr.loiter_direction_counter_clockwise,
|
||||
ground_speed,
|
||||
_wind_vel);
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
float roll_body = getCorrectedNpfgRollSetpoint();
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
float alt_sp = pos_sp_curr.alt;
|
||||
|
||||
@@ -1267,7 +1280,7 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
||||
|
||||
} else {
|
||||
// continue straight until vehicle has sufficient altitude
|
||||
_att_sp.roll_body = 0.0f;
|
||||
roll_body = 0.0f;
|
||||
}
|
||||
|
||||
_tecs.set_altitude_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
@@ -1282,6 +1295,11 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
|
||||
tecs_fw_thr_max,
|
||||
_param_sinkrate_target.get(),
|
||||
_param_climbrate_target.get());
|
||||
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
}
|
||||
|
||||
#ifdef CONFIG_FIGURE_OF_EIGHT
|
||||
@@ -1306,7 +1324,7 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
|
||||
|
||||
// Apply control
|
||||
_figure_eight.updateSetpoint(curr_pos_local, ground_speed, params, target_airspeed);
|
||||
_att_sp.roll_body = _figure_eight.getRollSetpoint();
|
||||
float roll_body = _figure_eight.getRollSetpoint();
|
||||
target_airspeed = _figure_eight.getAirspeedSetpoint();
|
||||
_target_bearing = _figure_eight.getTargetBearing();
|
||||
_closest_point_on_path = _figure_eight.getClosestPoint();
|
||||
@@ -1337,7 +1355,11 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c
|
||||
_param_climbrate_target.get());
|
||||
|
||||
// Yaw
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
}
|
||||
|
||||
void FixedwingPositionControl::publishFigureEightStatus(const position_setpoint_s pos_sp)
|
||||
@@ -1392,10 +1414,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
|
||||
0.0f;
|
||||
navigatePathTangent(curr_pos_local, curr_wp_local, velocity_2d.normalized(), ground_speed, _wind_vel, curvature);
|
||||
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
float roll_body = getCorrectedNpfgRollSetpoint();
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
pos_sp_curr.alt,
|
||||
@@ -1408,7 +1430,10 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const
|
||||
_param_climbrate_target.get());
|
||||
|
||||
_att_sp.thrust_body[0] = min(get_tecs_thrust(), tecs_fw_thr_max);
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
}
|
||||
|
||||
void
|
||||
@@ -1494,7 +1519,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
|
||||
navigateLine(start_pos_local, takeoff_bearing, local_2D_position, ground_speed, _wind_vel);
|
||||
|
||||
_att_sp.roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint());
|
||||
float roll_body = _runway_takeoff.getRoll(getCorrectedNpfgRollSetpoint());
|
||||
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
|
||||
@@ -1502,7 +1527,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
const float bearing = _npfg.getBearing();
|
||||
|
||||
// heading hold mode will override this bearing setpoint
|
||||
_att_sp.yaw_body = _runway_takeoff.getYaw(bearing);
|
||||
float yaw_body = _runway_takeoff.getYaw(bearing);
|
||||
|
||||
// update tecs
|
||||
const float pitch_max = _runway_takeoff.getMaxPitch(math::radians(_param_fw_p_lim_max.get()));
|
||||
@@ -1529,9 +1554,16 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
|
||||
_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
|
||||
|
||||
_att_sp.pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
|
||||
const float pitch_body = _runway_takeoff.getPitch(get_tecs_pitch());
|
||||
_att_sp.thrust_body[0] = _runway_takeoff.getThrottle(_param_fw_thr_idle.get(), get_tecs_thrust());
|
||||
|
||||
roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt);
|
||||
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
|
||||
_flaps_setpoint = _param_fw_flaps_to_scl.get();
|
||||
|
||||
// retract ladning gear once passed the climbout state
|
||||
if (_runway_takeoff.getState() > RunwayTakeoffState::CLIMBOUT) {
|
||||
_new_landing_gear_position = landing_gear_s::GEAR_UP;
|
||||
@@ -1588,7 +1620,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
||||
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
|
||||
navigateLine(launch_local_position, takeoff_bearing, local_2D_position, ground_speed, _wind_vel);
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
float roll_body = getCorrectedNpfgRollSetpoint();
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
|
||||
|
||||
@@ -1613,17 +1645,27 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
_att_sp.thrust_body[0] = get_tecs_thrust();
|
||||
}
|
||||
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
float pitch_body = get_tecs_pitch();
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt);
|
||||
|
||||
Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
|
||||
} else {
|
||||
/* Tell the attitude controller to stop integrating while we are waiting for the launch */
|
||||
_att_sp.reset_integral = true;
|
||||
|
||||
/* Set default roll and pitch setpoints during detection phase */
|
||||
_att_sp.roll_body = 0.0f;
|
||||
float roll_body = 0.0f;
|
||||
float yaw_body = _yaw;
|
||||
_att_sp.thrust_body[0] = _param_fw_thr_idle.get();
|
||||
_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
|
||||
float pitch_body = radians(_takeoff_pitch_min.get());
|
||||
roll_body = constrainRollNearGround(roll_body, _current_altitude, _takeoff_ground_alt);
|
||||
Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
|
||||
}
|
||||
|
||||
launch_detection_status_s launch_detection_status;
|
||||
@@ -1633,7 +1675,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo
|
||||
}
|
||||
|
||||
_flaps_setpoint = _param_fw_flaps_to_scl.get();
|
||||
_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, _takeoff_ground_alt);
|
||||
|
||||
if (!_vehicle_status.in_transition_to_fw) {
|
||||
publishLocalPositionSetpoint(pos_sp_curr);
|
||||
@@ -1738,10 +1779,10 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
|
||||
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
|
||||
navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
float roll_body = getCorrectedNpfgRollSetpoint();
|
||||
|
||||
// use npfg's bearing to commanded course, controlled via yaw angle while on runway
|
||||
_att_sp.yaw_body = _npfg.getBearing();
|
||||
float yaw_body = _npfg.getBearing();
|
||||
|
||||
/* longitudinal guidance */
|
||||
|
||||
@@ -1787,7 +1828,13 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
|
||||
/* set the attitude and throttle commands */
|
||||
|
||||
// TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
float pitch_body = get_tecs_pitch();
|
||||
|
||||
roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt);
|
||||
|
||||
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
|
||||
// enable direct yaw control using rudder/wheel
|
||||
_att_sp.fw_control_yaw_wheel = true;
|
||||
@@ -1817,7 +1864,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
|
||||
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
|
||||
navigateLine(local_approach_entrance, local_land_point, local_position, ground_speed, _wind_vel);
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
float roll_body = getCorrectedNpfgRollSetpoint();
|
||||
|
||||
/* longitudinal guidance */
|
||||
|
||||
@@ -1839,10 +1886,15 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
|
||||
|
||||
/* set the attitude and throttle commands */
|
||||
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
float pitch_body = get_tecs_pitch();
|
||||
|
||||
roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt);
|
||||
|
||||
// yaw is not controlled in nominal flight
|
||||
_att_sp.yaw_body = _yaw;
|
||||
float yaw_body = _yaw;
|
||||
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
|
||||
// enable direct yaw control using rudder/wheel
|
||||
_att_sp.fw_control_yaw_wheel = false;
|
||||
@@ -1852,8 +1904,6 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now,
|
||||
|
||||
_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
|
||||
|
||||
_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
|
||||
|
||||
_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
|
||||
_spoilers_setpoint = _param_fw_spoilers_lnd.get();
|
||||
|
||||
@@ -1911,6 +1961,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
|
||||
}
|
||||
|
||||
// the terrain estimate (if enabled) is always used to determine the flaring altitude
|
||||
float roll_body;
|
||||
float yaw_body;
|
||||
float pitch_body;
|
||||
|
||||
if ((_current_altitude < terrain_alt + flare_rel_alt) || _flare_states.flaring) {
|
||||
// flare and land with minimal speed
|
||||
|
||||
@@ -1943,9 +1997,9 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
|
||||
pos_sp_curr.loiter_direction_counter_clockwise,
|
||||
ground_speed, _wind_vel);
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
roll_body = getCorrectedNpfgRollSetpoint();
|
||||
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
/* longitudinal guidance */
|
||||
|
||||
@@ -1990,7 +2044,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
|
||||
/* set the attitude and throttle commands */
|
||||
|
||||
// TECS has authority (though constrained) over pitch during flare, throttle is hard set to idle
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
pitch_body = get_tecs_pitch();
|
||||
|
||||
// enable direct yaw control using rudder/wheel
|
||||
_att_sp.fw_control_yaw_wheel = true;
|
||||
@@ -2020,7 +2074,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
|
||||
pos_sp_curr.loiter_direction_counter_clockwise,
|
||||
ground_speed, _wind_vel);
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
roll_body = getCorrectedNpfgRollSetpoint();
|
||||
|
||||
/* longitudinal guidance */
|
||||
|
||||
@@ -2044,10 +2098,10 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
|
||||
|
||||
/* set the attitude and throttle commands */
|
||||
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
pitch_body = get_tecs_pitch();
|
||||
|
||||
// yaw is not controlled in nominal flight
|
||||
_att_sp.yaw_body = _yaw;
|
||||
yaw_body = _yaw;
|
||||
|
||||
// enable direct yaw control using rudder/wheel
|
||||
_att_sp.fw_control_yaw_wheel = false;
|
||||
@@ -2057,7 +2111,11 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now,
|
||||
|
||||
_tecs.set_equivalent_airspeed_min(_performance_model.getMinimumCalibratedAirspeed()); // reset after TECS calculation
|
||||
|
||||
_att_sp.roll_body = constrainRollNearGround(_att_sp.roll_body, _current_altitude, terrain_alt);
|
||||
roll_body = constrainRollNearGround(roll_body, _current_altitude, terrain_alt);
|
||||
|
||||
Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
|
||||
|
||||
_flaps_setpoint = _param_fw_flaps_lnd_scl.get();
|
||||
_spoilers_setpoint = _param_fw_spoilers_lnd.get();
|
||||
@@ -2104,11 +2162,14 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval,
|
||||
false,
|
||||
height_rate_sp);
|
||||
|
||||
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
float roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
}
|
||||
|
||||
void
|
||||
@@ -2137,6 +2198,11 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
|
||||
_time_last_xy_reset = _local_pos.timestamp;
|
||||
}
|
||||
|
||||
Eulerf current_setpoint(Quatf(_att_sp.q_d));
|
||||
float yaw_body = current_setpoint.psi();
|
||||
float roll_body = current_setpoint.phi();
|
||||
float pitch_body = current_setpoint.theta();
|
||||
|
||||
/* heading control */
|
||||
// TODO: either make it course hold (easier) or a real heading hold (minus all the complexity here)
|
||||
if (fabsf(_manual_control_setpoint.roll) < HDG_HOLD_MAN_INPUT_THRESH &&
|
||||
@@ -2177,10 +2243,10 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
|
||||
_npfg.setAirspeedNom(calibrated_airspeed_sp * _eas2tas);
|
||||
_npfg.setAirspeedMax(_performance_model.getMaximumCalibratedAirspeed() * _eas2tas);
|
||||
navigateLine(_hdg_hold_position, _hdg_hold_yaw, curr_pos_local, ground_speed, _wind_vel);
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
roll_body = getCorrectedNpfgRollSetpoint();
|
||||
calibrated_airspeed_sp = _npfg.getAirspeedRef() / _eas2tas;
|
||||
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2202,12 +2268,16 @@ FixedwingPositionControl::control_manual_position(const float control_interval,
|
||||
_hdg_hold_enabled = false;
|
||||
_yaw_lock_engaged = false;
|
||||
|
||||
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
roll_body = _manual_control_setpoint.roll * radians(_param_fw_r_lim.get());
|
||||
yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
}
|
||||
|
||||
_att_sp.thrust_body[0] = min(get_tecs_thrust(), throttle_max);
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
|
||||
pitch_body = get_tecs_pitch();
|
||||
|
||||
Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
}
|
||||
|
||||
void FixedwingPositionControl::control_backtransition(const float control_interval, const Vector2f &ground_speed,
|
||||
@@ -2230,10 +2300,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv
|
||||
|
||||
navigateLine(_lpos_where_backtrans_started, curr_wp_local, curr_pos_local, ground_speed, _wind_vel);
|
||||
|
||||
_att_sp.roll_body = getCorrectedNpfgRollSetpoint();
|
||||
float roll_body = getCorrectedNpfgRollSetpoint();
|
||||
target_airspeed = _npfg.getAirspeedRef() / _eas2tas;
|
||||
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
float yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
pos_sp_curr.alt,
|
||||
@@ -2246,7 +2316,10 @@ void FixedwingPositionControl::control_backtransition(const float control_interv
|
||||
_param_climbrate_target.get());
|
||||
|
||||
_att_sp.thrust_body[0] = (_landed) ? _param_fw_thr_min.get() : min(get_tecs_thrust(), _param_fw_thr_max.get());
|
||||
_att_sp.pitch_body = get_tecs_pitch();
|
||||
const float pitch_body = get_tecs_pitch();
|
||||
|
||||
const Quatf attitude_setpoint(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
attitude_setpoint.copyTo(_att_sp.q_d);
|
||||
}
|
||||
float
|
||||
FixedwingPositionControl::get_tecs_pitch()
|
||||
@@ -2556,12 +2629,16 @@ FixedwingPositionControl::Run()
|
||||
|
||||
|
||||
if (_control_mode_current != FW_POSCTRL_MODE_OTHER) {
|
||||
Eulerf attitude_setpoint(Quatf(_att_sp.q_d));
|
||||
float roll_body = attitude_setpoint.phi();
|
||||
float pitch_body = attitude_setpoint.theta();
|
||||
float yaw_body = attitude_setpoint.psi();
|
||||
|
||||
if (_control_mode.flag_control_manual_enabled) {
|
||||
_att_sp.roll_body = constrain(_att_sp.roll_body, -radians(_param_fw_r_lim.get()),
|
||||
radians(_param_fw_r_lim.get()));
|
||||
_att_sp.pitch_body = constrain(_att_sp.pitch_body, radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()));
|
||||
roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()),
|
||||
radians(_param_fw_r_lim.get()));
|
||||
pitch_body = constrain(pitch_body, radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()));
|
||||
}
|
||||
|
||||
if (_control_mode.flag_control_position_enabled ||
|
||||
@@ -2571,9 +2648,9 @@ FixedwingPositionControl::Run()
|
||||
_control_mode.flag_control_climb_rate_enabled) {
|
||||
|
||||
// roll slew rate
|
||||
_att_sp.roll_body = _roll_slew_rate.update(_att_sp.roll_body, control_interval);
|
||||
roll_body = _roll_slew_rate.update(roll_body, control_interval);
|
||||
|
||||
const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
|
||||
const Quatf q(Eulerf(roll_body, pitch_body, yaw_body));
|
||||
q.copyTo(_att_sp.q_d);
|
||||
|
||||
_att_sp.timestamp = hrt_absolute_time();
|
||||
@@ -3162,8 +3239,10 @@ float FixedwingPositionControl::getLoadFactor()
|
||||
{
|
||||
float load_factor_from_bank_angle = 1.0f;
|
||||
|
||||
if (PX4_ISFINITE(_att_sp.roll_body)) {
|
||||
load_factor_from_bank_angle = 1.0f / math::max(cosf(_att_sp.roll_body), FLT_EPSILON);
|
||||
float roll_body = Eulerf(Quatf(_att_sp.q_d)).phi();
|
||||
|
||||
if (PX4_ISFINITE(roll_body)) {
|
||||
load_factor_from_bank_angle = 1.0f / math::max(cosf(roll_body), FLT_EPSILON);
|
||||
}
|
||||
|
||||
return load_factor_from_bank_angle;
|
||||
|
||||
Reference in New Issue
Block a user