Skip to content

Commit

Permalink
renaming and more cleanup
Browse files Browse the repository at this point in the history 
Signed-off-by: RomanBapst <[email protected]>
  • Loading branch information
@RomanBapst
RomanBapst committed Jan 8, 2025
1 parent a65004e commit eb68cd0
Show file tree
Hide file tree
Showing 5 changed files with 42 additions and 40 deletions.
2 changes: 1 addition & 1 deletion msg/FwLateralControlSetpoint.msg
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
uint64 timestamp

float32 course_setpoint
float32 heading_setpoint
float32 airspeed_reference_direction # angle of desired airspeed vector [-pi, pi]
float32 lateral_acceleration_setpoint
float32 roll_sp # TODO: remove, only for testing

Expand Down
14 changes: 7 additions & 7 deletions src/lib/npfg/CourseToAirspeedRefMapper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,32 +35,32 @@
using matrix::Vector2f;

float
CourseToAirspeedRefMapper::mapBearingSetpointToHeadingSetpoint(const float bearing_setpoint, const Vector2f &wind_vel,
float airspeed) const
CourseToAirspeedRefMapper::mapCourseSetpointToHeadingSetpoint(const float bearing_setpoint, const Vector2f &wind_vel,
float airspeed_true) const
{

Vector2f bearing_vector = Vector2f{cosf(bearing_setpoint), sinf(bearing_setpoint)};
const float wind_cross_bearing = wind_vel.cross(bearing_vector);
const float wind_dot_bearing = wind_vel.dot(bearing_vector);

const Vector2f air_vel_ref = refAirVelocity(wind_vel, bearing_vector, wind_cross_bearing,
wind_dot_bearing, wind_vel.norm(), airspeed);
wind_dot_bearing, wind_vel.norm(), airspeed_true);

return atan2f(air_vel_ref(1), air_vel_ref(0));
}

matrix::Vector2f CourseToAirspeedRefMapper::refAirVelocity(const Vector2f &wind_vel, const Vector2f &bearing_vec,
const float wind_cross_bearing, const float wind_dot_bearing,
const float wind_speed, float airspeed) const
const float wind_speed, float airspeed_true) const
{
Vector2f air_vel_ref;

if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed)) {
const float airsp_dot_bearing = projectAirspOnBearing(airspeed, wind_cross_bearing);
if (bearingIsFeasible(wind_cross_bearing, wind_dot_bearing, airspeed_true, wind_speed)) {
const float airsp_dot_bearing = projectAirspOnBearing(airspeed_true, wind_cross_bearing);
air_vel_ref = solveWindTriangle(wind_cross_bearing, airsp_dot_bearing, bearing_vec);

} else {
air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed);
air_vel_ref = infeasibleAirVelRef(wind_vel, bearing_vec, wind_speed, airspeed_true);
}

return air_vel_ref;
Expand Down
6 changes: 3 additions & 3 deletions src/lib/npfg/CourseToAirspeedRefMapper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,8 +71,8 @@ class CourseToAirspeedRefMapper

~CourseToAirspeedRefMapper() = default;

float mapBearingSetpointToHeadingSetpoint(const float bearing_setpoint,
const matrix::Vector2f &wind_vel, float airspeed) const;
float mapCourseSetpointToHeadingSetpoint(const float bearing_setpoint,
const matrix::Vector2f &wind_vel, float airspeed_true) const;

private:
/*
Expand All @@ -90,7 +90,7 @@ class CourseToAirspeedRefMapper
*/
matrix::Vector2f refAirVelocity(const matrix::Vector2f &wind_vel, const matrix::Vector2f &bearing_vec,
const float wind_cross_bearing, const float wind_dot_bearing,
const float wind_speed, float airspeed) const;
const float wind_speed, float airspeed_true) const;
/*
* Projection of the air velocity vector onto the bearing line considering
* a connected wind triangle.
Expand Down
56 changes: 29 additions & 27 deletions src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,7 @@ static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s;
// [-] Can-run threshold needed to trigger the roll-constraining failsafe warning
static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f;

const fw_lateral_control_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course_setpoint = NAN, .heading_setpoint = NAN, .lateral_acceleration_setpoint = NAN, .roll_sp = NAN};
const fw_lateral_control_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course_setpoint = NAN, .airspeed_reference_direction = NAN, .lateral_acceleration_setpoint = NAN, .roll_sp = NAN};
const fw_longitudinal_control_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .height_rate_setpoint = NAN, .altitude_setpoint = NAN, .equivalent_airspeed_setpoint = NAN, .pitch_sp = NAN, .thrust_sp = NAN};

FwLateralLongitudinalControl::FwLateralLongitudinalControl(bool is_vtol) :
Expand Down Expand Up @@ -143,7 +143,6 @@ void FwLateralLongitudinalControl::Run()
updateTECSAltitudeTimeConstant(checkLowHeightConditions(), control_interval);
_tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState());

fw_longitudinal_control_setpoint_s longitudinal_sp = {empty_longitudinal_control_setpoint};

_long_control_limits_sub.update();
_lateral_control_limits_sub.update();
Expand All @@ -170,13 +169,15 @@ void FwLateralLongitudinalControl::Run()
}

float pitch_sp{NAN};
float thrust_sp{NAN};
float throttle_sp{NAN};

bool publish = false;

fw_longitudinal_control_setpoint_s longitudinal_sp = {empty_longitudinal_control_setpoint};
_fw_longitudinal_ctrl_sub.copy(&longitudinal_sp);

if (longitudinal_sp.timestamp > 0 && hrt_elapsed_time(&longitudinal_sp.timestamp) < 1_s) {
publish = true;
tecs_update_pitch_throttle(control_interval, longitudinal_sp.altitude_setpoint,
PX4_ISFINITE(longitudinal_sp.equivalent_airspeed_setpoint)
? longitudinal_sp.equivalent_airspeed_setpoint :
Expand All @@ -186,13 +187,12 @@ void FwLateralLongitudinalControl::Run()
_long_control_limits_sub.get().throttle_min,
_long_control_limits_sub.get().throttle_max,
_long_control_limits_sub.get().sink_rate_max,
_long_control_limits_sub.get().climb_rate_max, // TODO, replace
_long_control_limits_sub.get().climb_rate_max,
_long_control_limits_sub.get().disable_underspeed_protection,
longitudinal_sp.height_rate_setpoint
);
pitch_sp = PX4_ISFINITE(longitudinal_sp.pitch_sp) ? longitudinal_sp.pitch_sp : _tecs.get_pitch_setpoint();
thrust_sp = PX4_ISFINITE(longitudinal_sp.thrust_sp) ? longitudinal_sp.thrust_sp : _tecs.get_throttle_setpoint();
publish = true;
throttle_sp = PX4_ISFINITE(longitudinal_sp.thrust_sp) ? longitudinal_sp.thrust_sp : _tecs.get_throttle_setpoint();

fw_longitudinal_control_setpoint_s longitudinal_control_status {
.timestamp = hrt_absolute_time(),
Expand All @@ -205,35 +205,37 @@ void FwLateralLongitudinalControl::Run()
}


fw_lateral_control_setpoint_s sp = {empty_lateral_control_setpoint};
float roll_sp {NAN};
float yaw_sp {NAN};

_fw_lateral_ctrl_sub.copy(&sp);
fw_lateral_control_setpoint_s lateral_sp = {empty_lateral_control_setpoint};
_fw_lateral_ctrl_sub.copy(&lateral_sp);

if (sp.timestamp > 0 && hrt_elapsed_time(&sp.timestamp) < 1_s) {
float heading_setpoint{NAN};
if (lateral_sp.timestamp > 0 && hrt_elapsed_time(&lateral_sp.timestamp) < 1_s) {
float airspeed_reference_direction{NAN};
float lateral_accel_sp {NAN};

if (PX4_ISFINITE(sp.course_setpoint)) {
heading_setpoint = _course_to_airspeed.mapBearingSetpointToHeadingSetpoint(
sp.course_setpoint, _lateral_control_state.wind_speed, _long_control_state.airspeed_eas * _long_control_state.eas2tas);
if (PX4_ISFINITE(lateral_sp.course_setpoint)) {
airspeed_reference_direction = _course_to_airspeed.mapCourseSetpointToHeadingSetpoint(
lateral_sp.course_setpoint, _lateral_control_state.wind_speed,
_long_control_state.airspeed_eas * _long_control_state.eas2tas);
}

if (PX4_ISFINITE(sp.heading_setpoint)) {
heading_setpoint = PX4_ISFINITE(heading_setpoint) ? heading_setpoint + sp.heading_setpoint : sp.heading_setpoint;
if (PX4_ISFINITE(lateral_sp.airspeed_reference_direction)) {
airspeed_reference_direction = PX4_ISFINITE(airspeed_reference_direction) ? airspeed_reference_direction +
lateral_sp.airspeed_reference_direction : lateral_sp.airspeed_reference_direction;
}

if (PX4_ISFINITE(heading_setpoint)) {
if (PX4_ISFINITE(airspeed_reference_direction)) {
const Vector2f airspeed_vector = _lateral_control_state.ground_speed - _lateral_control_state.wind_speed;
const float heading = atan2f(airspeed_vector(1), airspeed_vector(0));
lateral_accel_sp = _airspeed_ref_control.controlHeading(heading_setpoint, heading,
lateral_accel_sp = _airspeed_ref_control.controlHeading(airspeed_reference_direction, heading,
_long_control_state.airspeed_eas * _long_control_state.eas2tas);
}

if (PX4_ISFINITE(sp.lateral_acceleration_setpoint)) {
lateral_accel_sp = PX4_ISFINITE(lateral_accel_sp) ? lateral_accel_sp + sp.lateral_acceleration_setpoint :
sp.lateral_acceleration_setpoint;
if (PX4_ISFINITE(lateral_sp.lateral_acceleration_setpoint)) {
lateral_accel_sp = PX4_ISFINITE(lateral_accel_sp) ? lateral_accel_sp + lateral_sp.lateral_acceleration_setpoint :
lateral_sp.lateral_acceleration_setpoint;
}

if (PX4_ISFINITE(lateral_accel_sp)) {
Expand All @@ -243,14 +245,14 @@ void FwLateralLongitudinalControl::Run()
roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp);
}

_att_sp.reset_integral = sp.reset_integral;
_att_sp.fw_control_yaw_wheel = PX4_ISFINITE(sp.heading_sp_runway_takeoff);
yaw_sp = sp.heading_sp_runway_takeoff;
_att_sp.reset_integral = lateral_sp.reset_integral;
_att_sp.fw_control_yaw_wheel = PX4_ISFINITE(lateral_sp.heading_sp_runway_takeoff);
yaw_sp = lateral_sp.heading_sp_runway_takeoff;

fw_lateral_control_setpoint_s status = {
.timestamp = sp.timestamp,
.course_setpoint = sp.course_setpoint,
.heading_setpoint = heading_setpoint,
.timestamp = lateral_sp.timestamp,
.course_setpoint = lateral_sp.course_setpoint,
.airspeed_reference_direction = airspeed_reference_direction,
.lateral_acceleration_setpoint = lateral_accel_sp,
.roll_sp = roll_sp // TODO: just for logging, can be removed later
};
Expand All @@ -264,7 +266,7 @@ void FwLateralLongitudinalControl::Run()
float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f;
float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f;
const float yaw_body = PX4_ISFINITE(yaw_sp) ? yaw_sp : _yaw;
const float thrust_body_x = PX4_ISFINITE(thrust_sp) ? thrust_sp : 0.0f;
const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f;

if (_control_mode_sub.get().flag_control_manual_enabled) {
roll_body = constrain(roll_body, -radians(_param_fw_r_lim.get()),
Expand Down
4 changes: 2 additions & 2 deletions src/modules/fw_pos_control/FixedwingPositionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ using matrix::Vector2d;
using matrix::Vector3f;
using matrix::wrap_pi;

const fw_lateral_control_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course_setpoint = NAN, .heading_setpoint = NAN, .lateral_acceleration_setpoint = NAN, .roll_sp = NAN, .heading_sp_runway_takeoff = NAN, .reset_integral = false};
const fw_lateral_control_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course_setpoint = NAN, .airspeed_reference_direction = NAN, .lateral_acceleration_setpoint = NAN, .roll_sp = NAN, .heading_sp_runway_takeoff = NAN, .reset_integral = false};
const fw_longitudinal_control_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .height_rate_setpoint = NAN, .altitude_setpoint = NAN, .equivalent_airspeed_setpoint = NAN, .pitch_sp = NAN, .thrust_sp = NAN};
FixedwingPositionControl::FixedwingPositionControl(bool vtol) :
ModuleParams(nullptr),
Expand Down Expand Up @@ -2121,7 +2121,7 @@ void FixedwingPositionControl::control_backtransition_heading_hold()

fw_lateral_control_setpoint_s fw_lateral_ctrl_sp{empty_lateral_control_setpoint};
fw_lateral_ctrl_sp.timestamp = hrt_absolute_time();
fw_lateral_ctrl_sp.heading_setpoint = _backtrans_heading;
fw_lateral_ctrl_sp.airspeed_reference_direction = _backtrans_heading;

_lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp);
}
Expand Down

0 comments on commit eb68cd0

Please sign in to comment.