Skip to content

Commit

Permalink
FW position control add takeoff minimum pitch
Browse files Browse the repository at this point in the history
  • Loading branch information
@dagar
dagar committed Aug 24, 2018
1 parent c997159 commit f9db41c
Show file tree
Hide file tree
Showing 11 changed files with 101 additions and 101 deletions.
1 change: 0 additions & 1 deletion msg/position_setpoint.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,6 @@ bool yawspeed_valid # true if yawspeed setpoint valid

float32 loiter_radius # loiter radius (only for fixed wing), in m
int8 loiter_direction # loiter direction: 1 = CW, -1 = CCW
float32 pitch_min # minimal pitch angle for fixed wing takeoff waypoints

float32 a_x # acceleration x setpoint
float32 a_y # acceleration y setpoint
Expand Down
134 changes: 72 additions & 62 deletions src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -315,13 +315,16 @@ FixedwingPositionControl::vehicle_control_mode_poll()
void
FixedwingPositionControl::vehicle_command_poll()
{
bool updated;
bool updated = false;

orb_check(_vehicle_command_sub, &updated);

if (updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &_vehicle_command);
handle_command();
vehicle_command_s vehicle_command;

if (orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &vehicle_command) == PX4_OK) {
handle_command(vehicle_command);
}
}
}

Expand Down Expand Up @@ -414,28 +417,33 @@ FixedwingPositionControl::airspeed_poll()
void
FixedwingPositionControl::vehicle_attitude_poll()
{
/* check if there is a new position */
bool updated;
bool updated = false;
orb_check(_vehicle_attitude_sub, &updated);

if (updated) {
orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
}
vehicle_attitude_s att;

/* set rotation matrix and euler angles */
_R_nb = Quatf(_att.q);
if (orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &att) == PX4_OK) {

// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
// between multirotor and fixed wing flight
if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
Dcmf R_offset = Eulerf(0, M_PI_2_F, 0);
_R_nb = _R_nb * R_offset;
}
if (_parameters.vtol_type == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
// between multirotor and fixed wing flight
Dcmf R_offset = Eulerf(0, M_PI_2_F, 0);
_R_nb = Quatf(att.q) * R_offset;
_yawspeed = att.rollspeed;

} else {
// otherwise, normal vehicle
_R_nb = Quatf(att.q);
_yawspeed = att.yawspeed;
}

Eulerf euler_angles(_R_nb);
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
Eulerf euler_angles(_R_nb);
_roll = euler_angles(0);
_pitch = euler_angles(1);
_yaw = euler_angles(2);
}
}
}

void
Expand Down Expand Up @@ -562,8 +570,8 @@ void
FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_setpoint_s &waypoint_prev,
position_setpoint_s &waypoint_next, bool flag_init)
{
position_setpoint_s temp_prev = waypoint_prev;
position_setpoint_s temp_next = waypoint_next;
position_setpoint_s temp_prev{};
position_setpoint_s temp_next{};

if (flag_init) {
// previous waypoint: HDG_HOLD_SET_BACK_DIST meters behind us
Expand All @@ -586,18 +594,19 @@ FixedwingPositionControl::get_waypoint_heading_distance(float heading, position_
-(HDG_HOLD_REACHED_DIST + HDG_HOLD_DIST_NEXT), &temp_next.lat, &temp_next.lon);
}

waypoint_prev = temp_prev;
waypoint_prev.timestamp = hrt_absolute_time();
waypoint_prev.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
waypoint_prev.alt = _hold_alt;
waypoint_prev.valid = true;

waypoint_next = temp_next;
waypoint_next.timestamp = hrt_absolute_time();
waypoint_next.type = position_setpoint_s::SETPOINT_TYPE_POSITION;
waypoint_next.alt = _hold_alt;
waypoint_next.valid = true;
}

float
FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt,
const vehicle_global_position_s &global_pos)
FixedwingPositionControl::get_terrain_altitude_takeoff(float takeoff_alt, const vehicle_global_position_s &global_pos)
{
if (PX4_ISFINITE(global_pos.terrain_alt) && global_pos.terrain_alt_valid) {
return global_pos.terrain_alt;
Expand Down Expand Up @@ -729,7 +738,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
Vector2f nav_speed_2d{0.0f, 0.0f};

// angle between air_speed_2d and ground_speed
float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));
const float air_gnd_angle = acosf((air_speed_2d * ground_speed) / (air_speed_2d.length() * ground_speed.length()));

// if angle > 90 degrees or groundspeed is less than threshold, replace groundspeed with airspeed projection
if ((fabsf(air_gnd_angle) > M_PI_2_F) || (ground_speed.length() < 3.0f)) {
Expand Down Expand Up @@ -779,7 +788,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
_tecs.set_time_const_throt(_parameters.time_const_throt);

/* current waypoint (the one currently heading for) */
Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
const Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);

/* Initialize attitude controller integrator reset flags to 0 */
_att_sp.roll_reset_integral = false;
Expand Down Expand Up @@ -842,15 +851,15 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
} else if (pos_sp_curr.type == position_setpoint_s::SETPOINT_TYPE_LOITER) {

/* waypoint is a loiter waypoint */
_l1_control.navigate_loiter(curr_wp, curr_pos, pos_sp_curr.loiter_radius,
pos_sp_curr.loiter_direction, nav_speed_2d);
_l1_control.navigate_loiter(curr_wp, curr_pos, pos_sp_curr.loiter_radius, pos_sp_curr.loiter_direction, nav_speed_2d);
_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();

float alt_sp = pos_sp_curr.alt;

if (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND && pos_sp_next.valid
&& _l1_control.circle_mode() && _parameters.land_early_config_change == 1) {
if (pos_sp_next.valid && (pos_sp_next.type == position_setpoint_s::SETPOINT_TYPE_LAND)
&& _l1_control.circle_mode() && (_parameters.land_early_config_change == 1)) {

// We're in a loiter directly before a landing WP. Enable our landing configuration (flaps,
// landing airspeed and potentially tighter throttle control) already such that we don't
// have to do this switch (which can cause significant altitude errors) close to the ground.
Expand Down Expand Up @@ -896,7 +905,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto

/* reset landing state */
if (pos_sp_curr.type != position_setpoint_s::SETPOINT_TYPE_LAND) {
reset_landing_state();
reset_landing_state(pos_sp_curr.type);
}

/* reset takeoff/launch state */
Expand Down Expand Up @@ -963,7 +972,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
fabsf(_manual.r) < HDG_HOLD_MAN_INPUT_THRESH) {

/* heading / roll is zero, lock onto current heading */
if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
if (fabsf(_yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
// little yaw movement, lock to current heading
_yaw_lock_engaged = true;

Expand All @@ -984,22 +993,21 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto
_hdg_hold_enabled = true;
_hdg_hold_yaw = _yaw;

get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
// update the previous and current position setpoint triplet
get_waypoint_heading_distance(_hdg_hold_yaw, _pos_sp_triplet.previous, _pos_sp_triplet.current, true);
}

/* we have a valid heading hold position, are we too close? */
float dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _hdg_hold_curr_wp.lat,
_hdg_hold_curr_wp.lon);
float dist = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, pos_sp_curr.lat, pos_sp_curr.lon);

if (dist < HDG_HOLD_REACHED_DIST) {
get_waypoint_heading_distance(_hdg_hold_yaw, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
// update the previous and current position setpoint triplet
get_waypoint_heading_distance(_hdg_hold_yaw, _pos_sp_triplet.previous, _pos_sp_triplet.current, false);
}

Vector2f prev_wp{(float)_hdg_hold_prev_wp.lat, (float)_hdg_hold_prev_wp.lon};
Vector2f curr_wp{(float)_hdg_hold_curr_wp.lat, (float)_hdg_hold_curr_wp.lon};

/* populate l1 control setpoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, curr_pos, ground_speed);
_l1_control.navigate_waypoints(Vector2f{(float)pos_sp_prev.lat, (float)pos_sp_prev.lon},
Vector2f{(float)pos_sp_curr.lat, (float)pos_sp_curr.lon}, curr_pos, ground_speed);

_att_sp.roll_body = _l1_control.get_roll_setpoint();
_att_sp.yaw_body = _l1_control.nav_bearing();
Expand Down Expand Up @@ -1074,7 +1082,7 @@ FixedwingPositionControl::control_position(const Vector2f &curr_pos, const Vecto

/* reset landing and takeoff state */
if (!_last_manual) {
reset_landing_state();
reset_landing_state(pos_sp_curr.type);
reset_takeoff_state();
}
}
Expand Down Expand Up @@ -1149,7 +1157,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
{
/* current waypoint (the one currently heading for) */
Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
const Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */

if (pos_sp_prev.valid) {
Expand Down Expand Up @@ -1178,8 +1186,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector

if (_runway_takeoff.runwayTakeoffEnabled()) {
if (!_runway_takeoff.isInitialized()) {
Eulerf euler(Quatf(_att.q));
_runway_takeoff.init(euler.psi(), _global_pos.lat, _global_pos.lon);
_runway_takeoff.init(_yaw, _global_pos.lat, _global_pos.lon);

/* need this already before takeoff is detected
* doesn't matter if it gets reset when takeoff is detected eventually */
Expand Down Expand Up @@ -1211,7 +1218,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
_parameters.throttle_max, // XXX should we also set runway_takeoff_throttle here?
_parameters.throttle_cruise,
_runway_takeoff.climbout(),
radians(_runway_takeoff.getMinPitch(pos_sp_curr.pitch_min, 10.0f, _parameters.pitch_limit_min)),
radians(_runway_takeoff.getMinPitch(_takeoff_pitch_min.get(), _parameters.pitch_limit_min)),
tecs_status_s::TECS_MODE_TAKEOFF);

// assign values
Expand Down Expand Up @@ -1282,7 +1289,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector
takeoff_throttle,
_parameters.throttle_cruise,
true,
max(radians(pos_sp_curr.pitch_min), radians(10.0f)),
radians(_takeoff_pitch_min.get()),
tecs_status_s::TECS_MODE_TAKEOFF);

/* limit roll motion to ensure enough lift */
Expand All @@ -1309,7 +1316,7 @@ FixedwingPositionControl::control_takeoff(const Vector2f &curr_pos, const Vector

/* Set default roll and pitch setpoints during detection phase */
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = max(radians(pos_sp_curr.pitch_min), radians(10.0f));
_att_sp.pitch_body = radians(_takeoff_pitch_min.get());
}
}
}
Expand All @@ -1320,7 +1327,7 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
{
/* current waypoint (the one currently heading for) */
Vector2f curr_wp((float)pos_sp_curr.lat, (float)pos_sp_curr.lon);
Vector2f prev_wp{0.0f, 0.0f}; /* previous waypoint */
Vector2f prev_wp{0.0f, 0.0f};

if (pos_sp_prev.valid) {
prev_wp(0) = (float)pos_sp_prev.lat;
Expand Down Expand Up @@ -1355,14 +1362,14 @@ FixedwingPositionControl::control_landing(const Vector2f &curr_pos, const Vector
float bearing_lastwp_currwp = bearing_airplane_currwp;

if (pos_sp_prev.valid) {
bearing_lastwp_currwp = get_bearing_to_next_waypoint((double)prev_wp(0), (double)prev_wp(1), (double)curr_wp(0),
(double)curr_wp(1));
bearing_lastwp_currwp = get_bearing_to_next_waypoint((double)prev_wp(0), (double)prev_wp(1),
(double)curr_wp(0), (double)curr_wp(1));
}

/* Horizontal landing control */
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1), (double)curr_wp(0),
(double)curr_wp(1));
float wp_distance = get_distance_to_next_waypoint((double)curr_pos(0), (double)curr_pos(1),
(double)curr_wp(0), (double)curr_wp(1));

/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
float wp_distance_save = wp_distance;
Expand Down Expand Up @@ -1607,13 +1614,11 @@ FixedwingPositionControl::get_tecs_thrust()
}

void
FixedwingPositionControl::handle_command()
FixedwingPositionControl::handle_command(const vehicle_command_s &vcmd)
{
if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
if (vcmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
// only abort landing before point of no return (horizontal and vertical)
if (_control_mode.flag_control_auto_enabled &&
_pos_sp_triplet.current.valid &&
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
if (_control_mode.flag_control_auto_enabled && (_time_started_landing != 0)) {

_fw_pos_ctrl_status.abort_landing = true;
mavlink_log_critical(&_mavlink_log_pub, "Landing aborted");
Expand Down Expand Up @@ -1715,14 +1720,19 @@ FixedwingPositionControl::run()

_sub_sensors.update();
airspeed_poll();
manual_control_setpoint_poll();
position_setpoint_triplet_poll();
vehicle_attitude_poll();
vehicle_command_poll();
vehicle_control_mode_poll();
vehicle_land_detected_poll();
vehicle_status_poll();

if (_control_mode.flag_control_auto_enabled) {
position_setpoint_triplet_poll();

} else if (_control_mode.flag_control_manual_enabled) {
manual_control_setpoint_poll();
}

Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
Vector2f ground_speed(_global_pos.vel_n, _global_pos.vel_e);

Expand Down Expand Up @@ -1812,7 +1822,7 @@ FixedwingPositionControl::reset_takeoff_state()
}

void
FixedwingPositionControl::reset_landing_state()
FixedwingPositionControl::reset_landing_state(const uint8_t pos_sp_type)
{
_time_started_landing = 0;

Expand All @@ -1827,7 +1837,7 @@ FixedwingPositionControl::reset_landing_state()

// reset abort land, unless loitering after an abort
if (_fw_pos_ctrl_status.abort_landing
&& _pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER) {
&& (pos_sp_type != position_setpoint_s::SETPOINT_TYPE_LOITER)) {

_fw_pos_ctrl_status.abort_landing = false;
}
Expand Down
Loading

0 comments on commit f9db41c

Please sign in to comment.