fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

navigator: extract breaking functionality out

Browse Source 
This only moves the breaking functionality out from the reposition
command handling to a function, so that it can get re-used in other
places.
This commit is contained in:
Julian Oes 2021-12-13 14:07:54 +01:00 committed by Daniel Agar
parent 0e0e0d8be7
commit f88dd28e85
2 changed files with 22 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/navigator/navigator.h
View File

@ -320,6 +320,8 @@ public:
void acquire_gimbal_control();
void release_gimbal_control();
void calculate_breaking_stop(double &lat, double &lon, float &yaw);
private:
struct traffic_buffer_s {

36
src/modules/navigator/navigator_main.cpp
View File

@ -311,22 +311,7 @@ void Navigator::run()
if (_vstatus.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& (get_position_setpoint_triplet()->current.type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF)) {
// For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back
double lat, lon;
float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx);
// predict braking distance
const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs,
_param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto);
waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, course_over_ground,
multirotor_braking_distance, &lat, &lon);
rep->current.lat = lat;
rep->current.lon = lon;
rep->current.yaw = get_local_position()->heading;
calculate_breaking_stop(rep->current.lat, rep->current.lon, rep->current.yaw);
rep->current.yaw_valid = true;
} else {
@ -1578,6 +1563,25 @@ bool Navigator::geofence_allows_position(const vehicle_global_position_s &pos)
return true;
}
void Navigator::calculate_breaking_stop(double &lat, double &lon, float &yaw)
{
// For multirotors we need to account for the braking distance, otherwise the vehicle will overshoot and go back
float course_over_ground = atan2f(_local_pos.vy, _local_pos.vx);
// predict braking distance
const float velocity_hor_abs = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
float multirotor_braking_distance = math::trajectory::computeBrakingDistanceFromVelocity(velocity_hor_abs,
_param_mpc_jerk_auto, _param_mpc_acc_hor, 0.6f * _param_mpc_jerk_auto);
waypoint_from_heading_and_distance(get_global_position()->lat, get_global_position()->lon, course_over_ground,
multirotor_braking_distance, &lat, &lon);
lat = lat;
lon = lon;
yaw = get_local_position()->heading;
}
int Navigator::print_usage(const char *reason)
{
if (reason) {