diff --git a/libraries/AC_WPNav/AC_Circle.cpp b/libraries/AC_WPNav/AC_Circle.cpp index b0bfc707bae..8e6563eb554 100644 --- a/libraries/AC_WPNav/AC_Circle.cpp +++ b/libraries/AC_WPNav/AC_Circle.cpp @@ -15,7 +15,7 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = { // @Range: 0 200000 // @Increment: 100 // @User: Standard - AP_GROUPINFO("RADIUS", 0, AC_Circle, _radius_parm, AC_CIRCLE_RADIUS_DEFAULT), + AP_GROUPINFO("RADIUS", 0, AC_Circle, _radius_parm_cm, AC_CIRCLE_RADIUS_DEFAULT), // @Param: RATE // @DisplayName: Circle rate @@ -24,7 +24,7 @@ const AP_Param::GroupInfo AC_Circle::var_info[] = { // @Range: -90 90 // @Increment: 1 // @User: Standard - AP_GROUPINFO("RATE", 1, AC_Circle, _rate_parm, AC_CIRCLE_RATE_DEFAULT), + AP_GROUPINFO("RATE", 1, AC_Circle, _rate_parm_degs, AC_CIRCLE_RATE_DEFAULT), // @Param: OPTIONS // @DisplayName: Circle options @@ -49,17 +49,17 @@ AC_Circle::AC_Circle(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_Po // init flags _flags.panorama = false; - _rate = _rate_parm; + _rate_degs = _rate_parm_degs; } /// init - initialise circle controller setting center specifically -/// set terrain_alt to true if center.z should be interpreted as an alt-above-terrain. Rate should be +ve in deg/sec for cw turn +/// set terrain_alt to true if center_neu_cm.z should be interpreted as an alt-above-terrain. Rate should be +ve in deg/sec for cw turn /// caller should set the position controller's x,y and z speeds and accelerations before calling this -void AC_Circle::init(const Vector3p& center, bool terrain_alt, float rate_deg_per_sec) +void AC_Circle::init_NEU_cm(const Vector3p& center_neu_cm, bool terrain_alt, float rate_degs) { - _center = center; + _center_neu_cm = center_neu_cm; _terrain_alt = terrain_alt; - _rate = rate_deg_per_sec; + _rate_degs = rate_degs; // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles) _pos_control.init_NE_controller_stopping_point(); @@ -77,9 +77,9 @@ void AC_Circle::init(const Vector3p& center, bool terrain_alt, float rate_deg_pe void AC_Circle::init() { // initialize radius and rate from params - _radius = _radius_parm; - _last_radius_param = _radius_parm; - _rate = _rate_parm; + _radius_cm = _radius_parm_cm; + _last_radius_param = _radius_parm_cm; + _rate_degs = _rate_parm_degs; // initialise position controller (sets target roll angle, pitch angle and I terms based on vehicle current lean angles) _pos_control.init_NE_controller_stopping_point(); @@ -89,10 +89,10 @@ void AC_Circle::init() const Vector3p& stopping_point = _pos_control.get_pos_desired_NEU_cm(); // set circle center to circle_radius ahead of stopping point - _center = stopping_point; + _center_neu_cm = stopping_point; if ((_options.get() & CircleOptions::INIT_AT_CENTER) == 0) { - _center.x += _radius * _ahrs.cos_yaw(); - _center.y += _radius * _ahrs.sin_yaw(); + _center_neu_cm.x += _radius_cm * _ahrs.cos_yaw(); + _center_neu_cm.y += _radius_cm * _ahrs.sin_yaw(); } _terrain_alt = false; @@ -108,40 +108,40 @@ void AC_Circle::set_center(const Location& center) { if (center.get_alt_frame() == Location::AltFrame::ABOVE_TERRAIN) { // convert Location with terrain altitude - Vector2f center_xy; + Vector2f center_ne_cm; int32_t terr_alt_cm; - if (center.get_vector_xy_from_origin_NE_cm(center_xy) && - center.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, terr_alt_cm)) { - set_center(Vector3f(center_xy.x, center_xy.y, terr_alt_cm), true); + if (center.get_vector_xy_from_origin_NE_cm(center_ne_cm) && + center.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, terr_alt_cm)) { + set_center_NEU_cm(Vector3f(center_ne_cm.x, center_ne_cm.y, terr_alt_cm), true); } else { // failed to convert location so set to current position and log error - set_center(_inav.get_position_neu_cm(), false); + set_center_NEU_cm(_inav.get_position_neu_cm(), false); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); } } else { // convert Location with alt-above-home, alt-above-origin or absolute alt - Vector3f circle_center_neu; - if (!center.get_vector_from_origin_NEU_cm(circle_center_neu)) { + Vector3f circle_center_neu_cm; + if (!center.get_vector_from_origin_NEU_cm(circle_center_neu_cm)) { // default to current position and log error - circle_center_neu = _inav.get_position_neu_cm(); + circle_center_neu_cm = _inav.get_position_neu_cm(); LOGGER_WRITE_ERROR(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_CIRCLE_INIT); } - set_center(circle_center_neu, false); + set_center_NEU_cm(circle_center_neu_cm, false); } } /// set_circle_rate - set circle rate in degrees per second -void AC_Circle::set_rate(float deg_per_sec) +void AC_Circle::set_rate_degs(float rate_degs) { - if (!is_equal(deg_per_sec, _rate)) { - _rate = deg_per_sec; + if (!is_equal(rate_degs, _rate_degs)) { + _rate_degs = rate_degs; } } /// set_circle_rate - set circle rate in degrees per second void AC_Circle::set_radius_cm(float radius_cm) { - _radius = constrain_float(radius_cm, 0, AC_CIRCLE_RADIUS_MAX); + _radius_cm = constrain_float(radius_cm, 0, AC_CIRCLE_RADIUS_MAX); } /// returns true if update has been run recently @@ -152,7 +152,7 @@ bool AC_Circle::is_active() const } /// update - update circle controller -bool AC_Circle::update(float climb_rate_cms) +bool AC_Circle::update_cms(float climb_rate_cms) { calc_velocities(false); @@ -160,57 +160,57 @@ bool AC_Circle::update(float climb_rate_cms) const float dt = _pos_control.get_dt(); // ramp angular velocity to maximum - if (_angular_vel < _angular_vel_max) { - _angular_vel += fabsf(_angular_accel) * dt; - _angular_vel = MIN(_angular_vel, _angular_vel_max); + if (_angular_vel_rads < _angular_vel_max_rads) { + _angular_vel_rads += fabsf(_angular_accel_radss) * dt; + _angular_vel_rads = MIN(_angular_vel_rads, _angular_vel_max_rads); } - if (_angular_vel > _angular_vel_max) { - _angular_vel -= fabsf(_angular_accel) * dt; - _angular_vel = MAX(_angular_vel, _angular_vel_max); + if (_angular_vel_rads > _angular_vel_max_rads) { + _angular_vel_rads -= fabsf(_angular_accel_radss) * dt; + _angular_vel_rads = MAX(_angular_vel_rads, _angular_vel_max_rads); } // update the target angle and total angle travelled - float angle_change = _angular_vel * dt; - _angle += angle_change; - _angle = wrap_PI(_angle); - _angle_total += angle_change; + float angle_change = _angular_vel_rads * dt; + _angle_rad += angle_change; + _angle_rad = wrap_PI(_angle_rad); + _angle_total_rad += angle_change; // calculate terrain adjustments float terr_offset = 0.0f; - if (_terrain_alt && !get_terrain_offset(terr_offset)) { + if (_terrain_alt && !get_terrain_offset_cm(terr_offset)) { return false; } // calculate z-axis target float target_z_cm; if (_terrain_alt) { - target_z_cm = _center.z + terr_offset; + target_z_cm = _center_neu_cm.z + terr_offset; } else { target_z_cm = _pos_control.get_pos_desired_U_cm(); } // if the circle_radius is zero we are doing panorama so no need to update loiter target Vector3p target { - _center.x, - _center.y, + _center_neu_cm.x, + _center_neu_cm.y, target_z_cm }; - if (!is_zero(_radius)) { + if (!is_zero(_radius_cm)) { // calculate target position - target.x += _radius * cosf(-_angle); - target.y += - _radius * sinf(-_angle); + target.x += _radius_cm * cosf(-_angle_rad); + target.y += - _radius_cm * sinf(-_angle_rad); // heading is from vehicle to center of circle - _yaw = get_bearing_cd(_pos_control.get_pos_desired_NEU_cm().xy().tofloat(), _center.tofloat().xy()); + _yaw_cd = get_bearing_cd(_pos_control.get_pos_desired_NEU_cm().xy().tofloat(), _center_neu_cm.tofloat().xy()); if ((_options.get() & CircleOptions::FACE_DIRECTION_OF_TRAVEL) != 0) { - _yaw += is_positive(_rate)?-9000.0f:9000.0f; - _yaw = wrap_360_cd(_yaw); + _yaw_cd += is_positive(_rate_degs)?-9000.0f:9000.0f; + _yaw_cd = wrap_360_cd(_yaw_cd); } } else { - // heading is same as _angle but converted to centi-degrees - _yaw = _angle * DEGX100; + // heading is same as _angle_rad but converted to centi-degrees + _yaw_cd = _angle_rad * DEGX100; } // update position controller target @@ -233,12 +233,12 @@ bool AC_Circle::update(float climb_rate_cms) return true; } -// get_closest_point_on_circle - returns closest point on the circle +// get_closest_point_on_circle_NEU_cm - returns closest point on the circle // circle's center should already have been set -// closest point on the circle will be placed in result, dist_cm will be updated with the 3D distance to the center -// result's altitude (i.e. z) will be set to the circle_center's altitude +// closest point on the circle will be placed in result_NEU_cm, dist_cm will be updated with the 3D distance to the center +// result_NEU_cm's altitude (i.e. z) will be set to the circle_center's altitude // if vehicle is at the center of the circle, the edge directly behind vehicle will be returned -void AC_Circle::get_closest_point_on_circle(Vector3f& result, float& dist_cm) const +void AC_Circle::get_closest_point_on_circle_NEU_cm(Vector3f& result_NEU_cm, float& dist_cm) const { // get current position Vector3p stopping_point; @@ -246,27 +246,27 @@ void AC_Circle::get_closest_point_on_circle(Vector3f& result, float& dist_cm) co _pos_control.get_stopping_point_U_cm(stopping_point.z); // calc vector from stopping point to circle center - Vector3f vec = (stopping_point - _center).tofloat(); + Vector3f vec = (stopping_point - _center_neu_cm).tofloat(); dist_cm = vec.length(); // return center if radius is zero - if (!is_positive(_radius)) { - result = _center.tofloat(); + if (!is_positive(_radius_cm)) { + result_NEU_cm = _center_neu_cm.tofloat(); return; } // if current location is exactly at the center of the circle return edge directly behind vehicle if (is_zero(dist_cm)) { - result.x = _center.x - _radius * _ahrs.cos_yaw(); - result.y = _center.y - _radius * _ahrs.sin_yaw(); - result.z = _center.z; + result_NEU_cm.x = _center_neu_cm.x - _radius_cm * _ahrs.cos_yaw(); + result_NEU_cm.y = _center_neu_cm.y - _radius_cm * _ahrs.sin_yaw(); + result_NEU_cm.z = _center_neu_cm.z; return; } // calculate closest point on edge of circle - result.x = _center.x + vec.x / dist_cm * _radius; - result.y = _center.y + vec.y / dist_cm * _radius; - result.z = _center.z; + result_NEU_cm.x = _center_neu_cm.x + vec.x / dist_cm * _radius_cm; + result_NEU_cm.y = _center_neu_cm.y + vec.y / dist_cm * _radius_cm; + result_NEU_cm.z = _center_neu_cm.z; } // calc_velocities - calculate angular velocity max and acceleration based on radius and rate @@ -275,24 +275,24 @@ void AC_Circle::get_closest_point_on_circle(Vector3f& result, float& dist_cm) co void AC_Circle::calc_velocities(bool init_velocity) { // if we are doing a panorama set the circle_angle to the current heading - if (_radius <= 0) { - _angular_vel_max = ToRad(_rate); - _angular_accel = MAX(fabsf(_angular_vel_max),ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN)); // reach maximum yaw velocity in 1 second + if (_radius_cm <= 0) { + _angular_vel_max_rads = ToRad(_rate_degs); + _angular_accel_radss = MAX(fabsf(_angular_vel_max_rads),ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN)); // reach maximum yaw velocity in 1 second }else{ // calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle - float velocity_max = MIN(_pos_control.get_max_speed_NE_cms(), safe_sqrt(0.5f*_pos_control.get_max_accel_NE_cmss()*_radius)); + float vel_max_cms = MIN(_pos_control.get_max_speed_NE_cms(), safe_sqrt(0.5f*_pos_control.get_max_accel_NE_cmss()*_radius_cm)); // angular_velocity in radians per second - _angular_vel_max = velocity_max/_radius; - _angular_vel_max = constrain_float(ToRad(_rate),-_angular_vel_max,_angular_vel_max); + _angular_vel_max_rads = vel_max_cms/_radius_cm; + _angular_vel_max_rads = constrain_float(ToRad(_rate_degs),-_angular_vel_max_rads,_angular_vel_max_rads); // angular_velocity in radians per second - _angular_accel = MAX(_pos_control.get_max_accel_NE_cmss()/_radius, ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN)); + _angular_accel_radss = MAX(_pos_control.get_max_accel_NE_cmss()/_radius_cm, ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN)); } // initialise angular velocity if (init_velocity) { - _angular_vel = 0; + _angular_vel_rads = 0; } } @@ -302,27 +302,27 @@ void AC_Circle::calc_velocities(bool init_velocity) void AC_Circle::init_start_angle(bool use_heading) { // initialise angle total - _angle_total = 0; + _angle_total_rad = 0; // if the radius is zero we are doing panorama so init angle to the current heading - if (_radius <= 0) { - _angle = _ahrs.yaw; + if (_radius_cm <= 0) { + _angle_rad = _ahrs.yaw; return; } // if use_heading is true if (use_heading) { - _angle = wrap_PI(_ahrs.yaw-M_PI); + _angle_rad = wrap_PI(_ahrs.yaw-M_PI); } 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) // curr_pos_desired is the position before we add offsets and terrain const Vector3f &curr_pos_desired= _pos_control.get_pos_desired_NEU_cm().tofloat(); - if (is_equal(curr_pos_desired.x,float(_center.x)) && is_equal(curr_pos_desired.y,float(_center.y))) { - _angle = wrap_PI(_ahrs.yaw-M_PI); + if (is_equal(curr_pos_desired.x,float(_center_neu_cm.x)) && is_equal(curr_pos_desired.y,float(_center_neu_cm.y))) { + _angle_rad = wrap_PI(_ahrs.yaw-M_PI); } else { // get bearing from circle center to vehicle in radians - float bearing_rad = atan2f(curr_pos_desired.y-_center.y, curr_pos_desired.x-_center.x); - _angle = wrap_PI(bearing_rad); + float bearing_rad = atan2f(curr_pos_desired.y-_center_neu_cm.y, curr_pos_desired.x-_center_neu_cm.x); + _angle_rad = wrap_PI(bearing_rad); } } } @@ -347,7 +347,7 @@ AC_Circle::TerrainSource AC_Circle::get_terrain_source() const } // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) -bool AC_Circle::get_terrain_offset(float& offset_cm) +bool AC_Circle::get_terrain_offset_cm(float& offset_cm) { // calculate offset based on source (rangefinder or terrain database) switch (get_terrain_source()) { @@ -377,8 +377,8 @@ bool AC_Circle::get_terrain_offset(float& offset_cm) void AC_Circle::check_param_change() { - if (!is_equal(_last_radius_param,_radius_parm.get())) { - _radius = _radius_parm; - _last_radius_param = _radius_parm; + if (!is_equal(_last_radius_param,_radius_parm_cm.get())) { + _radius_cm = _radius_parm_cm; + _last_radius_param = _radius_parm_cm; } } diff --git a/libraries/AC_WPNav/AC_Circle.h b/libraries/AC_WPNav/AC_Circle.h index 6cafbc4a96f..8ac7857fb80 100644 --- a/libraries/AC_WPNav/AC_Circle.h +++ b/libraries/AC_WPNav/AC_Circle.h @@ -20,9 +20,9 @@ public: AC_Circle(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control); /// init - initialise circle controller setting center specifically - /// set terrain_alt to true if center.z should be interpreted as an alt-above-terrain. Rate should be +ve in deg/sec for cw turn + /// set terrain_alt to true if center_neu_cm.z should be interpreted as an alt-above-terrain. Rate should be +ve in deg/sec for cw turn /// caller should set the position controller's x,y and z speeds and accelerations before calling this - void init(const Vector3p& center, bool terrain_alt, float rate_deg_per_sec); + void init_NEU_cm(const Vector3p& center_neu_cm, bool terrain_alt, float rate_degs); /// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading /// caller should set the position controller's x,y and z speeds and accelerations before calling this @@ -33,58 +33,58 @@ public: /// set_circle_center as a vector from ekf origin /// terrain_alt should be true if center.z is alt is above terrain - void set_center(const Vector3f& center, bool terrain_alt) { _center = center.topostype(); _terrain_alt = terrain_alt; } + void set_center_NEU_cm(const Vector3f& center_neu_cm, bool terrain_alt) { _center_neu_cm = center_neu_cm.topostype(); _terrain_alt = terrain_alt; } /// get_circle_center in cm from home - const Vector3p& get_center() const { return _center; } + const Vector3p& get_center_NEU_cm() const { return _center_neu_cm; } /// returns true if using terrain altitudes bool center_is_terrain_alt() const { return _terrain_alt; } /// get_radius - returns radius of circle in cm - float get_radius() const { return is_positive(_radius)?_radius:_radius_parm; } + float get_radius_cm() const { return is_positive(_radius_cm)?_radius_cm:_radius_parm_cm; } /// set_radius_cm - sets circle radius in cm void set_radius_cm(float radius_cm); - /// get_rate - returns target rate in deg/sec held in RATE parameter - float get_rate() const { return _rate; } + /// get_rate_degs - returns target rate in deg/sec held in RATE parameter + float get_rate_degs() const { return _rate_degs; } - /// get_rate_current - returns actual calculated rate target in deg/sec, which may be less than _rate - float get_rate_current() const { return ToDeg(_angular_vel); } + /// get_rate_current - returns actual calculated rate target in deg/sec, which may be less than _rate_degs + float get_rate_current() const { return ToDeg(_angular_vel_rads); } /// set_rate - set circle rate in degrees per second - void set_rate(float deg_per_sec); + void set_rate_degs(float rate_degs); - /// get_angle_total - return total angle in radians that vehicle has circled - float get_angle_total() const { return _angle_total; } + /// get_angle_total_rad - return total angle in radians that vehicle has circled + float get_angle_total_rad() const { return _angle_total_rad; } /// update - update circle controller /// returns false on failure which indicates a terrain failsafe - bool update(float climb_rate_cms = 0.0f) WARN_IF_UNUSED; + bool update_cms(float climb_rate_cms = 0.0f) WARN_IF_UNUSED; /// get desired roll, pitch which should be fed into stabilize controllers - float get_roll() const { return _pos_control.get_roll_cd(); } - float get_pitch() const { return _pos_control.get_pitch_cd(); } + float get_roll_cd() const { return _pos_control.get_roll_cd(); } + float get_pitch_cd() const { return _pos_control.get_pitch_cd(); } Vector3f get_thrust_vector() const { return _pos_control.get_thrust_vector(); } - float get_yaw() const { return _yaw; } + float get_yaw_cd() const { return _yaw_cd; } /// returns true if update has been run recently /// used by vehicle code to determine if get_yaw() is valid bool is_active() const; - // get_closest_point_on_circle - returns closest point on the circle + // get_closest_point_on_circle_NEU_cm - returns closest point on the circle // circle's center should already have been set // closest point on the circle will be placed in result, dist_cm will be updated with the distance to the center // result's altitude (i.e. z) will be set to the circle_center's altitude // if vehicle is at the center of the circle, the edge directly behind vehicle will be returned - void get_closest_point_on_circle(Vector3f& result, float& dist_cm) const; + void get_closest_point_on_circle_NEU_cm(Vector3f& result_NEU_cm, float& dist_cm) const; /// get horizontal distance to loiter target in cm - float get_distance_to_target() const { return _pos_control.get_pos_error_NE_cm(); } + float get_distance_to_target_cm() const { return _pos_control.get_pos_error_NE_cm(); } /// get bearing to target in centi-degrees - int32_t get_bearing_to_target() const { return _pos_control.get_bearing_to_target_cd(); } + int32_t get_bearing_to_target_cd() const { return _pos_control.get_bearing_to_target_cd(); } /// true if pilot control of radius and turn rate is enabled bool pilot_control_enabled() const { return (_options.get() & CircleOptions::MANUAL_CONTROL) != 0; } @@ -94,7 +94,7 @@ public: /// provide rangefinder based terrain offset /// terrain offset is the terrain's height above the EKF origin - void set_rangefinder_terrain_offset(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;} + void set_rangefinder_terrain_offset_cm(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;} /// check for a change in the radius params void check_param_change(); @@ -123,7 +123,7 @@ private: AC_Circle::TerrainSource get_terrain_source() const; // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) - bool get_terrain_offset(float& offset_cm); + bool get_terrain_offset_cm(float& offset_cm); // flags structure struct circle_flags { @@ -143,25 +143,25 @@ private: }; // parameters - AP_Float _radius_parm; // radius of circle in cm loaded from params - AP_Float _rate_parm; // rotation speed in deg/sec + AP_Float _radius_parm_cm; // radius of circle in cm loaded from params + AP_Float _rate_parm_degs; // rotation speed in deg/sec AP_Int16 _options; // stick control enable/disable // internal variables - Vector3p _center; // center of circle in cm from home - float _radius; // radius of circle in cm - float _rate; // rotation speed of circle in deg/sec. +ve for cw turn - float _yaw; // yaw heading (normally towards circle center) - float _angle; // current angular position around circle in radians (0=directly north of the center of the circle) - float _angle_total; // total angle travelled in radians - float _angular_vel; // angular velocity in radians/sec - float _angular_vel_max; // maximum velocity in radians/sec - float _angular_accel; // angular acceleration in radians/sec/sec + Vector3p _center_neu_cm; // center of circle in cm from home + float _radius_cm; // radius of circle in cm + float _rate_degs; // rotation speed of circle in deg/sec. +ve for cw turn + float _yaw_cd; // yaw heading (normally towards circle center) + float _angle_rad; // current angular position around circle in radians (0=directly north of the center of the circle) + float _angle_total_rad; // total angle traveled in radians + float _angular_vel_rads; // angular velocity in radians/sec + float _angular_vel_max_rads; // maximum velocity in radians/sec + float _angular_accel_radss; // angular acceleration in radians/sec/sec uint32_t _last_update_ms; // system time of last update float _last_radius_param; // last value of radius param, used to update radius on param change // terrain following variables - bool _terrain_alt; // true if _center.z is alt-above-terrain, false if alt-above-ekf-origin + bool _terrain_alt; // true if _center_neu_cm.z is alt-above-terrain, false if alt-above-ekf-origin bool _rangefinder_available; // true if range finder could be used bool _rangefinder_healthy; // true if range finder is healthy float _rangefinder_terrain_offset_cm; // latest rangefinder based terrain offset (e.g. terrain's height above EKF origin) diff --git a/libraries/AC_WPNav/AC_Loiter.cpp b/libraries/AC_WPNav/AC_Loiter.cpp index 995c879d204..60e14ee5c02 100644 --- a/libraries/AC_WPNav/AC_Loiter.cpp +++ b/libraries/AC_WPNav/AC_Loiter.cpp @@ -25,7 +25,7 @@ const AP_Param::GroupInfo AC_Loiter::var_info[] = { // @Range: 0 45 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("ANG_MAX", 1, AC_Loiter, _angle_max, 0.0f), + AP_GROUPINFO("ANG_MAX", 1, AC_Loiter, _angle_max_deg, 0.0f), // @Param: SPEED // @DisplayName: Loiter Horizontal Maximum Speed @@ -34,7 +34,7 @@ const AP_Param::GroupInfo AC_Loiter::var_info[] = { // @Range: 20 3500 // @Increment: 50 // @User: Standard - AP_GROUPINFO("SPEED", 2, AC_Loiter, _speed_cms, LOITER_SPEED_DEFAULT), + AP_GROUPINFO("SPEED", 2, AC_Loiter, _speed_max_ne_cms, LOITER_SPEED_DEFAULT), // @Param: ACC_MAX // @DisplayName: Loiter maximum correction acceleration @@ -43,7 +43,7 @@ const AP_Param::GroupInfo AC_Loiter::var_info[] = { // @Range: 100 981 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("ACC_MAX", 3, AC_Loiter, _accel_cmss, LOITER_ACCEL_MAX_DEFAULT), + AP_GROUPINFO("ACC_MAX", 3, AC_Loiter, _accel_max_ne_cmss, LOITER_ACCEL_MAX_DEFAULT), // @Param: BRK_ACCEL // @DisplayName: Loiter braking acceleration @@ -52,7 +52,7 @@ const AP_Param::GroupInfo AC_Loiter::var_info[] = { // @Range: 25 250 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("BRK_ACCEL", 4, AC_Loiter, _brake_accel_cmss, LOITER_BRAKE_ACCEL_DEFAULT), + AP_GROUPINFO("BRK_ACCEL", 4, AC_Loiter, _brake_accel_max_cmss, LOITER_BRAKE_ACCEL_DEFAULT), // @Param: BRK_JERK // @DisplayName: Loiter braking jerk @@ -70,7 +70,7 @@ const AP_Param::GroupInfo AC_Loiter::var_info[] = { // @Range: 0 2 // @Increment: 0.1 // @User: Advanced - AP_GROUPINFO("BRK_DELAY", 6, AC_Loiter, _brake_delay, LOITER_BRAKE_START_DELAY_DEFAULT), + AP_GROUPINFO("BRK_DELAY", 6, AC_Loiter, _brake_delay_s, LOITER_BRAKE_START_DELAY_DEFAULT), AP_GROUPEND }; @@ -88,26 +88,26 @@ AC_Loiter::AC_Loiter(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_Po AP_Param::setup_object_defaults(this, var_info); } -/// init_target to a position in cm from ekf origin -void AC_Loiter::init_target(const Vector2f& position) +/// initialise loiter target to a position in cm from ekf origin +void AC_Loiter::init_target_cm(const Vector2f& position_ne_cm) { sanity_check_params(); // initialise position controller speed and acceleration - _pos_control.set_correction_speed_accel_NE_cm(LOITER_VEL_CORRECTION_MAX, _accel_cmss); + _pos_control.set_correction_speed_accel_NE_cm(LOITER_VEL_CORRECTION_MAX, _accel_max_ne_cmss); _pos_control.set_pos_error_max_NE_cm(LOITER_POS_CORRECTION_MAX); // initialise position controller _pos_control.init_NE_controller_stopping_point(); // initialise desired acceleration and angles to zero to remain on station - _predicted_accel.zero(); - _desired_accel.zero(); - _predicted_euler_angle.zero(); - _brake_accel = 0.0f; + _predicted_accel_ne_cmss.zero(); + _desired_accel_ne_cmss.zero(); + _predicted_euler_angle_rad.zero(); + _brake_accel_cmss = 0.0f; // set target position - _pos_control.set_pos_desired_NE_cm(position); + _pos_control.set_pos_desired_NE_cm(position_ne_cm); } /// initialize's position and feed-forward velocity from current pos and velocity @@ -116,17 +116,17 @@ void AC_Loiter::init_target() sanity_check_params(); // initialise position controller speed and acceleration - _pos_control.set_correction_speed_accel_NE_cm(LOITER_VEL_CORRECTION_MAX, _accel_cmss); + _pos_control.set_correction_speed_accel_NE_cm(LOITER_VEL_CORRECTION_MAX, _accel_max_ne_cmss); _pos_control.set_pos_error_max_NE_cm(LOITER_POS_CORRECTION_MAX); // initialise position controller and move target accelerations smoothly towards zero _pos_control.relax_velocity_controller_NE(); // initialise predicted acceleration and angles from the position controller - _predicted_accel = _pos_control.get_accel_target_NEU_cmss().xy(); - _predicted_euler_angle.x = radians(_pos_control.get_roll_cd()*0.01f); - _predicted_euler_angle.y = radians(_pos_control.get_pitch_cd()*0.01f); - _brake_accel = 0.0f; + _predicted_accel_ne_cmss = _pos_control.get_accel_target_NEU_cmss().xy(); + _predicted_euler_angle_rad.x = radians(_pos_control.get_roll_cd() * 0.01f); + _predicted_euler_angle_rad.y = radians(_pos_control.get_pitch_cd() * 0.01f); + _brake_accel_cmss = 0.0f; } /// reduce response for landing @@ -137,52 +137,52 @@ void AC_Loiter::soften_for_landing() /// set pilot desired acceleration in centi-degrees // dt should be the time (in seconds) since the last call to this function -void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd) +void AC_Loiter::set_pilot_desired_acceleration_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd) { const float dt = _attitude_control.get_dt(); // Convert from centidegrees on public interface to radians - const float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f); - const float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f); + const float euler_roll_angle_rad = radians(euler_roll_angle_cd * 0.01f); + const float euler_pitch_angle_rad = radians(euler_pitch_angle_cd * 0.01f); // convert our desired attitude to an acceleration vector assuming we are not accelerating vertically - const Vector3f desired_euler {euler_roll_angle, euler_pitch_angle, _ahrs.yaw}; - const Vector3f desired_accel = _pos_control.lean_angles_to_accel(desired_euler); + const Vector3f desired_euler_rad {euler_roll_angle_rad, euler_pitch_angle_rad, _ahrs.yaw}; + const Vector3f desired_accel_NEU_cmss = _pos_control.lean_angles_to_accel_NEU_cmss(desired_euler_rad); - _desired_accel.x = desired_accel.x; - _desired_accel.y = desired_accel.y; + _desired_accel_ne_cmss.x = desired_accel_NEU_cmss.x; + _desired_accel_ne_cmss.y = desired_accel_NEU_cmss.y; // difference between where we think we should be and where we want to be - Vector2f angle_error(wrap_PI(euler_roll_angle - _predicted_euler_angle.x), wrap_PI(euler_pitch_angle - _predicted_euler_angle.y)); + Vector2f angle_error(wrap_PI(euler_roll_angle_rad - _predicted_euler_angle_rad.x), wrap_PI(euler_pitch_angle_rad - _predicted_euler_angle_rad.y)); // calculate the angular velocity that we would expect given our desired and predicted attitude _attitude_control.input_shaping_rate_predictor(angle_error, _predicted_euler_rate, dt); // update our predicted attitude based on our predicted angular velocity - _predicted_euler_angle += _predicted_euler_rate * dt; + _predicted_euler_angle_rad += _predicted_euler_rate * dt; // convert our predicted attitude to an acceleration vector assuming we are not accelerating vertically - const Vector3f predicted_euler {_predicted_euler_angle.x, _predicted_euler_angle.y, _ahrs.yaw}; - const Vector3f predicted_accel = _pos_control.lean_angles_to_accel(predicted_euler); + const Vector3f predicted_euler_rad {_predicted_euler_angle_rad.x, _predicted_euler_angle_rad.y, _ahrs.yaw}; + const Vector3f predicted_accel = _pos_control.lean_angles_to_accel_NEU_cmss(predicted_euler_rad); - _predicted_accel.x = predicted_accel.x; - _predicted_accel.y = predicted_accel.y; + _predicted_accel_ne_cmss.x = predicted_accel.x; + _predicted_accel_ne_cmss.y = predicted_accel.y; } /// get vector to stopping point based on a horizontal position and velocity -void AC_Loiter::get_stopping_point_xy(Vector2f& stopping_point) const +void AC_Loiter::get_stopping_point_NE_cm(Vector2f& stopping_point_ne_cm) const { - Vector2p stop; - _pos_control.get_stopping_point_NE_cm(stop); - stopping_point = stop.tofloat(); + Vector2p stop_ne_cm; + _pos_control.get_stopping_point_NE_cm(stop_ne_cm); + stopping_point_ne_cm = stop_ne_cm.tofloat(); } /// get maximum lean angle when using loiter float AC_Loiter::get_angle_max_cd() const { - if (!is_positive(_angle_max)) { - return MIN(_attitude_control.lean_angle_max_cd(), _pos_control.get_lean_angle_max_cd()) * (2.0f/3.0f); + if (!is_positive(_angle_max_deg)) { + return MIN(_attitude_control.lean_angle_max_cd(), _pos_control.get_lean_angle_max_cd()) * (2.0f / 3.0f); } - return MIN(_angle_max*100.0f, _pos_control.get_lean_angle_max_cd()); + return MIN(_angle_max_deg*100.0f, _pos_control.get_lean_angle_max_cd()); } /// run the loiter controller @@ -193,16 +193,16 @@ void AC_Loiter::update(bool avoidance_on) } //set maximum horizontal speed -void AC_Loiter::set_max_xy_speed(float max_xy_speed) +void AC_Loiter::set_speed_max_NE_cms(float speed_max_ne_cms) { - _speed_cms.set(MAX(max_xy_speed, LOITER_SPEED_MIN)); + _speed_max_ne_cms.set(MAX(speed_max_ne_cms, LOITER_SPEED_MIN)); } // sanity check parameters void AC_Loiter::sanity_check_params() { - _speed_cms.set(MAX(_speed_cms, LOITER_SPEED_MIN)); - _accel_cmss.set(MIN(_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max_cd() * 0.01f)))); + _speed_max_ne_cms.set(MAX(_speed_max_ne_cms, LOITER_SPEED_MIN)); + _accel_max_ne_cmss.set(MIN(_accel_max_ne_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max_cd() * 0.01f)))); } /// calc_desired_velocity - updates desired velocity (i.e. feed forward) with pilot requested acceleration and fake wind resistance @@ -216,7 +216,7 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) // calculate a loiter speed limit which is the minimum of the value set by the LOITER_SPEED // parameter and the value set by the EKF to observe optical flow limits - float gnd_speed_limit_cms = MIN(_speed_cms, ekfGndSpdLimit * 100.0f); + float gnd_speed_limit_cms = MIN(_speed_max_ne_cms, ekfGndSpdLimit * 100.0f); gnd_speed_limit_cms = MAX(gnd_speed_limit_cms, LOITER_SPEED_MIN); float pilot_acceleration_max = angle_to_accel(get_angle_max_cd() * 0.01) * 100; @@ -230,7 +230,7 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) Vector2f desired_vel = _pos_control.get_vel_desired_NEU_cms().xy(); // update the desired velocity using our predicted acceleration - desired_vel += _predicted_accel * dt; + desired_vel += _predicted_accel_ne_cmss * dt; Vector2f loiter_accel_brake; float desired_speed = desired_vel.length(); @@ -242,25 +242,25 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) // calculate a braking acceleration if sticks are at zero float loiter_brake_accel = 0.0f; - if (_desired_accel.is_zero()) { - if ((AP_HAL::millis() - _brake_timer) > _brake_delay * 1000.0f) { + if (_desired_accel_ne_cmss.is_zero()) { + if ((AP_HAL::millis() - _brake_timer) > _brake_delay_s * 1000.0f) { float brake_gain = _pos_control.get_vel_NE_pid().kP() * 0.5f; - loiter_brake_accel = constrain_float(sqrt_controller(desired_speed, brake_gain, _brake_jerk_max_cmsss, dt), 0.0f, _brake_accel_cmss); + loiter_brake_accel = constrain_float(sqrt_controller(desired_speed, brake_gain, _brake_jerk_max_cmsss, dt), 0.0f, _brake_accel_max_cmss); } } else { loiter_brake_accel = 0.0f; _brake_timer = AP_HAL::millis(); } - _brake_accel += constrain_float(loiter_brake_accel - _brake_accel, -_brake_jerk_max_cmsss * dt, _brake_jerk_max_cmsss * dt); - loiter_accel_brake = desired_vel_norm * _brake_accel; + _brake_accel_cmss += constrain_float(loiter_brake_accel - _brake_accel_cmss, -_brake_jerk_max_cmsss * dt, _brake_jerk_max_cmsss * dt); + loiter_accel_brake = desired_vel_norm * _brake_accel_cmss; // update the desired velocity using the drag and braking accelerations - desired_speed = MAX(desired_speed - (drag_decel + _brake_accel) * dt, 0.0f); + desired_speed = MAX(desired_speed - (drag_decel + _brake_accel_cmss) * dt, 0.0f); desired_vel = desired_vel_norm * desired_speed; } // add braking to the desired acceleration - _desired_accel -= loiter_accel_brake; + _desired_accel_ne_cmss -= loiter_accel_brake; // Apply EKF limit to desired velocity - this limit is calculated by the EKF and adjusted as required to ensure certain sensor limits are respected (eg optical flow sensing) float horizSpdDem = desired_vel.length(); @@ -271,11 +271,11 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) #if AP_AVOIDANCE_ENABLED && !APM_BUILD_TYPE(APM_BUILD_ArduPlane) if (avoidance_on) { // Limit the velocity to prevent fence violations - // TODO: We need to also limit the _desired_accel + // TODO: We need to also limit the _desired_accel_ne_cmss AC_Avoid *_avoid = AP::ac_avoid(); if (_avoid != nullptr) { Vector3f avoidance_vel_3d{desired_vel.x, desired_vel.y, 0.0f}; - _avoid->adjust_velocity(avoidance_vel_3d, _pos_control.get_pos_NE_p().kP(), _accel_cmss, _pos_control.get_pos_U_p().kP(), _pos_control.get_max_accel_U_cmss(), dt); + _avoid->adjust_velocity(avoidance_vel_3d, _pos_control.get_pos_NE_p().kP(), _accel_max_ne_cmss, _pos_control.get_pos_U_p().kP(), _pos_control.get_max_accel_U_cmss(), dt); desired_vel = Vector2f{avoidance_vel_3d.x, avoidance_vel_3d.y}; } } @@ -288,5 +288,5 @@ void AC_Loiter::calc_desired_velocity(bool avoidance_on) desired_pos += (desired_vel * dt).topostype(); // send adjusted feed forward acceleration and velocity back to the Position Controller - _pos_control.set_pos_vel_accel_NE_cm(desired_pos, desired_vel, _desired_accel); + _pos_control.set_pos_vel_accel_NE_cm(desired_pos, desired_vel, _desired_accel_ne_cmss); } diff --git a/libraries/AC_WPNav/AC_Loiter.h b/libraries/AC_WPNav/AC_Loiter.h index e70136edda8..7f1de2edc39 100644 --- a/libraries/AC_WPNav/AC_Loiter.h +++ b/libraries/AC_WPNav/AC_Loiter.h @@ -16,8 +16,8 @@ public: /// Constructor AC_Loiter(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control); - /// init_target to a position in cm from ekf origin - void init_target(const Vector2f& position); + /// initialise loiter target to a position in cm from ekf origin + void init_target_cm(const Vector2f& position_neu_cm); /// initialize's position and feed-forward velocity from current pos and velocity void init_target(); @@ -27,24 +27,24 @@ public: /// set pilot desired acceleration in centi-degrees // dt should be the time (in seconds) since the last call to this function - void set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd); + void set_pilot_desired_acceleration_cd(float euler_roll_angle_cd, float euler_pitch_angle_cd); - /// gets pilot desired acceleration, body frame, [forward,right] - Vector2f get_pilot_desired_acceleration() const { return Vector2f{_desired_accel.x, _desired_accel.y}; } + /// gets pilot desired acceleration in the earth frame + Vector2f get_pilot_desired_acceleration_NE_cmss() const { return Vector2f{_desired_accel_ne_cmss.x, _desired_accel_ne_cmss.y}; } /// clear pilot desired acceleration void clear_pilot_desired_acceleration() { - set_pilot_desired_acceleration(0, 0); + set_pilot_desired_acceleration_cd(0, 0); } /// get vector to stopping point based on a horizontal position and velocity - void get_stopping_point_xy(Vector2f& stopping_point) const; + void get_stopping_point_NE_cm(Vector2f& stopping_point) const; /// get horizontal distance to loiter target in cm - float get_distance_to_target() const { return _pos_control.get_pos_error_NE_cm(); } + float get_distance_to_target_cm() const { return _pos_control.get_pos_error_NE_cm(); } /// get bearing to target in centi-degrees - int32_t get_bearing_to_target() const { return _pos_control.get_bearing_to_target_cd(); } + int32_t get_bearing_to_target_cd() const { return _pos_control.get_bearing_to_target_cd(); } /// get maximum lean angle when using loiter float get_angle_max_cd() const; @@ -53,11 +53,11 @@ public: void update(bool avoidance_on = true); //set maximum horizontal speed - void set_max_xy_speed(float max_xy_speed); + void set_speed_max_NE_cms(float speed_max_NE_cms); /// get desired roll, pitch which should be fed into stabilize controllers - float get_roll() const { return _pos_control.get_roll_cd(); } - float get_pitch() const { return _pos_control.get_pitch_cd(); } + float get_roll_cd() const { return _pos_control.get_roll_cd(); } + float get_pitch_cd() const { return _pos_control.get_pitch_cd(); } Vector3f get_thrust_vector() const { return _pos_control.get_thrust_vector(); } static const struct AP_Param::GroupInfo var_info[]; @@ -78,18 +78,18 @@ protected: const AC_AttitudeControl& _attitude_control; // parameters - AP_Float _angle_max; // maximum pilot commanded angle in degrees. Set to zero for 2/3 Angle Max - AP_Float _speed_cms; // maximum horizontal speed in cm/s while in loiter - AP_Float _accel_cmss; // loiter's max acceleration in cm/s/s - AP_Float _brake_accel_cmss; // loiter's acceleration during braking in cm/s/s - AP_Float _brake_jerk_max_cmsss; - AP_Float _brake_delay; // delay (in seconds) before loiter braking begins after sticks are released + AP_Float _angle_max_deg; // maximum pilot commanded angle in degrees. Set to zero for 2/3 Angle Max + AP_Float _speed_max_ne_cms; // maximum horizontal speed in cm/s while in loiter + AP_Float _accel_max_ne_cmss; // loiter's max acceleration in cm/s/s + AP_Float _brake_accel_max_cmss; // loiter's maximum acceleration during braking in cm/s/s + AP_Float _brake_jerk_max_cmsss; // loiter's maximum jerk during braking in cm/s/s + AP_Float _brake_delay_s; // delay (in seconds) before loiter braking begins after sticks are released // loiter controller internal variables - Vector2f _desired_accel; // slewed pilot's desired acceleration in lat/lon frame - Vector2f _predicted_accel; - Vector2f _predicted_euler_angle; - Vector2f _predicted_euler_rate; - uint32_t _brake_timer; // system time that brake was initiated - float _brake_accel; // acceleration due to braking from previous iteration (used for jerk limiting) + Vector2f _desired_accel_ne_cmss; // slewed pilot's desired acceleration in lat/lon frame + Vector2f _predicted_accel_ne_cmss; // predicted acceleration in lat/lon frame based on pilot's desired acceleration + Vector2f _predicted_euler_angle_rad; // predicted roll/pitch angles in radians based on pilot's desired acceleration + Vector2f _predicted_euler_rate; // predicted roll/pitch rates in radians/sec based on pilot's desired acceleration + uint32_t _brake_timer; // system time that brake was initiated + float _brake_accel_cmss; // acceleration due to braking from previous iteration (used for jerk limiting) }; diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 94c87396df5..9dbc6b9d7f0 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -4,13 +4,13 @@ extern const AP_HAL::HAL& hal; // maximum velocities and accelerations -#define WPNAV_WP_SPEED 1000.0f // default horizontal speed between waypoints in cm/s -#define WPNAV_WP_SPEED_MIN 10.0f // minimum horizontal speed between waypoints in cm/s -#define WPNAV_WP_RADIUS 200.0f // default waypoint radius in cm -#define WPNAV_WP_RADIUS_MIN 5.0f // minimum waypoint radius in cm -#define WPNAV_WP_SPEED_UP 250.0f // default maximum climb velocity -#define WPNAV_WP_SPEED_DOWN 150.0f // default maximum descent velocity -#define WPNAV_WP_ACCEL_Z_DEFAULT 100.0f // default vertical acceleration between waypoints in cm/s/s +#define WPNAV_WP_SPEED_CMS 1000.0f // default horizontal speed between waypoints in cm/s +#define WPNAV_WP_SPEED_MIN_CMS 10.0f // minimum horizontal speed between waypoints in cm/s +#define WPNAV_WP_RADIUS_CM 200.0f // default waypoint radius in cm +#define WPNAV_WP_RADIUS_MIN_CM 5.0f // minimum waypoint radius in cm +#define WPNAV_WP_SPEED_UP_CMS 250.0f // default maximum climb velocity +#define WPNAV_WP_SPEED_DOWN_CMS 150.0f // default maximum descent velocity +#define WPNAV_WP_ACCEL_Z_DEFAULT_CMSS 100.0f // default vertical acceleration between waypoints in cm/s/s const AP_Param::GroupInfo AC_WPNav::var_info[] = { // index 0 was used for the old orientation matrix @@ -22,7 +22,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @Range: 10 2000 // @Increment: 50 // @User: Standard - AP_GROUPINFO("SPEED", 0, AC_WPNav, _wp_speed_cms, WPNAV_WP_SPEED), + AP_GROUPINFO("SPEED", 0, AC_WPNav, _wp_speed_cms, WPNAV_WP_SPEED_CMS), // @Param: RADIUS // @DisplayName: Waypoint Radius @@ -31,7 +31,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @Range: 5 1000 // @Increment: 1 // @User: Standard - AP_GROUPINFO("RADIUS", 1, AC_WPNav, _wp_radius_cm, WPNAV_WP_RADIUS), + AP_GROUPINFO("RADIUS", 1, AC_WPNav, _wp_radius_cm, WPNAV_WP_RADIUS_CM), // @Param: SPEED_UP // @DisplayName: Waypoint Climb Speed Target @@ -40,7 +40,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @Range: 10 1000 // @Increment: 50 // @User: Standard - AP_GROUPINFO("SPEED_UP", 2, AC_WPNav, _wp_speed_up_cms, WPNAV_WP_SPEED_UP), + AP_GROUPINFO("SPEED_UP", 2, AC_WPNav, _wp_speed_up_cms, WPNAV_WP_SPEED_UP_CMS), // @Param: SPEED_DN // @DisplayName: Waypoint Descent Speed Target @@ -49,7 +49,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @Range: 10 500 // @Increment: 10 // @User: Standard - AP_GROUPINFO("SPEED_DN", 3, AC_WPNav, _wp_speed_down_cms, WPNAV_WP_SPEED_DOWN), + AP_GROUPINFO("SPEED_DN", 3, AC_WPNav, _wp_speed_down_cms, WPNAV_WP_SPEED_DOWN_CMS), // @Param: ACCEL // @DisplayName: Waypoint Acceleration @@ -67,7 +67,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @Range: 50 500 // @Increment: 10 // @User: Standard - AP_GROUPINFO("ACCEL_Z", 6, AC_WPNav, _wp_accel_z_cmss, WPNAV_WP_ACCEL_Z_DEFAULT), + AP_GROUPINFO("ACCEL_Z", 6, AC_WPNav, _wp_accel_z_cmss, WPNAV_WP_ACCEL_Z_DEFAULT_CMSS), // @Param: RFND_USE // @DisplayName: Waypoint missions use rangefinder for terrain following @@ -82,7 +82,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @Units: m/s/s/s // @Range: 1 20 // @User: Standard - AP_GROUPINFO("JERK", 11, AC_WPNav, _wp_jerk, 1.0f), + AP_GROUPINFO("JERK", 11, AC_WPNav, _wp_jerk_msss, 1.0f), // @Param: TER_MARGIN // @DisplayName: Waypoint Terrain following altitude margin @@ -90,7 +90,7 @@ const AP_Param::GroupInfo AC_WPNav::var_info[] = { // @Units: m // @Range: 0.1 100 // @User: Advanced - AP_GROUPINFO("TER_MARGIN", 12, AC_WPNav, _terrain_margin, 10.0), + AP_GROUPINFO("TER_MARGIN", 12, AC_WPNav, _terrain_margin_m, 10.0), // @Param: ACCEL_C // @DisplayName: Waypoint Cornering Acceleration @@ -123,7 +123,7 @@ AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosC // initialise old WPNAV_SPEED values _last_wp_speed_cms = _wp_speed_cms; _last_wp_speed_up_cms = _wp_speed_up_cms; - _last_wp_speed_down_cms = get_default_speed_down(); + _last_wp_speed_down_cms = get_default_speed_down_cms(); } // get expected source of terrain data if alt-above-terrain command is executed (used by Copter's ModeRTL) @@ -149,17 +149,17 @@ AC_WPNav::TerrainSource AC_WPNav::get_terrain_source() const /// waypoint navigation /// -/// wp_and_spline_init - initialise straight line and spline waypoint controllers +/// wp_and_spline_init_cm - initialise straight line and spline waypoint controllers /// speed_cms should be a positive value or left at zero to use the default speed -/// stopping_point should be the vehicle's stopping point (equal to the starting point of the next segment) if know or left as zero -/// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination -void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point) +/// stopping_point_ne_cm should be the vehicle's stopping point (equal to the starting point of the next segment) if know or left as zero +/// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination_neu_cm +void AC_WPNav::wp_and_spline_init_cm(float speed_cms, Vector3f stopping_point_ne_cm) { // check _wp_radius_cm is reasonable - _wp_radius_cm.set_and_save_ifchanged(MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN)); + _wp_radius_cm.set_and_save_ifchanged(MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN_CM)); // check _wp_speed - _wp_speed_cms.set_and_save_ifchanged(MAX(_wp_speed_cms, WPNAV_WP_SPEED_MIN)); + _wp_speed_cms.set_and_save_ifchanged(MAX(_wp_speed_cms, WPNAV_WP_SPEED_MIN_CMS)); // initialise position controller _pos_control.init_U_controller_stopping_point(); @@ -167,18 +167,18 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point) // initialize the desired wp speed _check_wp_speed_change = !is_positive(speed_cms); - _wp_desired_speed_xy_cms = is_positive(speed_cms) ? speed_cms : _wp_speed_cms; - _wp_desired_speed_xy_cms = MAX(_wp_desired_speed_xy_cms, WPNAV_WP_SPEED_MIN); + _wp_desired_speed_ne_cms = is_positive(speed_cms) ? speed_cms : _wp_speed_cms; + _wp_desired_speed_ne_cms = MAX(_wp_desired_speed_ne_cms, WPNAV_WP_SPEED_MIN_CMS); // initialise position controller speed and acceleration - _pos_control.set_max_speed_accel_NE_cm(_wp_desired_speed_xy_cms, get_wp_acceleration()); - _pos_control.set_correction_speed_accel_NE_cm(_wp_desired_speed_xy_cms, get_wp_acceleration()); - _pos_control.set_max_speed_accel_U_cm(-get_default_speed_down(), _wp_speed_up_cms, _wp_accel_z_cmss); - _pos_control.set_correction_speed_accel_U_cmss(-get_default_speed_down(), _wp_speed_up_cms, _wp_accel_z_cmss); + _pos_control.set_max_speed_accel_NE_cm(_wp_desired_speed_ne_cms, get_wp_acceleration_cmss()); + _pos_control.set_correction_speed_accel_NE_cm(_wp_desired_speed_ne_cms, get_wp_acceleration_cmss()); + _pos_control.set_max_speed_accel_U_cm(-get_default_speed_down_cms(), _wp_speed_up_cms, _wp_accel_z_cmss); + _pos_control.set_correction_speed_accel_U_cmss(-get_default_speed_down_cms(), _wp_speed_up_cms, _wp_accel_z_cmss); // calculate scurve jerk and jerk time - if (!is_positive(_wp_jerk)) { - _wp_jerk.set(get_wp_acceleration()); + if (!is_positive(_wp_jerk_msss)) { + _wp_jerk_msss.set(get_wp_acceleration_cmss()); } calc_scurve_jerk_and_snap(); @@ -190,37 +190,37 @@ void AC_WPNav::wp_and_spline_init(float speed_cms, Vector3f stopping_point) _flags.reached_destination = true; _flags.fast_waypoint = false; - // initialise origin and destination to stopping point - if (stopping_point.is_zero()) { - get_wp_stopping_point(stopping_point); + // initialise origin and destination_neu_cm to stopping point + if (stopping_point_ne_cm.is_zero()) { + get_wp_stopping_point_NEU_cm(stopping_point_ne_cm); } - _origin = _destination = stopping_point; + _origin_neu_cm = _destination_neu_cm = stopping_point_ne_cm; _terrain_alt = false; _this_leg_is_spline = false; // initialise the terrain velocity to the current maximum velocity - _offset_vel = _wp_desired_speed_xy_cms; - _offset_accel = 0.0; + _offset_vel_cms = _wp_desired_speed_ne_cms; + _offset_accel_cmss = 0.0; _paused = false; // mark as active - _wp_last_update = AP_HAL::millis(); + _wp_last_update_ms = AP_HAL::millis(); } -/// set_speed_xy - allows main code to pass target horizontal velocity for wp navigation -void AC_WPNav::set_speed_xy(float speed_cms) +/// set_speed_NE_cms - allows main code to pass target horizontal velocity for wp navigation +void AC_WPNav::set_speed_NE_cms(float speed_cms) { // range check target speed and protect against divide by zero - if (speed_cms >= WPNAV_WP_SPEED_MIN && is_positive(_wp_desired_speed_xy_cms)) { + if (speed_cms >= WPNAV_WP_SPEED_MIN_CMS && is_positive(_wp_desired_speed_ne_cms)) { // update horizontal velocity speed offset scalar - _offset_vel = speed_cms * _offset_vel / _wp_desired_speed_xy_cms; + _offset_vel_cms = speed_cms * _offset_vel_cms / _wp_desired_speed_ne_cms; // initialize the desired wp speed - _wp_desired_speed_xy_cms = speed_cms; + _wp_desired_speed_ne_cms = speed_cms; // update position controller speed and acceleration - _pos_control.set_max_speed_accel_NE_cm(_wp_desired_speed_xy_cms, get_wp_acceleration()); - _pos_control.set_correction_speed_accel_NE_cm(_wp_desired_speed_xy_cms, get_wp_acceleration()); + _pos_control.set_max_speed_accel_NE_cm(_wp_desired_speed_ne_cms, get_wp_acceleration_cmss()); + _pos_control.set_correction_speed_accel_NE_cm(_wp_desired_speed_ne_cms, get_wp_acceleration_cmss()); // change track speed update_track_with_speed_accel_limits(); @@ -228,85 +228,85 @@ void AC_WPNav::set_speed_xy(float speed_cms) } /// set current target climb rate during wp navigation -void AC_WPNav::set_speed_up(float speed_up_cms) +void AC_WPNav::set_speed_up_cms(float speed_up_cms) { _pos_control.set_max_speed_accel_U_cm(_pos_control.get_max_speed_down_cms(), speed_up_cms, _pos_control.get_max_accel_U_cmss()); update_track_with_speed_accel_limits(); } /// set current target descent rate during wp navigation -void AC_WPNav::set_speed_down(float speed_down_cms) +void AC_WPNav::set_speed_down_cms(float speed_down_cms) { _pos_control.set_max_speed_accel_U_cm(speed_down_cms, _pos_control.get_max_speed_up_cms(), _pos_control.get_max_accel_U_cmss()); update_track_with_speed_accel_limits(); } -/// set_wp_destination waypoint using location class +/// set_wp_destination_NEU_cm waypoint using location class /// returns false if conversion from location to vector from ekf origin cannot be calculated -bool AC_WPNav::set_wp_destination_loc(const Location& destination) +bool AC_WPNav::set_wp_destination_loc(const Location& destination_neu_cm) { bool terr_alt; Vector3f dest_neu; - // convert destination location to vector - if (!get_vector_NEU(destination, dest_neu, terr_alt)) { + // convert destination_neu_cm location to vector + if (!get_vector_NEU_cm(destination_neu_cm, dest_neu, terr_alt)) { return false; } // set target as vector from EKF origin - return set_wp_destination(dest_neu, terr_alt); + return set_wp_destination_NEU_cm(dest_neu, terr_alt); } -/// set next destination using location class +/// set next destination_neu_cm using location class /// returns false if conversion from location to vector from ekf origin cannot be calculated -bool AC_WPNav::set_wp_destination_next_loc(const Location& destination) +bool AC_WPNav::set_wp_destination_next_loc(const Location& destination_neu_cm) { bool terr_alt; Vector3f dest_neu; - // convert destination location to vector - if (!get_vector_NEU(destination, dest_neu, terr_alt)) { + // convert destination_neu_cm location to vector + if (!get_vector_NEU_cm(destination_neu_cm, dest_neu, terr_alt)) { return false; } // set target as vector from EKF origin - return set_wp_destination_next(dest_neu, terr_alt); + return set_wp_destination_next_NEU_cm(dest_neu, terr_alt); } -// get destination as a location. Altitude frame will be above origin or above terrain -// returns false if unable to return a destination (for example if origin has not yet been set) -bool AC_WPNav::get_wp_destination_loc(Location& destination) const +// get destination_neu_cm as a location. Altitude frame will be above origin or above terrain +// returns false if unable to return a destination_neu_cm (for example if origin has not yet been set) +bool AC_WPNav::get_wp_destination_loc(Location& destination_neu_cm) const { - if (!AP::ahrs().get_origin(destination)) { + if (!AP::ahrs().get_origin(destination_neu_cm)) { return false; } - destination = Location{get_wp_destination(), _terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN}; + destination_neu_cm = Location{get_wp_destination_NEU_cm(), _terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN}; return true; } -/// set_wp_destination - set destination waypoints using position vectors (distance from ekf origin in cm) -/// terrain_alt should be true if destination.z is an altitude above terrain (false if alt-above-ekf-origin) +/// set_wp_destination_NEU_cm - set destination_neu_cm waypoints using position vectors (distance from ekf origin in cm) +/// terrain_alt should be true if destination_neu_cm.z is an altitude above terrain (false if alt-above-ekf-origin) /// returns false on failure (likely caused by missing terrain data) -bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) +bool AC_WPNav::set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool terrain_alt) { - // re-initialise if previous destination has been interrupted + // re-initialise if previous destination_neu_cm has been interrupted if (!is_active() || !_flags.reached_destination) { - wp_and_spline_init(_wp_desired_speed_xy_cms); + wp_and_spline_init_cm(_wp_desired_speed_ne_cms); } _scurve_prev_leg.init(); float origin_speed = 0.0f; - // use previous destination as origin - _origin = _destination; + // use previous destination_neu_cm as origin + _origin_neu_cm = _destination_neu_cm; if (terrain_alt == _terrain_alt) { if (_this_leg_is_spline) { // if previous leg was a spline we can use current target velocity vector for origin velocity vector - Vector3f curr_target_vel = _pos_control.get_vel_desired_NEU_cms(); - curr_target_vel.z -= _pos_control.get_vel_offset_U_cms(); - origin_speed = curr_target_vel.length(); + Vector3f curr_target_vel_neu_cms = _pos_control.get_vel_desired_NEU_cms(); + curr_target_vel_neu_cms.z -= _pos_control.get_vel_offset_U_cms(); + origin_speed = curr_target_vel_neu_cms.length(); } else { // store previous leg _scurve_prev_leg = _scurve_this_leg; @@ -315,33 +315,33 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) // get current alt above terrain float origin_terr_offset; - if (!get_terrain_offset(origin_terr_offset)) { + if (!get_terrain_offset_cm(origin_terr_offset)) { return false; } // convert origin to alt-above-terrain if necessary if (terrain_alt) { - // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin - _origin.z -= origin_terr_offset; + // new destination_neu_cm is alt-above-terrain, previous destination_neu_cm was alt-above-ekf-origin + _origin_neu_cm.z -= origin_terr_offset; _pos_control.init_pos_terrain_U_cm(origin_terr_offset); } else { - // new destination is alt-above-ekf-origin, previous destination was alt-above-terrain - _origin.z += origin_terr_offset; + // new destination_neu_cm is alt-above-ekf-origin, previous destination_neu_cm was alt-above-terrain + _origin_neu_cm.z += origin_terr_offset; _pos_control.init_pos_terrain_U_cm(0.0); } } - // update destination - _destination = destination; + // update destination_neu_cm + _destination_neu_cm = destination_neu_cm; _terrain_alt = terrain_alt; if (_flags.fast_waypoint && !_this_leg_is_spline && !_next_leg_is_spline && !_scurve_next_leg.finished()) { _scurve_this_leg = _scurve_next_leg; } else { - _scurve_this_leg.calculate_track(_origin, _destination, + _scurve_this_leg.calculate_track(_origin_neu_cm, _destination_neu_cm, _pos_control.get_max_speed_NE_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), - get_wp_acceleration(), _wp_accel_z_cmss, - _scurve_snap * 100.0f, _scurve_jerk * 100.0f); + get_wp_acceleration_cmss(), _wp_accel_z_cmss, + _scurve_snap_max_mssss * 100.0f, _scurve_jerk_max_msss * 100.0f); if (!is_zero(origin_speed)) { // rebuild start of scurve if we have a non-zero origin speed _scurve_this_leg.set_origin_speed_max(origin_speed); @@ -350,27 +350,27 @@ bool AC_WPNav::set_wp_destination(const Vector3f& destination, bool terrain_alt) _this_leg_is_spline = false; _scurve_next_leg.init(); - _next_destination.zero(); // clear next destination + _next_destination_neu_cm.zero(); // clear next destination_neu_cm _flags.fast_waypoint = false; // default waypoint back to slow _flags.reached_destination = false; return true; } -/// set next destination using position vector (distance from ekf origin in cm) -/// terrain_alt should be true if destination.z is a desired altitude above terrain -/// provide next_destination -bool AC_WPNav::set_wp_destination_next(const Vector3f& destination, bool terrain_alt) +/// set next destination_neu_cm using position vector (distance from ekf origin in cm) +/// terrain_alt should be true if destination_neu_cm.z is a desired altitude above terrain +/// provide next_destination_neu_cm +bool AC_WPNav::set_wp_destination_next_NEU_cm(const Vector3f& destination_neu_cm, bool terrain_alt) { // do not add next point if alt types don't match if (terrain_alt != _terrain_alt) { return true; } - _scurve_next_leg.calculate_track(_destination, destination, + _scurve_next_leg.calculate_track(_destination_neu_cm, destination_neu_cm, _pos_control.get_max_speed_NE_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), - get_wp_acceleration(), _wp_accel_z_cmss, - _scurve_snap * 100.0f, _scurve_jerk * 100.0); + get_wp_acceleration_cmss(), _wp_accel_z_cmss, + _scurve_snap_max_mssss * 100.0f, _scurve_jerk_max_msss * 100.0); if (_this_leg_is_spline) { const float this_leg_dest_speed_max = _spline_this_leg.get_destination_speed_max(); const float next_leg_origin_speed_max = _scurve_next_leg.set_origin_speed_max(this_leg_dest_speed_max); @@ -378,172 +378,172 @@ bool AC_WPNav::set_wp_destination_next(const Vector3f& destination, bool terrain } _next_leg_is_spline = false; - // next destination provided so fast waypoint + // next destination_neu_cm provided so fast waypoint _flags.fast_waypoint = true; - // record next destination - _next_destination = destination; + // record next destination_neu_cm + _next_destination_neu_cm = destination_neu_cm; return true; } -/// set waypoint destination using NED position vector from ekf origin in meters -bool AC_WPNav::set_wp_destination_NED(const Vector3f& destination_NED) +/// set waypoint destination_neu_cm using NED position vector from ekf origin in meters +bool AC_WPNav::set_wp_destination_NED_cm(const Vector3f& destination_NED_cm) { // convert NED to NEU and do not use terrain following - return set_wp_destination(Vector3f(destination_NED.x * 100.0f, destination_NED.y * 100.0f, -destination_NED.z * 100.0f), false); + return set_wp_destination_NEU_cm(Vector3f(destination_NED_cm.x * 100.0f, destination_NED_cm.y * 100.0f, -destination_NED_cm.z * 100.0f), false); } -/// set waypoint destination using NED position vector from ekf origin in meters -bool AC_WPNav::set_wp_destination_next_NED(const Vector3f& destination_NED) +/// set waypoint destination_neu_cm using NED position vector from ekf origin in meters +bool AC_WPNav::set_wp_destination_next_NED_cm(const Vector3f& destination_NED_cm) { // convert NED to NEU and do not use terrain following - return set_wp_destination_next(Vector3f(destination_NED.x * 100.0f, destination_NED.y * 100.0f, -destination_NED.z * 100.0f), false); + return set_wp_destination_next_NEU_cm(Vector3f(destination_NED_cm.x * 100.0f, destination_NED_cm.y * 100.0f, -destination_NED_cm.z * 100.0f), false); } -/// shifts the origin and destination horizontally to the current position +/// shifts the origin and destination_neu_cm horizontally to the current position /// used to reset the track when taking off without horizontal position control -/// relies on set_wp_destination or set_wp_origin_and_destination having been called first -void AC_WPNav::shift_wp_origin_and_destination_to_current_pos_xy() +/// relies on set_wp_destination_NEU_cm or set_wp_origin_and_destination having been called first +void AC_WPNav::shift_wp_origin_and_destination_to_current_pos_NE() { // Reset position controller to current location _pos_control.init_NE_controller(); // get current and target locations - const Vector2f& curr_pos = _inav.get_position_xy_cm(); + const Vector2f& curr_pos_neu_cm = _inav.get_position_xy_cm(); - // shift origin and destination horizontally - _origin.xy() = curr_pos; - _destination.xy() = curr_pos; + // shift origin and destination_neu_cm horizontally + _origin_neu_cm.xy() = curr_pos_neu_cm; + _destination_neu_cm.xy() = curr_pos_neu_cm; } -/// shifts the origin and destination horizontally to the achievable stopping point +/// shifts the origin and destination_neu_cm horizontally to the achievable stopping point /// used to reset the track when horizontal navigation is enabled after having been disabled (see Copter's wp_navalt_min) -/// relies on set_wp_destination or set_wp_origin_and_destination having been called first -void AC_WPNav::shift_wp_origin_and_destination_to_stopping_point_xy() +/// relies on set_wp_destination_NEU_cm or set_wp_origin_and_destination having been called first +void AC_WPNav::shift_wp_origin_and_destination_to_stopping_point_NE() { // relax position control in xy axis // removing velocity error also impacts stopping point calculation _pos_control.relax_velocity_controller_NE(); // get current and target locations - Vector2f stopping_point; - get_wp_stopping_point_xy(stopping_point); + Vector2f stopping_point_ne_cm; + get_wp_stopping_point_NE_cm(stopping_point_ne_cm); - // shift origin and destination horizontally - _origin.xy() = stopping_point; - _destination.xy() = stopping_point; + // shift origin and destination_neu_cm horizontally + _origin_neu_cm.xy() = stopping_point_ne_cm; + _destination_neu_cm.xy() = stopping_point_ne_cm; // move pos controller target horizontally - _pos_control.set_pos_desired_NE_cm(stopping_point); + _pos_control.set_pos_desired_NE_cm(stopping_point_ne_cm); } -/// get_wp_stopping_point_xy - returns vector to stopping point based on a horizontal position and velocity -void AC_WPNav::get_wp_stopping_point_xy(Vector2f& stopping_point) const +/// get_wp_stopping_point_NE_cm - returns vector to stopping point based on a horizontal position and velocity +void AC_WPNav::get_wp_stopping_point_NE_cm(Vector2f& stopping_point_ne_cm) const { - Vector2p stop; - _pos_control.get_stopping_point_NE_cm(stop); - stopping_point = stop.tofloat(); + Vector2p stop_ne_cm; + _pos_control.get_stopping_point_NE_cm(stop_ne_cm); + stopping_point_ne_cm = stop_ne_cm.tofloat(); } -/// get_wp_stopping_point - returns vector to stopping point based on 3D position and velocity -void AC_WPNav::get_wp_stopping_point(Vector3f& stopping_point) const +/// get_wp_stopping_point_NEU_cm - returns vector to stopping point based on 3D position and velocity +void AC_WPNav::get_wp_stopping_point_NEU_cm(Vector3f& stopping_point_neu_cm) const { - Vector3p stop; - _pos_control.get_stopping_point_NE_cm(stop.xy()); - _pos_control.get_stopping_point_U_cm(stop.z); - stopping_point = stop.tofloat(); + Vector3p stop_neu_cm; + _pos_control.get_stopping_point_NE_cm(stop_neu_cm.xy()); + _pos_control.get_stopping_point_U_cm(stop_neu_cm.z); + stopping_point_neu_cm = stop_neu_cm.tofloat(); } -/// advance_wp_target_along_track - move target location along track from origin to destination +/// advance_wp_target_along_track - move target location along track from origin to destination_neu_cm bool AC_WPNav::advance_wp_target_along_track(float dt) { // calculate terrain adjustments - float terr_offset = 0.0f; - if (_terrain_alt && !get_terrain_offset(terr_offset)) { + float terr_offset_u_cm = 0.0f; + if (_terrain_alt && !get_terrain_offset_cm(terr_offset_u_cm)) { return false; } - const float offset_z_scaler = _pos_control.pos_terrain_U_scaler(terr_offset, get_terrain_margin() * 100.0); + const float offset_u_scaler = _pos_control.pos_terrain_U_scaler(terr_offset_u_cm, get_terrain_margin_m() * 100.0); // input shape the terrain offset - _pos_control.set_pos_terrain_target_U_cm(terr_offset); + _pos_control.set_pos_terrain_target_U_cm(terr_offset_u_cm); // get position controller's position offset (post input shaping) so it can be used in position error calculation - const Vector3p& psc_pos_offset = _pos_control.get_pos_offset_NEU_cm(); + const Vector3p& psc_pos_offset_neu_cm = _pos_control.get_pos_offset_NEU_cm(); - // get current position and adjust altitude to origin and destination's frame (i.e. _frame) - Vector3f curr_pos = _inav.get_position_neu_cm() - psc_pos_offset.tofloat(); - curr_pos.z -= terr_offset; - Vector3f curr_target_vel = _pos_control.get_vel_desired_NEU_cms(); - curr_target_vel.z -= _pos_control.get_vel_offset_U_cms(); + // get current position and adjust altitude to origin and destination_neu_cm's frame (i.e. _frame) + Vector3f curr_pos_neu_cm = _inav.get_position_neu_cm() - psc_pos_offset_neu_cm.tofloat(); + curr_pos_neu_cm.z -= terr_offset_u_cm; + Vector3f curr_target_vel_neu_cms = _pos_control.get_vel_desired_NEU_cms(); + curr_target_vel_neu_cms.z -= _pos_control.get_vel_offset_U_cms(); // Use _track_scalar_dt to slow down progression of the position target moving too far in front of aircraft // _track_scalar_dt does not scale the velocity or acceleration float track_scaler_dt = 1.0f; // check target velocity is non-zero - if (is_positive(curr_target_vel.length_squared())) { - Vector3f track_direction = curr_target_vel.normalized(); - const float track_error = _pos_control.get_pos_error_NEU_cm().dot(track_direction); - const float track_velocity = _inav.get_velocity_neu_cms().dot(track_direction); + if (is_positive(curr_target_vel_neu_cms.length_squared())) { + Vector3f track_direction_neu = curr_target_vel_neu_cms.normalized(); + const float track_error_neu_cm = _pos_control.get_pos_error_NEU_cm().dot(track_direction_neu); + const float track_velocity_neu_cms = _inav.get_velocity_neu_cms().dot(track_direction_neu); // 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_NE_p().kP() * track_error) / curr_target_vel.length(), 0.0f, 1.0f); + track_scaler_dt = constrain_float(0.05f + (track_velocity_neu_cms - _pos_control.get_pos_NE_p().kP() * track_error_neu_cm) / curr_target_vel_neu_cms.length(), 0.0f, 1.0f); } // Use vel_scaler_dt to slow down the trajectory time // vel_scaler_dt scales the velocity and acceleration to be kinematically consistent float vel_scaler_dt = 1.0; - if (is_positive(_wp_desired_speed_xy_cms)) { - update_vel_accel(_offset_vel, _offset_accel, dt, 0.0, 0.0); - const float vel_input = !_paused ? _wp_desired_speed_xy_cms * offset_z_scaler : 0.0; - shape_vel_accel(vel_input, 0.0, _offset_vel, _offset_accel, -get_wp_acceleration(), get_wp_acceleration(), + if (is_positive(_wp_desired_speed_ne_cms)) { + update_vel_accel(_offset_vel_cms, _offset_accel_cmss, dt, 0.0, 0.0); + const float vel_input_cms = !_paused ? _wp_desired_speed_ne_cms * offset_u_scaler : 0.0; + shape_vel_accel(vel_input_cms, 0.0, _offset_vel_cms, _offset_accel_cmss, -get_wp_acceleration_cmss(), get_wp_acceleration_cmss(), _pos_control.get_shaping_jerk_NE_cmsss(), dt, true); - vel_scaler_dt = _offset_vel / _wp_desired_speed_xy_cms; + vel_scaler_dt = _offset_vel_cms / _wp_desired_speed_ne_cms; } // change s-curve time speed with a time constant of maximum acceleration / maximum jerk float track_scaler_tc = 1.0f; - if (!is_zero(_wp_jerk)) { - track_scaler_tc = 0.01f * get_wp_acceleration()/_wp_jerk; + if (!is_zero(_wp_jerk_msss)) { + track_scaler_tc = 0.01f * get_wp_acceleration_cmss()/_wp_jerk_msss; } _track_scalar_dt += (track_scaler_dt - _track_scalar_dt) * (dt / track_scaler_tc); // target position, velocity and acceleration from straight line or spline calculators - Vector3f target_pos, target_vel, target_accel; + Vector3f target_pos_neu_cm, target_vel_neu_cms, target_accel_neu_cmss; bool s_finished; if (!_this_leg_is_spline) { // update target position, velocity and acceleration - target_pos = _origin; - s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, get_corner_acceleration(), _flags.fast_waypoint, _track_scalar_dt * vel_scaler_dt * dt, target_pos, target_vel, target_accel); + target_pos_neu_cm = _origin_neu_cm; + s_finished = _scurve_this_leg.advance_target_along_track(_scurve_prev_leg, _scurve_next_leg, _wp_radius_cm, get_corner_acceleration_cmss(), _flags.fast_waypoint, _track_scalar_dt * vel_scaler_dt * dt, target_pos_neu_cm, target_vel_neu_cms, target_accel_neu_cmss); } else { // splinetarget_vel - target_vel = curr_target_vel; - _spline_this_leg.advance_target_along_track(_track_scalar_dt * vel_scaler_dt * dt, target_pos, target_vel); + target_vel_neu_cms = curr_target_vel_neu_cms; + _spline_this_leg.advance_target_along_track(_track_scalar_dt * vel_scaler_dt * dt, target_pos_neu_cm, target_vel_neu_cms); s_finished = _spline_this_leg.reached_destination(); } - Vector3f accel_offset; - if (is_positive(target_vel.length_squared())) { - Vector3f track_direction = target_vel.normalized(); - accel_offset = track_direction * _offset_accel * target_vel.length() / _wp_desired_speed_xy_cms; + Vector3f accel_offset_neu_cmss; + if (is_positive(target_vel_neu_cms.length_squared())) { + Vector3f track_direction_neu = target_vel_neu_cms.normalized(); + accel_offset_neu_cmss = track_direction_neu * _offset_accel_cmss * target_vel_neu_cms.length() / _wp_desired_speed_ne_cms; } - target_vel *= vel_scaler_dt; - target_accel *= sq(vel_scaler_dt); - target_accel += accel_offset; + target_vel_neu_cms *= vel_scaler_dt; + target_accel_neu_cmss *= sq(vel_scaler_dt); + target_accel_neu_cmss += accel_offset_neu_cmss; // pass new target to the position controller - _pos_control.set_pos_vel_accel_NEU_cm(target_pos.topostype(), target_vel, target_accel); + _pos_control.set_pos_vel_accel_NEU_cm(target_pos_neu_cm.topostype(), target_vel_neu_cms, target_accel_neu_cmss); // check if we've reached the waypoint if (!_flags.reached_destination) { if (s_finished) { - // "fast" waypoints are complete once the intermediate point reaches the destination + // "fast" waypoints are complete once the intermediate point reaches the destination_neu_cm if (_flags.fast_waypoint) { _flags.reached_destination = true; } else { // regular waypoints also require the copter to be within the waypoint radius - const Vector3f dist_to_dest = curr_pos - _destination; + const Vector3f dist_to_dest = curr_pos_neu_cm - _destination_neu_cm; if (dist_to_dest.length_squared() <= sq(_wp_radius_cm)) { _flags.reached_destination = true; } @@ -561,7 +561,7 @@ void AC_WPNav::update_track_with_speed_accel_limits() // update this leg if (_this_leg_is_spline) { _spline_this_leg.set_speed_accel(_pos_control.get_max_speed_NE_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), - get_wp_acceleration(), _wp_accel_z_cmss); + get_wp_acceleration_cmss(), _wp_accel_z_cmss); } else { _scurve_this_leg.set_speed_max(_pos_control.get_max_speed_NE_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms()); } @@ -569,22 +569,22 @@ void AC_WPNav::update_track_with_speed_accel_limits() // update next leg if (_next_leg_is_spline) { _spline_next_leg.set_speed_accel(_pos_control.get_max_speed_NE_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms(), - get_wp_acceleration(), _wp_accel_z_cmss); + get_wp_acceleration_cmss(), _wp_accel_z_cmss); } else { _scurve_next_leg.set_speed_max(_pos_control.get_max_speed_NE_cms(), _pos_control.get_max_speed_up_cms(), _pos_control.get_max_speed_down_cms()); } } -/// get_wp_distance_to_destination - get horizontal distance to destination in cm -float AC_WPNav::get_wp_distance_to_destination() const +/// get_wp_distance_to_destination - get horizontal distance to destination_neu_cm in cm +float AC_WPNav::get_wp_distance_to_destination_cm() const { - return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination.xy()); + return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination_neu_cm.xy()); } -/// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees -int32_t AC_WPNav::get_wp_bearing_to_destination() const +/// get_wp_bearing_to_destination_cd - get bearing to next waypoint in centi-degrees +int32_t AC_WPNav::get_wp_bearing_to_destination_cd() const { - return get_bearing_cd(_inav.get_position_xy_cm(), _destination.xy()); + return get_bearing_cd(_inav.get_position_xy_cm(), _destination_neu_cm.xy()); } /// update_wpnav - run the wp controller - should be called at 100hz or higher @@ -593,16 +593,16 @@ bool AC_WPNav::update_wpnav() // check for changes in speed parameter values if (_check_wp_speed_change) { if (!is_equal(_wp_speed_cms.get(), _last_wp_speed_cms)) { - set_speed_xy(_wp_speed_cms); + set_speed_NE_cms(_wp_speed_cms); _last_wp_speed_cms = _wp_speed_cms; } } if (!is_equal(_wp_speed_up_cms.get(), _last_wp_speed_up_cms)) { - set_speed_up(_wp_speed_up_cms); + set_speed_up_cms(_wp_speed_up_cms); _last_wp_speed_up_cms = _wp_speed_up_cms; } if (!is_equal(_wp_speed_down_cms.get(), _last_wp_speed_down_cms)) { - set_speed_down(_wp_speed_down_cms); + set_speed_down_cms(_wp_speed_down_cms); _last_wp_speed_down_cms = _wp_speed_down_cms; } @@ -615,7 +615,7 @@ bool AC_WPNav::update_wpnav() _pos_control.update_NE_controller(); - _wp_last_update = AP_HAL::millis(); + _wp_last_update_ms = AP_HAL::millis(); return ret; } @@ -623,10 +623,10 @@ bool AC_WPNav::update_wpnav() // returns true if update_wpnav has been run very recently bool AC_WPNav::is_active() const { - return (AP_HAL::millis() - _wp_last_update) < 200; + return (AP_HAL::millis() - _wp_last_update_ms) < 200; } -// force stopping at next waypoint. Used by Dijkstra's object avoidance when path from destination to next destination is not clear +// force stopping at next waypoint. Used by Dijkstra's object avoidance when path from destination_neu_cm to next destination_neu_cm is not clear // only affects regular (e.g. non-spline) waypoints // returns true if this had any affect on the path bool AC_WPNav::force_stop_at_next_wp() @@ -650,7 +650,7 @@ bool AC_WPNav::force_stop_at_next_wp() } // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) -bool AC_WPNav::get_terrain_offset(float& offset_cm) +bool AC_WPNav::get_terrain_offset_cm(float& offset_cm) { // calculate offset based on source (rangefinder or terrain database) switch (get_terrain_source()) { @@ -683,63 +683,63 @@ bool AC_WPNav::get_terrain_offset(float& offset_cm) /// spline methods /// -/// set_spline_destination waypoint using location class +/// set_spline_destination_NEU_cm waypoint using location class /// returns false if conversion from location to vector from ekf origin cannot be calculated -/// next_destination should be the next segment's destination -/// next_is_spline should be true if path to next_destination should be a spline -bool AC_WPNav::set_spline_destination_loc(const Location& destination, const Location& next_destination, bool next_is_spline) +/// next_destination_neu_cm should be the next segment's destination_neu_cm +/// next_is_spline should be true if path to next_destination_neu_cm should be a spline +bool AC_WPNav::set_spline_destination_loc(const Location& destination_neu_cm, const Location& next_destination_neu_cm, bool next_is_spline) { - // convert destination location to vector + // convert destination_neu_cm location to vector Vector3f dest_neu; bool dest_terr_alt; - if (!get_vector_NEU(destination, dest_neu, dest_terr_alt)) { + if (!get_vector_NEU_cm(destination_neu_cm, dest_neu, dest_terr_alt)) { return false; } - // convert next destination to vector + // convert next destination_neu_cm to vector Vector3f next_dest_neu; bool next_dest_terr_alt; - if (!get_vector_NEU(next_destination, next_dest_neu, next_dest_terr_alt)) { + if (!get_vector_NEU_cm(next_destination_neu_cm, next_dest_neu, next_dest_terr_alt)) { return false; } // set target as vector from EKF origin - return set_spline_destination(dest_neu, dest_terr_alt, next_dest_neu, next_dest_terr_alt, next_is_spline); + return set_spline_destination_NEU_cm(dest_neu, dest_terr_alt, next_dest_neu, next_dest_terr_alt, next_is_spline); } -/// set next destination (e.g. the one after the current destination) as a spline segment specified as a location +/// set next destination_neu_cm (e.g. the one after the current destination_neu_cm) as a spline segment specified as a location /// returns false if conversion from location to vector from ekf origin cannot be calculated -/// next_next_destination should be the next segment's destination -bool AC_WPNav::set_spline_destination_next_loc(const Location& next_destination, const Location& next_next_destination, bool next_next_is_spline) +/// next_next_destination_neu_cm should be the next segment's destination_neu_cm +bool AC_WPNav::set_spline_destination_next_loc(const Location& next_destination_neu_cm, const Location& next_next_destination_neu_cm, bool next_next_is_spline) { - // convert next_destination location to vector + // convert next_destination_neu_cm location to vector Vector3f next_dest_neu; bool next_dest_terr_alt; - if (!get_vector_NEU(next_destination, next_dest_neu, next_dest_terr_alt)) { + if (!get_vector_NEU_cm(next_destination_neu_cm, next_dest_neu, next_dest_terr_alt)) { return false; } - // convert next_next_destination to vector + // convert next_next_destination_neu_cm to vector Vector3f next_next_dest_neu; bool next_next_dest_terr_alt; - if (!get_vector_NEU(next_next_destination, next_next_dest_neu, next_next_dest_terr_alt)) { + if (!get_vector_NEU_cm(next_next_destination_neu_cm, next_next_dest_neu, next_next_dest_terr_alt)) { return false; } // set target as vector from EKF origin - return set_spline_destination_next(next_dest_neu, next_dest_terr_alt, next_next_dest_neu, next_next_dest_terr_alt, next_next_is_spline); + return set_spline_destination_next_NEU_cm(next_dest_neu, next_dest_terr_alt, next_next_dest_neu, next_next_dest_terr_alt, next_next_is_spline); } -/// set_spline_destination waypoint using position vector (distance from ekf origin in cm) -/// terrain_alt should be true if destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) -/// next_destination should be set to the next segment's destination -/// next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) -/// next_destination.z must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination should be too) -bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_alt, const Vector3f& next_destination, bool next_terrain_alt, bool next_is_spline) +/// set_spline_destination_NEU_cm waypoint using position vector (distance from ekf origin in cm) +/// terrain_alt should be true if destination_neu_cm.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) +/// next_destination_neu_cm should be set to the next segment's destination_neu_cm +/// next_terrain_alt should be true if next_destination_neu_cm.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) +/// next_destination_neu_cm.z must be in the same "frame" as destination_neu_cm.z (i.e. if destination_neu_cm is a alt-above-terrain, next_destination_neu_cm should be too) +bool AC_WPNav::set_spline_destination_NEU_cm(const Vector3f& destination_neu_cm, bool terrain_alt, const Vector3f& next_destination_neu_cm, bool next_terrain_alt, bool next_is_spline) { - // re-initialise if previous destination has been interrupted + // re-initialise if previous destination_neu_cm has been interrupted if (!is_active() || !_flags.reached_destination) { - wp_and_spline_init(_wp_desired_speed_xy_cms); + wp_and_spline_init_cm(_wp_desired_speed_ne_cms); } // update spline calculators speeds and accelerations @@ -747,75 +747,75 @@ bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_ _pos_control.get_max_accel_NE_cmss(), _pos_control.get_max_accel_U_cmss()); // calculate origin and origin velocity vector - Vector3f origin_vector; + Vector3f origin_vector_neu_cm; if (terrain_alt == _terrain_alt) { if (_flags.fast_waypoint) { // calculate origin vector if (_this_leg_is_spline) { - // if previous leg was a spline we can use destination velocity vector for origin velocity vector - origin_vector = _spline_this_leg.get_destination_vel(); + // if previous leg was a spline we can use destination_neu_cm velocity vector for origin velocity vector + origin_vector_neu_cm = _spline_this_leg.get_destination_vel(); } else { // use direction of the previous straight line segment - origin_vector = _destination - _origin; + origin_vector_neu_cm = _destination_neu_cm - _origin_neu_cm; } } - // use previous destination as origin - _origin = _destination; + // use previous destination_neu_cm as origin + _origin_neu_cm = _destination_neu_cm; } else { - // use previous destination as origin - _origin = _destination; + // use previous destination_neu_cm as origin + _origin_neu_cm = _destination_neu_cm; // get current alt above terrain float origin_terr_offset; - if (!get_terrain_offset(origin_terr_offset)) { + if (!get_terrain_offset_cm(origin_terr_offset)) { return false; } // convert origin to alt-above-terrain if necessary if (terrain_alt) { - // new destination is alt-above-terrain, previous destination was alt-above-ekf-origin - _origin.z -= origin_terr_offset; + // new destination_neu_cm is alt-above-terrain, previous destination_neu_cm was alt-above-ekf-origin + _origin_neu_cm.z -= origin_terr_offset; _pos_control.init_pos_terrain_U_cm(origin_terr_offset); } else { - // new destination is alt-above-ekf-origin, previous destination was alt-above-terrain - _origin.z += origin_terr_offset; + // new destination_neu_cm is alt-above-ekf-origin, previous destination_neu_cm was alt-above-terrain + _origin_neu_cm.z += origin_terr_offset; _pos_control.init_pos_terrain_U_cm(0.0); } } - // store destination location - _destination = destination; + // store destination_neu_cm location + _destination_neu_cm = destination_neu_cm; _terrain_alt = terrain_alt; - // calculate destination velocity vector - Vector3f destination_vector; + // calculate destination_neu_cm velocity vector + Vector3f destination_vector_neu_cm; if (terrain_alt == next_terrain_alt) { if (next_is_spline) { - // leave this segment moving parallel to vector from origin to next destination - destination_vector = next_destination - _origin; + // leave this segment moving parallel to vector from origin to next destination_neu_cm + destination_vector_neu_cm = next_destination_neu_cm - _origin_neu_cm; } else { // leave this segment moving parallel to next segment - destination_vector = next_destination - _destination; + destination_vector_neu_cm = next_destination_neu_cm - _destination_neu_cm; } } - _flags.fast_waypoint = !destination_vector.is_zero(); + _flags.fast_waypoint = !destination_vector_neu_cm.is_zero(); // setup spline leg - _spline_this_leg.set_origin_and_destination(_origin, _destination, origin_vector, destination_vector); + _spline_this_leg.set_origin_and_destination(_origin_neu_cm, _destination_neu_cm, origin_vector_neu_cm, destination_vector_neu_cm); _this_leg_is_spline = true; _flags.reached_destination = false; return true; } -/// set next destination (e.g. the one after the current destination) as an offset (in cm, NEU frame) from the EKF origin -/// next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) -/// next_next_destination should be set to the next segment's destination -/// next_next_terrain_alt should be true if next_next_destination.z is a desired altitude above terrain (false if it is desired altitude above ekf origin) -/// next_next_destination.z must be in the same "frame" as destination.z (i.e. if next_destination is a alt-above-terrain, next_next_destination should be too) -bool AC_WPNav::set_spline_destination_next(const Vector3f& next_destination, bool next_terrain_alt, const Vector3f& next_next_destination, bool next_next_terrain_alt, bool next_next_is_spline) +/// set next destination_neu_cm (e.g. the one after the current destination_neu_cm) as an offset (in cm, NEU frame) from the EKF origin +/// next_terrain_alt should be true if next_destination_neu_cm.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) +/// next_next_destination_neu_cm should be set to the next segment's destination_neu_cm +/// next_next_terrain_alt should be true if next_next_destination_neu_cm.z is a desired altitude above terrain (false if it is desired altitude above ekf origin) +/// next_next_destination_neu_cm.z must be in the same "frame" as destination_neu_cm.z (i.e. if next_destination_neu_cm is a alt-above-terrain, next_next_destination_neu_cm should be too) +bool AC_WPNav::set_spline_destination_next_NEU_cm(const Vector3f& next_destination_neu_cm, bool next_terrain_alt, const Vector3f& next_next_destination_neu_cm, bool next_next_terrain_alt, bool next_next_is_spline) { // do not add next point if alt types don't match if (next_terrain_alt != _terrain_alt) { @@ -823,24 +823,24 @@ bool AC_WPNav::set_spline_destination_next(const Vector3f& next_destination, boo } // calculate origin and origin velocity vector - Vector3f origin_vector; + Vector3f origin_vector_neu_cm; if (_this_leg_is_spline) { - // if previous leg was a spline we can use destination velocity vector for origin velocity vector - origin_vector = _spline_this_leg.get_destination_vel(); + // if previous leg was a spline we can use destination_neu_cm velocity vector for origin velocity vector + origin_vector_neu_cm = _spline_this_leg.get_destination_vel(); } else { // use the direction of the previous straight line segment - origin_vector = _destination - _origin; + origin_vector_neu_cm = _destination_neu_cm - _origin_neu_cm; } - // calculate destination velocity vector - Vector3f destination_vector; + // calculate destination_neu_cm velocity vector + Vector3f destination_vector_neu_cm; if (next_terrain_alt == next_next_terrain_alt) { if (next_next_is_spline) { - // leave this segment moving parallel to vector from this leg's origin (i.e. prev leg's destination) to next next destination - destination_vector = next_next_destination - _destination; + // leave this segment moving parallel to vector from this leg's origin (i.e. prev leg's destination_neu_cm) to next next destination_neu_cm + destination_vector_neu_cm = next_next_destination_neu_cm - _destination_neu_cm; } else { // leave this segment moving parallel to next segment - destination_vector = next_next_destination - next_destination; + destination_vector_neu_cm = next_next_destination_neu_cm - next_destination_neu_cm; } } @@ -849,10 +849,10 @@ bool AC_WPNav::set_spline_destination_next(const Vector3f& next_destination, boo _pos_control.get_max_accel_NE_cmss(), _pos_control.get_max_accel_U_cmss()); // setup next spline leg. Note this could be made local - _spline_next_leg.set_origin_and_destination(_destination, next_destination, origin_vector, destination_vector); + _spline_next_leg.set_origin_and_destination(_destination_neu_cm, next_destination_neu_cm, origin_vector_neu_cm, destination_vector_neu_cm); _next_leg_is_spline = true; - // next destination provided so fast waypoint + // next destination_neu_cm provided so fast waypoint _flags.fast_waypoint = true; // update this_leg's final velocity to match next spline leg @@ -867,11 +867,11 @@ bool AC_WPNav::set_spline_destination_next(const Vector3f& next_destination, boo // convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain // returns false if conversion failed (likely because terrain data was not available) -bool AC_WPNav::get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_alt) +bool AC_WPNav::get_vector_NEU_cm(const Location &loc, Vector3f &pos_from_origin_neu_cm, bool &terrain_alt) { // convert location to NE vector2f - Vector2f res_vec; - if (!loc.get_vector_xy_from_origin_NE_cm(res_vec)) { + Vector2f loc_pos_from_origin_neu_cm; + if (!loc.get_vector_xy_from_origin_NE_cm(loc_pos_from_origin_neu_cm)) { return false; } @@ -881,7 +881,7 @@ bool AC_WPNav::get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_ if (!loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, terr_alt)) { return false; } - vec.z = terr_alt; + pos_from_origin_neu_cm.z = terr_alt; terrain_alt = true; } else { terrain_alt = false; @@ -889,37 +889,37 @@ bool AC_WPNav::get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_ if (!loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, temp_alt)) { return false; } - vec.z = temp_alt; + pos_from_origin_neu_cm.z = temp_alt; terrain_alt = false; } // copy xy (we do this to ensure we do not adjust vector unless the overall conversion is successful - vec.x = res_vec.x; - vec.y = res_vec.y; + pos_from_origin_neu_cm.x = loc_pos_from_origin_neu_cm.x; + pos_from_origin_neu_cm.y = loc_pos_from_origin_neu_cm.y; return true; } // helper function to calculate scurve jerk and jerk_time values -// updates _scurve_jerk and _scurve_snap +// updates _scurve_jerk_max_msss and _scurve_snap_max_mssss void AC_WPNav::calc_scurve_jerk_and_snap() { // calculate jerk - _scurve_jerk = MIN(_attitude_control.get_ang_vel_roll_max_rads() * GRAVITY_MSS, _attitude_control.get_ang_vel_pitch_max_rads() * GRAVITY_MSS); - if (is_zero(_scurve_jerk)) { - _scurve_jerk = _wp_jerk; + _scurve_jerk_max_msss = MIN(_attitude_control.get_ang_vel_roll_max_rads() * GRAVITY_MSS, _attitude_control.get_ang_vel_pitch_max_rads() * GRAVITY_MSS); + if (is_zero(_scurve_jerk_max_msss)) { + _scurve_jerk_max_msss = _wp_jerk_msss; } else { - _scurve_jerk = MIN(_scurve_jerk, _wp_jerk); + _scurve_jerk_max_msss = MIN(_scurve_jerk_max_msss, _wp_jerk_msss); } // calculate maximum snap // Snap (the rate of change of jerk) uses the attitude control input time constant because multicopters // lean to accelerate. This means the change in angle is equivalent to the change in acceleration - _scurve_snap = (_scurve_jerk * M_PI) / (2.0 * MAX(_attitude_control.get_input_tc(), 0.1f)); + _scurve_snap_max_mssss = (_scurve_jerk_max_msss * M_PI) / (2.0 * MAX(_attitude_control.get_input_tc(), 0.1f)); const float snap = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS; if (is_positive(snap)) { - _scurve_snap = MIN(_scurve_snap, snap); + _scurve_snap_max_mssss = MIN(_scurve_snap_max_mssss, snap); } // reduce maximum snap by a factor of two from what the aircraft is capable of - _scurve_snap *= 0.5; + _scurve_snap_max_mssss *= 0.5; } diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 5e224959769..0f714c2df9e 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -24,7 +24,7 @@ public: /// provide rangefinder based terrain offset /// terrain offset is the terrain's height above the EKF origin - void set_rangefinder_terrain_offset(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;} + void set_rangefinder_terrain_offset_cm(bool use, bool healthy, float terrain_offset_cm) { _rangefinder_available = use; _rangefinder_healthy = healthy; _rangefinder_terrain_offset_cm = terrain_offset_cm;} // return true if range finder may be used for terrain following bool rangefinder_used() const { return _rangefinder_use; } @@ -39,27 +39,27 @@ public: AC_WPNav::TerrainSource get_terrain_source() const; // get terrain's altitude (in cm above the ekf origin) at the current position (+ve means terrain below vehicle is above ekf origin's altitude) - bool get_terrain_offset(float& offset_cm); + bool get_terrain_offset_cm(float& offset_cm); // return terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin - float get_terrain_margin() const { return MAX(_terrain_margin, 0.1); } + float get_terrain_margin_m() const { return MAX(_terrain_margin_m, 0.1); } // convert location to vector from ekf origin. terrain_alt is set to true if resulting vector's z-axis should be treated as alt-above-terrain // returns false if conversion failed (likely because terrain data was not available) - bool get_vector_NEU(const Location &loc, Vector3f &vec, bool &terrain_alt); + bool get_vector_NEU_cm(const Location &loc, Vector3f &pos_from_origin_NEU_cm, bool &terrain_alt); /// /// waypoint controller /// - /// wp_and_spline_init - initialise straight line and spline waypoint controllers + /// wp_and_spline_init_cm - initialise straight line and spline waypoint controllers /// speed_cms is the desired max speed to travel between waypoints. should be a positive value or omitted to use the default speed /// updates target roll, pitch targets and I terms based on vehicle lean angles /// should be called once before the waypoint controller is used but does not need to be called before subsequent updates to destination - void wp_and_spline_init(float speed_cms = 0.0f, Vector3f stopping_point = Vector3f{}); + void wp_and_spline_init_cm(float speed_cms = 0.0f, Vector3f stopping_point = Vector3f{}); /// set current target horizontal speed during wp navigation - void set_speed_xy(float speed_cms); + void set_speed_NE_cms(float speed_cms); /// set pause or resume during wp navigation void set_pause() { _paused = true; } @@ -69,39 +69,39 @@ public: bool paused() { return _paused; } /// set current target climb or descent rate during wp navigation - void set_speed_up(float speed_up_cms); - void set_speed_down(float speed_down_cms); + void set_speed_up_cms(float speed_up_cms); + void set_speed_down_cms(float speed_down_cms); /// get default target horizontal velocity during wp navigation - float get_default_speed_xy() const { return _wp_speed_cms; } + float get_default_speed_NE_cms() const { return _wp_speed_cms; } /// get default target climb speed in cm/s during missions - float get_default_speed_up() const { return _wp_speed_up_cms; } + float get_default_speed_up_cms() const { return _wp_speed_up_cms; } /// get default target descent rate in cm/s during missions. Note: always positive - float get_default_speed_down() const { return fabsf(_wp_speed_down_cms); } + float get_default_speed_down_cms() const { return fabsf(_wp_speed_down_cms); } - /// get_speed_z - returns target descent speed in cm/s during missions. Note: always positive - float get_accel_z() const { return _wp_accel_z_cmss; } + /// get_accel_U_cmss - returns vertical acceleration in cm/s/s during missions. Note: always positive + float get_accel_U_cmss() const { return _wp_accel_z_cmss; } /// get_wp_acceleration - returns acceleration in cm/s/s during missions - float get_wp_acceleration() const { return (is_positive(_wp_accel_cmss)) ? _wp_accel_cmss : WPNAV_ACCELERATION; } + float get_wp_acceleration_cmss() const { return (is_positive(_wp_accel_cmss)) ? _wp_accel_cmss : WPNAV_ACCELERATION; } - /// get_corner_acceleration - returns maximum acceleration in cm/s/s used during cornering in missions - float get_corner_acceleration() const { return (is_positive(_wp_accel_c_cmss)) ? _wp_accel_c_cmss : 2.0 * get_wp_acceleration(); } + /// get_corner_acceleration_cmss - returns maximum acceleration in cm/s/s used during cornering in missions + float get_corner_acceleration_cmss() const { return (is_positive(_wp_accel_c_cmss)) ? _wp_accel_c_cmss : 2.0 * get_wp_acceleration_cmss(); } - /// get_wp_destination waypoint using position vector + /// get_wp_destination_NEU_cm waypoint using position vector /// x,y are distance from ekf origin in cm /// z may be cm above ekf origin or terrain (see origin_and_destination_are_terrain_alt method) - const Vector3f &get_wp_destination() const { return _destination; } + const Vector3f &get_wp_destination_NEU_cm() const { return _destination_neu_cm; } /// get origin using position vector (distance from ekf origin in cm) - const Vector3f &get_wp_origin() const { return _origin; } + const Vector3f &get_wp_origin_NEU_cm() const { return _origin_neu_cm; } /// true if origin.z and destination.z are alt-above-terrain, false if alt-above-ekf-origin bool origin_and_destination_are_terrain_alt() const { return _terrain_alt; } - /// set_wp_destination waypoint using location class + /// set_wp_destination_NEU_cm waypoint using location class /// provide the next_destination if known /// returns false if conversion from location to vector from ekf origin cannot be calculated bool set_wp_destination_loc(const Location& destination); @@ -111,47 +111,47 @@ public: // returns false if unable to return a destination (for example if origin has not yet been set) bool get_wp_destination_loc(Location& destination) const; - // returns object avoidance adjusted destination which is always the same as get_wp_destination + // returns object avoidance adjusted destination which is always the same as get_wp_destination_NEU_cm // having this function unifies the AC_WPNav_OA and AC_WPNav interfaces making vehicle code simpler virtual bool get_oa_wp_destination(Location& destination) const { return get_wp_destination_loc(destination); } - /// set_wp_destination waypoint using position vector (distance from ekf origin in cm) + /// set_wp_destination_NEU_cm waypoint using position vector (distance from ekf origin in cm) /// terrain_alt should be true if destination.z is a desired altitude above terrain - virtual bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false); - bool set_wp_destination_next(const Vector3f& destination, bool terrain_alt = false); + virtual bool set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool terrain_alt = false); + bool set_wp_destination_next_NEU_cm(const Vector3f& destination_neu_cm, bool terrain_alt = false); /// set waypoint destination using NED position vector from ekf origin in meters /// provide next_destination_NED if known - bool set_wp_destination_NED(const Vector3f& destination_NED); - bool set_wp_destination_next_NED(const Vector3f& destination_NED); + bool set_wp_destination_NED_cm(const Vector3f& destination_NED_cm); + bool set_wp_destination_next_NED_cm(const Vector3f& destination_NED_cm); /// shifts the origin and destination horizontally to the current position /// used to reset the track when taking off without horizontal position control - /// relies on set_wp_destination or set_wp_origin_and_destination having been called first - void shift_wp_origin_and_destination_to_current_pos_xy(); + /// relies on set_wp_destination_NEU_cm or set_wp_origin_and_destination having been called first + void shift_wp_origin_and_destination_to_current_pos_NE(); // todo: Not used /// shifts the origin and destination horizontally to the achievable stopping point /// used to reset the track when horizontal navigation is enabled after having been disabled (see Copter's wp_navalt_min) - /// relies on set_wp_destination or set_wp_origin_and_destination having been called first - void shift_wp_origin_and_destination_to_stopping_point_xy(); + /// relies on set_wp_destination_NEU_cm or set_wp_origin_and_destination having been called first + void shift_wp_origin_and_destination_to_stopping_point_NE(); // todo: Not used - /// get_wp_stopping_point_xy - calculates stopping point based on current position, velocity, waypoint acceleration + /// get_wp_stopping_point_cm - calculates stopping point based on current position, velocity, waypoint acceleration /// results placed in stopping_position vector - void get_wp_stopping_point_xy(Vector2f& stopping_point) const; - void get_wp_stopping_point(Vector3f& stopping_point) const; + void get_wp_stopping_point_NE_cm(Vector2f& stopping_point_NE_cm) const; + void get_wp_stopping_point_NEU_cm(Vector3f& stopping_point) const; /// get_wp_distance_to_destination - get horizontal distance to destination in cm - virtual float get_wp_distance_to_destination() const; + virtual float get_wp_distance_to_destination_cm() const; /// get_bearing_to_destination - get bearing to next waypoint in centi-degrees - virtual int32_t get_wp_bearing_to_destination() const; + virtual int32_t get_wp_bearing_to_destination_cd() const; /// reached_destination - true when we have come within RADIUS cm of the waypoint virtual bool reached_wp_destination() const { return _flags.reached_destination; } - // reached_wp_destination_xy - true if within RADIUS_CM of waypoint in x/y - bool reached_wp_destination_xy() const { - return get_wp_distance_to_destination() < _wp_radius_cm; + // reached_wp_destination_NE - true if within RADIUS_CM of waypoint in x/y + bool reached_wp_destination_NE() const { + return get_wp_distance_to_destination_cm() < _wp_radius_cm; } // get wp_radius parameter value in cm @@ -172,7 +172,7 @@ public: /// spline methods /// - /// set_spline_destination waypoint using location class + /// set_spline_destination_NEU_cm waypoint using location class /// returns false if conversion from location to vector from ekf origin cannot be calculated /// next_destination should be the next segment's destination /// next_is_spline should be true if next_destination is a spline segment @@ -184,13 +184,13 @@ public: /// next_next_is_spline should be true if next_next_destination is a spline segment bool set_spline_destination_next_loc(const Location& next_destination, const Location& next_next_destination, bool next_next_is_spline); - /// set_spline_destination waypoint using position vector (distance from ekf origin in cm) + /// set_spline_destination_NEU_cm waypoint using position vector (distance from ekf origin in cm) /// terrain_alt should be true if destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) /// next_destination is the next segment's destination /// next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) /// next_destination.z must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination must be too) /// next_is_spline should be true if next_destination is a spline segment - bool set_spline_destination(const Vector3f& destination, bool terrain_alt, const Vector3f& next_destination, bool next_terrain_alt, bool next_is_spline); + bool set_spline_destination_NEU_cm(const Vector3f& destination_neu_cm, bool terrain_alt, const Vector3f& next_destination_neu_cm, bool next_terrain_alt, bool next_is_spline); /// set next destination (e.g. the one after the current destination) as an offset (in cm, NEU frame) from the EKF origin /// next_terrain_alt should be true if next_destination.z is a desired altitude above terrain (false if its desired altitudes above ekf origin) @@ -198,7 +198,7 @@ public: /// next_next_terrain_alt should be true if next_next_destination.z is a desired altitude above terrain (false if it is desired altitude above ekf origin) /// next_next_destination.z must be in the same "frame" as destination.z (i.e. if next_destination is a alt-above-terrain, next_next_destination must be too) /// next_next_is_spline should be true if next_next_destination is a spline segment - bool set_spline_destination_next(const Vector3f& next_destination, bool next_terrain_alt, const Vector3f& next_next_destination, bool next_next_terrain_alt, bool next_next_is_spline); + bool set_spline_destination_next_NEU_cm(const Vector3f& next_destination_neu_cm, bool next_terrain_alt, const Vector3f& next_next_destination_neu_cm, bool next_next_terrain_alt, bool next_next_is_spline); /// /// shared methods @@ -232,7 +232,7 @@ protected: } _flags; // helper function to calculate scurve jerk and jerk_time values - // updates _scurve_jerk and _scurve_snap + // updates _scurve_jerk_max_msss and _scurve_snap_max_mssss void calc_scurve_jerk_and_snap(); // references and pointers to external libraries @@ -242,52 +242,52 @@ protected: const AC_AttitudeControl& _attitude_control; // parameters - AP_Float _wp_speed_cms; // default maximum horizontal speed in cm/s during missions - AP_Float _wp_speed_up_cms; // default maximum climb rate in cm/s - AP_Float _wp_speed_down_cms; // default maximum descent rate in cm/s - AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached - AP_Float _wp_accel_cmss; // horizontal acceleration in cm/s/s during missions - AP_Float _wp_accel_c_cmss; // cornering acceleration in cm/s/s during missions - AP_Float _wp_accel_z_cmss; // vertical acceleration in cm/s/s during missions - AP_Float _wp_jerk; // maximum jerk used to generate scurve trajectories in m/s/s/s - AP_Float _terrain_margin; // terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin + AP_Float _wp_speed_cms; // default maximum horizontal speed in cm/s during missions + AP_Float _wp_speed_up_cms; // default maximum climb rate in cm/s + AP_Float _wp_speed_down_cms; // default maximum descent rate in cm/s + AP_Float _wp_radius_cm; // distance from a waypoint in cm that, when crossed, indicates the wp has been reached + AP_Float _wp_accel_cmss; // horizontal acceleration in cm/s/s during missions + AP_Float _wp_accel_c_cmss; // cornering acceleration in cm/s/s during missions + AP_Float _wp_accel_z_cmss; // vertical acceleration in cm/s/s during missions + AP_Float _wp_jerk_msss; // maximum jerk used to generate scurve trajectories in m/s/s/s + AP_Float _terrain_margin_m; // terrain following altitude margin. vehicle will stop if distance from target altitude is larger than this margin // WPNAV_SPEED param change checker - bool _check_wp_speed_change; // if true WPNAV_SPEED param should be checked for changes in-flight - float _last_wp_speed_cms; // last recorded WPNAV_SPEED, used for changing speed in-flight - float _last_wp_speed_up_cms; // last recorded WPNAV_SPEED_UP, used for changing speed in-flight + bool _check_wp_speed_change; // if true WPNAV_SPEED param should be checked for changes in-flight + float _last_wp_speed_cms; // last recorded WPNAV_SPEED, used for changing speed in-flight + float _last_wp_speed_up_cms; // last recorded WPNAV_SPEED_UP, used for changing speed in-flight float _last_wp_speed_down_cms; // last recorded WPNAV_SPEED_DN, used for changing speed in-flight // scurve - SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory - SCurve _scurve_this_leg; // current scurve trajectory - SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory - float _scurve_jerk; // scurve jerk max in m/s/s/s - float _scurve_snap; // scurve snap in m/s/s/s/s + SCurve _scurve_prev_leg; // previous scurve trajectory used to blend with current scurve trajectory + SCurve _scurve_this_leg; // current scurve trajectory + SCurve _scurve_next_leg; // next scurve trajectory used to blend with current scurve trajectory + float _scurve_jerk_max_msss; // scurve jerk max in m/s/s/s + float _scurve_snap_max_mssss; // scurve snap in m/s/s/s/s // spline curves - SplineCurve _spline_this_leg; // spline curve for current segment - SplineCurve _spline_next_leg; // spline curve for next segment + SplineCurve _spline_this_leg; // spline curve for current segment + SplineCurve _spline_next_leg; // spline curve for next segment // the type of this leg - bool _this_leg_is_spline; // true if this leg is a spline - bool _next_leg_is_spline; // true if the next leg is a spline + bool _this_leg_is_spline; // true if this leg is a spline + bool _next_leg_is_spline; // true if the next leg is a spline // waypoint controller internal variables - uint32_t _wp_last_update; // time of last update_wpnav call - float _wp_desired_speed_xy_cms; // desired wp speed in cm/sec - Vector3f _origin; // starting point of trip to next waypoint in cm from ekf origin - Vector3f _destination; // target destination in cm from ekf origin - Vector3f _next_destination; // next target destination in cm from ekf origin - float _track_scalar_dt; // time compression multiplier to slow the progress along the track - float _offset_vel; // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain - float _offset_accel; // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain - bool _paused; // flag for pausing waypoint controller + uint32_t _wp_last_update_ms; // time of last update_wpnav call (in ms) + float _wp_desired_speed_ne_cms; // desired wp speed in cm/sec + Vector3f _origin_neu_cm; // starting point of trip to next waypoint in cm from ekf origin + Vector3f _destination_neu_cm; // target destination in cm from ekf origin + Vector3f _next_destination_neu_cm; // next target destination in cm from ekf origin + float _track_scalar_dt; // time compression multiplier to slow the progress along the track + float _offset_vel_cms; // horizontal velocity reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain + float _offset_accel_cmss; // horizontal acceleration reference used to slow the aircraft for pause and to ensure the aircraft can maintain height above terrain + bool _paused; // flag for pausing waypoint controller // terrain following variables - bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin - bool _rangefinder_available; // true if rangefinder is enabled (user switch can turn this true/false) - AP_Int8 _rangefinder_use; // parameter that specifies if the range finder should be used for terrain following commands - bool _rangefinder_healthy; // true if rangefinder distance is healthy (i.e. between min and maximum) + bool _terrain_alt; // true if origin and destination.z are alt-above-terrain, false if alt-above-ekf-origin + bool _rangefinder_available; // true if rangefinder is enabled (user switch can turn this true/false) + AP_Int8 _rangefinder_use; // parameter that specifies if the range finder should be used for terrain following commands + bool _rangefinder_healthy; // true if rangefinder distance is healthy (i.e. between min and maximum) float _rangefinder_terrain_offset_cm; // latest rangefinder based terrain offset (e.g. terrain's height above EKF origin) }; diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index 069dc2b35ea..2d030c23294 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -25,12 +25,12 @@ bool AC_WPNav_OA::get_oa_wp_destination(Location& destination) const return true; } -/// set_wp_destination waypoint using position vector (distance from ekf origin in cm) +/// set_wp_destination_NEU_cm waypoint using position vector (distance from ekf origin in cm) /// terrain_alt should be true if destination.z is a desired altitude above terrain /// returns false on failure (likely caused by missing terrain data) -bool AC_WPNav_OA::set_wp_destination(const Vector3f& destination, bool terrain_alt) +bool AC_WPNav_OA::set_wp_destination_NEU_cm(const Vector3f& destination_neu_cm, bool terrain_alt) { - const bool ret = AC_WPNav::set_wp_destination(destination, terrain_alt); + const bool ret = AC_WPNav::set_wp_destination_NEU_cm(destination_neu_cm, terrain_alt); if (ret) { // reset object avoidance state @@ -42,24 +42,24 @@ bool AC_WPNav_OA::set_wp_destination(const Vector3f& destination, bool terrain_a /// get_wp_distance_to_destination - get horizontal distance to destination in cm /// always returns distance to final destination (i.e. does not use oa adjusted destination) -float AC_WPNav_OA::get_wp_distance_to_destination() const +float AC_WPNav_OA::get_wp_distance_to_destination_cm() const { if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) { - return AC_WPNav::get_wp_distance_to_destination(); + return AC_WPNav::get_wp_distance_to_destination_cm(); } - return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination_oabak.xy()); + return get_horizontal_distance_cm(_inav.get_position_xy_cm(), _destination_oabak_neu_cm.xy()); } /// get_wp_bearing_to_destination - get bearing to next waypoint in centi-degrees /// always returns bearing to final destination (i.e. does not use oa adjusted destination) -int32_t AC_WPNav_OA::get_wp_bearing_to_destination() const +int32_t AC_WPNav_OA::get_wp_bearing_to_destination_cd() const { if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) { - return AC_WPNav::get_wp_bearing_to_destination(); + return AC_WPNav::get_wp_bearing_to_destination_cd(); } - return get_bearing_cd(_inav.get_position_xy_cm(), _destination_oabak.xy()); + return get_bearing_cd(_inav.get_position_xy_cm(), _destination_oabak_neu_cm.xy()); } /// true when we have come within RADIUS cm of the waypoint @@ -76,18 +76,18 @@ bool AC_WPNav_OA::update_wpnav() Location current_loc; if ((oa_ptr != nullptr) && AP::ahrs().get_location(current_loc)) { - // backup _origin and _destination when not doing oa + // backup _origin and _destination_neu_cm when not doing oa if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) { - _origin_oabak = _origin; - _destination_oabak = _destination; + _origin_oabak_neu_cm = _origin_neu_cm; + _destination_oabak_neu_cm = _destination_neu_cm; _terrain_alt_oabak = _terrain_alt; - _next_destination_oabak = _next_destination; + _next_destination_oabak_neu_cm = _next_destination_neu_cm; } // convert origin, destination and next_destination to Locations and pass into oa - const Location origin_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); - const Location destination_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); - const Location next_destination_loc(_next_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + const Location origin_loc(_origin_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + const Location destination_loc(_destination_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + const Location next_destination_loc(_next_destination_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); Location oa_origin_new, oa_destination_new, oa_next_destination_new; bool dest_to_next_dest_clear = true; AP_OAPathPlanner::OAPathPlannerUsed path_planner_used = AP_OAPathPlanner::OAPathPlannerUsed::None; @@ -106,7 +106,7 @@ bool AC_WPNav_OA::update_wpnav() case AP_OAPathPlanner::OA_NOT_REQUIRED: if (_oa_state != oa_retstate) { // object avoidance has become inactive so reset target to original destination - if (!set_wp_destination(_destination_oabak, _terrain_alt_oabak)) { + if (!set_wp_destination_NEU_cm(_destination_oabak_neu_cm, _terrain_alt_oabak)) { // trigger terrain failsafe return false; } @@ -114,8 +114,8 @@ bool AC_WPNav_OA::update_wpnav() // if path from destination to next_destination is clear if (dest_to_next_dest_clear && (oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS)) { // set next destination if non-zero - if (!_next_destination_oabak.is_zero()) { - set_wp_destination_next(_next_destination_oabak); + if (!_next_destination_oabak_neu_cm.is_zero()) { + set_wp_destination_next_NEU_cm(_next_destination_oabak_neu_cm); } } _oa_state = oa_retstate; @@ -141,10 +141,10 @@ bool AC_WPNav_OA::update_wpnav() if ((_oa_state != AP_OAPathPlanner::OA_PROCESSING) && (_oa_state != AP_OAPathPlanner::OA_ERROR)) { // calculate stopping point Vector3f stopping_point; - get_wp_stopping_point(stopping_point); + get_wp_stopping_point_NEU_cm(stopping_point); _oa_destination = Location(stopping_point, Location::AltFrame::ABOVE_ORIGIN); _oa_next_destination.zero(); - if (set_wp_destination(stopping_point, false)) { + if (set_wp_destination_NEU_cm(stopping_point, false)) { _oa_state = oa_retstate; } } @@ -163,8 +163,8 @@ bool AC_WPNav_OA::update_wpnav() case AP_OAPathPlanner::OAPathPlannerUsed::Dijkstras: // Dijkstra's. Action is only needed if path planner has just became active or the target destination's lat or lon has changed if ((_oa_state != AP_OAPathPlanner::OA_SUCCESS) || !oa_destination_new.same_latlon_as(_oa_destination)) { - Location origin_oabak_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); - Location destination_oabak_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + Location origin_oabak_loc(_origin_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + Location destination_oabak_loc(_destination_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); oa_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc); // set new OA adjusted destination @@ -180,7 +180,7 @@ bool AC_WPNav_OA::update_wpnav() if ((oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS) && !oa_next_destination_new.is_zero()) { // calculate oa_next_destination_new's altitude using linear interpolation between original origin and destination // this "next destination" is still an intermediate point between the origin and destination - Location next_destination_oabak_loc(_next_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + Location next_destination_oabak_loc(_next_destination_oabak_neu_cm, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); oa_next_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc); if (set_wp_destination_next_loc(oa_next_destination_new)) { _oa_next_destination = oa_next_destination_new; @@ -200,7 +200,7 @@ bool AC_WPNav_OA::update_wpnav() // correct target_alt_loc's alt-above-ekf-origin if using terrain altitudes // positive terr_offset means terrain below vehicle is above ekf origin's altitude float terr_offset = 0; - if (_terrain_alt_oabak && !get_terrain_offset(terr_offset)) { + if (_terrain_alt_oabak && !get_terrain_offset_cm(terr_offset)) { // trigger terrain failsafe return false; } diff --git a/libraries/AC_WPNav/AC_WPNav_OA.h b/libraries/AC_WPNav/AC_WPNav_OA.h index 67569d14f4c..55de9195b83 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.h +++ b/libraries/AC_WPNav/AC_WPNav_OA.h @@ -22,15 +22,15 @@ public: /// set_wp_destination waypoint using position vector (distance from ekf origin in cm) /// terrain_alt should be true if destination.z is a desired altitude above terrain /// returns false on failure (likely caused by missing terrain data) - bool set_wp_destination(const Vector3f& destination, bool terrain_alt = false) override; + bool set_wp_destination_NEU_cm(const Vector3f& destination, bool terrain_alt = false) override; /// get horizontal distance to destination in cm /// always returns distance to final destination (i.e. does not use oa adjusted destination) - float get_wp_distance_to_destination() const override; + float get_wp_distance_to_destination_cm() const override; /// get bearing to next waypoint in centi-degrees /// always returns bearing to final destination (i.e. does not use oa adjusted destination) - int32_t get_wp_bearing_to_destination() const override; + int32_t get_wp_bearing_to_destination_cd() const override; /// true when we have come within RADIUS cm of the final destination bool reached_wp_destination() const override; @@ -42,9 +42,9 @@ protected: // oa path planning variables AP_OAPathPlanner::OA_RetState _oa_state; // state of object avoidance, if OA_SUCCESS we use _oa_destination to avoid obstacles - Vector3f _origin_oabak; // backup of _origin so it can be restored when oa completes - Vector3f _destination_oabak; // backup of _destination so it can be restored when oa completes - Vector3f _next_destination_oabak;// backup of _next_destination so it can be restored when oa completes + Vector3f _origin_oabak_neu_cm; // backup of _origin_neu_cm so it can be restored when oa completes + Vector3f _destination_oabak_neu_cm; // backup of _destination_neu_cm so it can be restored when oa completes + Vector3f _next_destination_oabak_neu_cm;// backup of _next_destination_neu_cm so it can be restored when oa completes bool _terrain_alt_oabak; // true if backup origin and destination z-axis are terrain altitudes Location _oa_destination; // intermediate destination during avoidance Location _oa_next_destination; // intermediate next destination during avoidance