Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: INAV rename for neu & cm/cms

Browse Source
This commit is contained in:
Josh Henderson 2021-11-22 21:10:53 -05:00 committed by Andrew Tridgell
parent bd9361b701
commit e10edabd5d
4 changed files with 16 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AC_WPNav/AC_Circle.cpp
View File

@ -114,7 +114,7 @@ void AC_Circle::set_center(const Location& center)
set_center(Vector3f(center_xy.x, center_xy.y, terr_alt_cm), true); set_center(Vector3f(center_xy.x, center_xy.y, terr_alt_cm), true);
} else { } else {
// failed to convert location so set to current position and log error // failed to convert location so set to current position and log error
set_center(_inav.get_position(), false); set_center(_inav.get_position_neu_cm(), false);
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT);
} }
} else { } else {
@ -122,7 +122,7 @@ void AC_Circle::set_center(const Location& center)
Vector3f circle_center_neu; Vector3f circle_center_neu;
if (!center.get_vector_from_origin_NEU(circle_center_neu)) { if (!center.get_vector_from_origin_NEU(circle_center_neu)) {
// default to current position and log error // default to current position and log error
circle_center_neu = _inav.get_position(); circle_center_neu = _inav.get_position_neu_cm();
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT);
} }
set_center(circle_center_neu, false); set_center(circle_center_neu, false);
@ -200,7 +200,7 @@ bool AC_Circle::update(float climb_rate_cms)
target.y += - _radius * sinf(-_angle); target.y += - _radius * sinf(-_angle);
// heading is from vehicle to center of circle // heading is from vehicle to center of circle
_yaw = get_bearing_cd(_inav.get_position_xy(), _center.tofloat().xy()); _yaw = get_bearing_cd(_inav.get_position_xy_cm(), _center.tofloat().xy());
if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) { if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) {
_yaw += is_positive(_rate)?-9000.0f:9000.0f; _yaw += is_positive(_rate)?-9000.0f:9000.0f;
@ -313,7 +313,7 @@ void AC_Circle::init_start_angle(bool use_heading)
_angle = wrap_PI(_ahrs.yaw-M_PI); _angle = wrap_PI(_ahrs.yaw-M_PI);
} else { } else {
// if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading) // if we are exactly at the center of the circle, init angle to directly behind vehicle (so vehicle will backup but not change heading)
const Vector3f &curr_pos = _inav.get_position(); const Vector3f &curr_pos = _inav.get_position_neu_cm();
if (is_equal(curr_pos.x,float(_center.x)) && is_equal(curr_pos.y,float(_center.y))) { if (is_equal(curr_pos.x,float(_center.x)) && is_equal(curr_pos.y,float(_center.y))) {
_angle = wrap_PI(_ahrs.yaw-M_PI); _angle = wrap_PI(_ahrs.yaw-M_PI);
} else { } else {
@ -352,7 +352,7 @@ bool AC_Circle::get_terrain_offset(float& offset_cm)
return false; return false;
case AC_Circle::TerrainSource::TERRAIN_FROM_RANGEFINDER: case AC_Circle::TerrainSource::TERRAIN_FROM_RANGEFINDER:
if (_rangefinder_healthy) { if (_rangefinder_healthy) {
offset_cm = _inav.get_altitude() - _rangefinder_alt_cm; offset_cm = _inav.get_position_z_up_cm() - _rangefinder_alt_cm;
return true; return true;
} }
return false; return false;
@ -361,7 +361,7 @@ bool AC_Circle::get_terrain_offset(float& offset_cm)
float terr_alt = 0.0f; float terr_alt = 0.0f;
AP_Terrain *terrain = AP_Terrain::get_singleton(); AP_Terrain *terrain = AP_Terrain::get_singleton();
if (terrain != nullptr && terrain->height_above_terrain(terr_alt, true)) { if (terrain != nullptr && terrain->height_above_terrain(terr_alt, true)) {
offset_cm = _inav.get_altitude() - (terr_alt * 100.0f); offset_cm = _inav.get_position_z_up_cm() - (terr_alt * 100.0);
return true; return true;
} }
#endif #endif

2
libraries/AC_WPNav/AC_Loiter.cpp
View File

@ -131,7 +131,7 @@ void AC_Loiter::init_target()
/// reduce response for landing /// reduce response for landing
void AC_Loiter::soften_for_landing() void AC_Loiter::soften_for_landing()
{ {
const Vector3f& curr_pos = _inav.get_position(); const Vector3f& curr_pos = _inav.get_position_neu_cm();
// set target position to current position // set target position to current position
_pos_control.set_pos_target_xy_cm(curr_pos.x, curr_pos.y); _pos_control.set_pos_target_xy_cm(curr_pos.x, curr_pos.y);

14
libraries/AC_WPNav/AC_WPNav.cpp
View File

@ -399,7 +399,7 @@ void AC_WPNav::shift_wp_origin_and_destination_to_current_pos_xy()
_pos_control.init_xy_controller(); _pos_control.init_xy_controller();
// get current and target locations // get current and target locations
const Vector2f& curr_pos = _inav.get_position_xy(); const Vector2f& curr_pos = _inav.get_position_xy_cm();
// shift origin and destination horizontally // shift origin and destination horizontally
_origin.xy() = curr_pos; _origin.xy() = curr_pos;
@ -458,7 +458,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
_pos_control.update_pos_offset_z(terr_offset); _pos_control.update_pos_offset_z(terr_offset);
// get current position and adjust altitude to origin and destination's frame (i.e. _frame) // get current position and adjust altitude to origin and destination's frame (i.e. _frame)
const Vector3f &curr_pos = _inav.get_position() - Vector3f{0, 0, terr_offset}; const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{0, 0, terr_offset};
// Use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of aircraft // Use _track_scalar_dt to slow down S-Curve time to prevent target moving too far in front of aircraft
Vector3f curr_target_vel = _pos_control.get_vel_desired_cms(); Vector3f curr_target_vel = _pos_control.get_vel_desired_cms();
@ -469,7 +469,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt)
if (is_positive(curr_target_vel.length())) { if (is_positive(curr_target_vel.length())) {
Vector3f track_direction = curr_target_vel.normalized(); Vector3f track_direction = curr_target_vel.normalized();
const float track_error = _pos_control.get_pos_error_cm().dot(track_direction); const float track_error = _pos_control.get_pos_error_cm().dot(track_direction);
const float track_velocity = _inav.get_velocity().dot(track_direction); const float track_velocity = _inav.get_velocity_neu_cms().dot(track_direction);
// set time scaler to be consistent with the achievable aircraft speed with a 5% buffer for short term variation. // set time scaler to be consistent with the achievable aircraft speed with a 5% buffer for short term variation.
track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f); track_scaler_dt = constrain_float(0.05f + (track_velocity - _pos_control.get_pos_xy_p().kP() * track_error) / curr_target_vel.length(), 0.1f, 1.0f);
} }
@ -559,13 +559,13 @@ void AC_WPNav::update_track_with_speed_accel_limits()
/// get_wp_distance_to_destination - get horizontal distance to destination in cm /// get_wp_distance_to_destination - get horizontal distance to destination in cm
float AC_WPNav::get_wp_distance_to_destination() const float AC_WPNav::get_wp_distance_to_destination() const
{ {
return get_horizontal_distance_cm(_inav.get_position_xy(), _destination.xy()); return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination.xy());
} }
/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
int32_t AC_WPNav::get_wp_bearing_to_destination() const int32_t AC_WPNav::get_wp_bearing_to_destination() const
{ {
return get_bearing_cd(_inav.get_position_xy(), _destination.xy()); return get_bearing_cd(_inav.get_position_xy_cm(), _destination.xy());
} }
/// update_wpnav - run the wp controller - should be called at 100hz or higher /// update_wpnav - run the wp controller - should be called at 100hz or higher
@ -614,7 +614,7 @@ bool AC_WPNav::get_terrain_offset(float& offset_cm)
return false; return false;
case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER: case AC_WPNav::TerrainSource::TERRAIN_FROM_RANGEFINDER:
if (_rangefinder_healthy) { if (_rangefinder_healthy) {
offset_cm = _inav.get_altitude() - _rangefinder_alt_cm; offset_cm = _inav.get_position_z_up_cm() - _rangefinder_alt_cm;
return true; return true;
} }
return false; return false;
@ -624,7 +624,7 @@ bool AC_WPNav::get_terrain_offset(float& offset_cm)
AP_Terrain *terrain = AP::terrain(); AP_Terrain *terrain = AP::terrain();
if (terrain != nullptr && if (terrain != nullptr &&
terrain->height_above_terrain(terr_alt, true)) { terrain->height_above_terrain(terr_alt, true)) {
offset_cm = _inav.get_altitude() - (terr_alt * 100.0f); offset_cm = _inav.get_position_z_up_cm() - (terr_alt * 100.0);
return true; return true;
} }
#endif #endif

4
libraries/AC_WPNav/AC_WPNav_OA.cpp
View File

@ -44,7 +44,7 @@ float AC_WPNav_OA::get_wp_distance_to_destination() const
return AC_WPNav::get_wp_distance_to_destination(); return AC_WPNav::get_wp_distance_to_destination();
} }
return get_horizontal_distance_cm(_inav.get_position_xy(), _destination_oabak.xy()); return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination_oabak.xy());
} }
/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees
@ -55,7 +55,7 @@ int32_t AC_WPNav_OA::get_wp_bearing_to_destination() const
return AC_WPNav::get_wp_bearing_to_destination(); return AC_WPNav::get_wp_bearing_to_destination();
} }
return get_bearing_cd(_inav.get_position_xy(), _destination_oabak.xy()); return get_bearing_cd(_inav.get_position_xy_cm(), _destination_oabak.xy());
} }
/// true when we have come within RADIUS cm of the waypoint /// true when we have come within RADIUS cm of the waypoint