|
|
@ -1,5 +1,7 @@ |
|
|
|
#include "Copter.h" |
|
|
|
#include "Copter.h" |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#if MODE_GUIDED_ENABLED == ENABLED |
|
|
|
|
|
|
|
|
|
|
|
/*
|
|
|
|
/*
|
|
|
|
* Init and run calls for guided flight mode |
|
|
|
* Init and run calls for guided flight mode |
|
|
|
*/ |
|
|
|
*/ |
|
|
@ -666,7 +668,7 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const |
|
|
|
// limit the velocity to prevent fence violations
|
|
|
|
// limit the velocity to prevent fence violations
|
|
|
|
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy(), curr_vel_des, G_Dt); |
|
|
|
copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy(), curr_vel_des, G_Dt); |
|
|
|
// get avoidance adjusted climb rate
|
|
|
|
// get avoidance adjusted climb rate
|
|
|
|
curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);
|
|
|
|
curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z); |
|
|
|
#endif |
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
// update position controller with new target
|
|
|
|
// update position controller with new target
|
|
|
@ -785,3 +787,5 @@ float Copter::ModeGuided::crosstrack_error() const |
|
|
|
return 0; |
|
|
|
return 0; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#endif |
|
|
|