From 726074be9123675db06f675e56190519f822edf2 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 13 Apr 2021 15:09:53 +0930 Subject: [PATCH] Copter: Support Thrust Vector based navigation --- ArduCopter/mode.cpp | 18 +++++++++--------- ArduCopter/mode_auto.cpp | 10 +++++----- ArduCopter/mode_brake.cpp | 2 +- ArduCopter/mode_circle.cpp | 8 ++------ ArduCopter/mode_guided.cpp | 18 +++++++++--------- ArduCopter/mode_loiter.cpp | 6 +++--- ArduCopter/mode_rtl.cpp | 10 +++++----- ArduCopter/mode_smart_rtl.cpp | 8 ++++---- ArduCopter/takeoff.cpp | 7 +++---- 9 files changed, 41 insertions(+), 46 deletions(-) diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index a9601a59b0..4e54c35f67 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -603,8 +603,7 @@ void Mode::land_run_horizontal_control() // run loiter controller loiter_nav->update(); - float nav_roll = loiter_nav->get_roll(); - float nav_pitch = loiter_nav->get_pitch(); + Vector3f thrust_vector = loiter_nav->get_thrust_vector(); if (g2.wp_navalt_min > 0) { // user has requested an altitude below which navigation @@ -615,11 +614,12 @@ void Mode::land_run_horizontal_control() // interpolate for 1m above that float attitude_limit_cd = linear_interpolate(700, copter.aparm.angle_max, get_alt_above_ground_cm(), g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); - float total_angle_cd = norm(nav_roll, nav_pitch); - if (total_angle_cd > attitude_limit_cd) { - float ratio = attitude_limit_cd / total_angle_cd; - nav_roll *= ratio; - nav_pitch *= ratio; + float thrust_vector_max = sinf(radians(attitude_limit_cd / 100.0f)) * GRAVITY_MSS * 100.0f; + float thrust_vector_mag = norm(thrust_vector.x, thrust_vector.y); + if (thrust_vector_mag > thrust_vector_max) { + float ratio = thrust_vector_max / thrust_vector_mag; + thrust_vector.x *= ratio; + thrust_vector.y *= ratio; // tell position controller we are applying an external limit pos_control->set_limit_accel_xy(); @@ -629,10 +629,10 @@ void Mode::land_run_horizontal_control() // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(thrust_vector, target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(nav_roll, nav_pitch, auto_yaw.yaw(), true); + attitude_control->input_thrust_vector_heading(thrust_vector, auto_yaw.yaw()); } } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index dc37ad011e..6d57016892 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -832,10 +832,10 @@ void ModeAuto::wp_run() // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), auto_yaw.rate_cds()); + attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw(), auto_yaw.rate_cds()); } } @@ -889,10 +889,10 @@ void ModeAuto::circle_run() if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(copter.circle_nav->get_thrust_vector(), target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), auto_yaw.yaw(), true); + attitude_control->input_thrust_vector_heading(copter.circle_nav->get_thrust_vector(), auto_yaw.yaw()); } } @@ -930,7 +930,7 @@ void ModeAuto::loiter_run() copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); pos_control->update_z_controller(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate); } // auto_loiter_run - loiter to altitude in AUTO flight mode diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 02f6dd6d4c..39364d66de 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -51,7 +51,7 @@ void ModeBrake::run() pos_control->update_xy_controller(); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f); + attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), 0.0f); // update altitude target and call position controller // protects heli's from inflight motor interlock disable diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 62a420be83..fc8dc93208 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -108,13 +108,9 @@ void ModeCircle::run() // call attitude controller if (pilot_yaw_override) { - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(copter.circle_nav->get_roll(), - copter.circle_nav->get_pitch(), - target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(copter.circle_nav->get_thrust_vector(), target_yaw_rate); } else { - attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), - copter.circle_nav->get_pitch(), - copter.circle_nav->get_yaw(), true); + attitude_control->input_thrust_vector_heading(copter.circle_nav->get_thrust_vector(), copter.circle_nav->get_yaw()); } // update altitude target and call position controller diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 74fbd03abd..a7cd3b997a 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -455,13 +455,13 @@ void ModeGuided::pos_control_run() // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate); } else if (auto_yaw.mode() == AUTO_YAW_RATE) { // roll & pitch from waypoint controller, yaw rate from mavlink command or mission item - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.rate_cds()); + attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), auto_yaw.rate_cds()); } else { // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true); + attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw()); } } @@ -518,13 +518,13 @@ void ModeGuided::vel_control_run() // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); } else if (auto_yaw.mode() == AUTO_YAW_RATE) { // roll & pitch from velocity controller, yaw rate from mavlink command or mission item - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds()); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw()); } } @@ -583,13 +583,13 @@ void ModeGuided::posvel_control_run() // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), target_yaw_rate); } else if (auto_yaw.mode() == AUTO_YAW_RATE) { // roll & pitch from position-velocity controller, yaw rate from mavlink command or mission item - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds()); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), auto_yaw.rate_cds()); } else { // roll, pitch from waypoint controller, yaw heading from GCS or auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(), true); + attitude_control->input_thrust_vector_heading(pos_control->get_thrust_vector(), auto_yaw.yaw()); } } diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index dfc0556562..1972698445 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -121,7 +121,7 @@ void ModeLoiter::run() attitude_control->set_yaw_target_to_current_heading(); pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero loiter_nav->init_target(); - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); pos_control->update_z_controller(); break; @@ -141,7 +141,7 @@ void ModeLoiter::run() loiter_nav->update(); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); // update altitude target and call position controller pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); @@ -175,7 +175,7 @@ void ModeLoiter::run() loiter_nav->update(); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); // adjust climb rate using rangefinder target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate); diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index bb96501296..8d664ad1bf 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -177,10 +177,10 @@ void ModeRTL::climb_return_run() // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true); + attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw()); } // check if we've completed this stage of RTL @@ -234,10 +234,10 @@ void ModeRTL::loiterathome_run() // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate); }else{ // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true); + attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw()); } // check if we've completed this stage of RTL @@ -340,7 +340,7 @@ void ModeRTL::descent_run() pos_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(loiter_nav->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; diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index 83e789b9dc..f0bc06e0f1 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -70,7 +70,7 @@ void ModeSmartRTL::wait_cleanup_run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); wp_nav->update_wpnav(); pos_control->update_z_controller(); - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true); + attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw()); // check if return path is computed and if yes, begin journey home if (g2.smart_rtl.request_thorough_cleanup()) { @@ -143,10 +143,10 @@ void ModeSmartRTL::path_follow_run() // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), target_yaw_rate); } else { // roll, pitch from waypoint controller, yaw heading from auto_heading() - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true); + attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw()); } } @@ -169,7 +169,7 @@ void ModeSmartRTL::pre_land_position_run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); wp_nav->update_wpnav(); pos_control->update_z_controller(); - attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(), true); + attitude_control->input_thrust_vector_heading(wp_nav->get_thrust_vector(), auto_yaw.yaw()); } // save current position for use by the smart_rtl flight mode diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 7175c14591..54fb27e00e 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -175,7 +175,6 @@ void Mode::auto_takeoff_run() } // check if we are not navigating because of low altitude - float nav_roll = 0.0f, nav_pitch = 0.0f; if (auto_takeoff_no_nav_active) { // check if vehicle has reached no_nav_alt threshold if (inertial_nav.get_altitude() >= auto_takeoff_no_nav_alt_cm) { @@ -192,16 +191,16 @@ void Mode::auto_takeoff_run() // run waypoint controller copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); + Vector3f thrustvector{0, 0, -GRAVITY_MSS * 100.0f}; if (!auto_takeoff_no_nav_active) { - nav_roll = wp_nav->get_roll(); - nav_pitch = wp_nav->get_pitch(); + thrustvector = wp_nav->get_thrust_vector(); } // call z-axis position controller (wpnav should have already updated it's alt target) copter.pos_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate); + attitude_control->input_thrust_vector_rate_heading(thrustvector, target_yaw_rate); } void Mode::auto_takeoff_set_start_alt(void)