From b5a4f2455918c63ab48659c84dc77d8ab5728272 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 2 Feb 2022 14:10:27 +1030 Subject: [PATCH] Copter: use position controller for landing reposition --- ArduCopter/mode.cpp | 68 ++++++++++++++++++++++++++++----------- ArduCopter/mode.h | 1 + ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_rtl.cpp | 19 +++++------ 4 files changed, 59 insertions(+), 31 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 7eda3f3c93..4e7054c888 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -431,6 +431,39 @@ void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, floa // roll_out and pitch_out are returned } +// transform pilot's roll or pitch input into a desired velocity +Vector2f Mode::get_pilot_desired_velocity(float vel_max) const +{ + Vector2f vel; + + // throttle failsafe check + if (copter.failsafe.radio || !copter.ap.rc_receiver_present) { + return vel; + } + // fetch roll and pitch inputs + float roll_out = channel_roll->get_control_in(); + float pitch_out = channel_pitch->get_control_in(); + + // convert roll and pitch inputs to -1 to +1 range + float scaler = 1.0 / (float)ROLL_PITCH_YAW_INPUT_MAX; + roll_out *= scaler; + pitch_out *= scaler; + + // convert roll and pitch inputs into velocity in NE frame + vel = Vector2f(-pitch_out, roll_out); + if (vel.is_zero()) { + return vel; + } + copter.rotate_body_frame_to_NE(vel.x, vel.y); + + // Transform square input range to circular output + // vel_scaler is the vector to the edge of the +- 1.0 square in the direction of the current input + Vector2f vel_scaler = vel / MAX(fabsf(vel.x), fabsf(vel.y)); + // We scale the output by the ratio of the distance to the square to the unit circle and multiply by vel_max + vel *= vel_max / vel_scaler.length(); + return vel; +} + bool Mode::_TakeOff::triggered(const float target_climb_rate) const { if (!copter.ap.land_complete) { @@ -598,13 +631,12 @@ void Mode::land_run_vertical_control(bool pause_descent) void Mode::land_run_horizontal_control() { - float target_roll = 0.0f; - float target_pitch = 0.0f; + Vector2f vel_correction; float target_yaw_rate = 0; // relax loiter target if we might be landed if (copter.ap.land_complete_maybe) { - loiter_nav->soften_for_landing(); + pos_control->soften_for_landing_xy(); } // process pilot inputs @@ -621,11 +653,14 @@ void Mode::land_run_horizontal_control() // apply SIMPLE mode transform to pilot inputs update_simple_mode(); - // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); + // convert pilot input to reposition velocity + // use half maximum acceleration as the maximum velocity to ensure aircraft will + // stop from full reposition speed in less than 1 second. + const float max_pilot_vel = wp_nav->get_wp_acceleration() * 0.5; + vel_correction = get_pilot_desired_velocity(max_pilot_vel); // record if pilot has overridden roll or pitch - if (!is_zero(target_roll) || !is_zero(target_pitch)) { + if (!vel_correction.is_zero()) { if (!copter.ap.land_repo_active) { AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); } @@ -641,11 +676,11 @@ void Mode::land_run_horizontal_control() } // this variable will be updated if prec land target is in sight and pilot isn't trying to reposition the vehicle - bool doing_precision_landing = false; + copter.ap.prec_land_active = false; #if PRECISION_LANDING == ENABLED - doing_precision_landing = !copter.ap.land_repo_active && copter.precland.target_acquired(); + copter.ap.prec_land_active = !copter.ap.land_repo_active && copter.precland.target_acquired(); // run precision landing - if (doing_precision_landing) { + if (copter.ap.prec_land_active) { Vector2f target_pos, target_vel; if (!copter.precland.get_target_position_cm(target_pos)) { target_pos = inertial_nav.get_position_xy_cm(); @@ -657,12 +692,10 @@ void Mode::land_run_horizontal_control() Vector2p landing_pos = target_pos.topostype(); // target vel will remain zero if landing target is stationary pos_control->input_pos_vel_accel_xy(landing_pos, target_vel, zero); - // run pos controller - pos_control->update_xy_controller(); } #endif - if (!doing_precision_landing) { + if (!copter.ap.prec_land_active) { if (copter.ap.prec_land_active) { // precland isn't active anymore but was active a while back // lets run an init again @@ -671,15 +704,12 @@ void Mode::land_run_horizontal_control() loiter_nav->get_stopping_point_xy(stopping_point); loiter_nav->init_target(stopping_point); } - // process roll, pitch inputs - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); - - // run loiter controller - loiter_nav->update(); + Vector2f accel; + pos_control->input_vel_accel_xy(vel_correction, accel); } - copter.ap.prec_land_active = doing_precision_landing; - + // run pos controller + pos_control->update_xy_controller(); Vector3f thrust_vector = pos_control->get_thrust_vector(); if (g2.wp_navalt_min > 0) { diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index cc27ab6247..4c9ee573ae 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -90,6 +90,7 @@ public: // pilot input processing void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const; + Vector2f get_pilot_desired_velocity(float vel_max) const; float get_pilot_desired_yaw_rate(float yaw_in); float get_pilot_desired_throttle() const; diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 29fe7803d0..dae8ecdb65 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -47,7 +47,7 @@ void ModeBrake::run() // relax stop target if we might be landed if (copter.ap.land_complete_maybe) { - loiter_nav->soften_for_landing(); + pos_control->soften_for_landing_xy(); } // use position controller to stop diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 9515334c3f..6c148e57d0 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -297,8 +297,7 @@ void ModeRTL::descent_start() // called by rtl_run at 100hz or more void ModeRTL::descent_run() { - float target_roll = 0.0f; - float target_pitch = 0.0f; + Vector2f vel_correction; float target_yaw_rate = 0.0f; // if not armed set throttle to zero and exit immediately @@ -321,11 +320,11 @@ void ModeRTL::descent_run() // apply SIMPLE mode transform to pilot inputs update_simple_mode(); - // convert pilot input to lean angles - get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max_cd()); + // convert pilot input to reposition velocity + vel_correction = get_pilot_desired_velocity(wp_nav->get_wp_acceleration() * 0.5); // record if pilot has overridden roll or pitch - if (!is_zero(target_roll) || !is_zero(target_pitch)) { + if (!vel_correction.is_zero()) { if (!copter.ap.land_repo_active) { AP::logger().Write_Event(LogEvent::LAND_REPO_ACTIVE); } @@ -342,11 +341,9 @@ void ModeRTL::descent_run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // process roll, pitch inputs - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); - - // run loiter controller - loiter_nav->update(); + Vector2f accel; + pos_control->input_vel_accel_xy(vel_correction, accel); + pos_control->update_xy_controller(); // WP_Nav has set the vertical position control targets // run the vertical position controller and set output throttle @@ -354,7 +351,7 @@ void ModeRTL::descent_run() pos_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); // check if we've reached within 20cm of final altitude _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;