7 #ifndef GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM 8 # define GUIDED_LOOK_AT_TARGET_MIN_DISTANCE_CM 500 // point nose at target if it is more than 5m away 11 #define GUIDED_POSVEL_TIMEOUT_MS 3000 // guided mode's position-velocity controller times out after 3seconds with no new updates 12 #define GUIDED_ATTITUDE_TIMEOUT_MS 1000 // guided mode's attitude controller times out after 1 second with no new updates 41 if (
copter.position_ok() || ignore_checks) {
43 auto_yaw.set_mode_to_default(
false);
62 if (!wp_nav->set_wp_destination(target_loc)) {
73 set_throttle_takeoff();
76 copter.auto_takeoff_set_start_alt();
88 wp_nav->wp_and_spline_init();
92 wp_nav->get_wp_stopping_point(stopping_point);
95 wp_nav->set_wp_destination(stopping_point,
false);
98 auto_yaw.set_mode_to_default(
false);
108 pos_control->set_speed_xy(wp_nav->get_speed_xy());
109 pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
112 pos_control->set_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
113 pos_control->set_accel_z(g.pilot_accel_z);
116 pos_control->init_vel_controller_xyz();
125 pos_control->init_xy_controller();
128 pos_control->set_speed_xy(wp_nav->get_speed_xy());
129 pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
131 const Vector3f& curr_pos = inertial_nav.get_position();
132 const Vector3f& curr_vel = inertial_nav.get_velocity();
135 pos_control->set_xy_target(curr_pos.
x, curr_pos.
y);
136 pos_control->set_desired_velocity_xy(curr_vel.
x, curr_vel.
y);
139 pos_control->set_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
140 pos_control->set_accel_z(wp_nav->get_accel_z());
153 pos_control->set_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
154 pos_control->set_accel_z(wp_nav->get_accel_z());
157 if (!pos_control->is_active_z()) {
158 pos_control->set_alt_target_to_current_alt();
159 pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
185 #if AC_FENCE == ENABLED 188 if (!
copter.fence.check_destination_within_fence(dest_loc)) {
196 set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
199 wp_nav->set_wp_destination(destination,
false);
202 copter.Log_Write_GuidedTarget(guided_mode, destination,
Vector3f());
211 return wp_nav->get_wp_destination(destination);
224 #if AC_FENCE == ENABLED 227 if (!
copter.fence.check_destination_within_fence(dest_loc)) {
234 if (!wp_nav->set_wp_destination(dest_loc)) {
242 set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
258 set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
261 guided_vel_target_cms = velocity;
275 posvel_control_start();
278 #if AC_FENCE == ENABLED 281 if (!
copter.fence.check_destination_within_fence(dest_loc)) {
289 set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
292 guided_pos_target_cm = destination;
293 guided_vel_target_cms = velocity;
295 copter.pos_control->set_pos_target(guided_pos_target_cm);
298 copter.Log_Write_GuidedTarget(guided_mode, destination, velocity);
307 angle_control_start();
323 copter.set_auto_armed(
true);
327 copter.Log_Write_GuidedTarget(guided_mode,
337 switch (guided_mode) {
356 posvel_control_run();
373 wp_nav->shift_wp_origin_to_current_pos();
374 zero_throttle_and_relax_ac();
376 set_throttle_takeoff();
381 float target_yaw_rate = 0;
382 if (!
copter.failsafe.radio) {
384 target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
387 #if FRAME_CONFIG == HELI_FRAME 389 if (
motors->rotor_runup_complete()) {
390 set_land_complete(
false);
393 wp_nav->shift_wp_origin_to_current_pos();
396 set_land_complete(
false);
403 copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
406 copter.pos_control->update_z_controller();
409 copter.auto_takeoff_attitude_run(target_yaw_rate);
418 zero_throttle_and_relax_ac();
423 float target_yaw_rate = 0;
424 if (!
copter.failsafe.radio) {
426 target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
427 if (!
is_zero(target_yaw_rate)) {
436 copter.failsafe_terrain_set_status(wp_nav->update_wpnav());
439 pos_control->update_z_controller();
444 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate);
447 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.rate_cds());
450 attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),
true);
461 pos_control->init_vel_controller_xyz();
462 zero_throttle_and_relax_ac();
467 float target_yaw_rate = 0;
468 if (!
copter.failsafe.radio) {
470 target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
471 if (!
is_zero(target_yaw_rate)) {
482 if (!pos_control->get_desired_velocity().is_zero()) {
483 set_desired_velocity_with_accel_and_fence_limits(
Vector3f(0.0
f, 0.0
f, 0.0
f));
486 auto_yaw.set_rate(0.0
f);
489 set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
493 pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
498 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate);
501 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds());
504 attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(),
true);
515 pos_control->set_pos_target(inertial_nav.get_position());
516 pos_control->set_desired_velocity(
Vector3f(0,0,0));
517 zero_throttle_and_relax_ac();
522 float target_yaw_rate = 0;
524 if (!
copter.failsafe.radio) {
526 target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
527 if (!
is_zero(target_yaw_rate)) {
538 guided_vel_target_cms.
zero();
540 auto_yaw.set_rate(0.0
f);
545 float dt = pos_control->time_since_last_xy_update();
553 guided_pos_target_cm += guided_vel_target_cms * dt;
556 pos_control->set_pos_target(guided_pos_target_cm);
557 pos_control->set_desired_velocity_xy(guided_vel_target_cms.
x, guided_vel_target_cms.
y);
560 pos_control->update_xy_controller(ekfNavVelGainScaler);
561 pos_control->update_z_controller();
566 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate);
569 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.rate_cds());
572 attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), auto_yaw.yaw(),
true);
582 #if FRAME_CONFIG == HELI_FRAME 583 attitude_control->set_yaw_target_to_current_heading();
585 zero_throttle_and_relax_ac();
586 pos_control->relax_alt_hold_controllers(0.0
f);
593 float total_in =
norm(roll_in, pitch_in);
594 float angle_max =
MIN(attitude_control->get_althold_lean_angle_max(),
copter.aparm.angle_max);
595 if (total_in > angle_max) {
596 float ratio = angle_max / total_in;
609 climb_rate_cms = get_avoidance_adjusted_climbrate(climb_rate_cms);
616 climb_rate_cms = 0.0f;
625 attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in);
627 attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in,
true);
631 pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt,
false);
632 pos_control->update_z_controller();
639 Vector3f curr_vel_des = pos_control->get_desired_velocity();
642 Vector3f vel_delta = vel_des - curr_vel_des;
646 float vel_delta_xy_max = G_Dt * pos_control->get_accel_xy();
647 float ratio_xy = 1.0f;
648 if (!
is_zero(vel_delta_xy) && (vel_delta_xy > vel_delta_xy_max)) {
649 ratio_xy = vel_delta_xy_max / vel_delta_xy;
651 curr_vel_des.x += (vel_delta.
x * ratio_xy);
652 curr_vel_des.y += (vel_delta.
y * ratio_xy);
655 float vel_delta_z_max = G_Dt * pos_control->get_accel_z();
656 curr_vel_des.z +=
constrain_float(vel_delta.
z, -vel_delta_z_max, vel_delta_z_max);
660 copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
662 curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);
666 pos_control->set_desired_velocity(curr_vel_des);
673 auto_yaw.set_fixed_yaw(yaw_cd / 100.0
f, 0.0
f, 0, relative_angle);
674 }
else if (use_yaw_rate) {
675 auto_yaw.set_rate(yaw_rate_cds);
720 const Vector3f& curr_pos = inertial_nav.get_position();
749 return wp_nav->get_wp_distance_to_destination();
752 return pos_control->get_distance_to_target();
763 return wp_nav->get_wp_bearing_to_destination();
766 return pos_control->get_bearing_to_target();
float norm(const T first, const U second, const Params... parameters)
void to_euler(float &roll, float &pitch, float &yaw) const
struct Guided_Limit guided_limit
bool set_destination_posvel(const Vector3f &destination, const Vector3f &velocity, bool use_yaw=false, float yaw_cd=0.0, bool use_yaw_rate=false, float yaw_rate_cds=0.0, bool yaw_relative=false)
#define GUIDED_ATTITUDE_TIMEOUT_MS
float safe_sqrt(const T v)
void set_desired_velocity_with_accel_and_fence_limits(const Vector3f &vel_des)
auto wrap_180_cd(const T angle) -> decltype(wrap_180(angle, 100.f))
#define ERROR_CODE_DEST_OUTSIDE_FENCE
void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle)
static uint32_t vel_update_time_ms
AP_MotorsMatrix motors(400)
void set_velocity(const Vector3f &velocity, bool use_yaw=false, float yaw_cd=0.0, bool use_yaw_rate=false, float yaw_rate_cds=0.0, bool yaw_relative=false, bool log_request=true)
static Vector3f guided_pos_target_cm
bool get_interlock() const
bool init(bool ignore_checks) override
bool get_wp(Location_Class &loc) override
#define ERROR_CODE_FAILED_TO_SET_DESTINATION
float get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
void posvel_control_run()
void limit_init_time_and_pos()
bool is_zero(const T fVal1)
void set_angle(const Quaternion &q, float climb_rate_cms, bool use_yaw_rate, float yaw_rate_rads)
static Vector3f guided_vel_target_cms
void limit_set(uint32_t timeout_ms, float alt_min_cm, float alt_max_cm, float horiz_max_cm)
struct @14 guided_angle_state
DESIRED_THROTTLE_UNLIMITED
void posvel_control_start()
float constrain_float(const float amt, const float low, const float high)
void set_alt_cm(int32_t alt_cm, ALT_FRAME frame)
bool set_destination(const Vector3f &destination, bool use_yaw=false, float yaw_cd=0.0, bool use_yaw_rate=false, float yaw_rate_cds=0.0, bool yaw_relative=false)
#define GUIDED_POSVEL_TIMEOUT_MS
uint32_t wp_distance() const override
AP_HAL_MAIN_CALLBACKS & copter
bool takeoff_start(float final_alt_above_home)
int32_t wp_bearing() const override
virtual void set_desired_spool_state(enum spool_up_down_desired spool)
void angle_control_start()
static uint32_t posvel_update_time_ms
#define ERROR_SUBSYSTEM_NAVIGATION