diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index 7e30f6919b..c6a79ad2e2 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -501,7 +501,7 @@ static const struct LogStructure log_structure[] = { { LOG_STATUS_MSG, sizeof(log_Status), "STAT", "QBfBBBBBB", "TimeUS,isFlying,isFlyProb,Armed,Safety,Crash,Still,Stage,Hit" }, { LOG_QTUN_MSG, sizeof(QuadPlane::log_QControl_Tuning), - "QTUN", "Qffffehhff", "TimeUS,AngBst,ThrOut,DAlt,Alt,BarAlt,DCRt,CRt,DVx,DVy" }, + "QTUN", "Qffffehhffff", "TimeUS,AngBst,ThrOut,DAlt,Alt,BarAlt,DCRt,CRt,DVx,DVy,DAx,DAy" }, #if OPTFLOW == ENABLED { LOG_OPTFLOW_MSG, sizeof(log_Optflow), "OF", "QBffff", "TimeUS,Qual,flowX,flowY,bodyX,bodyY" }, diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index bad20614d4..93de8565fd 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1050,19 +1050,9 @@ void QuadPlane::control_auto(const Location &loc) plane.nav_pitch_cd = pos_control->get_pitch(); } else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND && land_state == QLAND_POSITION1) { - /* - for initial land repositioning and descent we run the - velocity controller. This allows us to smoothly control the - velocity change from fixed wing flight to VTOL flight - */ - float ekfGndSpdLimit, ekfNavVelGainScaler; - ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); + Vector2f diff_wp = location_diff(plane.current_loc, loc); - Vector2f diff_wp; - diff_wp = location_diff(plane.current_loc, loc); - float distance = diff_wp.length(); - - if (land_speed_scale <= 0) { + if (land.speed_scale <= 0) { // initialise scaling so we start off targeting our // current linear speed towards the target. If this is // less than the wpnav speed then the wpnav speed is used @@ -1076,41 +1066,67 @@ void QuadPlane::control_auto(const Location &loc) float speed_towards_target = diff_wp.length() - newdiff.length(); // setup land_speed_scale so at current distance we maintain speed towards target, and slow down as // we approach - land_speed_scale = MAX(speed_towards_target, wp_nav->get_speed_xy() * 0.01) / MAX(distance, 1); + float distance = diff_wp.length(); + land.speed_scale = MAX(speed_towards_target, wp_nav->get_speed_xy() * 0.01) / MAX(distance, 1); } - // set target vertical velocity to zero. The aircraft may - // naturally try to climb or descend a bit depending on its - // approach speed and angle. We let it do that, just asking - // the quad motors to aim for zero climb rate - pos_control->set_desired_velocity_z(0); + // run fixed wing navigation + plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc); + + /* + calculate target velocity, not dropping it below 2m/s + */ + const float final_speed = 2.0f; + Vector2f target_speed = diff_wp * land.speed_scale; + if (target_speed.length() < final_speed) { + target_speed = target_speed.normalized() * final_speed; + } + pos_control->set_desired_velocity_xy(target_speed.x*100, + target_speed.y*100); + + float ekfGndSpdLimit, ekfNavVelGainScaler; + ahrs.getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler); - // set the desired XY velocity to bring us to the waypoint - pos_control->set_desired_velocity_xy(diff_wp.x*land_speed_scale*100, - diff_wp.y*land_speed_scale*100); pos_control->update_vel_controller_xyz(ekfNavVelGainScaler); + + const Vector3f& curr_pos = inertial_nav.get_position(); + pos_control->set_xy_target(curr_pos.x, curr_pos.y); + + pos_control->freeze_ff_xy(); // nav roll and pitch are controller by position controller plane.nav_roll_cd = pos_control->get_roll(); plane.nav_pitch_cd = pos_control->get_pitch(); - // initially constrain pitch so we don't nose down too - // much. The velocity controller tends to want to nose down - // initially - land_wp_proportion = constrain_float(MAX(land_wp_proportion, plane.auto_state.wp_proportion), 0, 1); - int32_t limit = MIN(land_wp_proportion * plane.aparm.pitch_limit_min_cd, -500); - plane.nav_pitch_cd = constrain_int32(plane.nav_pitch_cd, limit, plane.aparm.pitch_limit_max_cd); + /* + limit the pitch down with an expanding envelope. This + prevents the velocity controller demanding nose down during + the initial slowdown if the target velocity curve is higher + than the actual velocity curve (for a high drag + aircraft). Nose down will cause a lot of downforce on the + wings which will draw a lot of current and also cause the + aircraft to lose altitude rapidly. + */ + float pitch_limit_cd = linear_interpolate(-300, plane.aparm.pitch_limit_min_cd, + plane.auto_state.wp_proportion, 0, 1); + if (plane.nav_pitch_cd < pitch_limit_cd) { + plane.nav_pitch_cd = pitch_limit_cd; + // tell the pos controller we have limited the pitch to + // stop integrator buildup + pos_control->set_limit_accel_xy(); + } // call attitude controller attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_smooth(plane.nav_roll_cd, plane.nav_pitch_cd, - get_pilot_input_yaw_rate_cds(), + desired_auto_yaw_rate_cds(), smoothing_gain); - if (distance < 5 || plane.auto_state.wp_proportion >= 1) { - // move to loiter controller at 5m or if we overshoot the waypoint + if (plane.auto_state.wp_proportion >= 1 || + plane.auto_state.wp_distance < 5) { land_state = QLAND_POSITION2; + wp_nav->init_loiter_target(); plane.gcs_send_text_fmt(MAV_SEVERITY_INFO,"Land position2 started v=%.1f d=%.1f", - ahrs.groundspeed(), distance); + ahrs.groundspeed(), plane.auto_state.wp_distance); } } else if (plane.mission.get_current_nav_cmd().id == MAV_CMD_NAV_VTOL_LAND) { /* @@ -1226,12 +1242,11 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) // initially aim for current altitude plane.next_WP_loc.alt = plane.current_loc.alt; land_state = QLAND_POSITION1; - land_speed_scale = 0; - pos_control->init_vel_controller_xyz(); - + land.speed_scale = 0; + wp_nav->init_loiter_target(); + throttle_wait = false; - land_yaw_cd = get_bearing_cd(plane.prev_WP_loc, plane.next_WP_loc); - land_wp_proportion = 0; + land.yaw_cd = get_bearing_cd(plane.prev_WP_loc, plane.next_WP_loc); motors_lower_limit_start_ms = 0; Location origin = inertial_nav.get_origin(); Vector2f diff2d; @@ -1240,7 +1255,6 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) target.x = diff2d.x * 100; target.y = diff2d.y * 100; target.z = plane.next_WP_loc.alt - origin.alt; - wp_nav->set_wp_origin_and_destination(inertial_nav.get_position(), target); pos_control->set_alt_target(inertial_nav.get_altitude()); // also update nav_controller for status output @@ -1310,6 +1324,7 @@ bool QuadPlane::verify_vtol_land(const AP_Mission::Mission_Command &cmd) void QuadPlane::Log_Write_QControl_Tuning() { const Vector3f &desired_velocity = pos_control->get_desired_velocity(); + const Vector3f &accel_target = pos_control->get_accel_target(); struct log_QControl_Tuning pkt = { LOG_PACKET_HEADER_INIT(LOG_QTUN_MSG), time_us : AP_HAL::micros64(), @@ -1322,6 +1337,8 @@ void QuadPlane::Log_Write_QControl_Tuning() climb_rate : (int16_t)inertial_nav.get_velocity_z(), dvx : desired_velocity.x*0.01f, dvy : desired_velocity.y*0.01f, + dax : accel_target.x*0.01f, + day : accel_target.y*0.01f, }; plane.DataFlash.WriteBlock(&pkt, sizeof(pkt)); } diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 4268fed9e2..cfa8b41d01 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -70,6 +70,8 @@ public: int16_t climb_rate; float dvx; float dvy; + float dax; + float day; }; private: @@ -206,9 +208,11 @@ private: QLAND_FINAL, QLAND_COMPLETE } land_state; - int32_t land_yaw_cd; - float land_wp_proportion; - float land_speed_scale; + struct { + int32_t yaw_cd; + float speed_scale; + Vector2f target_velocity; + } land; enum frame_class { FRAME_CLASS_QUAD=0,