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

Merge pull request #475 from PX4/fw_autoland 

Autolanding for fixed wing
This commit is contained in:
Lorenz Meier 2013-10-26 16:01:57 -07:00
parent 2d4fc9eea1 411eb1f4ef
commit 59c04adada
6 changed files with 183 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Documentation/fixed_wing_control.odg Normal file
View File

Binary file not shown.

BIN
Documentation/l1_control.odg
View File

Binary file not shown.

5
src/lib/external_lgpl/tecs/tecs.h
View File

@ -94,6 +94,11 @@ public:
// Rate of change of velocity along X body axis in m/s^2
float get_VXdot(void) { return _vel_dot; }
float get_speed_weight() {
return _spdWeight;
}
// log data on internal state of the controller. Called at 10Hz
// void log_data(DataFlash_Class &dataflash, uint8_t msgid);

169
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -158,6 +158,12 @@ private:
float _launch_alt;
bool _launch_valid;
/* land states */
/* not in non-abort mode for landing yet */
bool land_noreturn;
/* heading hold */
float target_bearing;
/* throttle and airspeed states */
float _airspeed_error; ///< airspeed error to setpoint in m/s
bool _airspeed_valid; ///< flag if a valid airspeed estimate exists
@ -197,6 +203,8 @@ private:
float throttle_max;
float throttle_cruise;
float throttle_land_max;
float loiter_hold_radius;
} _parameters; /**< local copies of interesting parameters */
@ -229,6 +237,8 @@ private:
param_t throttle_max;
param_t throttle_cruise;
param_t throttle_land_max;
param_t loiter_hold_radius;
} _parameter_handles; /**< handles for interesting parameters */
@ -327,7 +337,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_airspeed_error(0.0f),
_airspeed_valid(false),
_groundspeed_undershoot(0.0f),
_global_pos_valid(false)
_global_pos_valid(false),
land_noreturn(false)
{
_nav_capabilities.turn_distance = 0.0f;
@ -345,6 +356,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.throttle_min = param_find("FW_THR_MIN");
_parameter_handles.throttle_max = param_find("FW_THR_MAX");
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
@ -408,6 +420,8 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.time_const, &(_parameters.time_const));
param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
@ -630,6 +644,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
/* no throttle limit as default */
float throttle_max = 1.0f;
/* AUTONOMOUS FLIGHT */
// XXX this should only execute if auto AND safety off (actuators active),
@ -639,11 +656,12 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_parameters.speed_weight);
/* execute navigation once we have a setpoint */
if (_setpoint_valid) {
float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
/* current waypoint (the one currently heading for) */
math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
@ -711,27 +729,87 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
if (wp_distance < 15.0f || land_noreturn) {
/* heading hold, along the line connecting this and the last waypoint */
// if (global_triplet.previous_valid) {
// target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
// } else {
if (!land_noreturn)
target_bearing = _att.yaw;
//}
warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
if (altitude_error > -5.0f)
land_noreturn = true;
} else {
/* normal navigation */
_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
}
/* do not go down too early */
if (wp_distance > 50.0f) {
altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
}
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// XXX this could make a great param
if (altitude_error > -20.0f) {
float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1)
float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
float land_pitch_min = math::radians(5.0f);
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = _parameters.airspeed_min;
float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
if (altitude_error > -4.0f) {
/* land with minimal speed */
/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
_tecs.set_speed_weight(2.0f);
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, flare_angle_rad,
false, flare_angle_rad,
0.0f, _parameters.throttle_max, throttle_land,
math::radians(-10.0f), math::radians(15.0f));
/* kill the throttle if param requests it */
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
} else if (wp_distance < 60.0f && altitude_error > -20.0f) {
/* minimize speed to approach speed */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, flare_angle_rad,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f));
} else {
/* normal cruise speed */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
@ -790,7 +868,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_loiter_hold = true;
}
float altitude_error = _loiter_hold_alt - _global_pos.alt;
altitude_error = _loiter_hold_alt - _global_pos.alt;
math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
@ -801,7 +879,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_att_sp.yaw_body = _l1_control.nav_bearing();
/* climb with full throttle if the altitude error is bigger than 5 meters */
bool climb_out = (altitude_error > 5);
bool climb_out = (altitude_error > 3);
float min_pitch;
@ -824,11 +902,53 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
}
/* reset land state */
if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
land_noreturn = false;
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
}
} else if (0/* easy mode enabled */) {
/** EASY FLIGHT **/
if (0/* switched from another mode to easy */) {
_seatbelt_hold_heading = _att.yaw;
}
if (0/* easy on and manual control yaw non-zero */) {
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
}
/* climb out control */
bool climb_out = false;
/* user wants to climb out */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
climb_out = true;
}
/* if in seatbelt mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float seatbelt_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
seatbelt_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
} else if (0/* seatbelt mode enabled */) {
/** SEATBELT FLIGHT **/
@ -848,13 +968,28 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
/* user switched off throttle */
if (_manual.throttle < 0.1f) {
throttle_max = 0.0f;
/* switch to pure pitch based altitude control, give up speed */
_tecs.set_speed_weight(0.0f);
}
/* climb out control */
bool climb_out = false;
/* user wants to climb out */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
climb_out = true;
}
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_att_sp.roll_body = _manual.roll;
_att_sp.yaw_body = _manual.yaw;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
seatbelt_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
climb_out, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
@ -867,7 +1002,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
_att_sp.thrust = _tecs.get_throttle_demand();
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
return setpoint;
}

1
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -75,6 +75,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);

37
src/modules/mavlink/waypoints.c
View File

@ -398,7 +398,14 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
if (time_elapsed) {
/* safeguard against invalid missions with last wp autocontinue on */
if (wpm->current_active_wp_id == wpm->size - 1) {
/* stop handling missions here */
cur_wp->autocontinue = false;
}
if (cur_wp->autocontinue) {
cur_wp->current = 0;
float navigation_lat = -1.0f;
@ -419,13 +426,16 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
navigation_frame = MAV_FRAME_LOCAL_NED;
}
/* guard against missions without final land waypoint */
/* only accept supported navigation waypoints, skip unknown ones */
do {
/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
/* this is a navigation waypoint */
navigation_frame = cur_wp->frame;
@ -434,17 +444,20 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
navigation_alt = cur_wp->z;
}
if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
/* the last waypoint was reached, if auto continue is
* activated keep the system loitering there.
*/
cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
cur_wp->frame = navigation_frame;
cur_wp->x = navigation_lat;
cur_wp->y = navigation_lon;
cur_wp->z = navigation_alt;
cur_wp->autocontinue = false;
if (wpm->current_active_wp_id == wpm->size - 1) {
/* if we're not landing at the last nav waypoint, we're falling back to loiter */
if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
/* the last waypoint was reached, if auto continue is
* activated AND it is NOT a land waypoint, keep the system loitering there.
*/
cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
cur_wp->frame = navigation_frame;
cur_wp->x = navigation_lat;
cur_wp->y = navigation_lon;
cur_wp->z = navigation_alt;
}
/* we risk an endless loop for missions without navigation waypoints, abort. */
break;