Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Support Thrust Vector based navigation

This commit is contained in:
Leonard Hall 2021-04-13 15:09:53 +09:30 committed by Randy Mackay
parent 939d8e3ed4
commit 726074be91
9 changed files with 41 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/mode.cpp
View File

@ -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());
}
}

10
ArduCopter/mode_auto.cpp
View File

@ -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

2
ArduCopter/mode_brake.cpp
View File

@ -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

8
ArduCopter/mode_circle.cpp
View File

@ -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

18
ArduCopter/mode_guided.cpp
View File

@ -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());
}
}

6
ArduCopter/mode_loiter.cpp
View File

@ -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);

10
ArduCopter/mode_rtl.cpp
View File

@ -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;

8
ArduCopter/mode_smart_rtl.cpp
View File

@ -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

7
ArduCopter/takeoff.cpp
View File

@ -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)