fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 13:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

rover: auto mode stopping conditions

Browse Source
This commit is contained in:
chfriedrich98
2025-03-25 10:38:30 +01:00
committed by chfriedrich98
parent 735777862d
commit c2706f5406
8 changed files with 192 additions and 251 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/rover_control/RoverControl.cpp
+2 -4
View File
@@ -215,8 +215,7 @@ float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate
}
void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned,
position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos,
MapProjection &global_ned_proj_ref)
position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, MapProjection &global_ned_proj_ref)
{
if (position_setpoint_triplet.current.valid && PX4_ISFINITE(position_setpoint_triplet.current.lat)
&& PX4_ISFINITE(position_setpoint_triplet.current.lon)) {
@@ -241,8 +240,7 @@ void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned,
next_wp_ned = global_ned_proj_ref.project(position_setpoint_triplet.next.lat, position_setpoint_triplet.next.lon);
} else {
next_wp_ned = home_pos.isAllFinite() ? global_ned_proj_ref.project(home_pos(0), home_pos(1)) : Vector2f(NAN,
NAN); // Enables corner slow down with RTL
next_wp_ned = Vector2f(NAN, NAN);
}
}
src/lib/rover_control/RoverControl.hpp
+1 -2
View File
@@ -118,11 +118,10 @@ float rateControl(SlewRate<float> &adjusted_yaw_rate_setpoint, PID &pid_yaw_rate
* @param next_wp_ned Next waypoint in NED frame (Updated by this function)
* @param position_setpoint_triplet Position Setpoint Triplet
* @param curr_pos Current position of the rover in global frame
* @param home_pos Home position in global frame
* @param global_ned_proj_ref Global to ned projection
*/
void globalToLocalSetpointTriplet(Vector2f &curr_wp_ned, Vector2f &prev_wp_ned, Vector2f &next_wp_ned,
position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned, Vector2d &home_pos,
position_setpoint_triplet_s position_setpoint_triplet, Vector2f &curr_pos_ned,
MapProjection &global_ned_proj_ref);
/**