Plane: added APIs for lua ship landing

This commit is contained in:
Andrew Tridgell 2022-01-04 15:53:45 +11:00 committed by Randy Mackay
parent 0e70270f49
commit 2b257663dc
8 changed files with 147 additions and 70 deletions

View File

@ -738,6 +738,7 @@ bool Plane::get_target_location(Location& target_loc)
case Mode::Number::GUIDED:
case Mode::Number::AUTO:
case Mode::Number::LOITER:
case Mode::Number::TAKEOFF:
#if HAL_QUADPLANE_ENABLED
case Mode::Number::QLOITER:
case Mode::Number::QLAND:
@ -751,6 +752,39 @@ bool Plane::get_target_location(Location& target_loc)
}
return false;
}
/*
update_target_location() works in all auto navigation modes
*/
bool Plane::update_target_location(const Location &old_loc, const Location &new_loc)
{
if (!old_loc.same_latlon_as(next_WP_loc)) {
return false;
}
ftype alt_diff;
if (!old_loc.get_alt_distance(next_WP_loc, alt_diff) ||
!is_zero(alt_diff)) {
return false;
}
next_WP_loc = new_loc;
next_WP_loc.change_alt_frame(old_loc.get_alt_frame());
return true;
}
// allow for velocity matching in VTOL
bool Plane::set_velocity_match(const Vector2f &velocity)
{
#if HAL_QUADPLANE_ENABLED
if (quadplane.in_vtol_mode() || quadplane.in_vtol_land_sequence()) {
quadplane.poscontrol.velocity_match = velocity;
quadplane.poscontrol.last_velocity_match_ms = AP_HAL::millis();
return true;
}
#endif
return false;
}
#endif // AP_SCRIPTING_ENABLED
#if OSD_ENABLED

View File

@ -643,7 +643,7 @@ void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status,
#if HAL_ADSB_ENABLED
plane.avoidance_adsb.handle_msg(msg);
#endif
#if SHIP_LANDING_ENABLED
#if AP_SCRIPTING_ENABLED
// pass message to follow library
plane.g2.follow.handle_msg(msg);
#endif
@ -879,7 +879,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_packet(const mavlink_command_in
return handle_command_int_guided_slew_commands(packet);
case MAV_CMD_DO_FOLLOW:
#if SHIP_LANDING_ENABLED
#if AP_SCRIPTING_ENABLED
// param1: sysid of target to follow
if ((packet.param1 > 0) && (packet.param1 <= 255)) {
plane.g2.follow.set_target_sysid((uint8_t)packet.param1);
@ -1089,7 +1089,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_long_packet(const mavlink_command_l
}
return MAV_RESULT_ACCEPTED;
#if SHIP_LANDING_ENABLED
#if AP_SCRIPTING_ENABLED
case MAV_CMD_DO_FOLLOW:
// param1: sysid of target to follow
if ((packet.param1 > 0) && (packet.param1 <= 255)) {

View File

@ -23,7 +23,7 @@ void Plane::Log_Write_Attitude(void)
} else {
ahrs.Write_Attitude(targets);
}
if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight()) {
if (quadplane.in_vtol_mode() || quadplane.in_assisted_flight() || quadplane.in_vtol_airbrake()) {
// log quadplane PIDs separately from fixed wing PIDs
logger.Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info());
logger.Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info());

View File

@ -1230,7 +1230,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Bitmask: 0: Servo 1, 1: Servo 2, 2: Servo 3, 3: Servo 4, 4: Servo 5, 5: Servo 6, 6: Servo 7, 7: Servo 8, 8: Servo 9, 9: Servo 10, 10: Servo 11, 11: Servo 12, 12: Servo 13, 13: Servo 14, 14: Servo 15
AP_GROUPINFO("ONESHOT_MASK", 32, ParametersG2, oneshot_mask, 0),
#if SHIP_LANDING_ENABLED
#if AP_SCRIPTING_ENABLED
// @Group: FOLL
// @Path: ../libraries/AP_Follow/AP_Follow.cpp
AP_SUBGROUPINFO(follow, "FOLL", 33, ParametersG2, AP_Follow),

View File

@ -545,7 +545,7 @@ public:
AC_PID guidedHeading{5000.0, 0.0, 0.0, 0 , 10.0, 5.0, 5.0 , 5.0 , 0.2};
#endif
#if SHIP_LANDING_ENABLED
#if AP_SCRIPTING_ENABLED
AP_Follow follow;
#endif

View File

@ -1210,6 +1210,8 @@ public:
#if AP_SCRIPTING_ENABLED
bool set_target_location(const Location& target_loc) override;
bool get_target_location(Location& target_loc) override;
bool update_target_location(const Location &old_loc, const Location &new_loc) override;
bool set_velocity_match(const Vector2f &velocity) override;
#endif // AP_SCRIPTING_ENABLED
};

View File

@ -1713,7 +1713,7 @@ void QuadPlane::update(void)
}
const uint32_t now = AP_HAL::millis();
if (!in_vtol_mode()) {
if (!in_vtol_mode() && !in_vtol_airbrake()) {
// we're in a fixed wing mode, cope with transitions and check
// for assistance needed
if (plane.control_mode == &plane.mode_manual ||
@ -1732,7 +1732,7 @@ void QuadPlane::update(void)
} else {
assisted_flight = false;
assisted_flight = in_vtol_airbrake();
// output to motors
motors_output();
@ -2011,6 +2011,9 @@ bool QuadPlane::in_vtol_auto(void) const
/*
are we in a VTOL mode? This is used to decide if we run the
transition handling code or not
note that AIRBRAKE is not considered in_vtol_mode even though the
VTOL motors are running
*/
bool QuadPlane::in_vtol_mode(void) const
{
@ -2018,7 +2021,7 @@ bool QuadPlane::in_vtol_mode(void) const
return false;
}
if (in_vtol_land_sequence()) {
return poscontrol.get_state() != QPOS_APPROACH;
return poscontrol.get_state() != QPOS_APPROACH && poscontrol.get_state() != QPOS_AIRBRAKE;
}
if (plane.control_mode->is_vtol_mode()) {
return true;
@ -2033,7 +2036,7 @@ bool QuadPlane::in_vtol_mode(void) const
return true;
}
if (in_vtol_auto()) {
if (!plane.auto_state.vtol_loiter || poscontrol.get_state() > QPOS_APPROACH) {
if (!plane.auto_state.vtol_loiter || poscontrol.get_state() > QPOS_AIRBRAKE) {
return true;
}
}
@ -2082,7 +2085,8 @@ void QuadPlane::update_land_positioning(void)
poscontrol.target_vel_cms = Vector3f(-pitch_in, roll_in, 0) * speed_max_cms;
poscontrol.target_vel_cms.rotate_xy(ahrs_view->yaw);
poscontrol.target_cm += (poscontrol.target_vel_cms * dt).topostype();
// integrate our corrected position
poscontrol.xy_correction += poscontrol.target_vel_cms.xy() * dt * 0.01;
poscontrol.pilot_correction_active = (!is_zero(roll_in) || !is_zero(pitch_in));
if (poscontrol.pilot_correction_active) {
@ -2095,9 +2099,16 @@ void QuadPlane::update_land_positioning(void)
*/
void QuadPlane::run_xy_controller(void)
{
float accel_cmss = wp_nav->get_wp_acceleration();
if (in_vtol_land_sequence() &&
(poscontrol.get_state() == QPOS_POSITION1 ||
poscontrol.get_state() == QPOS_POSITION2)) {
accel_cmss = MAX(accel_cmss, transition_decel*100);
}
const float speed_cms = wp_nav->get_default_speed_xy();
pos_control->set_max_speed_accel_xy(speed_cms, accel_cmss);
pos_control->set_correction_speed_accel_xy(speed_cms, accel_cmss);
if (!pos_control->is_active_xy()) {
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->init_xy_controller();
}
pos_control->update_xy_controller();
@ -2130,6 +2141,8 @@ void QuadPlane::poscontrol_init_approach(void)
}
poscontrol.thrust_loss_start_ms = 0;
}
poscontrol.pilot_correction_done = false;
poscontrol.xy_correction.zero();
}
/*
@ -2158,13 +2171,6 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
// if it is active then the rate control should not be reset at all
qp.attitude_control->reset_yaw_target_and_rate(false);
pos1_start_speed = plane.ahrs.groundspeed_vector().length();
} else if (s == QPOS_POSITION2) {
// POSITION2 changes target speed, so we need to change it
// back to normal
qp.pos_control->set_max_speed_accel_xy(qp.wp_nav->get_default_speed_xy(),
qp.wp_nav->get_wp_acceleration());
qp.pos_control->set_correction_speed_accel_xy(qp.wp_nav->get_default_speed_xy(),
qp.wp_nav->get_wp_acceleration());
} else if (s == QPOS_AIRBRAKE) {
// start with zero integrator on vertical throttle
qp.pos_control->get_accel_z_pid().set_integrator(0);
@ -2208,6 +2214,11 @@ void QuadPlane::vtol_position_controller(void)
// and tilt is more than tilt max
bool suppress_z_controller = false;
Vector2f landing_velocity;
if (now_ms - poscontrol.last_velocity_match_ms < 1000) {
landing_velocity = poscontrol.velocity_match;
}
// horizontal position control
switch (poscontrol.get_state()) {
@ -2419,19 +2430,11 @@ void QuadPlane::vtol_position_controller(void)
if (distance > 0.1) {
target_speed_xy_cms = diff_wp.normalized() * target_speed * 100;
}
if (!tailsitter.enabled()) {
// this method ignores pos-control velocity/accel limtis
pos_control->set_vel_desired_xy_cms(target_speed_xy_cms);
// reset position controller xy target to current position
// because we only want velocity control (no position control)
const Vector2f& curr_pos = inertial_nav.get_position_xy_cm();
pos_control->set_pos_target_xy_cm(curr_pos.x, curr_pos.y);
pos_control->set_accel_desired_xy_cmss(Vector2f());
} else {
// tailsitters use input shaping and abide by velocity limits
pos_control->input_vel_accel_xy(target_speed_xy_cms, Vector2f());
}
target_speed_xy_cms += landing_velocity * 100;
// use input shaping and abide by accel and jerk limits
pos_control->input_vel_accel_xy(target_speed_xy_cms, Vector2f());
// run horizontal velocity controller
run_xy_controller();
@ -2461,35 +2464,19 @@ void QuadPlane::vtol_position_controller(void)
case QPOS_POSITION2:
case QPOS_LAND_DESCEND: {
setup_target_position();
/*
for final land repositioning and descent we run the position controller
*/
if (poscontrol.pilot_correction_done) {
// if the pilot has repositioned the vehicle then we switch to velocity control. This prevents the vehicle
// shifting position in the event of GPS glitches.
Vector2f zero;
pos_control->input_vel_accel_xy(poscontrol.target_vel_cms.xy(), zero);
} else {
Vector2f zero;
pos_control->input_pos_vel_accel_xy(poscontrol.target_cm.xy(), zero, zero);
}
Vector2f zero;
Vector2f vel_cms = poscontrol.target_vel_cms.xy() + landing_velocity*100;
pos_control->input_pos_vel_accel_xy(poscontrol.target_cm.xy(), vel_cms, zero);
// also run fixed wing navigation
plane.nav_controller->update_waypoint(plane.current_loc, loc);
update_land_positioning();
/*
apply the same asymmetric speed limits from POSITION1, so we
don't suddenly speed up when we change to POSITION2 and
LAND_DESCEND
*/
const Vector2f diff_wp = plane.current_loc.get_distance_NE(loc);
const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle()));
pos_control->set_max_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration());
run_xy_controller();
// nav roll and pitch are controlled by position controller
@ -2516,7 +2503,8 @@ void QuadPlane::vtol_position_controller(void)
} else {
// we use velocity control in QPOS_LAND_FINAL to allow for GPS glitch handling
Vector2f zero;
pos_control->input_vel_accel_xy(poscontrol.target_vel_cms.xy(), zero);
Vector2f vel_cms = poscontrol.target_vel_cms.xy() + landing_velocity*100;
pos_control->input_vel_accel_xy(vel_cms, zero);
}
run_xy_controller();
@ -2677,20 +2665,12 @@ void QuadPlane::setup_target_position(void)
set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
}
const Vector2f diff2d = origin.get_distance_NE(loc);
Vector2f diff2d = origin.get_distance_NE(loc);
diff2d += poscontrol.xy_correction;
poscontrol.target_cm.x = diff2d.x * 100;
poscontrol.target_cm.y = diff2d.y * 100;
poscontrol.target_cm.z = plane.next_WP_loc.alt - origin.alt;
const uint32_t now = AP_HAL::millis();
if (!loc.same_latlon_as(last_auto_target) ||
plane.next_WP_loc.alt != last_auto_target.alt ||
now - last_loiter_ms > 500) {
wp_nav->set_wp_destination(poscontrol.target_cm.tofloat());
last_auto_target = loc;
}
last_loiter_ms = now;
// set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
@ -2701,18 +2681,24 @@ void QuadPlane::setup_target_position(void)
*/
void QuadPlane::takeoff_controller(void)
{
if (!hal.util->get_soft_armed()) {
return;
}
/*
for takeoff we use the position controller
*/
setup_target_position();
// set position controller desired velocity and acceleration to zero
pos_control->set_vel_desired_xy_cms(Vector2f());
pos_control->set_accel_desired_xy_cmss(Vector2f());
// set position control target and update
Vector2f zero;
pos_control->input_vel_accel_xy(zero, zero);
Vector2f vel, zero;
if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) {
vel = poscontrol.velocity_match * 100;
}
pos_control->set_accel_desired_xy_cmss(zero);
pos_control->set_vel_desired_xy_cms(vel);
pos_control->input_vel_accel_xy(vel, zero);
run_xy_controller();
@ -2753,6 +2739,16 @@ void QuadPlane::waypoint_controller(void)
{
setup_target_position();
const Location &loc = plane.next_WP_loc;
const uint32_t now = AP_HAL::millis();
if (!loc.same_latlon_as(last_auto_target) ||
plane.next_WP_loc.alt != last_auto_target.alt ||
now - last_loiter_ms > 500) {
wp_nav->set_wp_destination(poscontrol.target_cm.tofloat());
last_auto_target = loc;
}
last_loiter_ms = now;
/*
this is full copter control of auto flight
*/
@ -3082,10 +3078,18 @@ bool QuadPlane::verify_vtol_land(void)
const float dist = (inertial_nav.get_position_neu_cm().topostype() - poscontrol.target_cm).xy().length() * 0.01;
reached_position = dist < descend_dist_threshold;
}
Vector2f target_vel;
if (AP_HAL::millis() - poscontrol.last_velocity_match_ms < 1000) {
target_vel = poscontrol.velocity_match;
}
Vector3f vel_ned;
UNUSED_RESULT(plane.ahrs.get_velocity_NED(vel_ned));
if (reached_position &&
plane.ahrs.groundspeed() < descend_speed_threshold) {
(vel_ned.xy() - target_vel).length() < descend_speed_threshold) {
poscontrol.set_state(QPOS_LAND_DESCEND);
poscontrol.pilot_correction_done = false;
poscontrol.xy_correction.zero();
#if AC_FENCE == ENABLED
plane.fence.auto_disable_fence_for_landing();
#endif
@ -3638,6 +3642,23 @@ bool QuadPlane::in_vtol_land_poscontrol(void) const
return false;
}
/*
see if we are in the airbrake phase of a VTOL landing
*/
bool QuadPlane::in_vtol_airbrake(void) const
{
if (plane.control_mode == &plane.mode_qrtl &&
poscontrol.get_state() == QPOS_AIRBRAKE) {
return true;
}
if (plane.control_mode == &plane.mode_auto &&
is_vtol_land(plane.mission.get_current_nav_cmd().id) &&
poscontrol.get_state() == QPOS_AIRBRAKE) {
return true;
}
return false;
}
// return true if we should show VTOL view
bool QuadPlane::show_vtol_view() const
{
@ -3873,4 +3894,15 @@ MAV_VTOL_STATE SLT_Transition::get_mav_vtol_state() const
return MAV_VTOL_STATE_UNDEFINED;
}
/*
see if we are in a VTOL takeoff
*/
bool QuadPlane::in_vtol_takeoff(void) const
{
if (in_vtol_auto() && is_vtol_takeoff(plane.mission.get_current_nav_cmd().id)) {
return true;
}
return false;
}
#endif // HAL_QUADPLANE_ENABLED

View File

@ -108,6 +108,7 @@ public:
bool verify_vtol_land(void);
bool in_vtol_auto(void) const;
bool in_vtol_mode(void) const;
bool in_vtol_takeoff(void) const;
bool in_vtol_posvel_mode(void) const;
void update_throttle_hover();
bool show_vtol_view() const;
@ -433,6 +434,7 @@ private:
return AP_HAL::millis() - last_state_change_ms;
}
Vector3p target_cm;
Vector2f xy_correction;
Vector3f target_vel_cms;
bool slow_descent:1;
bool pilot_correction_active;
@ -442,6 +444,8 @@ private:
bool reached_wp_speed;
uint32_t last_run_ms;
float pos1_start_speed;
Vector2f velocity_match;
uint32_t last_velocity_match_ms;
private:
uint32_t last_state_change_ms;
enum position_control_state state;
@ -564,6 +568,11 @@ private:
see if we are in the VTOL position control phase of a landing
*/
bool in_vtol_land_poscontrol(void) const;
/*
are we in the airbrake phase of a VTOL landing?
*/
bool in_vtol_airbrake(void) const;
// Q assist state, can be enabled, disabled or force. Default to enabled
Q_ASSIST_STATE_ENUM q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED;