From 2b257663dce327950800aa1e81aa6c17063a14e7 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 4 Jan 2022 15:53:45 +1100 Subject: [PATCH] Plane: added APIs for lua ship landing --- ArduPlane/ArduPlane.cpp | 34 ++++++++ ArduPlane/GCS_Mavlink.cpp | 6 +- ArduPlane/Log.cpp | 2 +- ArduPlane/Parameters.cpp | 2 +- ArduPlane/Parameters.h | 2 +- ArduPlane/Plane.h | 2 + ArduPlane/quadplane.cpp | 160 +++++++++++++++++++++++--------------- ArduPlane/quadplane.h | 9 +++ 8 files changed, 147 insertions(+), 70 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 52f8f32411..7d67005f27 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -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 diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index d3fb8117a0..39f2a06e4e 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -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)) { diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index ad3ba064f6..7904d67400 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -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()); diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index c09f77752a..c38483f0b1 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -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), diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 59f8b08e2c..bae6c1a3de 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -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 diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 4f6030b78c..8a74bb1e3c 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -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 }; diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 3db9d6ae9e..865522a292 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -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 diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 7b184afc00..284c6f99da 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -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;