fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 22:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW implement MAV_CMD_DO_GO_AROUND

Browse Source
This commit is contained in:
Daniel Agar
2016-08-06 20:56:53 +02:00
committed by Lorenz Meier
parent aa3ffa28c8
commit 2a15578f8d
9 changed files with 186 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+88 -29
View File
@@ -94,6 +94,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/tecs_status.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
@@ -164,6 +165,7 @@ private:
int _pos_sp_triplet_sub;
int _ctrl_state_sub; /**< control state subscription */
int _control_mode_sub; /**< control mode subscription */
int _vehicle_command_sub; /**< vehicle command subscription */
int _vehicle_status_sub; /**< vehicle status subscription */
int _vehicle_land_detected_sub; /**< vehicle land detected subscription */
int _params_sub; /**< notification of parameter updates */
@@ -181,6 +183,7 @@ private:
struct fw_pos_ctrl_status_s _fw_pos_ctrl_status; /**< navigation capabilities */
struct manual_control_setpoint_s _manual; /**< r/c channel data */
struct vehicle_control_mode_s _control_mode; /**< control mode */
struct vehicle_command_s _vehicle_command; /**< vehicle commands */
struct vehicle_status_s _vehicle_status; /**< vehicle status */
struct vehicle_land_detected_s _vehicle_land_detected; /**< vehicle land detected */
struct vehicle_global_position_s _global_pos; /**< global vehicle position */
@@ -387,6 +390,11 @@ private:
*/
void vehicle_control_mode_poll();
/**
* Check for new in vehicle commands
*/
void vehicle_command_poll();
/**
* Check for changes in vehicle status.
*/
@@ -477,6 +485,11 @@ private:
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d,
const struct position_setpoint_triplet_s &pos_sp_triplet);
/**
* Handle incoming vehicle commands
*/
void handle_command();
/**
* Shim for calling task_main from task_create.
*/
@@ -533,6 +546,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_pos_sp_triplet_sub(-1),
_ctrl_state_sub(-1),
_control_mode_sub(-1),
_vehicle_command_sub(-1),
_vehicle_status_sub(-1),
_vehicle_land_detected_sub(-1),
_params_sub(-1),
@@ -553,6 +567,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_fw_pos_ctrl_status(),
_manual(),
_control_mode(),
_vehicle_command(),
_vehicle_status(),
_vehicle_land_detected(),
_global_pos(),
@@ -830,6 +845,19 @@ FixedwingPositionControl::vehicle_control_mode_poll()
}
}
void
FixedwingPositionControl::vehicle_command_poll()
{
bool updated;
orb_check(_vehicle_command_sub, &updated);
if (updated) {
orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &_vehicle_command);
handle_command();
}
}
void
FixedwingPositionControl::vehicle_status_poll()
{
@@ -1396,34 +1424,36 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
float alt_sp;
if (_fw_pos_ctrl_status.abort_landing == true) {
// if we entered loiter due to an aborted landing, demand
// altitude setpoint well above landing waypoint
alt_sp = pos_sp_triplet.current.alt + 2.0f * _parameters.climbout_diff;
} else {
// use altitude given by waypoint
alt_sp = pos_sp_triplet.current.alt;
}
float alt_sp = pos_sp_triplet.current.alt;
if (in_takeoff_situation()) {
alt_sp = math::max(alt_sp, _takeoff_ground_alt + _parameters.climbout_diff);
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-5.0f), math::radians(5.0f));
}
if (in_takeoff_situation() ||
((_global_pos.alt < pos_sp_triplet.current.alt + _parameters.climbout_diff)
&& _fw_pos_ctrl_status.abort_landing == true)) {
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f),
math::radians(10.0f));
if (_fw_pos_ctrl_status.abort_landing) {
if (pos_sp_triplet.current.alt - _global_pos.alt < _parameters.climbout_diff) {
// aborted landing complete, normal loiter over landing point
_fw_pos_ctrl_status.abort_landing = false;
} else {
// continue straight until vehicle has sufficient altitude
_att_sp.roll_body = 0.0f;
}
}
tecs_update_pitch_throttle(alt_sp, calculate_target_airspeed(mission_airspeed), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
tecs_update_pitch_throttle(alt_sp,
calculate_target_airspeed(mission_airspeed),
eas2tas,
math::radians(_parameters.pitch_limit_min),
math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min,
_parameters.throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
_global_pos.alt,
ground_speed);
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
@@ -1431,9 +1461,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// if they have been enabled using the corresponding parameter
_att_sp.apply_flaps = true;
// save time at which we started landing
// save time at which we started landing and reset abort_landing
if (_time_started_landing == 0) {
_time_started_landing = hrt_absolute_time();
_fw_pos_ctrl_status.abort_landing = false;
}
float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
@@ -2037,13 +2069,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// reset hold altitude
_hold_alt = _global_pos.alt;
// reset terrain estimation relevant values
_time_started_landing = 0;
_time_last_t_alt = 0;
// reset lading abort state
_fw_pos_ctrl_status.abort_landing = false;
/* reset landing and takeoff state */
if (!_last_manual) {
reset_landing_state();
@@ -2147,6 +2172,25 @@ FixedwingPositionControl::get_tecs_thrust()
}
}
void
FixedwingPositionControl::handle_command()
{
if (_vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {
// only abort landing before point of no return (horizontal and vertical)
if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
if (_land_noreturn_vertical) {
mavlink_log_info(&_mavlink_log_pub, "#Landing, can't abort after flare");
} else {
_fw_pos_ctrl_status.abort_landing = true;
mavlink_log_info(&_mavlink_log_pub, "#Landing, aborted");
}
}
}
}
void
FixedwingPositionControl::task_main()
{
@@ -2159,6 +2203,7 @@ FixedwingPositionControl::task_main()
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -2210,6 +2255,9 @@ FixedwingPositionControl::task_main()
/* check vehicle control mode for changes to publication state */
vehicle_control_mode_poll();
/* check for new vehicle commands */
vehicle_command_poll();
/* check vehicle status for changes to publication state */
vehicle_status_poll();
@@ -2321,12 +2369,23 @@ void FixedwingPositionControl::reset_landing_state()
{
_time_started_landing = 0;
// reset terrain estimation relevant values
_time_last_t_alt = 0;
_land_noreturn_horizontal = false;
_land_noreturn_vertical = false;
_land_stayonground = false;
_land_motor_lim = false;
_land_onslope = false;
_land_useterrain = false;
// reset abort land, unless loitering after an abort
if (_fw_pos_ctrl_status.abort_landing == true
&& _pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER) {
_fw_pos_ctrl_status.abort_landing = false;
}
}
void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,