diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 4f1413a0d16..f859c4d3463 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -95,7 +95,7 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // @Description: Position (vertical) controller P gain. Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller // @Range: 1.000 3.000 // @User: Standard - AP_SUBGROUPINFO(_p_pos_u_cm, "_POSZ_", 2, AC_PosControl, AC_P_1D), + AP_SUBGROUPINFO(_p_pos_u_m, "_POSZ_", 2, AC_PosControl, AC_P_1D), // @Param: _VELZ_P // @DisplayName: Velocity (vertical) controller P gain @@ -241,7 +241,7 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // @Description: Position controller P gain. Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller // @Range: 0.500 2.000 // @User: Standard - AP_SUBGROUPINFO(_p_pos_ne_cm, "_POSXY_", 5, AC_PosControl, AC_P_2D), + AP_SUBGROUPINFO(_p_pos_ne_m, "_POSXY_", 5, AC_PosControl, AC_P_2D), // @Param: _VELXY_P // @DisplayName: Velocity (horizontal) P gain @@ -312,7 +312,7 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // @Range: 1 20 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("_JERK_XY", 10, AC_PosControl, _shaping_jerk_ne_msss, POSCONTROL_JERK_NE), + AP_GROUPINFO("_JERK_XY", 10, AC_PosControl, _shaping_jerk_ne_msss, POSCONTROL_JERK_NE_MSSS), // @Param: _JERK_Z // @DisplayName: Jerk limit for the vertical kinematic input shaping @@ -321,7 +321,7 @@ const AP_Param::GroupInfo AC_PosControl::var_info[] = { // @Range: 5 50 // @Increment: 1 // @User: Advanced - AP_GROUPINFO("_JERK_Z", 11, AC_PosControl, _shaping_jerk_u_msss, POSCONTROL_JERK_U), + AP_GROUPINFO("_JERK_Z", 11, AC_PosControl, _shaping_jerk_u_msss, POSCONTROL_JERK_U_MSSS), AP_GROUPEND }; @@ -334,18 +334,18 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_Motors& motors, AC_Att _ahrs(ahrs), _motors(motors), _attitude_control(attitude_control), - _p_pos_ne_cm(POSCONTROL_POS_XY_P), - _p_pos_u_cm(POSCONTROL_POS_Z_P), + _p_pos_ne_m(POSCONTROL_POS_XY_P), + _p_pos_u_m(POSCONTROL_POS_Z_P), _pid_vel_ne_cm(POSCONTROL_VEL_XY_P, POSCONTROL_VEL_XY_I, POSCONTROL_VEL_XY_D, 0.0f, POSCONTROL_VEL_XY_IMAX, POSCONTROL_VEL_XY_FILT_HZ, POSCONTROL_VEL_XY_FILT_D_HZ), _pid_vel_u_cm(POSCONTROL_VEL_Z_P, 0.0f, 0.0f, 0.0f, POSCONTROL_VEL_Z_IMAX, POSCONTROL_VEL_Z_FILT_HZ, POSCONTROL_VEL_Z_FILT_D_HZ), _pid_accel_u_cm_to_kt(POSCONTROL_ACC_Z_P, POSCONTROL_ACC_Z_I, POSCONTROL_ACC_Z_D, 0.0f, POSCONTROL_ACC_Z_IMAX, 0.0f, POSCONTROL_ACC_Z_FILT_HZ, 0.0f), - _vel_max_ne_cms(POSCONTROL_SPEED), - _vel_max_up_cms(POSCONTROL_SPEED_UP), - _vel_max_down_cms(POSCONTROL_SPEED_DOWN), - _accel_max_ne_cmss(POSCONTROL_ACCEL_NE), - _accel_max_u_cmss(POSCONTROL_ACCEL_U), - _jerk_max_ne_cmsss(POSCONTROL_JERK_NE * 100.0), - _jerk_max_u_cmsss(POSCONTROL_JERK_U * 100.0) + _vel_max_ne_ms(POSCONTROL_SPEED_MS), + _vel_max_up_ms(POSCONTROL_SPEED_UP_MS), + _vel_max_down_ms(POSCONTROL_SPEED_DOWN_MS), + _accel_max_ne_mss(POSCONTROL_ACCEL_NE_MSS), + _accel_max_u_mss(POSCONTROL_ACCEL_U_MSS), + _jerk_max_ne_msss(POSCONTROL_JERK_NE_MSSS), + _jerk_max_u_msss(POSCONTROL_JERK_U_MSSS) { AP_Param::setup_object_defaults(this, var_info); @@ -357,116 +357,132 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_Motors& motors, AC_Att /// 3D position shaper /// -/// input_pos_NEU_cm - computes a jerk-limited trajectory from the current NEU position, velocity, and acceleration to a new position input (in cm). +/// input_pos_NEU_m - computes a jerk-limited trajectory from the current NEU position, velocity, and acceleration to a new position input (in cm). /// This function updates the desired acceleration using a smooth kinematic path constrained by acceleration and jerk limits. /// The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// The jerk limit also defines the time taken to achieve the maximum acceleration. /// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time. void AC_PosControl::input_pos_NEU_cm(const Vector3p& pos_neu_cm, float pos_terrain_target_u_cm, float terrain_buffer_cm) +{ + input_pos_NEU_m(pos_neu_cm * 0.01, pos_terrain_target_u_cm * 0.01, terrain_buffer_cm * 0.01); +} +void AC_PosControl::input_pos_NEU_m(const Vector3p& pos_neu_m, float pos_terrain_target_u_m, float terrain_buffer_m) { // Terrain following velocity scalar must be calculated before we remove the position offset - const float offset_u_scalar = pos_terrain_U_scaler_cm(pos_terrain_target_u_cm, terrain_buffer_cm); - set_pos_terrain_target_U_cm(pos_terrain_target_u_cm); + const float offset_u_scalar = pos_terrain_U_scaler_m(pos_terrain_target_u_m, terrain_buffer_m); + set_pos_terrain_target_U_m(pos_terrain_target_u_m); // calculated increased maximum acceleration and jerk if over speed const float overspeed_gain = calculate_overspeed_gain(); - const float accel_max_u_cmss = _accel_max_u_cmss * overspeed_gain; - const float jerk_max_u_cmsss = _jerk_max_u_cmsss * overspeed_gain; + const float accel_max_u_mss = _accel_max_u_mss * overspeed_gain; + const float jerk_max_u_msss = _jerk_max_u_msss * overspeed_gain; - update_pos_vel_accel_xy(_pos_desired_neu_cm.xy(), _vel_desired_neu_cms.xy(), _accel_desired_neu_cmss.xy(), _dt_s, _limit_vector.xy(), _p_pos_ne_cm.get_error(), _pid_vel_ne_cm.get_error()); + update_pos_vel_accel_xy(_pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), _dt_s, _limit_vector_neu.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_cm.get_error()); // adjust desired altitude if motors have not hit their limits - update_pos_vel_accel(_pos_desired_neu_cm.z, _vel_desired_neu_cms.z, _accel_desired_neu_cmss.z, _dt_s, _limit_vector.z, _p_pos_u_cm.get_error(), _pid_vel_u_cm.get_error()); + update_pos_vel_accel(_pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, _dt_s, _limit_vector_neu.z, _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); - // calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos_ne_cm - float vel_max_ne_cms = 0.0f; - float vel_max_u_cms = 0.0f; - Vector3f travel_dir_unit = (pos_neu_cm - _pos_desired_neu_cm).tofloat(); + // calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos_ne_m + float vel_max_ne_ms = 0.0f; + float vel_max_u_ms = 0.0f; + Vector3f travel_dir_unit = (pos_neu_m - _pos_desired_neu_m).tofloat(); if (is_positive(travel_dir_unit.length_squared()) ) { travel_dir_unit.normalize(); float travel_dir_unit_ne_length = travel_dir_unit.xy().length(); - float vel_max_cms = kinematic_limit(travel_dir_unit, _vel_max_ne_cms, _vel_max_up_cms, _vel_max_down_cms); - vel_max_ne_cms = vel_max_cms * travel_dir_unit_ne_length; - vel_max_u_cms = fabsf(vel_max_cms * travel_dir_unit.z); + float vel_max_ms = kinematic_limit(travel_dir_unit, _vel_max_ne_ms, _vel_max_up_ms, _vel_max_down_ms); + vel_max_ne_ms = vel_max_ms * travel_dir_unit_ne_length; + vel_max_u_ms = fabsf(vel_max_ms * travel_dir_unit.z); } // reduce speed if we are reaching the edge of our vertical buffer - vel_max_ne_cms *= offset_u_scalar; + vel_max_ne_ms *= offset_u_scalar; - Vector2f vel_ne_cms; - Vector2f accel_ne_cmss; - shape_pos_vel_accel_xy(pos_neu_cm.xy(), vel_ne_cms, accel_ne_cmss, _pos_desired_neu_cm.xy(), _vel_desired_neu_cms.xy(), _accel_desired_neu_cmss.xy(), - vel_max_ne_cms, _accel_max_ne_cmss, _jerk_max_ne_cmsss, _dt_s, false); + Vector2f vel_ne_ms; + Vector2f accel_ne_mss; + shape_pos_vel_accel_xy(pos_neu_m.xy(), vel_ne_ms, accel_ne_mss, _pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), + vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, false); - float pos_u_cm = pos_neu_cm.z; - shape_pos_vel_accel(pos_u_cm, 0, 0, - _pos_desired_neu_cm.z, _vel_desired_neu_cms.z, _accel_desired_neu_cmss.z, - -vel_max_u_cms, vel_max_u_cms, - -constrain_float(accel_max_u_cmss, 0.0f, 750.0f), accel_max_u_cmss, - jerk_max_u_cmsss, _dt_s, false); + float pos_u_m = pos_neu_m.z; + shape_pos_vel_accel(pos_u_m, 0, 0, + _pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, + -vel_max_u_ms, vel_max_u_ms, + -constrain_float(accel_max_u_mss, 0.0, 7.5), accel_max_u_mss, + jerk_max_u_msss, _dt_s, false); } -/// pos_terrain_U_scaler_cm - computes a scaling factor applied to horizontal velocity limits to ensure the vertical position controller remains within its terrain buffer. +/// pos_terrain_U_scaler_m - computes a scaling factor applied to horizontal velocity limits to ensure the vertical position controller remains within its terrain buffer. float AC_PosControl::pos_terrain_U_scaler_cm(float pos_terrain_u_cm, float pos_terrain_u_buffer_cm) const { - if (is_zero(pos_terrain_u_buffer_cm)) { + return pos_terrain_U_scaler_m(pos_terrain_u_cm * 0.01, pos_terrain_u_buffer_cm * 0.01); +} +float AC_PosControl::pos_terrain_U_scaler_m(float pos_terrain_u_m, float pos_terrain_u_buffer_m) const +{ + if (is_zero(pos_terrain_u_buffer_m)) { return 1.0; } - float pos_offset_error_u_cm = _pos_estimate_neu_cm.z - (_pos_target_neu_cm.z + (pos_terrain_u_cm - _pos_terrain_u_cm)); - return constrain_float((1.0 - (fabsf(pos_offset_error_u_cm) - 0.5 * pos_terrain_u_buffer_cm) / (0.5 * pos_terrain_u_buffer_cm)), 0.01, 1.0); + float pos_offset_error_u_m = _pos_estimate_neu_m.z - (_pos_target_neu_m.z + (pos_terrain_u_m - _pos_terrain_u_m)); + return constrain_float((1.0 - (fabsf(pos_offset_error_u_m) - 0.5 * pos_terrain_u_buffer_m) / (0.5 * pos_terrain_u_buffer_m)), 0.01, 1.0); } /// /// Lateral position controller /// -/// set_max_speed_accel_NE_cm - set the maximum horizontal speed in cm/s and acceleration in cm/s/s +/// set_max_speed_accel_NE_m - set the maximum horizontal speed in cm/s and acceleration in cm/s/s /// This function only needs to be called if using the kinematic shaping. /// This can be done at any time as changes in these parameters are handled smoothly /// by the kinematic shaping. void AC_PosControl::set_max_speed_accel_NE_cm(float speed_ne_cms, float accel_ne_cmss) { - _vel_max_ne_cms = speed_ne_cms; - _accel_max_ne_cmss = accel_ne_cmss; + set_max_speed_accel_NE_m(speed_ne_cms * 0.01, accel_ne_cmss * 0.01); +} +void AC_PosControl::set_max_speed_accel_NE_m(float speed_ne_ms, float accel_ne_mss) +{ + _vel_max_ne_ms = speed_ne_ms; + _accel_max_ne_mss = accel_ne_mss; // ensure the horizontal jerk is less than the vehicle is capable of - const float jerk_max_cmsss = MIN(_attitude_control.get_ang_vel_roll_max_rads(), _attitude_control.get_ang_vel_pitch_max_rads()) * GRAVITY_MSS * 100.0; - const float snap_max_cmssss = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS * 100.0; + const float jerk_max_msss = MIN(_attitude_control.get_ang_vel_roll_max_rads(), _attitude_control.get_ang_vel_pitch_max_rads()) * GRAVITY_MSS; + const float snap_max_mssss = MIN(_attitude_control.get_accel_roll_max_radss(), _attitude_control.get_accel_pitch_max_radss()) * GRAVITY_MSS; // get specified jerk limit - _jerk_max_ne_cmsss = _shaping_jerk_ne_msss * 100.0; + _jerk_max_ne_msss = _shaping_jerk_ne_msss; // limit maximum jerk based on maximum angular rate - if (is_positive(jerk_max_cmsss) && _attitude_control.get_bf_feedforward()) { - _jerk_max_ne_cmsss = MIN(_jerk_max_ne_cmsss, jerk_max_cmsss); + if (is_positive(jerk_max_msss) && _attitude_control.get_bf_feedforward()) { + _jerk_max_ne_msss = MIN(_jerk_max_ne_msss, jerk_max_msss); } // limit maximum jerk to maximum possible average jerk based on angular acceleration - if (is_positive(snap_max_cmssss) && _attitude_control.get_bf_feedforward()) { - _jerk_max_ne_cmsss = MIN(0.5 * safe_sqrt(_accel_max_ne_cmss * snap_max_cmssss), _jerk_max_ne_cmsss); + if (is_positive(snap_max_mssss) && _attitude_control.get_bf_feedforward()) { + _jerk_max_ne_msss = MIN(0.5 * safe_sqrt(_accel_max_ne_mss * snap_max_mssss), _jerk_max_ne_msss); } } -/// set_correction_speed_accel_xy_cm - set the position controller correction velocity and acceleration limit +/// set_correction_speed_accel_xy_m - set the position controller correction velocity and acceleration limit /// This should be done only during initialisation to avoid discontinuities void AC_PosControl::set_correction_speed_accel_NE_cm(float speed_ne_cms, float accel_ne_cmss) { - _p_pos_ne_cm.set_limits(speed_ne_cms, accel_ne_cmss, 0.0f); + set_correction_speed_accel_NE_m(speed_ne_cms * 0.01, accel_ne_cmss * 0.01); +} +void AC_PosControl::set_correction_speed_accel_NE_m(float speed_ne_ms, float accel_ne_mss) +{ + _p_pos_ne_m.set_limits(speed_ne_ms, accel_ne_mss, 0.0f); } /// init_NE_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration. /// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position. -/// The starting position can be retrieved by getting the position target using get_pos_desired_NEU_cm() after calling this function. +/// The starting position can be retrieved by getting the position target using get_pos_desired_NEU_m() after calling this function. void AC_PosControl::init_NE_controller_stopping_point() { init_NE_controller(); - get_stopping_point_NE_cm(_pos_desired_neu_cm.xy()); - _pos_target_neu_cm.xy() = _pos_desired_neu_cm.xy() + _pos_offset_neu_cm.xy(); - _vel_desired_neu_cms.xy().zero(); - _accel_desired_neu_cmss.xy().zero(); + get_stopping_point_NE_m(_pos_desired_neu_m.xy()); + _pos_target_neu_m.xy() = _pos_desired_neu_m.xy() + _pos_offset_neu_m.xy(); + _vel_desired_neu_ms.xy().zero(); + _accel_desired_neu_mss.xy().zero(); } // relax_velocity_controller_NE - initialise the position controller to the current position and velocity with decaying acceleration. @@ -477,7 +493,7 @@ void AC_PosControl::relax_velocity_controller_NE() // this will be reset by init_NE_controller() if !is_active_NE() if (is_positive(_dt_s)) { float decay = 1.0 - _dt_s / (_dt_s + POSCONTROL_RELAX_TC); - _accel_target_neu_cmss.xy() *= decay; + _accel_target_neu_mss.xy() *= decay; } init_NE_controller(); @@ -488,8 +504,8 @@ void AC_PosControl::soften_for_landing_NE() { // decay position error to zero if (is_positive(_dt_s)) { - _pos_target_neu_cm.xy() += (_pos_estimate_neu_cm.xy() - _pos_target_neu_cm.xy()) * (_dt_s / (_dt_s + POSCONTROL_RELAX_TC)); - _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); + _pos_target_neu_m.xy() += (_pos_estimate_neu_m.xy() - _pos_target_neu_m.xy()) * (_dt_s / (_dt_s + POSCONTROL_RELAX_TC)); + _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); } // Prevent I term build up in xy velocity controller. @@ -512,29 +528,29 @@ void AC_PosControl::init_NE_controller() _yaw_rate_target_rads = 0.0f; _angle_max_override_rad = 0.0; - _pos_target_neu_cm.xy() = _pos_estimate_neu_cm.xy(); - _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); + _pos_target_neu_m.xy() = _pos_estimate_neu_m.xy(); + _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); - _vel_target_neu_cms.xy() = _vel_estimate_neu_cms.xy(); - _vel_desired_neu_cms.xy() = _vel_target_neu_cms.xy() - _vel_offset_neu_cms.xy(); + _vel_target_neu_ms.xy() = _vel_estimate_neu_ms.xy(); + _vel_desired_neu_ms.xy() = _vel_target_neu_ms.xy() - _vel_offset_neu_ms.xy(); // Set desired acceleration to zero because raw acceleration is prone to noise - _accel_desired_neu_cmss.xy().zero(); + _accel_desired_neu_mss.xy().zero(); if (!is_active_NE()) { - lean_angles_to_accel_NE_cmss(_accel_target_neu_cmss.x, _accel_target_neu_cmss.y); + lean_angles_to_accel_NE_mss(_accel_target_neu_mss.x, _accel_target_neu_mss.y); } // limit acceleration using maximum lean angles const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad()); - const float accel_max_cmss = angle_rad_to_accel_mss(angle_max_rad) * 100.0; - _accel_target_neu_cmss.xy().limit_length(accel_max_cmss); + const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad); + _accel_target_neu_mss.xy().limit_length(accel_max_mss); // initialise I terms from lean angles _pid_vel_ne_cm.reset_filter(); - // initialise the I term to (_accel_target_neu_cmss - _accel_desired_neu_cmss) - // _accel_desired_neu_cmss is zero and can be removed from the equation - _pid_vel_ne_cm.set_integrator(_accel_target_neu_cmss.xy() - _vel_target_neu_cms.xy() * _pid_vel_ne_cm.ff()); + // initialise the I term to (_accel_target_neu_mss - _accel_desired_neu_mss) + // _accel_desired_neu_mss is zero and can be removed from the equation + _pid_vel_ne_cm.set_integrator((_accel_target_neu_mss.xy() - _vel_target_neu_ms.xy() * _pid_vel_ne_cm.ff()) * 100.0); // initialise ekf xy reset handler init_ekf_NE_reset(); @@ -543,84 +559,102 @@ void AC_PosControl::init_NE_controller() _last_update_ne_ticks = AP::scheduler().ticks32(); } -/// input_accel_NE_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. +/// input_accel_NE_m - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_ne. /// The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// The jerk limit also defines the time taken to achieve the maximum acceleration. void AC_PosControl::input_accel_NE_cm(const Vector3f& accel_neu_cmss) { - update_pos_vel_accel_xy(_pos_desired_neu_cm.xy(), _vel_desired_neu_cms.xy(), _accel_desired_neu_cmss.xy(), _dt_s, _limit_vector.xy(), _p_pos_ne_cm.get_error(), _pid_vel_ne_cm.get_error()); - shape_accel_xy(accel_neu_cmss.xy(), _accel_desired_neu_cmss.xy(), _jerk_max_ne_cmsss, _dt_s); + input_accel_NE_m(accel_neu_cmss * 0.01); +} +void AC_PosControl::input_accel_NE_m(const Vector3f& accel_neu_mss) +{ + update_pos_vel_accel_xy(_pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), _dt_s, _limit_vector_neu.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_cm.get_error()); + shape_accel_xy(accel_neu_mss.xy(), _accel_desired_neu_mss.xy(), _jerk_max_ne_msss, _dt_s); } -/// input_vel_accel_NE_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. +/// input_vel_accel_NE_m - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_ne. -/// The function modifies vel_ne_cms to follow the kinematic trajectory toward accel_cmss. +/// The function modifies vel_ne_ms to follow the kinematic trajectory toward accel_mss. /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. void AC_PosControl::input_vel_accel_NE_cm(Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output) { - update_pos_vel_accel_xy(_pos_desired_neu_cm.xy(), _vel_desired_neu_cms.xy(), _accel_desired_neu_cmss.xy(), _dt_s, _limit_vector.xy(), _p_pos_ne_cm.get_error(), _pid_vel_ne_cm.get_error()); + Vector2f vel_ne_ms = vel_ne_cms * 0.01; + input_vel_accel_NE_m(vel_ne_ms, accel_ne_cmss * 0.01, limit_output); + vel_ne_cms = vel_ne_ms * 100.0; +} +void AC_PosControl::input_vel_accel_NE_m(Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output) +{ + update_pos_vel_accel_xy(_pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), _dt_s, _limit_vector_neu.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_cm.get_error()); - shape_vel_accel_xy(vel_ne_cms, accel_ne_cmss, _vel_desired_neu_cms.xy(), _accel_desired_neu_cmss.xy(), - _accel_max_ne_cmss, _jerk_max_ne_cmsss, _dt_s, limit_output); + shape_vel_accel_xy(vel_ne_ms, accel_ne_mss, _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), + _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, limit_output); - update_vel_accel_xy(vel_ne_cms, accel_ne_cmss, _dt_s, Vector2f(), Vector2f()); + update_vel_accel_xy(vel_ne_ms, accel_ne_mss, _dt_s, Vector2f(), Vector2f()); } -/// input_pos_vel_accel_NE_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. +/// input_pos_vel_accel_NE_m - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_ne. -/// The function modifies pos_ne_cm and vel_ne_cms to follow the jerk-limited trajectory defined by accel_ne_cmss. +/// The function modifies pos_ne_m and vel_ne_ms to follow the jerk-limited trajectory defined by accel_ne_mss. /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. void AC_PosControl::input_pos_vel_accel_NE_cm(Vector2p& pos_ne_cm, Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output) { - update_pos_vel_accel_xy(_pos_desired_neu_cm.xy(), _vel_desired_neu_cms.xy(), _accel_desired_neu_cmss.xy(), _dt_s, _limit_vector.xy(), _p_pos_ne_cm.get_error(), _pid_vel_ne_cm.get_error()); + Vector2p pos_ne_m = pos_ne_cm * 0.01; + Vector2f vel_ne_ms = vel_ne_cms * 0.01; + input_pos_vel_accel_NE_m(pos_ne_m, vel_ne_ms, accel_ne_cmss * 0.01, limit_output); + pos_ne_cm = pos_ne_m * 100.0; + vel_ne_cms = vel_ne_ms * 100.0; +} +void AC_PosControl::input_pos_vel_accel_NE_m(Vector2p& pos_ne_m, Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output) +{ + update_pos_vel_accel_xy(_pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), _dt_s, _limit_vector_neu.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_cm.get_error()); - shape_pos_vel_accel_xy(pos_ne_cm, vel_ne_cms, accel_ne_cmss, _pos_desired_neu_cm.xy(), _vel_desired_neu_cms.xy(), _accel_desired_neu_cmss.xy(), - _vel_max_ne_cms, _accel_max_ne_cmss, _jerk_max_ne_cmsss, _dt_s, limit_output); + shape_pos_vel_accel_xy(pos_ne_m, vel_ne_ms, accel_ne_mss, _pos_desired_neu_m.xy(), _vel_desired_neu_ms.xy(), _accel_desired_neu_mss.xy(), + _vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, limit_output); - update_pos_vel_accel_xy(pos_ne_cm, vel_ne_cms, accel_ne_cmss, _dt_s, Vector2f(), Vector2f(), Vector2f()); + update_pos_vel_accel_xy(pos_ne_m, vel_ne_ms, accel_ne_mss, _dt_s, Vector2f(), Vector2f(), Vector2f()); } /// update the horizontal position and velocity offsets -/// this moves the offsets (e.g _pos_offset_neu_cm, _vel_offset_neu_cms, _accel_offset_neu_cmss) towards the targets (e.g. _pos_offset_target_neu_cm, _vel_offset_target_neu_cms, _accel_offset_target_neu_cmss) +/// this moves the offsets (e.g _pos_offset_neu_m, _vel_offset_neu_ms, _accel_offset_neu_mss) towards the targets (e.g. _pos_offset_target_neu_m, _vel_offset_target_neu_ms, _accel_offset_target_neu_mss) void AC_PosControl::update_offsets_NE() { // check for offset target timeout uint32_t now_ms = AP_HAL::millis(); if (now_ms - _posvelaccel_offset_target_ne_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { - _pos_offset_target_neu_cm.xy().zero(); - _vel_offset_target_neu_cms.xy().zero(); - _accel_offset_target_neu_cmss.xy().zero(); + _pos_offset_target_neu_m.xy().zero(); + _vel_offset_target_neu_ms.xy().zero(); + _accel_offset_target_neu_mss.xy().zero(); } // update position, velocity, acceleration offsets for this iteration - update_pos_vel_accel_xy(_pos_offset_target_neu_cm.xy(), _vel_offset_target_neu_cms.xy(), _accel_offset_target_neu_cmss.xy(), _dt_s, Vector2f(), Vector2f(), Vector2f()); - update_pos_vel_accel_xy(_pos_offset_neu_cm.xy(), _vel_offset_neu_cms.xy(), _accel_offset_neu_cmss.xy(), _dt_s, _limit_vector.xy(), _p_pos_ne_cm.get_error(), _pid_vel_ne_cm.get_error()); + update_pos_vel_accel_xy(_pos_offset_target_neu_m.xy(), _vel_offset_target_neu_ms.xy(), _accel_offset_target_neu_mss.xy(), _dt_s, Vector2f(), Vector2f(), Vector2f()); + update_pos_vel_accel_xy(_pos_offset_neu_m.xy(), _vel_offset_neu_ms.xy(), _accel_offset_neu_mss.xy(), _dt_s, _limit_vector_neu.xy(), _p_pos_ne_m.get_error(), _pid_vel_ne_cm.get_error()); // input shape horizontal position, velocity and acceleration offsets - shape_pos_vel_accel_xy(_pos_offset_target_neu_cm.xy(), _vel_offset_target_neu_cms.xy(), _accel_offset_target_neu_cmss.xy(), - _pos_offset_neu_cm.xy(), _vel_offset_neu_cms.xy(), _accel_offset_neu_cmss.xy(), - _vel_max_ne_cms, _accel_max_ne_cmss, _jerk_max_ne_cmsss, _dt_s, false); + shape_pos_vel_accel_xy(_pos_offset_target_neu_m.xy(), _vel_offset_target_neu_ms.xy(), _accel_offset_target_neu_mss.xy(), + _pos_offset_neu_m.xy(), _vel_offset_neu_ms.xy(), _accel_offset_neu_mss.xy(), + _vel_max_ne_ms, _accel_max_ne_mss, _jerk_max_ne_msss, _dt_s, false); } /// stop_pos_NE_stabilisation - sets the target to the current position to remove any position corrections from the system void AC_PosControl::stop_pos_NE_stabilisation() { - _pos_target_neu_cm.xy() = _pos_estimate_neu_cm.xy(); - _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); + _pos_target_neu_m.xy() = _pos_estimate_neu_m.xy(); + _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); } /// stop_vel_NE_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system void AC_PosControl::stop_vel_NE_stabilisation() { - _pos_target_neu_cm.xy() = _pos_estimate_neu_cm.xy(); - _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); + _pos_target_neu_m.xy() = _pos_estimate_neu_m.xy(); + _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); - _vel_target_neu_cms.xy() = _vel_estimate_neu_cms.xy(); - _vel_desired_neu_cms.xy() = _vel_target_neu_cms.xy() - _vel_offset_neu_cms.xy(); + _vel_target_neu_ms.xy() = _vel_estimate_neu_ms.xy(); + _vel_desired_neu_ms.xy() = _vel_target_neu_ms.xy() - _vel_offset_neu_ms.xy(); // initialise I terms from lean angles _pid_vel_ne_cm.reset_filter(); @@ -661,61 +695,61 @@ void AC_PosControl::update_NE_controller() // Position Controller - _pos_target_neu_cm.xy() = _pos_desired_neu_cm.xy() + _pos_offset_neu_cm.xy(); + _pos_target_neu_m.xy() = _pos_desired_neu_m.xy() + _pos_offset_neu_m.xy(); // determine the combined position of the actual position and the disturbance from system ID mode // calculate the target velocity correction - Vector2p comb_pos = _pos_estimate_neu_cm.xy(); - comb_pos += _disturb_pos_ne_cm.topostype(); + Vector2p comb_pos_ne_m = _pos_estimate_neu_m.xy(); + comb_pos_ne_m += _disturb_pos_ne_m.topostype(); - Vector2f vel_target = _p_pos_ne_cm.update_all(_pos_target_neu_cm.xy(), comb_pos); - _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); + Vector2f vel_target_ne_ms = _p_pos_ne_m.update_all(_pos_target_neu_m.xy(), comb_pos_ne_m); + _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); // Velocity Controller // add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise - vel_target *= ahrsControlScaleXY; - vel_target *= _ne_control_scale_factor; + vel_target_ne_ms *= ahrsControlScaleXY; + vel_target_ne_ms *= _ne_control_scale_factor; - _vel_target_neu_cms.xy() = vel_target; - _vel_target_neu_cms.xy() += _vel_desired_neu_cms.xy() + _vel_offset_neu_cms.xy(); + _vel_target_neu_ms.xy() = vel_target_ne_ms; + _vel_target_neu_ms.xy() += _vel_desired_neu_ms.xy() + _vel_offset_neu_ms.xy(); // determine the combined velocity of the actual velocity and the disturbance from system ID mode // Velocity Controller - Vector2f comb_vel = _vel_estimate_neu_cms.xy(); - comb_vel += _disturb_vel_ne_cms; + Vector2f comb_vel_ne_ms = _vel_estimate_neu_ms.xy(); + comb_vel_ne_ms += _disturb_vel_ne_ms; - Vector2f accel_target_ne_cmss = _pid_vel_ne_cm.update_all(_vel_target_neu_cms.xy(), comb_vel, _dt_s, _limit_vector.xy()); + Vector2f accel_target_ne_mss = _pid_vel_ne_cm.update_all(_vel_target_neu_ms.xy() * 100.0, comb_vel_ne_ms * 100.0, _dt_s, _limit_vector_neu.xy()) * 0.01; // Acceleration Controller // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise - accel_target_ne_cmss *= ahrsControlScaleXY; - accel_target_ne_cmss *= _ne_control_scale_factor; + accel_target_ne_mss *= ahrsControlScaleXY; + accel_target_ne_mss *= _ne_control_scale_factor; _ne_control_scale_factor = 1.0; // pass the correction acceleration to the target acceleration output - _accel_target_neu_cmss.xy() = accel_target_ne_cmss; - _accel_target_neu_cmss.xy() += _accel_desired_neu_cmss.xy() + _accel_offset_neu_cmss.xy(); + _accel_target_neu_mss.xy() = accel_target_ne_mss; + _accel_target_neu_mss.xy() += _accel_desired_neu_mss.xy() + _accel_offset_neu_mss.xy(); // limit acceleration using maximum lean angles const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad()); - const float accel_max_cmss = angle_rad_to_accel_mss(angle_max_rad) * 100.0; - // Define the limit vector before we constrain _accel_target_neu_cmss - _limit_vector.xy() = _accel_target_neu_cmss.xy(); - if (!limit_accel_xy(_vel_desired_neu_cms.xy(), _accel_target_neu_cmss.xy(), accel_max_cmss)) { - // _accel_target_neu_cmss was not limited so we can zero the xy limit vector - _limit_vector.xy().zero(); + const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad); + // Define the limit vector before we constrain _accel_target_neu_mss + _limit_vector_neu.xy() = _accel_target_neu_mss.xy(); + if (!limit_accel_xy(_vel_desired_neu_ms.xy(), _accel_target_neu_mss.xy(), accel_max_mss)) { + // _accel_target_neu_mss was not limited so we can zero the xy limit vector + _limit_vector_neu.xy().zero(); } // update angle targets that will be passed to stabilize controller - accel_NE_cmss_to_lean_angles_rad(_accel_target_neu_cmss.x, _accel_target_neu_cmss.y, _roll_target_rad, _pitch_target_rad); + accel_NE_mss_to_lean_angles_rad(_accel_target_neu_mss.x, _accel_target_neu_mss.y, _roll_target_rad, _pitch_target_rad); calculate_yaw_and_rate_yaw(); // reset the disturbance from system ID mode to zero - _disturb_pos_ne_cm.zero(); - _disturb_vel_ne_cms.zero(); + _disturb_pos_ne_m.zero(); + _disturb_vel_ne_ms.zero(); } @@ -723,44 +757,52 @@ void AC_PosControl::update_NE_controller() /// Vertical position controller /// -/// set_max_speed_accel_U_cm - set the maximum vertical speed in cm/s and acceleration in cm/s/s -/// speed_down_cms can be positive or negative but will always be interpreted as a descent speed. +/// set_max_speed_accel_U_m - set the maximum vertical speed in m/s and acceleration in m/s/s +/// speed_down_ms can be positive or negative but will always be interpreted as a descent speed. /// This function only needs to be called if using the kinematic shaping. /// This can be done at any time as changes in these parameters are handled smoothly /// by the kinematic shaping. void AC_PosControl::set_max_speed_accel_U_cm(float speed_down_cms, float speed_up_cms, float accel_cmss) { - // ensure speed_down_cms is always negative - speed_down_cms = -fabsf(speed_down_cms); + set_max_speed_accel_U_m(speed_down_cms * 0.01, speed_up_cms * 0.01, accel_cmss * 0.01); +} +void AC_PosControl::set_max_speed_accel_U_m(float speed_down_ms, float speed_up_ms, float accel_mss) +{ + // ensure speed_down_ms is always negative + speed_down_ms = -fabsf(speed_down_ms); // sanity check and update - if (is_negative(speed_down_cms)) { - _vel_max_down_cms = speed_down_cms; + if (is_negative(speed_down_ms)) { + _vel_max_down_ms = speed_down_ms; } - if (is_positive(speed_up_cms)) { - _vel_max_up_cms = speed_up_cms; + if (is_positive(speed_up_ms)) { + _vel_max_up_ms = speed_up_ms; } - if (is_positive(accel_cmss)) { - _accel_max_u_cmss = accel_cmss; + if (is_positive(accel_mss)) { + _accel_max_u_mss = accel_mss; } // ensure the vertical Jerk is not limited by the filters in the Z acceleration PID object - _jerk_max_u_cmsss = _shaping_jerk_u_msss * 100.0; + _jerk_max_u_msss = _shaping_jerk_u_msss; if (is_positive(_pid_accel_u_cm_to_kt.filt_T_hz())) { - _jerk_max_u_cmsss = MIN(_jerk_max_u_cmsss, MIN(GRAVITY_MSS * 100.0, _accel_max_u_cmss) * (M_2PI * _pid_accel_u_cm_to_kt.filt_T_hz()) / 5.0); + _jerk_max_u_msss = MIN(_jerk_max_u_msss, MIN(GRAVITY_MSS, _accel_max_u_mss) * (M_2PI * _pid_accel_u_cm_to_kt.filt_T_hz()) / 5.0); } if (is_positive(_pid_accel_u_cm_to_kt.filt_E_hz())) { - _jerk_max_u_cmsss = MIN(_jerk_max_u_cmsss, MIN(GRAVITY_MSS * 100.0, _accel_max_u_cmss) * (M_2PI * _pid_accel_u_cm_to_kt.filt_E_hz()) / 5.0); + _jerk_max_u_msss = MIN(_jerk_max_u_msss, MIN(GRAVITY_MSS, _accel_max_u_mss) * (M_2PI * _pid_accel_u_cm_to_kt.filt_E_hz()) / 5.0); } } -/// set_correction_speed_accel_U_cmss - set the position controller correction velocity and acceleration limit -/// speed_down_cms can be positive or negative but will always be interpreted as a descent speed. +/// set_correction_speed_accel_U_mss - set the position controller correction velocity and acceleration limit +/// speed_down_ms can be positive or negative but will always be interpreted as a descent speed. /// This should be done only during initialisation to avoid discontinuities void AC_PosControl::set_correction_speed_accel_U_cmss(float speed_down_cms, float speed_up_cms, float accel_cmss) +{ + set_correction_speed_accel_U_mss(speed_down_cms * 0.01, speed_up_cms * 0.01, accel_cmss * 0.01); +} +void AC_PosControl::set_correction_speed_accel_U_mss(float speed_down_ms, float speed_up_ms, float accel_mss) { // define maximum position error and maximum first and second differential limits - _p_pos_u_cm.set_limits(-fabsf(speed_down_cms), speed_up_cms, accel_cmss, 0.0f); + _p_pos_u_m.set_limits(-fabsf(speed_down_ms), speed_up_ms, accel_mss, 0.0f); } /// init_U_controller - initialise the position controller to the current position, velocity, acceleration and attitude. @@ -772,28 +814,28 @@ void AC_PosControl::init_U_controller_no_descent() init_U_controller(); // remove all descent if present - _vel_target_neu_cms.z = MAX(0.0, _vel_target_neu_cms.z); - _vel_desired_neu_cms.z = MAX(0.0, _vel_desired_neu_cms.z); - _vel_terrain_u_cms = MAX(0.0, _vel_terrain_u_cms); - _vel_offset_neu_cms.z = MAX(0.0, _vel_offset_neu_cms.z); - _accel_target_neu_cmss.z = MAX(0.0, _accel_target_neu_cmss.z); - _accel_desired_neu_cmss.z = MAX(0.0, _accel_desired_neu_cmss.z); - _accel_terrain_u_cmss = MAX(0.0, _accel_terrain_u_cmss); - _accel_offset_neu_cmss.z = MAX(0.0, _accel_offset_neu_cmss.z); + _vel_target_neu_ms.z = MAX(0.0, _vel_target_neu_ms.z); + _vel_desired_neu_ms.z = MAX(0.0, _vel_desired_neu_ms.z); + _vel_terrain_u_ms = MAX(0.0, _vel_terrain_u_ms); + _vel_offset_neu_ms.z = MAX(0.0, _vel_offset_neu_ms.z); + _accel_target_neu_mss.z = MAX(0.0, _accel_target_neu_mss.z); + _accel_desired_neu_mss.z = MAX(0.0, _accel_desired_neu_mss.z); + _accel_terrain_u_mss = MAX(0.0, _accel_terrain_u_mss); + _accel_offset_neu_mss.z = MAX(0.0, _accel_offset_neu_mss.z); } /// init_U_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration. /// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position. -/// The starting position can be retrieved by getting the position target using get_pos_target_NEU_cm() after calling this function. +/// The starting position can be retrieved by getting the position target using get_pos_target_NEU_m() after calling this function. void AC_PosControl::init_U_controller_stopping_point() { // Initialise the position controller to the current throttle, position, velocity and acceleration. init_U_controller(); - get_stopping_point_U_cm(_pos_desired_neu_cm.z); - _pos_target_neu_cm.z = _pos_desired_neu_cm.z + _pos_offset_neu_cm.z; - _vel_desired_neu_cms.z = 0.0f; - _accel_desired_neu_cmss.z = 0.0f; + get_stopping_point_U_m(_pos_desired_neu_m.z); + _pos_target_neu_m.z = _pos_desired_neu_m.z + _pos_offset_neu_m.z; + _vel_desired_neu_ms.z = 0.0f; + _accel_desired_neu_mss.z = 0.0f; } // relax_U_controller - initialise the position controller to the current position and velocity with decaying acceleration. @@ -805,7 +847,7 @@ void AC_PosControl::relax_U_controller(float throttle_setting) // init_U_controller has set the acceleration PID I term to generate the current throttle set point // Use relax_integrator to decay the throttle set point to throttle_setting - _pid_accel_u_cm_to_kt.relax_integrator((throttle_setting - _motors.get_throttle_hover()) * 1000.0f, _dt_s, POSCONTROL_RELAX_TC); + _pid_accel_u_cm_to_kt.relax_integrator((throttle_setting - _motors.get_throttle_hover()) * 10.0 * 100.0, _dt_s, POSCONTROL_RELAX_TC); } /// init_U_controller - initialise the position controller to the current position, velocity, acceleration and attitude. @@ -819,26 +861,26 @@ void AC_PosControl::init_U_controller() // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated. init_offsets_U(); - _pos_target_neu_cm.z = _pos_estimate_neu_cm.z; - _pos_desired_neu_cm.z = _pos_target_neu_cm.z - _pos_offset_neu_cm.z; + _pos_target_neu_m.z = _pos_estimate_neu_m.z; + _pos_desired_neu_m.z = _pos_target_neu_m.z - _pos_offset_neu_m.z; - _vel_target_neu_cms.z = _vel_estimate_neu_cms.z; - _vel_desired_neu_cms.z = _vel_target_neu_cms.z - _vel_offset_neu_cms.z; + _vel_target_neu_ms.z = _vel_estimate_neu_ms.z; + _vel_desired_neu_ms.z = _vel_target_neu_ms.z - _vel_offset_neu_ms.z; // Reset I term of velocity PID _pid_vel_u_cm.reset_filter(); _pid_vel_u_cm.set_integrator(0.0f); - _accel_target_neu_cmss.z = constrain_float(get_measured_accel_U_cmss(), -_accel_max_u_cmss, _accel_max_u_cmss); - _accel_desired_neu_cmss.z = _accel_target_neu_cmss.z - (_accel_offset_neu_cmss.z + _accel_terrain_u_cmss); + _accel_target_neu_mss.z = constrain_float(get_measured_accel_U_mss(), -_accel_max_u_mss, _accel_max_u_mss); + _accel_desired_neu_mss.z = _accel_target_neu_mss.z - (_accel_offset_neu_mss.z + _accel_terrain_u_mss); _pid_accel_u_cm_to_kt.reset_filter(); // Set acceleration PID I term based on the current throttle - // Remove the expected P term due to _accel_desired_neu_cmss.z being constrained to _accel_max_u_cmss - // Remove the expected FF term due to non-zero _accel_target_neu_cmss.z - _pid_accel_u_cm_to_kt.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 1000.0f - - _pid_accel_u_cm_to_kt.kP() * (_accel_target_neu_cmss.z - get_measured_accel_U_cmss()) - - _pid_accel_u_cm_to_kt.ff() * _accel_target_neu_cmss.z); + // Remove the expected P term due to _accel_desired_neu_mss.z being constrained to _accel_max_u_mss + // Remove the expected FF term due to non-zero _accel_target_neu_mss.z + _pid_accel_u_cm_to_kt.set_integrator((_attitude_control.get_throttle_in() - _motors.get_throttle_hover()) * 10.0 * 100.0 + - _pid_accel_u_cm_to_kt.kP() * (_accel_target_neu_mss.z - get_measured_accel_U_mss()) * 100.0 + - _pid_accel_u_cm_to_kt.ff() * _accel_target_neu_mss.z * 100.0); // initialise ekf z reset handler init_ekf_U_reset(); @@ -847,96 +889,126 @@ void AC_PosControl::init_U_controller() _last_update_u_ticks = AP::scheduler().ticks32(); } -/// input_accel_U_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. +/// input_accel_U_m - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. -/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_U_cmss. +/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_U_mss. void AC_PosControl::input_accel_U_cm(float accel_cmss) +{ + input_accel_U_m(accel_cmss * 0.01); +} +void AC_PosControl::input_accel_U_m(float accel_mss) { // calculated increased maximum jerk if over speed - float jerk_max_u_cmsss = _jerk_max_u_cmsss * calculate_overspeed_gain(); + float jerk_max_u_msss = _jerk_max_u_msss * calculate_overspeed_gain(); // adjust desired alt if motors have not hit their limits - update_pos_vel_accel(_pos_desired_neu_cm.z, _vel_desired_neu_cms.z, _accel_desired_neu_cmss.z, _dt_s, _limit_vector.z, _p_pos_u_cm.get_error(), _pid_vel_u_cm.get_error()); + update_pos_vel_accel(_pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, _dt_s, _limit_vector_neu.z, _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); - shape_accel(accel_cmss, _accel_desired_neu_cmss.z, jerk_max_u_cmsss, _dt_s); + shape_accel(accel_mss, _accel_desired_neu_mss.z, jerk_max_u_msss, _dt_s); } -/// input_vel_accel_U_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. +/// input_vel_accel_U_m - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. -/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_U_cmss. -/// The function modifies vel_u_cms to follow the jerk-limited trajectory defined by accel_u_cmss. +/// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_U_mss. +/// The function modifies vel_u_ms to follow the jerk-limited trajectory defined by accel_u_mss. /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. void AC_PosControl::input_vel_accel_U_cm(float &vel_u_cms, float accel_cmss, bool limit_output) +{ + float vel_u_ms = vel_u_cms * 0.01; + input_vel_accel_U_m(vel_u_ms, accel_cmss * 0.01, limit_output); + vel_u_cms = vel_u_ms * 100.0; +} +void AC_PosControl::input_vel_accel_U_m(float &vel_u_ms, float accel_mss, bool limit_output) { // calculated increased maximum acceleration and jerk if over speed const float overspeed_gain = calculate_overspeed_gain(); - const float accel_max_u_cmss = _accel_max_u_cmss * overspeed_gain; - const float jerk_max_u_cmsss = _jerk_max_u_cmsss * overspeed_gain; + const float accel_max_u_mss = _accel_max_u_mss * overspeed_gain; + const float jerk_max_u_msss = _jerk_max_u_msss * overspeed_gain; // adjust desired alt if motors have not hit their limits - update_pos_vel_accel(_pos_desired_neu_cm.z, _vel_desired_neu_cms.z, _accel_desired_neu_cmss.z, _dt_s, _limit_vector.z, _p_pos_u_cm.get_error(), _pid_vel_u_cm.get_error()); + update_pos_vel_accel(_pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, _dt_s, _limit_vector_neu.z, _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); - shape_vel_accel(vel_u_cms, accel_cmss, - _vel_desired_neu_cms.z, _accel_desired_neu_cmss.z, - -constrain_float(accel_max_u_cmss, 0.0f, 750.0f), accel_max_u_cmss, - jerk_max_u_cmsss, _dt_s, limit_output); + shape_vel_accel(vel_u_ms, accel_mss, + _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, + -constrain_float(accel_max_u_mss, 0.0, 7.5), accel_max_u_mss, + jerk_max_u_msss, _dt_s, limit_output); - update_vel_accel(vel_u_cms, accel_cmss, _dt_s, 0.0, 0.0); + update_vel_accel(vel_u_ms, accel_mss, _dt_s, 0.0, 0.0); } -/// set_pos_target_U_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s +/// set_pos_target_U_from_climb_rate_m - adjusts target up or down using a commanded climb rate in cm/s /// using the default position control kinematic path. /// The zero target altitude is varied to follow pos_offset_z void AC_PosControl::set_pos_target_U_from_climb_rate_cm(float vel_u_cms) { - input_vel_accel_U_cm(vel_u_cms, 0.0); + set_pos_target_U_from_climb_rate_m(vel_u_cms * 0.01); +} +void AC_PosControl::set_pos_target_U_from_climb_rate_m(float vel_u_ms) +{ + input_vel_accel_U_m(vel_u_ms, 0.0); } -/// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s +/// land_at_climb_rate_m - adjusts target up or down using a commanded climb rate in cm/s /// using the default position control kinematic path. /// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be false unless landing. void AC_PosControl::land_at_climb_rate_cm(float vel_u_cms, bool ignore_descent_limit) +{ + land_at_climb_rate_m(vel_u_cms * 0.01, ignore_descent_limit); +} +void AC_PosControl::land_at_climb_rate_m(float vel_u_ms, bool ignore_descent_limit) { if (ignore_descent_limit) { // turn off limits in the negative z direction - _limit_vector.z = MAX(_limit_vector.z, 0.0f); + _limit_vector_neu.z = MAX(_limit_vector_neu.z, 0.0f); } - input_vel_accel_U_cm(vel_u_cms, 0.0); + input_vel_accel_U_m(vel_u_ms, 0.0); } -/// input_pos_vel_accel_U_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. -/// The pos_u_cm and vel_u_cms are projected forwards in time based on a time step of dt and acceleration accel_cmss. +/// input_pos_vel_accel_U_m - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. +/// The pos_u_m and vel_u_ms are projected forwards in time based on a time step of dt and acceleration accel_mss. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. -/// The function alters the pos_u_cm and vel_u_cms to be the kinematic path based on accel_cmss +/// The function alters the pos_u_m and vel_u_ms to be the kinematic path based on accel_mss /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. void AC_PosControl::input_pos_vel_accel_U_cm(float &pos_u_cm, float &vel_u_cms, float accel_cmss, bool limit_output) +{ + float pos_u_m = pos_u_cm * 0.01; + float vel_u_ms = vel_u_cms * 0.01; + input_pos_vel_accel_U_m(pos_u_m, vel_u_ms, accel_cmss * 0.01, limit_output); + pos_u_cm = pos_u_m * 100.0; + vel_u_cms = vel_u_ms * 100.0; +} +void AC_PosControl::input_pos_vel_accel_U_m(float &pos_u_m, float &vel_u_ms, float accel_mss, bool limit_output) { // calculated increased maximum acceleration and jerk if over speed const float overspeed_gain = calculate_overspeed_gain(); - const float accel_max_u_cmss = _accel_max_u_cmss * overspeed_gain; - const float jerk_max_u_cmsss = _jerk_max_u_cmsss * overspeed_gain; + const float accel_max_u_mss = _accel_max_u_mss * overspeed_gain; + const float jerk_max_u_msss = _jerk_max_u_msss * overspeed_gain; // adjust desired altitude if motors have not hit their limits - update_pos_vel_accel(_pos_desired_neu_cm.z, _vel_desired_neu_cms.z, _accel_desired_neu_cmss.z, _dt_s, _limit_vector.z, _p_pos_u_cm.get_error(), _pid_vel_u_cm.get_error()); + update_pos_vel_accel(_pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, _dt_s, _limit_vector_neu.z, _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); - shape_pos_vel_accel(pos_u_cm, vel_u_cms, accel_cmss, - _pos_desired_neu_cm.z, _vel_desired_neu_cms.z, _accel_desired_neu_cmss.z, - _vel_max_down_cms, _vel_max_up_cms, - -constrain_float(accel_max_u_cmss, 0.0f, 750.0f), accel_max_u_cmss, - jerk_max_u_cmsss, _dt_s, limit_output); + shape_pos_vel_accel(pos_u_m, vel_u_ms, accel_mss, + _pos_desired_neu_m.z, _vel_desired_neu_ms.z, _accel_desired_neu_mss.z, + _vel_max_down_ms, _vel_max_up_ms, + -constrain_float(accel_max_u_mss, 0.0, 7.5), accel_max_u_mss, + jerk_max_u_msss, _dt_s, limit_output); - postype_t posp = pos_u_cm; - update_pos_vel_accel(posp, vel_u_cms, accel_cmss, _dt_s, 0.0, 0.0, 0.0); - pos_u_cm = posp; + postype_t posp = pos_u_m; + update_pos_vel_accel(posp, vel_u_ms, accel_mss, _dt_s, 0.0, 0.0, 0.0); + pos_u_m = posp; } -/// set_alt_target_with_slew_cm - adjusts target up or down using a commanded altitude in cm +/// set_alt_target_with_slew_m - adjusts target up or down using a commanded altitude in cm /// using the default position control kinematic path. void AC_PosControl::set_alt_target_with_slew_cm(float pos_u_cm) +{ + set_alt_target_with_slew_m(pos_u_cm * 0.01); +} +void AC_PosControl::set_alt_target_with_slew_m(float pos_u_m) { float zero = 0; - input_pos_vel_accel_U_cm(pos_u_cm, zero, 0); + input_pos_vel_accel_U_m(pos_u_m, zero, 0); } /// update_offsets_U - updates the vertical offsets used by terrain following @@ -945,26 +1017,26 @@ void AC_PosControl::update_offsets_U() // check for offset target timeout uint32_t now_ms = AP_HAL::millis(); if (now_ms - _posvelaccel_offset_target_u_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { - _pos_offset_target_neu_cm.z = 0.0; - _vel_offset_target_neu_cms.z = 0.0; - _accel_offset_target_neu_cmss.z = 0.0; + _pos_offset_target_neu_m.z = 0.0; + _vel_offset_target_neu_ms.z = 0.0; + _accel_offset_target_neu_mss.z = 0.0; } // update position, velocity, acceleration offsets for this iteration - postype_t p_offset_u_cm = _pos_offset_neu_cm.z; - update_pos_vel_accel(p_offset_u_cm, _vel_offset_neu_cms.z, _accel_offset_neu_cmss.z, _dt_s, MIN(_limit_vector.z, 0.0f), _p_pos_u_cm.get_error(), _pid_vel_u_cm.get_error()); - _pos_offset_neu_cm.z = p_offset_u_cm; + postype_t p_offset_u_m = _pos_offset_neu_m.z; + update_pos_vel_accel(p_offset_u_m, _vel_offset_neu_ms.z, _accel_offset_neu_mss.z, _dt_s, MIN(_limit_vector_neu.z, 0.0f), _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); + _pos_offset_neu_m.z = p_offset_u_m; // input shape vertical position, velocity and acceleration offsets - shape_pos_vel_accel(_pos_offset_target_neu_cm.z, _vel_offset_target_neu_cms.z, _accel_offset_target_neu_cmss.z, - _pos_offset_neu_cm.z, _vel_offset_neu_cms.z, _accel_offset_neu_cmss.z, - get_max_speed_down_cms(), get_max_speed_up_cms(), - -get_max_accel_U_cmss(), get_max_accel_U_cmss(), - _jerk_max_u_cmsss, _dt_s, false); + shape_pos_vel_accel(_pos_offset_target_neu_m.z, _vel_offset_target_neu_ms.z, _accel_offset_target_neu_mss.z, + _pos_offset_neu_m.z, _vel_offset_neu_ms.z, _accel_offset_neu_mss.z, + get_max_speed_down_ms(), get_max_speed_up_ms(), + -get_max_accel_U_mss(), get_max_accel_U_mss(), + _jerk_max_u_msss, _dt_s, false); - p_offset_u_cm = _pos_offset_target_neu_cm.z; - update_pos_vel_accel(p_offset_u_cm, _vel_offset_target_neu_cms.z, _accel_offset_target_neu_cmss.z, _dt_s, 0.0, 0.0, 0.0); - _pos_offset_target_neu_cm.z = p_offset_u_cm; + p_offset_u_m = _pos_offset_target_neu_m.z; + update_pos_vel_accel(p_offset_u_m, _vel_offset_target_neu_ms.z, _accel_offset_target_neu_mss.z, _dt_s, 0.0, 0.0, 0.0); + _pos_offset_target_neu_m.z = p_offset_u_m; } // is_active_U - returns true if the z position controller has been run in the previous loop @@ -996,43 +1068,43 @@ void AC_PosControl::update_U_controller() // update the position, velocity and acceleration offsets update_offsets_U(); update_terrain(); - _pos_target_neu_cm.z = _pos_desired_neu_cm.z + _pos_offset_neu_cm.z + _pos_terrain_u_cm; + _pos_target_neu_m.z = _pos_desired_neu_m.z + _pos_offset_neu_m.z + _pos_terrain_u_m; // calculate the target velocity correction - float pos_target_zf = _pos_target_neu_cm.z; + float pos_target_zf = _pos_target_neu_m.z; - _vel_target_neu_cms.z = _p_pos_u_cm.update_all(pos_target_zf, _pos_estimate_neu_cm.z); - _vel_target_neu_cms.z *= AP::ahrs().getControlScaleZ(); + _vel_target_neu_ms.z = _p_pos_u_m.update_all(pos_target_zf, _pos_estimate_neu_m.z); + _vel_target_neu_ms.z *= AP::ahrs().getControlScaleZ(); - _pos_target_neu_cm.z = pos_target_zf; - _pos_desired_neu_cm.z = _pos_target_neu_cm.z - (_pos_offset_neu_cm.z + _pos_terrain_u_cm); + _pos_target_neu_m.z = pos_target_zf; + _pos_desired_neu_m.z = _pos_target_neu_m.z - (_pos_offset_neu_m.z + _pos_terrain_u_m); // add feed forward component - _vel_target_neu_cms.z += _vel_desired_neu_cms.z + _vel_offset_neu_cms.z + _vel_terrain_u_cms; + _vel_target_neu_ms.z += _vel_desired_neu_ms.z + _vel_offset_neu_ms.z + _vel_terrain_u_ms; // Velocity Controller - _accel_target_neu_cmss.z = _pid_vel_u_cm.update_all(_vel_target_neu_cms.z, _vel_estimate_neu_cms.z, _dt_s, _motors.limit.throttle_lower, _motors.limit.throttle_upper); - _accel_target_neu_cmss.z *= AP::ahrs().getControlScaleZ(); + _accel_target_neu_mss.z = _pid_vel_u_cm.update_all(_vel_target_neu_ms.z * 100.0, _vel_estimate_neu_ms.z * 100.0, _dt_s, _motors.limit.throttle_lower, _motors.limit.throttle_upper) * 0.01; + _accel_target_neu_mss.z *= AP::ahrs().getControlScaleZ(); // add feed forward component - _accel_target_neu_cmss.z += _accel_desired_neu_cmss.z + _accel_offset_neu_cmss.z + _accel_terrain_u_cmss; + _accel_target_neu_mss.z += _accel_desired_neu_mss.z + _accel_offset_neu_mss.z + _accel_terrain_u_mss; // Acceleration Controller // Calculate vertical acceleration - const float measured_accel_u_cmss = get_measured_accel_U_cmss(); + const float measured_accel_u_mss = get_measured_accel_U_mss(); // ensure imax is always large enough to overpower hover throttle - if (_motors.get_throttle_hover() * 1000.0f > _pid_accel_u_cm_to_kt.imax()) { - _pid_accel_u_cm_to_kt.set_imax(_motors.get_throttle_hover() * 1000.0f); + if (_motors.get_throttle_hover() * 1000.0 > _pid_accel_u_cm_to_kt.imax()) { + _pid_accel_u_cm_to_kt.set_imax(_motors.get_throttle_hover() * 1000.0); } float thr_out; if (_vibe_comp_enabled) { thr_out = get_throttle_with_vibration_override(); } else { - thr_out = _pid_accel_u_cm_to_kt.update_all(_accel_target_neu_cmss.z, measured_accel_u_cmss, _dt_s, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001f; - thr_out += _pid_accel_u_cm_to_kt.get_ff() * 0.001f; + thr_out = _pid_accel_u_cm_to_kt.update_all(_accel_target_neu_mss.z * 100.0, measured_accel_u_mss * 100.0, _dt_s, (_motors.limit.throttle_lower || _motors.limit.throttle_upper)) * 0.001; + thr_out += _pid_accel_u_cm_to_kt.get_ff() * 0.001; } thr_out += _motors.get_throttle_hover(); @@ -1043,18 +1115,18 @@ void AC_PosControl::update_U_controller() // Check for vertical controller health - // _speed_down_cms is checked to be non-zero when set - float error_ratio = _pid_vel_u_cm.get_error() / _vel_max_down_cms; + // _speed_down_ms is checked to be non-zero when set + float error_ratio = _pid_vel_u_cm.get_error() * 0.01 / _vel_max_down_ms; _vel_u_control_ratio += _dt_s * 0.1f * (0.5 - error_ratio); _vel_u_control_ratio = constrain_float(_vel_u_control_ratio, 0.0f, 2.0f); // set vertical component of the limit vector if (_motors.limit.throttle_upper) { - _limit_vector.z = 1.0f; + _limit_vector_neu.z = 1.0f; } else if (_motors.limit.throttle_lower) { - _limit_vector.z = -1.0f; + _limit_vector_neu.z = -1.0f; } else { - _limit_vector.z = 0.0f; + _limit_vector_neu.z = 0.0f; } } @@ -1078,21 +1150,33 @@ float AC_PosControl::get_lean_angle_max_rad() const /// set the desired position, velocity and acceleration targets void AC_PosControl::set_pos_vel_accel_NEU_cm(const Vector3p& pos_neu_cm, const Vector3f& vel_neu_cms, const Vector3f& accel_neu_cmss) { - _pos_desired_neu_cm = pos_neu_cm; - _vel_desired_neu_cms = vel_neu_cms; - _accel_desired_neu_cmss = accel_neu_cmss; + set_pos_vel_accel_NEU_m(pos_neu_cm * 0.01, vel_neu_cms * 0.01, accel_neu_cmss * 0.01); +} +void AC_PosControl::set_pos_vel_accel_NEU_m(const Vector3p& pos_neu_m, const Vector3f& vel_neu_ms, const Vector3f& accel_neu_mss) +{ + _pos_desired_neu_m = pos_neu_m; + _vel_desired_neu_ms = vel_neu_ms; + _accel_desired_neu_mss = accel_neu_mss; } /// set the desired position, velocity and acceleration targets void AC_PosControl::set_pos_vel_accel_NE_cm(const Vector2p& pos_ne_cm, const Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss) { - _pos_desired_neu_cm.xy() = pos_ne_cm; - _vel_desired_neu_cms.xy() = vel_ne_cms; - _accel_desired_neu_cmss.xy() = accel_ne_cmss; + set_pos_vel_accel_NE_m(pos_ne_cm * 0.01, vel_ne_cms * 0.01, accel_ne_cmss * 0.01); +} +void AC_PosControl::set_pos_vel_accel_NE_m(const Vector2p& pos_ne_m, const Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss) +{ + _pos_desired_neu_m.xy() = pos_ne_m; + _vel_desired_neu_ms.xy() = vel_ne_ms; + _accel_desired_neu_mss.xy() = accel_ne_mss; } // get_lean_angles_to_accel - convert roll, pitch lean target angles to lat/lon frame accelerations in cm/s/s -Vector3f AC_PosControl::lean_angles_to_accel_NEU_cmss(const Vector3f& att_target_euler_rad) const +Vector3f AC_PosControl::lean_angles_rad_to_accel_NEU_cmss(const Vector3f& att_target_euler_rad) const +{ + return lean_angles_rad_to_accel_NEU_mss(att_target_euler_rad) * 100.0; +} +Vector3f AC_PosControl::lean_angles_rad_to_accel_NEU_mss(const Vector3f& att_target_euler_rad) const { // rotate our roll, pitch angles into lat/lon frame const float sin_roll = sinf(att_target_euler_rad.x); @@ -1103,9 +1187,9 @@ Vector3f AC_PosControl::lean_angles_to_accel_NEU_cmss(const Vector3f& att_target const float cos_yaw = cosf(att_target_euler_rad.z); return Vector3f{ - (GRAVITY_MSS * 100.0f) * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f), - (GRAVITY_MSS * 100.0f) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f), - (GRAVITY_MSS * 100.0f) + GRAVITY_MSS * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f), + GRAVITY_MSS * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f), + GRAVITY_MSS }; } @@ -1116,22 +1200,26 @@ Vector3f AC_PosControl::lean_angles_to_accel_NEU_cmss(const Vector3f& att_target void AC_PosControl::init_terrain() { // set terrain position and target to zero - _pos_terrain_target_u_cm = 0.0; - _pos_terrain_u_cm = 0.0; + _pos_terrain_target_u_m = 0.0; + _pos_terrain_u_m = 0.0; // set velocity offset to zero - _vel_terrain_u_cms = 0.0; + _vel_terrain_u_ms = 0.0; // set acceleration offset to zero - _accel_terrain_u_cmss = 0.0; + _accel_terrain_u_mss = 0.0; } -// init_pos_terrain_U_cm - initialises the current terrain altitude and target altitude to pos_offset_terrain_cm +// init_pos_terrain_U_m - initialises the current terrain altitude and target altitude to pos_offset_terrain_m void AC_PosControl::init_pos_terrain_U_cm(float pos_terrain_u_cm) { - _pos_desired_neu_cm.z -= (pos_terrain_u_cm - _pos_terrain_u_cm); - _pos_terrain_target_u_cm = pos_terrain_u_cm; - _pos_terrain_u_cm = pos_terrain_u_cm; + init_pos_terrain_U_m(pos_terrain_u_cm * 0.01); +} +void AC_PosControl::init_pos_terrain_U_m(float pos_terrain_u_m) +{ + _pos_desired_neu_m.z -= (pos_terrain_u_m - _pos_terrain_u_m); + _pos_terrain_target_u_m = pos_terrain_u_m; + _pos_terrain_u_m = pos_terrain_u_m; } @@ -1144,19 +1232,19 @@ void AC_PosControl::init_offsets_NE() // check for offset target timeout uint32_t now_ms = AP_HAL::millis(); if (now_ms - _posvelaccel_offset_target_ne_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { - _pos_offset_target_neu_cm.xy().zero(); - _vel_offset_target_neu_cms.xy().zero(); - _accel_offset_target_neu_cmss.xy().zero(); + _pos_offset_target_neu_m.xy().zero(); + _vel_offset_target_neu_ms.xy().zero(); + _accel_offset_target_neu_mss.xy().zero(); } // set position offset to target - _pos_offset_neu_cm.xy() = _pos_offset_target_neu_cm.xy(); + _pos_offset_neu_m.xy() = _pos_offset_target_neu_m.xy(); // set velocity offset to target - _vel_offset_neu_cms.xy() = _vel_offset_target_neu_cms.xy(); + _vel_offset_neu_ms.xy() = _vel_offset_target_neu_ms.xy(); // set acceleration offset to target - _accel_offset_neu_cmss.xy() = _accel_offset_target_neu_cmss.xy(); + _accel_offset_neu_mss.xy() = _accel_offset_target_neu_mss.xy(); } /// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame @@ -1166,63 +1254,71 @@ void AC_PosControl::init_offsets_U() // check for offset target timeout uint32_t now_ms = AP_HAL::millis(); if (now_ms - _posvelaccel_offset_target_u_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) { - _pos_offset_target_neu_cm.z = 0.0; - _vel_offset_target_neu_cms.z = 0.0; - _accel_offset_target_neu_cmss.z = 0.0; + _pos_offset_target_neu_m.z = 0.0; + _vel_offset_target_neu_ms.z = 0.0; + _accel_offset_target_neu_mss.z = 0.0; } // set position offset to target - _pos_offset_neu_cm.z = _pos_offset_target_neu_cm.z; + _pos_offset_neu_m.z = _pos_offset_target_neu_m.z; // set velocity offset to target - _vel_offset_neu_cms.z = _vel_offset_target_neu_cms.z; + _vel_offset_neu_ms.z = _vel_offset_target_neu_ms.z; // set acceleration offset to target - _accel_offset_neu_cmss.z = _accel_offset_target_neu_cmss.z; + _accel_offset_neu_mss.z = _accel_offset_target_neu_mss.z; } #if AP_SCRIPTING_ENABLED // add an additional offset to vehicle's target position, velocity and acceleration // units are m, m/s and m/s/s in NED frame // Z-axis is not currently supported and is ignored -bool AC_PosControl::set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED) +// Used in LUA +bool AC_PosControl::set_posvelaccel_offset(const Vector3f &pos_offset_NED_m, const Vector3f &vel_offset_NED_ms, const Vector3f &accel_offset_NED_mss) { - set_posvelaccel_offset_target_NE_cm(pos_offset_NED.topostype().xy() * 100.0, vel_offset_NED.xy() * 100.0, accel_offset_NED.xy() * 100.0); - set_posvelaccel_offset_target_U_cm(-pos_offset_NED.topostype().z * 100.0, -vel_offset_NED.z * 100, -accel_offset_NED.z * 100.0); + set_posvelaccel_offset_target_NE_m(pos_offset_NED_m.topostype().xy(), vel_offset_NED_ms.xy(), accel_offset_NED_mss.xy()); + set_posvelaccel_offset_target_U_m(-pos_offset_NED_m.topostype().z, -vel_offset_NED_ms.z, -accel_offset_NED_mss.z); return true; } // get position and velocity offset to vehicle's target velocity and acceleration // units are m and m/s in NED frame -bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED) +// Used in LUA +bool AC_PosControl::get_posvelaccel_offset(Vector3f &pos_offset_NED_m, Vector3f &vel_offset_NED_ms, Vector3f &accel_offset_NED_mss) { - pos_offset_NED.xy() = _pos_offset_target_neu_cm.xy().tofloat() * 0.01; - pos_offset_NED.z = -_pos_offset_target_neu_cm.z * 0.01; - vel_offset_NED.xy() = _vel_offset_target_neu_cms.xy() * 0.01; - vel_offset_NED.z = -_vel_offset_target_neu_cms.z * 0.01; - accel_offset_NED.xy() = _accel_offset_target_neu_cmss.xy() * 0.01; - accel_offset_NED.z = -_accel_offset_target_neu_cmss.z * 0.01; + pos_offset_NED_m.xy() = _pos_offset_target_neu_m.xy().tofloat(); + pos_offset_NED_m.z = -_pos_offset_target_neu_m.z; + + vel_offset_NED_ms.xy() = _vel_offset_target_neu_ms.xy(); + vel_offset_NED_ms.z = -_vel_offset_target_neu_ms.z; + + accel_offset_NED_mss.xy() = _accel_offset_target_neu_mss.xy(); + accel_offset_NED_mss.z = -_accel_offset_target_neu_mss.z; return true; } // get target velocity in m/s in NED frame -bool AC_PosControl::get_vel_target(Vector3f &vel_target_NED) +// Used in LUA +bool AC_PosControl::get_vel_target(Vector3f &vel_target_NED_ms) { if (!is_active_NE() || !is_active_U()) { return false; } - vel_target_NED.xy() = _vel_target_neu_cms.xy() * 0.01; - vel_target_NED.z = -_vel_target_neu_cms.z * 0.01; + vel_target_NED_ms.xy() = _vel_target_neu_ms.xy(); + vel_target_NED_ms.z = -_vel_target_neu_ms.z; return true; } // get target acceleration in m/s/s in NED frame -bool AC_PosControl::get_accel_target(Vector3f &accel_target_NED) +// Used in LUA +bool AC_PosControl::get_accel_target(Vector3f &accel_target_NED_mss) { if (!is_active_NE() || !is_active_U()) { return false; } - accel_target_NED.xy() = _accel_target_neu_cmss.xy() * 0.01; - accel_target_NED.z = -_accel_target_neu_cmss.z * 0.01; + + // Convert NEU → NED by inverting Z + accel_target_NED_mss.xy() = _accel_target_neu_mss.xy(); + accel_target_NED_mss.z = -_accel_target_neu_mss.z; return true; } #endif @@ -1230,15 +1326,19 @@ bool AC_PosControl::get_accel_target(Vector3f &accel_target_NED) /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame /// these must be set every 3 seconds (or less) or they will timeout and return to zero void AC_PosControl::set_posvelaccel_offset_target_NE_cm(const Vector2p& pos_offset_target_ne_cm, const Vector2f& vel_offset_target_ne_cms, const Vector2f& accel_offset_target_ne_cmss) +{ + set_posvelaccel_offset_target_NE_m(pos_offset_target_ne_cm * 0.01, vel_offset_target_ne_cms * 0.01, accel_offset_target_ne_cmss * 0.01); +} +void AC_PosControl::set_posvelaccel_offset_target_NE_m(const Vector2p& pos_offset_target_ne_m, const Vector2f& vel_offset_target_ne_ms, const Vector2f& accel_offset_target_ne_mss) { // set position offset target - _pos_offset_target_neu_cm.xy() = pos_offset_target_ne_cm; + _pos_offset_target_neu_m.xy() = pos_offset_target_ne_m; // set velocity offset target - _vel_offset_target_neu_cms.xy() = vel_offset_target_ne_cms; + _vel_offset_target_neu_ms.xy() = vel_offset_target_ne_ms; // set acceleration offset target - _accel_offset_target_neu_cmss.xy() = accel_offset_target_ne_cmss; + _accel_offset_target_neu_mss.xy() = accel_offset_target_ne_mss; // record time of update so we can detect timeouts _posvelaccel_offset_target_ne_ms = AP_HAL::millis(); @@ -1247,15 +1347,19 @@ void AC_PosControl::set_posvelaccel_offset_target_NE_cm(const Vector2p& pos_offs /// set the vertical position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame /// these must be set every 3 seconds (or less) or they will timeout and return to zero void AC_PosControl::set_posvelaccel_offset_target_U_cm(float pos_offset_target_u_cm, float vel_offset_target_u_cms, const float accel_offset_target_u_cmss) +{ + set_posvelaccel_offset_target_U_m(pos_offset_target_u_cm * 0.01, vel_offset_target_u_cms * 0.01, accel_offset_target_u_cmss * 0.01); +} +void AC_PosControl::set_posvelaccel_offset_target_U_m(float pos_offset_target_u_m, float vel_offset_target_u_ms, const float accel_offset_target_u_mss) { // set position offset target - _pos_offset_target_neu_cm.z = pos_offset_target_u_cm; + _pos_offset_target_neu_m.z = pos_offset_target_u_m; // set velocity offset target - _vel_offset_target_neu_cms.z = vel_offset_target_u_cms; + _vel_offset_target_neu_ms.z = vel_offset_target_u_ms; // set acceleration offset target - _accel_offset_target_neu_cmss.z = accel_offset_target_u_cmss; + _accel_offset_target_neu_mss.z = accel_offset_target_u_mss; // record time of update so we can detect timeouts _posvelaccel_offset_target_u_ms = AP_HAL::millis(); @@ -1264,21 +1368,27 @@ void AC_PosControl::set_posvelaccel_offset_target_U_cm(float pos_offset_target_u // returns the NED target acceleration vector for attitude control Vector3f AC_PosControl::get_thrust_vector() const { - Vector3f accel_target_neu_cmss = get_accel_target_NEU_cmss(); - accel_target_neu_cmss.z = -GRAVITY_MSS * 100.0f; - return accel_target_neu_cmss; + Vector3f accel_target_neu_mss = get_accel_target_NEU_mss(); + accel_target_neu_mss.z = -GRAVITY_MSS; + return accel_target_neu_mss; } -/// get_stopping_point_NE_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration +/// get_stopping_point_NE_m - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration /// function does not change the z axis void AC_PosControl::get_stopping_point_NE_cm(Vector2p &stopping_point_neu_cm) const +{ + Vector2p stopping_point_neu_m = stopping_point_neu_cm * 0.01; + get_stopping_point_NE_m(stopping_point_neu_m); + stopping_point_neu_cm = stopping_point_neu_m * 100.0; +} +void AC_PosControl::get_stopping_point_NE_m(Vector2p &stopping_point_neu_m) const { // todo: we should use the current target position and velocity if we are currently running the position controller - stopping_point_neu_cm = _pos_estimate_neu_cm.xy(); - stopping_point_neu_cm -= _pos_offset_neu_cm.xy(); + stopping_point_neu_m = _pos_estimate_neu_m.xy(); + stopping_point_neu_m -= _pos_offset_neu_m.xy(); - Vector2f curr_vel = _vel_estimate_neu_cms.xy(); - curr_vel -= _vel_offset_neu_cms.xy(); + Vector2f curr_vel = _vel_estimate_neu_ms.xy(); + curr_vel -= _vel_offset_neu_ms.xy(); // calculate current velocity float vel_total = curr_vel.length(); @@ -1287,8 +1397,8 @@ void AC_PosControl::get_stopping_point_NE_cm(Vector2p &stopping_point_neu_cm) co return; } - float kP = _p_pos_ne_cm.kP(); - const float stopping_dist = stopping_distance(constrain_float(vel_total, 0.0, _vel_max_ne_cms), kP, _accel_max_ne_cmss); + float kP = _p_pos_ne_m.kP(); + const float stopping_dist = stopping_distance(constrain_float(vel_total, 0.0, _vel_max_ne_ms), kP, _accel_max_ne_mss); if (!is_positive(stopping_dist)) { return; } @@ -1296,31 +1406,37 @@ void AC_PosControl::get_stopping_point_NE_cm(Vector2p &stopping_point_neu_cm) co // convert the stopping distance into a stopping point using velocity vector // todo: convert velocity to a unit vector instead. const float t = stopping_dist / vel_total; - stopping_point_neu_cm += (curr_vel * t).topostype(); + stopping_point_neu_m += (curr_vel * t).topostype(); } -/// get_stopping_point_U_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration +/// get_stopping_point_U_m - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration void AC_PosControl::get_stopping_point_U_cm(postype_t &stopping_point_u_cm) const { - float curr_pos_u_cm = _pos_estimate_neu_cm.z; - curr_pos_u_cm -= _pos_offset_neu_cm.z; + postype_t stopping_point_u_m = stopping_point_u_cm * 0.01; + get_stopping_point_U_m(stopping_point_u_m); + stopping_point_u_cm = stopping_point_u_m * 100.0; +} +void AC_PosControl::get_stopping_point_U_m(postype_t &stopping_point_u_m) const +{ + float curr_pos_u_m = _pos_estimate_neu_m.z; + curr_pos_u_m -= _pos_offset_neu_m.z; - float curr_vel_u_cms = _vel_estimate_neu_cms.z; - curr_vel_u_cms -= _vel_offset_neu_cms.z; + float curr_vel_u_ms = _vel_estimate_neu_ms.z; + curr_vel_u_ms -= _vel_offset_neu_ms.z; // avoid divide by zero by using current position if kP is very low or acceleration is zero - if (!is_positive(_p_pos_u_cm.kP()) || !is_positive(_accel_max_u_cmss)) { - stopping_point_u_cm = curr_pos_u_cm; + if (!is_positive(_p_pos_u_m.kP()) || !is_positive(_accel_max_u_mss)) { + stopping_point_u_m = curr_pos_u_m; return; } - stopping_point_u_cm = curr_pos_u_cm + constrain_float(stopping_distance(curr_vel_u_cms, _p_pos_u_cm.kP(), _accel_max_u_cmss), - POSCONTROL_STOPPING_DIST_DOWN_MAX, POSCONTROL_STOPPING_DIST_UP_MAX); + stopping_point_u_m = curr_pos_u_m + constrain_float(stopping_distance(curr_vel_u_ms, _p_pos_u_m.kP(), _accel_max_u_mss), - POSCONTROL_STOPPING_DIST_DOWN_MAX_M, POSCONTROL_STOPPING_DIST_UP_MAX_M); } /// get_bearing_to_target_rad - get bearing to target position in radians float AC_PosControl::get_bearing_to_target_rad() const { - return get_bearing_rad(Vector2f{0.0, 0.0}, (_pos_target_neu_cm.xy() - _pos_estimate_neu_cm.xy()).tofloat()); + return (_pos_target_neu_m.xy() - _pos_estimate_neu_m.xy()).angle(); } @@ -1340,8 +1456,8 @@ void AC_PosControl::update_estimates(bool high_vibes) pos_estimate_ned_m.z = posD; } } - _pos_estimate_neu_cm.xy() = pos_estimate_ned_m.xy() * 100.0; - _pos_estimate_neu_cm.z = -pos_estimate_ned_m.z * 100.0; + _pos_estimate_neu_m.xy() = pos_estimate_ned_m.xy(); + _pos_estimate_neu_m.z = -pos_estimate_ned_m.z; Vector3f vel_estimate_ned_ms; if (!AP::ahrs().get_velocity_NED(vel_estimate_ned_ms) || high_vibes) { @@ -1350,20 +1466,20 @@ void AC_PosControl::update_estimates(bool high_vibes) vel_estimate_ned_ms.z = rate_z; } } - _vel_estimate_neu_cms.xy() = vel_estimate_ned_ms.xy() * 100.0; - _vel_estimate_neu_cms.z = -vel_estimate_ned_ms.z * 100.0; + _vel_estimate_neu_ms.xy() = vel_estimate_ned_ms.xy(); + _vel_estimate_neu_ms.z = -vel_estimate_ned_ms.z; } // get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain) float AC_PosControl::get_throttle_with_vibration_override() { - const float thr_per_accel_u_cmss = _motors.get_throttle_hover() / (GRAVITY_MSS * 100.0f); + const float thr_per_accel_u_mss = _motors.get_throttle_hover() / GRAVITY_MSS; // during vibration compensation use feed forward with manually calculated gain // ToDo: clear pid_info P, I and D terms for logging if (!(_motors.limit.throttle_lower || _motors.limit.throttle_upper) || ((is_positive(_pid_accel_u_cm_to_kt.get_i()) && is_negative(_pid_vel_u_cm.get_error())) || (is_negative(_pid_accel_u_cm_to_kt.get_i()) && is_positive(_pid_vel_u_cm.get_error())))) { - _pid_accel_u_cm_to_kt.set_integrator(_pid_accel_u_cm_to_kt.get_i() + _dt_s * thr_per_accel_u_cmss * 1000.0f * _pid_vel_u_cm.get_error() * _pid_vel_u_cm.kP() * POSCONTROL_VIBE_COMP_I_GAIN); + _pid_accel_u_cm_to_kt.set_integrator(_pid_accel_u_cm_to_kt.get_i() + _dt_s * (thr_per_accel_u_mss / 100.0) * 1000.0 * _pid_vel_u_cm.get_error() * _pid_vel_u_cm.kP() * POSCONTROL_VIBE_COMP_I_GAIN); } - return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accel_u_cmss * _accel_target_neu_cmss.z + _pid_accel_u_cm_to_kt.get_i() * 0.001f; + return POSCONTROL_VIBE_COMP_P_GAIN * thr_per_accel_u_mss * _accel_target_neu_mss.z + _pid_accel_u_cm_to_kt.get_i() * 0.001; } /// standby_NEU_reset - resets I terms and removes position error @@ -1376,7 +1492,7 @@ void AC_PosControl::standby_NEU_reset() _pid_accel_u_cm_to_kt.set_integrator(0.0f); // Set the target position to the current pos. - _pos_target_neu_cm = _pos_estimate_neu_cm; + _pos_target_neu_m = _pos_estimate_neu_m; // Set _pid_vel_ne_cm integrator and derivative to zero. _pid_vel_ne_cm.reset_filter(); @@ -1390,33 +1506,33 @@ void AC_PosControl::standby_NEU_reset() void AC_PosControl::write_log() { if (is_active_NE()) { - float accel_n_cmss, accel_e_cmss; - lean_angles_to_accel_NE_cmss(accel_n_cmss, accel_e_cmss); - Write_PSCN(_pos_desired_neu_cm.x, _pos_target_neu_cm.x, _pos_estimate_neu_cm.x , - _vel_desired_neu_cms.x, _vel_target_neu_cms.x, _vel_estimate_neu_cms.x, - _accel_desired_neu_cmss.x, _accel_target_neu_cmss.x, accel_n_cmss); - Write_PSCE(_pos_desired_neu_cm.y, _pos_target_neu_cm.y, _pos_estimate_neu_cm.y, - _vel_desired_neu_cms.y, _vel_target_neu_cms.y, _vel_estimate_neu_cms.y, - _accel_desired_neu_cmss.y, _accel_target_neu_cmss.y, accel_e_cmss); + float accel_n_mss, accel_e_mss; + lean_angles_to_accel_NE_mss(accel_n_mss, accel_e_mss); + Write_PSCN(_pos_desired_neu_m.x, _pos_target_neu_m.x, _pos_estimate_neu_m.x , + _vel_desired_neu_ms.x, _vel_target_neu_ms.x, _vel_estimate_neu_ms.x, + _accel_desired_neu_mss.x, _accel_target_neu_mss.x, accel_n_mss); + Write_PSCE(_pos_desired_neu_m.y, _pos_target_neu_m.y, _pos_estimate_neu_m.y, + _vel_desired_neu_ms.y, _vel_target_neu_ms.y, _vel_estimate_neu_ms.y, + _accel_desired_neu_mss.y, _accel_target_neu_mss.y, accel_e_mss); // log offsets if they are being used - if (!_pos_offset_neu_cm.xy().is_zero()) { - Write_PSON(_pos_offset_target_neu_cm.x, _pos_offset_neu_cm.x, _vel_offset_target_neu_cms.x, _vel_offset_neu_cms.x, _accel_offset_target_neu_cmss.x, _accel_offset_neu_cmss.x); - Write_PSOE(_pos_offset_target_neu_cm.y, _pos_offset_neu_cm.y, _vel_offset_target_neu_cms.y, _vel_offset_neu_cms.y, _accel_offset_target_neu_cmss.y, _accel_offset_neu_cmss.y); + if (!_pos_offset_neu_m.xy().is_zero()) { + Write_PSON(_pos_offset_target_neu_m.x, _pos_offset_neu_m.x, _vel_offset_target_neu_ms.x, _vel_offset_neu_ms.x, _accel_offset_target_neu_mss.x, _accel_offset_neu_mss.x); + Write_PSOE(_pos_offset_target_neu_m.y, _pos_offset_neu_m.y, _vel_offset_target_neu_ms.y, _vel_offset_neu_ms.y, _accel_offset_target_neu_mss.y, _accel_offset_neu_mss.y); } } if (is_active_U()) { - Write_PSCD(-_pos_desired_neu_cm.z, -_pos_target_neu_cm.z, -_pos_estimate_neu_cm.z, - -_vel_desired_neu_cms.z, -_vel_target_neu_cms.z, -_vel_estimate_neu_cms.z, - -_accel_desired_neu_cmss.z, -_accel_target_neu_cmss.z, -get_measured_accel_U_cmss()); + Write_PSCD(-_pos_desired_neu_m.z, -_pos_target_neu_m.z, -_pos_estimate_neu_m.z, + -_vel_desired_neu_ms.z, -_vel_target_neu_ms.z, -_vel_estimate_neu_ms.z, + -_accel_desired_neu_mss.z, -_accel_target_neu_mss.z, -get_measured_accel_U_mss()); // log down and terrain offsets if they are being used - if (!is_zero(_pos_offset_neu_cm.z)) { - Write_PSOD(-_pos_offset_target_neu_cm.z, -_pos_offset_neu_cm.z, -_vel_offset_target_neu_cms.z, -_vel_offset_neu_cms.z, -_accel_offset_target_neu_cmss.z, -_accel_offset_neu_cmss.z); + if (!is_zero(_pos_offset_neu_m.z)) { + Write_PSOD(-_pos_offset_target_neu_m.z, -_pos_offset_neu_m.z, -_vel_offset_target_neu_ms.z, -_vel_offset_neu_ms.z, -_accel_offset_target_neu_mss.z, -_accel_offset_neu_mss.z); } - if (!is_zero(_pos_terrain_u_cm)) { - Write_PSOT(-_pos_terrain_target_u_cm, -_pos_terrain_u_cm, 0, -_vel_terrain_u_cms, 0, -_accel_terrain_u_cmss); + if (!is_zero(_pos_terrain_u_m)) { + Write_PSOT(-_pos_terrain_target_u_m, -_pos_terrain_u_m, 0, -_vel_terrain_u_ms, 0, -_accel_terrain_u_mss); } } } @@ -1425,13 +1541,13 @@ void AC_PosControl::write_log() /// crosstrack_error - returns horizontal error to the closest point to the current track float AC_PosControl::crosstrack_error() const { - const Vector2f pos_error = (_pos_target_neu_cm.xy() - _pos_estimate_neu_cm.xy()).tofloat(); - if (is_zero(_vel_desired_neu_cms.xy().length_squared())) { + const Vector2f pos_error = (_pos_target_neu_m.xy() - _pos_estimate_neu_m.xy()).tofloat(); + if (is_zero(_vel_desired_neu_ms.xy().length_squared())) { // crosstrack is the horizontal distance to target when stationary return pos_error.length(); } else { // crosstrack is the horizontal distance to the closest point to the current track - const Vector2f vel_unit = _vel_desired_neu_cms.xy().normalized(); + const Vector2f vel_unit = _vel_desired_neu_ms.xy().normalized(); const float dot_error = pos_error * vel_unit; // todo: remove MAX of zero when safe_sqrt fixed @@ -1443,16 +1559,16 @@ float AC_PosControl::crosstrack_error() const /// returns true when the forward pitch demand is limited by the maximum allowed tilt bool AC_PosControl::get_fwd_pitch_is_limited() const { - if (_limit_vector.xy().is_zero()) { + if (_limit_vector_neu.xy().is_zero()) { return false; } const float angle_max_rad = MIN(_attitude_control.get_althold_lean_angle_max_rad(), get_lean_angle_max_rad()); - const float accel_max_cmss = angle_rad_to_accel_mss(angle_max_rad) * 100.0; + const float accel_max_mss = angle_rad_to_accel_mss(angle_max_rad); // Check for pitch limiting in the forward direction - const float accel_fwd_unlimited_cmss = _limit_vector.x * _ahrs.cos_yaw() + _limit_vector.y * _ahrs.sin_yaw(); - const float pitch_target_unlimited_deg = accel_mss_to_angle_deg(- MIN(accel_fwd_unlimited_cmss, accel_max_cmss) * 0.01f); - const float accel_fwd_limited = _accel_target_neu_cmss.x * _ahrs.cos_yaw() + _accel_target_neu_cmss.y * _ahrs.sin_yaw(); - const float pitch_target_limited_deg = accel_mss_to_angle_deg(- accel_fwd_limited * 0.01f); + const float accel_fwd_unlimited_mss = _limit_vector_neu.x * _ahrs.cos_yaw() + _limit_vector_neu.y * _ahrs.sin_yaw(); + const float pitch_target_unlimited_deg = accel_mss_to_angle_deg(- MIN(accel_fwd_unlimited_mss, accel_max_mss)); + const float accel_fwd_limited = _accel_target_neu_mss.x * _ahrs.cos_yaw() + _accel_target_neu_mss.y * _ahrs.sin_yaw(); + const float pitch_target_limited_deg = accel_mss_to_angle_deg(- accel_fwd_limited); return is_negative(pitch_target_unlimited_deg) && pitch_target_unlimited_deg < pitch_target_limited_deg; } @@ -1468,27 +1584,31 @@ bool AC_PosControl::get_fwd_pitch_is_limited() const void AC_PosControl::update_terrain() { // update position, velocity, acceleration offsets for this iteration - postype_t pos_terrain_u_cm = _pos_terrain_u_cm; - update_pos_vel_accel(pos_terrain_u_cm, _vel_terrain_u_cms, _accel_terrain_u_cmss, _dt_s, MIN(_limit_vector.z, 0.0f), _p_pos_u_cm.get_error(), _pid_vel_u_cm.get_error()); - _pos_terrain_u_cm = pos_terrain_u_cm; + postype_t pos_terrain_u_m = _pos_terrain_u_m; + update_pos_vel_accel(pos_terrain_u_m, _vel_terrain_u_ms, _accel_terrain_u_mss, _dt_s, MIN(_limit_vector_neu.z, 0.0f), _p_pos_u_m.get_error(), _pid_vel_u_cm.get_error()); + _pos_terrain_u_m = pos_terrain_u_m; // input shape horizontal position, velocity and acceleration offsets - shape_pos_vel_accel(_pos_terrain_target_u_cm, 0.0, 0.0, - _pos_terrain_u_cm, _vel_terrain_u_cms, _accel_terrain_u_cmss, - get_max_speed_down_cms(), get_max_speed_up_cms(), - -get_max_accel_U_cmss(), get_max_accel_U_cmss(), - _jerk_max_u_cmsss, _dt_s, false); + shape_pos_vel_accel(_pos_terrain_target_u_m, 0.0, 0.0, + _pos_terrain_u_m, _vel_terrain_u_ms, _accel_terrain_u_mss, + get_max_speed_down_ms(), get_max_speed_up_ms(), + -get_max_accel_U_mss(), get_max_accel_U_mss(), + _jerk_max_u_msss, _dt_s, false); - // we do not have to update _pos_terrain_target_u_cm because we assume the target velocity and acceleration are zero - // if we know how fast the terain altitude is changing we would add update_pos_vel_accel for _pos_terrain_target_u_cm here + // we do not have to update _pos_terrain_target_u_m because we assume the target velocity and acceleration are zero + // if we know how fast the terain altitude is changing we would add update_pos_vel_accel for _pos_terrain_target_u_m here } // get_lean_angles_to_accel - convert NE frame accelerations in cm/s/s to roll, pitch lean angles in centi-degrees void AC_PosControl::accel_NE_cmss_to_lean_angles_rad(float accel_n_cmss, float accel_e_cmss, float& roll_target_rad, float& pitch_target_rad) const +{ + accel_NE_mss_to_lean_angles_rad(accel_n_cmss * 0.01, accel_e_cmss * 0.01, roll_target_rad, pitch_target_rad); +} +void AC_PosControl::accel_NE_mss_to_lean_angles_rad(float accel_n_mss, float accel_e_mss, float& roll_target_rad, float& pitch_target_rad) const { // rotate accelerations into body forward-right frame - const float accel_forward_mss = (accel_n_cmss * _ahrs.cos_yaw() + accel_e_cmss * _ahrs.sin_yaw()) * 0.01; - const float accel_right_mss = (-accel_n_cmss * _ahrs.sin_yaw() + accel_e_cmss * _ahrs.cos_yaw()) * 0.01; + const float accel_forward_mss = accel_n_mss * _ahrs.cos_yaw() + accel_e_mss * _ahrs.sin_yaw(); + const float accel_right_mss = -accel_n_mss * _ahrs.sin_yaw() + accel_e_mss * _ahrs.cos_yaw(); // update angle targets that will be passed to stabilize controller pitch_target_rad = accel_mss_to_angle_rad(-accel_forward_mss); @@ -1496,17 +1616,24 @@ void AC_PosControl::accel_NE_cmss_to_lean_angles_rad(float accel_n_cmss, float a roll_target_rad = accel_mss_to_angle_rad(accel_right_mss * cos_pitch_target); } -// lean_angles_to_accel_NE_cmss - convert roll, pitch lean target angles to NE frame accelerations in cm/s/s +// lean_angles_to_accel_NE_mss - convert roll, pitch lean target angles to NE frame accelerations in cm/s/s // todo: this should be based on thrust vector attitude control void AC_PosControl::lean_angles_to_accel_NE_cmss(float& accel_n_cmss, float& accel_e_cmss) const +{ + float accel_n_mss, accel_e_mss; + lean_angles_to_accel_NE_mss(accel_n_mss, accel_e_mss); + accel_n_cmss = accel_n_mss * 100.0; + accel_e_cmss = accel_e_mss * 100.0; +} +void AC_PosControl::lean_angles_to_accel_NE_mss(float& accel_n_mss, float& accel_e_mss) const { // rotate our roll, pitch angles into lat/lon frame Vector3f att_target_euler_rad = _attitude_control.get_att_target_euler_rad(); att_target_euler_rad.z = _ahrs.yaw; - Vector3f accel_ne_cmss = lean_angles_to_accel_NEU_cmss(att_target_euler_rad); + Vector3f accel_ne_mss = lean_angles_rad_to_accel_NEU_mss(att_target_euler_rad); - accel_n_cmss = accel_ne_cmss.x; - accel_e_cmss = accel_ne_cmss.y; + accel_n_mss = accel_ne_mss.x; + accel_e_mss = accel_ne_mss.y; } // calculate_yaw_and_rate_yaw - update the calculated the vehicle yaw and rate of yaw. @@ -1514,20 +1641,20 @@ void AC_PosControl::calculate_yaw_and_rate_yaw() { // Calculate the turn rate float turn_rate_rads = 0.0f; - const float vel_desired_length_ne_cms = _vel_desired_neu_cms.xy().length(); - if (is_positive(vel_desired_length_ne_cms)) { - const float accel_forward_cmss = (_accel_desired_neu_cmss.x * _vel_desired_neu_cms.x + _accel_desired_neu_cmss.y * _vel_desired_neu_cms.y) / vel_desired_length_ne_cms; - const Vector2f accel_turn_ne_cmss = _accel_desired_neu_cmss.xy() - _vel_desired_neu_cms.xy() * accel_forward_cmss / vel_desired_length_ne_cms; - const float accel_turn_length_ne_cmss = accel_turn_ne_cmss.length(); - turn_rate_rads = accel_turn_length_ne_cmss / vel_desired_length_ne_cms; - if ((accel_turn_ne_cmss.y * _vel_desired_neu_cms.x - accel_turn_ne_cmss.x * _vel_desired_neu_cms.y) < 0.0) { + const float vel_desired_length_ne_ms = _vel_desired_neu_ms.xy().length(); + if (is_positive(vel_desired_length_ne_ms)) { + const float accel_forward_mss = (_accel_desired_neu_mss.x * _vel_desired_neu_ms.x + _accel_desired_neu_mss.y * _vel_desired_neu_ms.y) / vel_desired_length_ne_ms; + const Vector2f accel_turn_ne_mss = _accel_desired_neu_mss.xy() - _vel_desired_neu_ms.xy() * accel_forward_mss / vel_desired_length_ne_ms; + const float accel_turn_length_ne_mss = accel_turn_ne_mss.length(); + turn_rate_rads = accel_turn_length_ne_mss / vel_desired_length_ne_ms; + if ((accel_turn_ne_mss.y * _vel_desired_neu_ms.x - accel_turn_ne_mss.x * _vel_desired_neu_ms.y) < 0.0) { turn_rate_rads = -turn_rate_rads; } } - // update the target yaw if velocity is greater than 5% _vel_max_ne_cms - if (vel_desired_length_ne_cms > _vel_max_ne_cms * 0.05f) { - _yaw_target_rad = _vel_desired_neu_cms.xy().angle(); + // update the target yaw if velocity is greater than 5% _vel_max_ne_ms + if (vel_desired_length_ne_ms > _vel_max_ne_ms * 0.05f) { + _yaw_target_rad = _vel_desired_neu_ms.xy().angle(); _yaw_rate_target_rads = turn_rate_rads; return; } @@ -1540,11 +1667,11 @@ void AC_PosControl::calculate_yaw_and_rate_yaw() // calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected float AC_PosControl::calculate_overspeed_gain() { - if (_vel_desired_neu_cms.z < _vel_max_down_cms && !is_zero(_vel_max_down_cms)) { - return POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_neu_cms.z / _vel_max_down_cms; + if (_vel_desired_neu_ms.z < _vel_max_down_ms && !is_zero(_vel_max_down_ms)) { + return POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_neu_ms.z / _vel_max_down_ms; } - if (_vel_desired_neu_cms.z > _vel_max_up_cms && !is_zero(_vel_max_up_cms)) { - return POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_neu_cms.z / _vel_max_up_cms; + if (_vel_desired_neu_ms.z > _vel_max_up_ms && !is_zero(_vel_max_up_ms)) { + return POSCONTROL_OVERSPEED_GAIN_U * _vel_desired_neu_ms.z / _vel_max_up_ms; } return 1.0; } @@ -1568,10 +1695,10 @@ void AC_PosControl::handle_ekf_NE_reset() // for this we need some sort of switch to select what type of EKF handling we want to use // To zero real position shift during relative position modes like Loiter, PosHold, Guided velocity and accleration control. - _pos_target_neu_cm.xy() = _pos_estimate_neu_cm.xy() + _p_pos_ne_cm.get_error().topostype(); - _pos_desired_neu_cm.xy() = _pos_target_neu_cm.xy() - _pos_offset_neu_cm.xy(); - _vel_target_neu_cms.xy() = _vel_estimate_neu_cms.xy() + _pid_vel_ne_cm.get_error(); - _vel_desired_neu_cms.xy() = _vel_target_neu_cms.xy() - _vel_offset_neu_cms.xy(); + _pos_target_neu_m.xy() = _pos_estimate_neu_m.xy() + _p_pos_ne_m.get_error().topostype(); + _pos_desired_neu_m.xy() = _pos_target_neu_m.xy() - _pos_offset_neu_m.xy(); + _vel_target_neu_ms.xy() = _vel_estimate_neu_ms.xy() + _pid_vel_ne_cm.get_error() * 0.01; + _vel_desired_neu_ms.xy() = _vel_target_neu_ms.xy() - _vel_offset_neu_ms.xy(); _ekf_ne_reset_ms = reset_ms; } @@ -1596,10 +1723,10 @@ void AC_PosControl::handle_ekf_U_reset() // for this we need some sort of switch to select what type of EKF handling we want to use // To zero real position shift during relative position modes like Loiter, PosHold, Guided velocity and accleration control. - _pos_target_neu_cm.z = _pos_estimate_neu_cm.z + _p_pos_u_cm.get_error(); - _pos_desired_neu_cm.z = _pos_target_neu_cm.z - (_pos_offset_neu_cm.z + _pos_terrain_u_cm); - _vel_target_neu_cms.z = _vel_estimate_neu_cms.z + _pid_vel_u_cm.get_error(); - _vel_desired_neu_cms.z = _vel_target_neu_cms.z - (_vel_offset_neu_cms.z + _vel_terrain_u_cms); + _pos_target_neu_m.z = _pos_estimate_neu_m.z + _p_pos_u_m.get_error(); + _pos_desired_neu_m.z = _pos_target_neu_m.z - (_pos_offset_neu_m.z + _pos_terrain_u_m); + _vel_target_neu_ms.z = _vel_estimate_neu_ms.z + _pid_vel_u_cm.get_error() * 0.01; + _vel_desired_neu_ms.z = _vel_target_neu_ms.z - (_vel_offset_neu_ms.z + _vel_terrain_u_ms); _ekf_u_reset_ms = reset_ms; } diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 76cb4cde199..995b198ed63 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -16,18 +16,18 @@ #include // position controller default definitions -#define POSCONTROL_ACCEL_NE 100.0f // default horizontal acceleration in cm/s/s. This is overwritten by waypoint and loiter controllers -#define POSCONTROL_JERK_NE 5.0f // default horizontal jerk m/s/s/s +#define POSCONTROL_ACCEL_NE_MSS 1.0f // default horizontal acceleration in m/s/s. This is overwritten by waypoint and loiter controllers +#define POSCONTROL_JERK_NE_MSSS 5.0f // default horizontal jerk m/s/s/s -#define POSCONTROL_STOPPING_DIST_UP_MAX 300.0f // max stopping distance (in cm) vertically while climbing -#define POSCONTROL_STOPPING_DIST_DOWN_MAX 200.0f // max stopping distance (in cm) vertically while descending +#define POSCONTROL_STOPPING_DIST_UP_MAX_M 3.0f // max stopping distance (in m) vertically while climbing +#define POSCONTROL_STOPPING_DIST_DOWN_MAX_M 2.0f // max stopping distance (in m) vertically while descending -#define POSCONTROL_SPEED 500.0f // default horizontal speed in cm/s -#define POSCONTROL_SPEED_DOWN -150.0f // default descent rate in cm/s -#define POSCONTROL_SPEED_UP 250.0f // default climb rate in cm/s +#define POSCONTROL_SPEED_MS 5.0f // default horizontal speed in m/s +#define POSCONTROL_SPEED_DOWN_MS -1.5f // default descent rate in m/s +#define POSCONTROL_SPEED_UP_MS 2.5f // default climb rate in m/s -#define POSCONTROL_ACCEL_U 250.0f // default vertical acceleration in cm/s/s. -#define POSCONTROL_JERK_U 5.0f // default vertical jerk m/s/s/s +#define POSCONTROL_ACCEL_U_MSS 2.5f // default vertical acceleration in m/s/s. +#define POSCONTROL_JERK_U_MSSS 5.0f // default vertical jerk m/s/s/s #define POSCONTROL_THROTTLE_CUTOFF_FREQ_HZ 2.0f // low-pass filter on acceleration error (unit: Hz) @@ -45,7 +45,7 @@ public: // do not allow copying CLASS_NO_COPY(AC_PosControl); - /// set_dt_s / get_dt_s - dt is the time in seconds since the last time the position controllers were updated + /// set_dt / get_dt - dt is the time in seconds since the last time the position controllers were updated /// _dt_s should be set based on the time of the last IMU read used by these controllers /// the position controller should run updates for active controllers on each loop to ensure normal operation void set_dt_s(float dt) { _dt_s = dt; } @@ -56,50 +56,59 @@ public: // When high_vibes is true, forces use of vertical fallback for velocity. void update_estimates(bool high_vibes = false); - /// get_shaping_jerk_NE_cmsss - gets the jerk limit of the ne kinematic path generation in cm/s/s/s - float get_shaping_jerk_NE_cmsss() const { return _shaping_jerk_ne_msss * 100.0; } + /// get_shaping_jerk_NE_msss - gets the jerk limit of the ne kinematic path generation in cm/s/s/s + float get_shaping_jerk_NE_cmsss() const { return get_shaping_jerk_NE_msss() * 100.0; } + float get_shaping_jerk_NE_msss() const { return _shaping_jerk_ne_msss; } /// /// 3D position shaper /// - /// input_pos_NEU_cm - computes a jerk-limited trajectory from the current NEU position, velocity, and acceleration to a new position input (in cm). + /// input_pos_NEU_m - computes a jerk-limited trajectory from the current NEU position, velocity, and acceleration to a new position input (in cm). /// This function updates the desired acceleration using a smooth kinematic path constrained by acceleration and jerk limits. void input_pos_NEU_cm(const Vector3p& pos_neu_cm, float pos_terrain_target_alt_cm, float terrain_buffer_cm); + void input_pos_NEU_m(const Vector3p& pos_neu_m, float pos_terrain_target_alt_m, float terrain_buffer_m); - /// pos_terrain_U_scaler_cm - computes a scaling factor applied to horizontal velocity limits to ensure the vertical position controller remains within its terrain buffer. + /// pos_terrain_U_scaler_m - computes a scaling factor applied to horizontal velocity limits to ensure the vertical position controller remains within its terrain buffer. float pos_terrain_U_scaler_cm(float pos_terrain_u_cm, float pos_terrain_u_buffer_cm) const; + float pos_terrain_U_scaler_m(float pos_terrain_u_m, float pos_terrain_u_buffer_m) const; /// /// Lateral position controller /// - /// set_max_speed_accel_NE_cm - set the maximum horizontal speed in cm/s and acceleration in cm/s/s + /// set_max_speed_accel_NE_m - set the maximum horizontal speed in cm/s and acceleration in cm/s/s /// This function only needs to be called if using the kinematic shaping. /// This can be done at any time as changes in these parameters are handled smoothly /// by the kinematic shaping. void set_max_speed_accel_NE_cm(float speed_cms, float accel_cmss); + void set_max_speed_accel_NE_m(float speed_ms, float accel_mss); - /// set_correction_speed_accel_NE_cm - set the position controller correction velocity and acceleration limit + /// set_correction_speed_accel_NE_m - set the position controller correction velocity and acceleration limit /// This should be done only during initialisation to avoid discontinuities void set_correction_speed_accel_NE_cm(float speed_cms, float accel_cmss); + void set_correction_speed_accel_NE_m(float speed_ms, float accel_mss); - /// get_max_speed_NE_cms - get the maximum horizontal speed in cm/s - float get_max_speed_NE_cms() const { return _vel_max_ne_cms; } + /// get_max_speed_NE_ms - get the maximum horizontal speed in cm/s + float get_max_speed_NE_cms() const { return get_max_speed_NE_ms() * 100.0; } + float get_max_speed_NE_ms() const { return _vel_max_ne_ms; } - /// get_max_accel_NE_cmss - get the maximum horizontal acceleration in cm/s/s - float get_max_accel_NE_cmss() const { return _accel_max_ne_cmss; } + /// get_max_accel_NE_mss - get the maximum horizontal acceleration in cm/s/s + float get_max_accel_NE_cmss() const { return get_max_accel_NE_mss() * 100.0; } + float get_max_accel_NE_mss() const { return _accel_max_ne_mss; } - // set_pos_error_max_NE_cm - set the maximum horizontal position error that will be allowed in the horizontal plane - void set_pos_error_max_NE_cm(float error_max_cm) { _p_pos_ne_cm.set_error_max(error_max_cm); } + // set_pos_error_max_NE_m - set the maximum horizontal position error that will be allowed in the horizontal plane + void set_pos_error_max_NE_cm(float error_max_cm) { set_pos_error_max_NE_m(error_max_cm * 0.01); } + void set_pos_error_max_NE_m(float error_max_m) { _p_pos_ne_m.set_error_max(error_max_m); } - // get_pos_error_max_NE_cm - return the maximum horizontal position error that will be allowed in the horizontal plane - float get_pos_error_max_NE_cm() { return _p_pos_ne_cm.get_error_max(); } + // get_pos_error_max_NE_m - return the maximum horizontal position error that will be allowed in the horizontal plane + float get_pos_error_max_NE_cm() { return get_pos_error_max_NE_m() * 100.0; } + float get_pos_error_max_NE_m() { return _p_pos_ne_m.get_error_max(); } /// init_NE_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration. /// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position. - /// The starting position can be retrieved by getting the position target using get_pos_target_NEU_cm() after calling this function. + /// The starting position can be retrieved by getting the position target using get_pos_target_NEU_m() after calling this function. void init_NE_controller_stopping_point(); // relax_velocity_controller_NE - initialise the position controller to the current position and velocity with decaying acceleration. @@ -114,26 +123,29 @@ public: /// This function is private and contains all the shared ne axis initialisation functions void init_NE_controller(); - /// input_accel_NE_cm - computes a jerk-limited trajectory to smoothly reach the specified acceleration in the NE plane from the current position, velocity, and acceleration. + /// input_accel_NE_m - computes a jerk-limited trajectory to smoothly reach the specified acceleration in the NE plane from the current position, velocity, and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_ne. /// The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// The jerk limit also defines the time taken to achieve the maximum acceleration. void input_accel_NE_cm(const Vector3f& accel_neu_cmsss); + void input_accel_NE_m(const Vector3f& accel_neu_msss); - /// input_vel_accel_NE_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. + /// input_vel_accel_NE_m - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_ne. - /// The function modifies vel_ne_cms to follow the kinematic trajectory toward accel_cmss. + /// The function modifies vel_ne_ms to follow the kinematic trajectory toward accel_mss. /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. void input_vel_accel_NE_cm(Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output = true); + void input_vel_accel_NE_m(Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output = true); - /// input_pos_vel_accel_NE_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. + /// input_pos_vel_accel_NE_m - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_ne. - /// The function modifies pos_ne_cm and vel_ne_cms to follow the jerk-limited trajectory defined by accel_ne_cmss. + /// The function modifies pos_ne_m and vel_ne_ms to follow the jerk-limited trajectory defined by accel_ne_mss. /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. void input_pos_vel_accel_NE_cm(Vector2p& pos_ne_cm, Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss, bool limit_output = true); + void input_pos_vel_accel_NE_m(Vector2p& pos_ne_m, Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss, bool limit_output = true); // is_active_NE - returns true if the ne position controller has been run in the previous 5 loop times bool is_active_NE() const; @@ -159,31 +171,38 @@ public: /// Vertical position controller /// - /// set_max_speed_accel_U_cmss - set the maximum vertical speed in cm/s and acceleration in cm/s/s - /// speed_down_cms may be positive or negative, but it is always interpreted as a descent rate. + /// set_max_speed_accel_U_mss - set the maximum vertical speed in cm/s and acceleration in cm/s/s + /// speed_down_ms may be positive or negative, but it is always interpreted as a descent rate. /// This can be done at any time as changes in these parameters are handled smoothly /// by the kinematic shaping. void set_max_speed_accel_U_cm(float speed_down_cms, float speed_up_cms, float accel_cmss); + void set_max_speed_accel_U_m(float speed_down_ms, float speed_up_ms, float accel_mss); - /// set_correction_speed_accel_U_cmss - set the position controller correction velocity and acceleration limit - /// speed_down_cms may be positive or negative, but it is always interpreted as a descent rate. + /// set_correction_speed_accel_U_mss - set the position controller correction velocity and acceleration limit + /// speed_down_ms may be positive or negative, but it is always interpreted as a descent rate. /// This should be done only during initialisation to avoid discontinuities void set_correction_speed_accel_U_cmss(float speed_down_cms, float speed_up_cms, float accel_cmss); + void set_correction_speed_accel_U_mss(float speed_down_ms, float speed_up_ms, float accel_mss); - /// get_max_accel_U_cmss - get the maximum vertical acceleration in cm/s/s - float get_max_accel_U_cmss() const { return _accel_max_u_cmss; } + /// get_max_accel_U_mss - get the maximum vertical acceleration in cm/s/s + float get_max_accel_U_cmss() const { return get_max_accel_U_mss() * 100.0; } + float get_max_accel_U_mss() const { return _accel_max_u_mss; } - // get_pos_error_up_cm - get the allowed upper bound of vertical position error (positive direction) - float get_pos_error_up_cm() { return _p_pos_u_cm.get_error_max(); } + // get_pos_error_up_m - get the allowed upper bound of vertical position error (positive direction) + float get_pos_error_up_cm() { return get_pos_error_up_m() * 100.0; } + float get_pos_error_up_m() { return _p_pos_u_m.get_error_max(); } - // get_pos_error_down_cm - get the allowed lower bound of vertical position error (negative direction) - float get_pos_error_down_cm() { return _p_pos_u_cm.get_error_min(); } + // get_pos_error_down_m - get the allowed lower bound of vertical position error (negative direction) + float get_pos_error_down_cm() { return get_pos_error_down_m() * 100.0; } + float get_pos_error_down_m() { return _p_pos_u_m.get_error_min(); } - /// get_max_speed_up_cms - accessors for current maximum up speed in cm/s - float get_max_speed_up_cms() const { return _vel_max_up_cms; } + /// get_max_speed_up_ms - accessors for current maximum up speed in cm/s + float get_max_speed_up_cms() const { return get_max_speed_up_ms() * 100.0; } + float get_max_speed_up_ms() const { return _vel_max_up_ms; } - /// get_max_speed_down_cms - accessors for current maximum down speed in cm/s. Will be a negative number - float get_max_speed_down_cms() const { return _vel_max_down_cms; } + /// get_max_speed_down_ms - accessors for current maximum down speed in cm/s. Will be a negative number + float get_max_speed_down_cms() const { return get_max_speed_down_ms() * 100.0; } + float get_max_speed_down_ms() const { return _vel_max_down_ms; } /// init_U_controller_no_descent - initialise the position controller to the current position, velocity, acceleration and attitude. /// This function is the default initialisation for any position control that provides position, velocity and acceleration. @@ -192,7 +211,7 @@ public: /// init_U_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration. /// This function should be used when the expected kinematic path assumes a stationary initial condition but does not specify a specific starting position. - /// The starting position can be retrieved by getting the position target using get_pos_target_NEU_cm() after calling this function. + /// The starting position can be retrieved by getting the position target using get_pos_target_NEU_m() after calling this function. void init_U_controller_stopping_point(); // relax_U_controller - initialise the position controller to the current position and velocity with decaying acceleration. @@ -204,37 +223,43 @@ public: /// This function is private and contains all the shared z axis initialisation functions void init_U_controller(); - /// input_accel_U_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. + /// input_accel_U_m - calculate a jerk limited path from the current position, velocity and acceleration to an input acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z. virtual void input_accel_U_cm(float accel_u_cmss); + virtual void input_accel_U_m(float accel_u_mss); - /// input_vel_accel_U_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. + /// input_vel_accel_U_m - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_z. - /// The function modifies vel_u_cms to follow the jerk-limited trajectory defined by accel_u_cmss. + /// The function modifies vel_u_ms to follow the jerk-limited trajectory defined by accel_u_mss. /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. virtual void input_vel_accel_U_cm(float &vel_u_cms, float accel_u_cmss, bool limit_output = true); + virtual void input_vel_accel_U_m(float &vel_u_ms, float accel_u_mss, bool limit_output = true); - /// set_pos_target_U_from_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s + /// set_pos_target_U_from_climb_rate_m - adjusts target up or down using a commanded climb rate in cm/s /// using the default jerk-limited kinematic shaping method. /// The zero target altitude is varied to follow pos_offset_u void set_pos_target_U_from_climb_rate_cm(float vel_u_cms); + void set_pos_target_U_from_climb_rate_m(float vel_u_ms); - /// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s + /// land_at_climb_rate_m - adjusts target up or down using a commanded climb rate in cm/s /// using the default jerk-limited kinematic shaping method. /// ignore_descent_limit turns off output saturation handling to aid in landing detection. ignore_descent_limit should be true unless landing. void land_at_climb_rate_cm(float vel_u_cms, bool ignore_descent_limit); + void land_at_climb_rate_m(float vel_u_ms, bool ignore_descent_limit); - /// input_pos_vel_accel_U_cm - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. + /// input_pos_vel_accel_U_m - calculate a jerk limited path from the current position, velocity and acceleration to an input position velocity and acceleration. /// The function takes the current position, velocity, and acceleration and calculates the required jerk limited adjustment to the acceleration for the next time dt. - /// The function alters the pos_u_cm and vel_u_cms to be the kinematic path based on accel_u_cmss + /// The function alters the pos_u_m and vel_u_ms to be the kinematic path based on accel_u_mss /// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. void input_pos_vel_accel_U_cm(float &pos_u_cm, float &vel_u_cms, float accel_u_cmss, bool limit_output = true); + void input_pos_vel_accel_U_m(float &pos_u_m, float &vel_u_ms, float accel_u_mss, bool limit_output = true); - /// set_alt_target_with_slew_cm - adjusts target up or down using a commanded altitude in cm + /// set_alt_target_with_slew_m - adjusts target up or down using a commanded altitude in cm /// using the default jerk-limited kinematic shaping method. void set_alt_target_with_slew_cm(float pos_u_cm); + void set_alt_target_with_slew_m(float pos_u_m); // is_active_U - returns true if the z position controller has been run in the previous 5 loop times bool is_active_U() const; @@ -253,97 +278,124 @@ public: /// set commanded position (cm), velocity (cm/s) and acceleration (cm/s/s) inputs when the path is created externally. void set_pos_vel_accel_NEU_cm(const Vector3p& pos_neu_cm, const Vector3f& vel_neu_cms, const Vector3f& accel_neu_cmss); + void set_pos_vel_accel_NEU_m(const Vector3p& pos_neu_m, const Vector3f& vel_neu_ms, const Vector3f& accel_neu_mss); void set_pos_vel_accel_NE_cm(const Vector2p& pos_ne_cm, const Vector2f& vel_ne_cms, const Vector2f& accel_ne_cmss); + void set_pos_vel_accel_NE_m(const Vector2p& pos_ne_m, const Vector2f& vel_ne_ms, const Vector2f& accel_ne_mss); /// Position - /// get_pos_estimate_NEU_cm - returns the current position estimate, frame NEU in cm relative to the EKF origin - const Vector3p& get_pos_estimate_NEU_cm() const { return _pos_estimate_neu_cm; } + /// get_pos_estimate_NEU_m - returns the current position estimate, frame NEU in cm relative to the EKF origin + const Vector3p get_pos_estimate_NEU_cm() const { return get_pos_estimate_NEU_m() * 100.0; } + const Vector3p& get_pos_estimate_NEU_m() const { return _pos_estimate_neu_m; } - /// get_pos_target_NEU_cm - returns the position target, frame NEU in cm relative to the EKF origin - const Vector3p& get_pos_target_NEU_cm() const { return _pos_target_neu_cm; } + /// get_pos_target_NEU_m - returns the position target, frame NEU in cm relative to the EKF origin + const Vector3p get_pos_target_NEU_cm() const { return get_pos_target_NEU_m() * 100.0; } + const Vector3p& get_pos_target_NEU_m() const { return _pos_target_neu_m; } - /// set_pos_desired_NE_cm - sets the position target, frame NEU in cm relative to the EKF origin - void set_pos_desired_NE_cm(const Vector2f& pos_desired_ne_cm) { _pos_desired_neu_cm.xy() = pos_desired_ne_cm.topostype(); } + /// set_pos_desired_NE_m - sets the position target, frame NEU in cm relative to the EKF origin + void set_pos_desired_NE_cm(const Vector2f& pos_desired_ne_cm) { set_pos_desired_NE_m(pos_desired_ne_cm * 0.01); } + void set_pos_desired_NE_m(const Vector2f& pos_desired_ne_m) { _pos_desired_neu_m.xy() = pos_desired_ne_m.topostype(); } - /// get_pos_desired_NEU_cm - returns the position desired, frame NEU in cm relative to the EKF origin - const Vector3p& get_pos_desired_NEU_cm() const { return _pos_desired_neu_cm; } + /// get_pos_desired_NEU_m - returns the position desired, frame NEU in cm relative to the EKF origin + const Vector3p get_pos_desired_NEU_cm() const { return get_pos_desired_NEU_m() * 100.0; } + const Vector3p& get_pos_desired_NEU_m() const { return _pos_desired_neu_m; } - /// get_pos_target_U_cm - get target altitude (in cm above the EKF origin) - float get_pos_target_U_cm() const { return _pos_target_neu_cm.z; } + /// get_pos_target_U_m - get target altitude (in cm above the EKF origin) + float get_pos_target_U_cm() const { return get_pos_target_U_m() * 100.0; } + float get_pos_target_U_m() const { return _pos_target_neu_m.z; } - /// set_pos_desired_U_cm - set altitude target in cm above the EKF origin - void set_pos_desired_U_cm(float pos_desired_u_cm) { _pos_desired_neu_cm.z = pos_desired_u_cm; } + /// set_pos_desired_U_m - set altitude target in cm above the EKF origin + void set_pos_desired_U_cm(float pos_desired_u_cm) { set_pos_desired_U_m(pos_desired_u_cm * 0.01); } + void set_pos_desired_U_m(float pos_desired_u_m) { _pos_desired_neu_m.z = pos_desired_u_m; } - /// get_pos_desired_U_cm - get target altitude (in cm above the EKF origin) - float get_pos_desired_U_cm() const { return _pos_desired_neu_cm.z; } + /// get_pos_desired_U_m - get target altitude (in cm above the EKF origin) + float get_pos_desired_U_cm() const { return get_pos_desired_U_m() * 100.0; } + float get_pos_desired_U_m() const { return _pos_desired_neu_m.z; } /// Stopping Point - /// get_stopping_point_NE_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration + /// get_stopping_point_NE_m - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration void get_stopping_point_NE_cm(Vector2p &stopping_point_neu_cm) const; + void get_stopping_point_NE_m(Vector2p &stopping_point_neu_m) const; - /// get_stopping_point_U_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration + /// get_stopping_point_U_m - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration void get_stopping_point_U_cm(postype_t &stopping_point_u_cm) const; + void get_stopping_point_U_m(postype_t &stopping_point_u_m) const; /// Position Error - /// get_pos_error_NEU_cm - returns the 3D position error vector between the current and target NEU positions. - const Vector3f get_pos_error_NEU_cm() const { return Vector3f(_p_pos_ne_cm.get_error().x, _p_pos_ne_cm.get_error().y, _p_pos_u_cm.get_error()); } + /// get_pos_error_NEU_m - returns the 3D position error vector between the current and target NEU positions. + const Vector3f get_pos_error_NEU_cm() const { return get_pos_error_NEU_m() * 100.0; } + const Vector3f get_pos_error_NEU_m() const { return Vector3f(_p_pos_ne_m.get_error().x, _p_pos_ne_m.get_error().y, _p_pos_u_m.get_error()); } - /// get_pos_error_NE_cm - get the length of the position error vector in the ne plane - float get_pos_error_NE_cm() const { return _p_pos_ne_cm.get_error().length(); } + /// get_pos_error_NE_m - get the length of the position error vector in the ne plane + float get_pos_error_NE_cm() const { return get_pos_error_NE_m() * 100.0; } + float get_pos_error_NE_m() const { return _p_pos_ne_m.get_error().length(); } - /// get_pos_error_U_cm - returns altitude error in cm - float get_pos_error_U_cm() const { return _p_pos_u_cm.get_error(); } + /// get_pos_error_U_m - returns altitude error in cm + float get_pos_error_U_cm() const { return get_pos_error_U_m() * 100.0; } + float get_pos_error_U_m() const { return _p_pos_u_m.get_error(); } /// Velocity - /// get_vel_estimate_NEU_cms - returns current velocity estimate in cm/s in NEU - const Vector3f& get_vel_estimate_NEU_cms() const { return _vel_estimate_neu_cms; } + /// get_vel_estimate_NEU_ms - returns current velocity estimate in cm/s in NEU + const Vector3f get_vel_estimate_NEU_cms() const { return get_vel_estimate_NEU_ms() * 100.0; } + const Vector3f& get_vel_estimate_NEU_ms() const { return _vel_estimate_neu_ms; } - /// set_vel_desired_NEU_cms - sets desired velocity in NEU cm/s - void set_vel_desired_NEU_cms(const Vector3f &vel_desired_neu_cms) { _vel_desired_neu_cms = vel_desired_neu_cms; } + /// set_vel_desired_NEU_ms - sets desired velocity in NEU cm/s + void set_vel_desired_NEU_cms(const Vector3f &vel_desired_neu_cms) { set_vel_desired_NEU_ms(vel_desired_neu_cms * 0.01); } + void set_vel_desired_NEU_ms(const Vector3f &vel_desired_neu_ms) { _vel_desired_neu_ms = vel_desired_neu_ms; } - /// set_vel_desired_NE_cms - sets the desired horizontal velocity (NE only) in cm/s. - void set_vel_desired_NE_cms(const Vector2f &vel_desired_ne_cms) {_vel_desired_neu_cms.xy() = vel_desired_ne_cms; } + /// set_vel_desired_NE_ms - sets the desired horizontal velocity (NE only) in cm/s. + void set_vel_desired_NE_cms(const Vector2f &vel_desired_ne_cms) { set_vel_desired_NE_ms(vel_desired_ne_cms * 0.01); } + void set_vel_desired_NE_ms(const Vector2f &vel_desired_ne_ms) { _vel_desired_neu_ms.xy() = vel_desired_ne_ms; } - /// get_vel_desired_NEU_cms - returns desired velocity in cm/s in NEU - const Vector3f& get_vel_desired_NEU_cms() const { return _vel_desired_neu_cms; } + /// get_vel_desired_NEU_ms - returns desired velocity in cm/s in NEU + const Vector3f get_vel_desired_NEU_cms() const { return get_vel_desired_NEU_ms() * 100.0; } + const Vector3f& get_vel_desired_NEU_ms() const { return _vel_desired_neu_ms; } - // get_vel_target_NEU_cms - returns the target velocity in NEU cm/s - const Vector3f& get_vel_target_NEU_cms() const { return _vel_target_neu_cms; } + // get_vel_target_NEU_ms - returns the target velocity in NEU cm/s + const Vector3f get_vel_target_NEU_cms() const { return get_vel_target_NEU_ms() * 100.0; } + const Vector3f& get_vel_target_NEU_ms() const { return _vel_target_neu_ms; } - /// set_vel_desired_U_cms - sets desired velocity in cm/s in z axis - void set_vel_desired_U_cms(float vel_desired_u_cms) {_vel_desired_neu_cms.z = vel_desired_u_cms;} + /// set_vel_desired_U_ms - sets desired velocity in cm/s in z axis + void set_vel_desired_U_cms(float vel_desired_u_cms) { set_vel_desired_U_ms(vel_desired_u_cms * 0.01); } - /// get_vel_target_U_cms - returns target vertical speed in cm/s - float get_vel_target_U_cms() const { return _vel_target_neu_cms.z; } + void set_vel_desired_U_ms(float vel_desired_u_ms) { _vel_desired_neu_ms.z = vel_desired_u_ms; } + + /// get_vel_target_U_ms - returns target vertical speed in cm/s + float get_vel_target_U_cms() const { return get_vel_target_U_ms() * 100.0; } + float get_vel_target_U_ms() const { return _vel_target_neu_ms.z; } /// Acceleration - // set_accel_desired_NE_cmss - set desired acceleration in cm/s in ne axis - void set_accel_desired_NE_cmss(const Vector2f &accel_desired_neu_cmss) { _accel_desired_neu_cmss.xy() = accel_desired_neu_cmss; } + // set_accel_desired_NE_mss - set desired acceleration in cm/s in ne axis + void set_accel_desired_NE_cmss(const Vector2f &accel_desired_neu_cmss) { set_accel_desired_NE_mss(accel_desired_neu_cmss * 0.01); } + void set_accel_desired_NE_mss(const Vector2f &accel_desired_neu_mss) { _accel_desired_neu_mss.xy() = accel_desired_neu_mss; } - // get_accel_target_NEU_cmss - returns the target acceleration in NEU cm/s/s - const Vector3f& get_accel_target_NEU_cmss() const { return _accel_target_neu_cmss; } + // get_accel_target_NEU_mss - returns the target acceleration in NEU cm/s/s + const Vector3f get_accel_target_NEU_cmss() const { return get_accel_target_NEU_mss() * 100.0; } + const Vector3f& get_accel_target_NEU_mss() const { return _accel_target_neu_mss; } /// Terrain - // set_pos_terrain_target_U_cm - set target terrain altitude in cm - void set_pos_terrain_target_U_cm(float pos_terrain_target_u_cm) {_pos_terrain_target_u_cm = pos_terrain_target_u_cm;} + // set_pos_terrain_target_U_m - set target terrain altitude in cm + void set_pos_terrain_target_U_cm(float pos_terrain_target_u_cm) { set_pos_terrain_target_U_m(pos_terrain_target_u_cm * 0.01); } + void set_pos_terrain_target_U_m(float pos_terrain_target_u_m) { _pos_terrain_target_u_m = pos_terrain_target_u_m; } - // init_pos_terrain_U_cm - initialises the current terrain altitude and target altitude to pos_offset_terrain_cm + // init_pos_terrain_U_m - initialises the current terrain altitude and target altitude to pos_offset_terrain_m void init_pos_terrain_U_cm(float pos_terrain_u_cm); + void init_pos_terrain_U_m(float pos_terrain_u_m); - // get_pos_terrain_U_cm - returns the current terrain altitude in cm - float get_pos_terrain_U_cm() const { return _pos_terrain_u_cm; } + // get_pos_terrain_U_m - returns the current terrain altitude in cm + float get_pos_terrain_U_cm() const { return get_pos_terrain_U_m() * 100.0; } + float get_pos_terrain_U_m() const { return _pos_terrain_u_m; } /// Offset @@ -352,37 +404,49 @@ public: // position, velocity and acceleration offset target (only used by scripting) // gets or sets an additional offset to the vehicle's target position, velocity and acceleration // units are m, m/s and m/s/s in NED frame - bool set_posvelaccel_offset(const Vector3f &pos_offset_NED, const Vector3f &vel_offset_NED, const Vector3f &accel_offset_NED); - bool get_posvelaccel_offset(Vector3f &pos_offset_NED, Vector3f &vel_offset_NED, Vector3f &accel_offset_NED); + bool set_posvelaccel_offset(const Vector3f &pos_offset_NED_m, const Vector3f &vel_offset_NED_ms, const Vector3f &accel_offset_NED_mss); + + bool get_posvelaccel_offset(Vector3f &pos_offset_NED_m, Vector3f &vel_offset_NED_ms, Vector3f &accel_offset_NED_mss); // get target velocity in m/s in NED frame - bool get_vel_target(Vector3f &vel_target_NED); + bool get_vel_target(Vector3f &vel_target_NED_ms); // get target acceleration in m/s/s in NED frame - bool get_accel_target(Vector3f &accel_target_NED); + bool get_accel_target(Vector3f &accel_target_NED_mss); #endif /// set the horizontal position, velocity and acceleration offset targets in cm, cms and cm/s/s from EKF origin in NE frame. /// These offsets must be updated at least every 3 seconds or they will timeout and revert to zero. void set_posvelaccel_offset_target_NE_cm(const Vector2p& pos_offset_target_ne_cm, const Vector2f& vel_offset_target_ne_cms, const Vector2f& accel_offset_target_ne_cmss); + void set_posvelaccel_offset_target_NE_m(const Vector2p& pos_offset_target_ne_m, const Vector2f& vel_offset_target_ne_ms, const Vector2f& accel_offset_target_ne_mss); + void set_posvelaccel_offset_target_U_cm(float pos_offset_target_u_cm, float vel_offset_target_u_cms, float accel_offset_target_u_cmss); + void set_posvelaccel_offset_target_U_m(float pos_offset_target_u_m, float vel_offset_target_u_ms, float accel_offset_target_u_mss); /// get the position, velocity or acceleration offets in cm from EKF origin in NEU frame - const Vector3p& get_pos_offset_NEU_cm() const { return _pos_offset_neu_cm; } - const Vector3f& get_vel_offset_NEU_cms() const { return _vel_offset_neu_cms; } - const Vector3f& get_accel_offset_NEU_cmss() const { return _accel_offset_neu_cmss; } + const Vector3p get_pos_offset_NEU_cm() const { return get_pos_offset_NEU_m() * 100.0; } + const Vector3p& get_pos_offset_NEU_m() const { return _pos_offset_neu_m; } + + const Vector3f get_vel_offset_NEU_cms() const { return get_vel_offset_NEU_ms() * 100.0; } + const Vector3f& get_vel_offset_NEU_ms() const { return _vel_offset_neu_ms; } + + const Vector3f get_accel_offset_NEU_cmss() const { return get_accel_offset_NEU_mss() * 100.0; } + const Vector3f& get_accel_offset_NEU_mss() const { return _accel_offset_neu_mss; } - /// set_pos_offset_U_cm - set altitude offset in cm above the EKF origin - void set_pos_offset_U_cm(float pos_offset_u) { _pos_offset_neu_cm.z = pos_offset_u; } + /// set_pos_offset_U_m - set altitude offset in cm above the EKF origin + void set_pos_offset_U_m(float pos_offset_u_m) { _pos_offset_neu_m.z = pos_offset_u_m; } - /// get_pos_offset_U_cm - returns altitude offset in cm above the EKF origin - float get_pos_offset_U_cm() const { return _pos_offset_neu_cm.z; } + /// get_pos_offset_U_m - returns altitude offset in cm above the EKF origin + float get_pos_offset_U_cm() const { return get_pos_offset_U_m() * 100.0; } + float get_pos_offset_U_m() const { return _pos_offset_neu_m.z; } - /// get_vel_offset_U_cms - returns current vertical offset speed in cm/s - float get_vel_offset_U_cms() const { return _vel_offset_neu_cms.z; } + /// get_vel_offset_U_ms - returns current vertical offset speed in cm/s + float get_vel_offset_U_cms() const { return get_vel_offset_U_ms() * 100.0; } + float get_vel_offset_U_ms() const { return _vel_offset_neu_ms.z; } - /// get_accel_offset_U_cmss - returns current vertical offset acceleration in cm/s/s - float get_accel_offset_U_cmss() const { return _accel_offset_neu_cmss.z; } + /// get_accel_offset_U_mss - returns current vertical offset acceleration in cm/s/s + float get_accel_offset_U_cmss() const { return get_accel_offset_U_mss() * 100.0; } + float get_accel_offset_U_mss() const { return _accel_offset_neu_mss.z; } /// Outputs @@ -434,18 +498,19 @@ public: /// Other /// get pid controllers - AC_P_2D& get_pos_NE_p() { return _p_pos_ne_cm; } - AC_P_1D& get_pos_U_p() { return _p_pos_u_cm; } + AC_P_2D& get_pos_NE_p() { return _p_pos_ne_m; } + AC_P_1D& get_pos_U_p() { return _p_pos_u_m; } AC_PID_2D& get_vel_NE_pid() { return _pid_vel_ne_cm; } AC_PID_Basic& get_vel_U_pid() { return _pid_vel_u_cm; } AC_PID& get_accel_U_pid() { return _pid_accel_u_cm_to_kt; } /// set_externally_limited_NE - mark that accel has been limited /// this prevents integrator windup during external acceleration saturation - void set_externally_limited_NE() { _limit_vector.x = _accel_target_neu_cmss.x; _limit_vector.y = _accel_target_neu_cmss.y; } + void set_externally_limited_NE() { _limit_vector_neu.x = _accel_target_neu_mss.x; _limit_vector_neu.y = _accel_target_neu_mss.y; } - // lean_angles_to_accel_NEU_cmss - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s - Vector3f lean_angles_to_accel_NEU_cmss(const Vector3f& att_target_euler_rad) const; + // lean_angles_rad_to_accel_NEU_mss - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s + Vector3f lean_angles_rad_to_accel_NEU_cmss(const Vector3f& att_target_euler_rad) const; + Vector3f lean_angles_rad_to_accel_NEU_mss(const Vector3f& att_target_euler_rad) const; // write PSC and/or PSCZ logs void write_log(); @@ -470,27 +535,30 @@ public: /// aircraft when in standby. void standby_NEU_reset(); - // get_measured_accel_U_cmss - returns the vertical (Up) acceleration in the earth frame, gravity-compensated, in cm/s/s (+ve = upward) - float get_measured_accel_U_cmss() const { return -(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f; } + // get_measured_accel_U_mss - returns the vertical (Up) acceleration in the earth frame, gravity-compensated, in cm/s/s (+ve = upward) + float get_measured_accel_U_cmss() const { return get_measured_accel_U_mss() * 100.0; } + float get_measured_accel_U_mss() const { return -(_ahrs.get_accel_ef().z + GRAVITY_MSS); } /// returns true when the forward pitch demand is limited by the maximum allowed tilt bool get_fwd_pitch_is_limited() const; - // set_disturb_pos_NE_cm - set the position disturbance in the north east plane - void set_disturb_pos_NE_cm(Vector2f disturb_pos) {_disturb_pos_ne_cm = disturb_pos;} + // set_disturb_pos_NE_m - set the position disturbance in the north east plane + void set_disturb_pos_NE_cm(Vector2f disturb_pos_cm) { set_disturb_pos_NE_m(disturb_pos_cm * 0.01); } + void set_disturb_pos_NE_m(Vector2f disturb_pos_m) { _disturb_pos_ne_m = disturb_pos_m; } - // set_disturb_vel_NE_cms - set the velocity disturbance in the north east plane - void set_disturb_vel_NE_cms(Vector2f disturb_vel) {_disturb_vel_ne_cms = disturb_vel;} + // set_disturb_vel_NE_ms - set the velocity disturbance in the north east plane + void set_disturb_vel_NE_cms(Vector2f disturb_vel_cms) { set_disturb_vel_NE_ms(disturb_vel_cms * 0.01); } + void set_disturb_vel_NE_ms(Vector2f disturb_vel_ms) { _disturb_vel_ne_ms = disturb_vel_ms; } static const struct AP_Param::GroupInfo var_info[]; - static void Write_PSCN(float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss); - static void Write_PSCE(float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss); - static void Write_PSCD(float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss); - static void Write_PSON(float pos_target_offset_cm, float pos_offset_cm, float vel_target_offset_cms, float vel_offset_cms, float accel_target_offset_cmss, float accel_offset_cmss); - static void Write_PSOE(float pos_target_offset_cm, float pos_offset_cm, float vel_target_offset_cms, float vel_offset_cms, float accel_target_offset_cmss, float accel_offset_cmss); - static void Write_PSOD(float pos_target_offset_cm, float pos_offset_cm, float vel_target_offset_cms, float vel_offset_cms, float accel_target_offset_cmss, float accel_offset_cmss); - static void Write_PSOT(float pos_target_offset_cm, float pos_offset_cm, float vel_target_offset_cms, float vel_offset_cms, float accel_target_offset_cmss, float accel_offset_cmss); + static void Write_PSCN(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss); + static void Write_PSCE(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss); + static void Write_PSCD(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss); + static void Write_PSON(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss); + static void Write_PSOE(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss); + static void Write_PSOD(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss); + static void Write_PSOT(float pos_target_offset_m, float pos_offset_m, float vel_target_offset_ms, float vel_offset_ms, float accel_target_offset_mss, float accel_offset_mss); // singleton static AC_PosControl *get_singleton(void) { return _singleton; } @@ -500,11 +568,13 @@ protected: // get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain) float get_throttle_with_vibration_override(); - // accel_NE_cmss_to_lean_angles_rad - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s + // accel_NE_mss_to_lean_angles_rad - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s void accel_NE_cmss_to_lean_angles_rad(float accel_n_cmss, float accel_e_cmss, float& roll_target_rad, float& pitch_target_rad) const; + void accel_NE_mss_to_lean_angles_rad(float accel_n_mss, float accel_e_mss, float& roll_target_rad, float& pitch_target_rad) const; - // lean_angles_to_accel_NE_cmss - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s + // lean_angles_to_accel_NE_mss - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s void lean_angles_to_accel_NE_cmss(float& accel_n_cmss, float& accel_e_cmss) const; + void lean_angles_to_accel_NE_mss(float& accel_n_mss, float& accel_e_mss) const; // calculate_yaw_and_rate_yaw - calculate the vehicle yaw and rate of yaw. void calculate_yaw_and_rate_yaw(); @@ -521,7 +591,7 @@ protected: void init_terrain(); /// update_terrain - updates the terrain position, velocity and acceleration estimation - /// this moves the estimated terrain position _pos_terrain_u_cm towards the target _pos_terrain_target_u_cm + /// this moves the estimated terrain position _pos_terrain_u_m towards the target _pos_terrain_target_u_m void update_terrain(); @@ -534,7 +604,7 @@ protected: void init_offsets_U(); /// update_offsets - update the position and velocity offsets - /// this moves the offsets (e.g _pos_offset_neu_cm, _vel_offset_neu_cms, _accel_offset_neu_cmss) towards the targets (e.g. _pos_offset_target_neu_cm or _vel_offset_target_neu_cms) + /// this moves the offsets (e.g _pos_offset_neu_m, _vel_offset_neu_ms, _accel_offset_neu_mss) towards the targets (e.g. _pos_offset_target_neu_m or _vel_offset_target_neu_ms) void update_offsets_NE(); void update_offsets_U(); @@ -553,26 +623,26 @@ protected: AP_Float _lean_angle_max_deg; // Maximum autopilot commanded angle (in degrees). Set to zero for Angle Max AP_Float _shaping_jerk_ne_msss; // Jerk limit of the ne kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target AP_Float _shaping_jerk_u_msss; // Jerk limit of the u kinematic path generation in m/s^3 used to determine how quickly the aircraft varies the acceleration target - AC_P_2D _p_pos_ne_cm; // XY axis position controller to convert target distance (cm) to target velocity (cm/s) - AC_P_1D _p_pos_u_cm; // Z axis position controller to convert target altitude (cm) to target climb rate (cm/s) + AC_P_2D _p_pos_ne_m; // XY axis position controller to convert target distance (cm) to target velocity (cm/s) + AC_P_1D _p_pos_u_m; // Z axis position controller to convert target altitude (cm) to target climb rate (cm/s) AC_PID_2D _pid_vel_ne_cm; // XY axis velocity controller to convert target velocity (cm/s) to target acceleration (cm/s^2) AC_PID_Basic _pid_vel_u_cm; // Z axis velocity controller to convert target climb rate (cm/s) to target acceleration (cm/s^2) AC_PID _pid_accel_u_cm_to_kt; // Z axis acceleration controller to convert target acceleration (cm/s^2) to throttle output (0 to 1000) // internal variables - float _dt_s; // time difference (in seconds) since the last loop time - uint32_t _last_update_ne_ticks; // ticks of last last update_NE_controller call - uint32_t _last_update_u_ticks; // ticks of last update_z_controller call - float _vel_max_ne_cms; // max horizontal speed in cm/s used for kinematic shaping - float _vel_max_up_cms; // max climb rate in cm/s used for kinematic shaping - float _vel_max_down_cms; // max descent rate in cm/s used for kinematic shaping - float _accel_max_ne_cmss; // max horizontal acceleration in cm/s/s used for kinematic shaping - float _accel_max_u_cmss; // max vertical acceleration in cm/s/s used for kinematic shaping - float _jerk_max_ne_cmsss; // Jerk limit of the ne kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target - float _jerk_max_u_cmsss; // Jerk limit of the z kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target + float _dt_s; // time difference (in seconds) since the last loop time + uint32_t _last_update_ne_ticks; // ticks of last last update_NE_controller call + uint32_t _last_update_u_ticks; // ticks of last update_z_controller call + float _vel_max_ne_ms; // max horizontal speed in cm/s used for kinematic shaping + float _vel_max_up_ms; // max climb rate in cm/s used for kinematic shaping + float _vel_max_down_ms; // max descent rate in cm/s used for kinematic shaping + float _accel_max_ne_mss; // max horizontal acceleration in cm/s/s used for kinematic shaping + float _accel_max_u_mss; // max vertical acceleration in cm/s/s used for kinematic shaping + float _jerk_max_ne_msss; // Jerk limit of the ne kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target + float _jerk_max_u_msss; // Jerk limit of the z kinematic path generation in cm/s^3 used to determine how quickly the aircraft varies the acceleration target float _vel_u_control_ratio = 2.0f; // confidence that we have control in the vertical axis - Vector2f _disturb_pos_ne_cm; // position disturbance generated by system ID mode - Vector2f _disturb_vel_ne_cms; // velocity disturbance generated by system ID mode + Vector2f _disturb_pos_ne_m; // position disturbance generated by system ID mode + Vector2f _disturb_vel_ne_ms; // velocity disturbance generated by system ID mode float _ne_control_scale_factor = 1.0; // single loop scale factor for XY control // output from controller @@ -582,29 +652,30 @@ protected: float _yaw_rate_target_rads; // desired yaw rate in radians per second calculated by position controller // position controller internal variables - Vector3p _pos_estimate_neu_cm; - Vector3p _pos_desired_neu_cm; // desired location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_target minus offsets - Vector3p _pos_target_neu_cm; // target location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_desired_neu_cm plus offsets - Vector3f _vel_estimate_neu_cms; - Vector3f _vel_desired_neu_cms; // desired velocity in NEU cm/s - Vector3f _vel_target_neu_cms; // velocity target in NEU cm/s calculated by pos_to_rate step - Vector3f _accel_desired_neu_cmss; // desired acceleration in NEU cm/s/s (feed forward) - Vector3f _accel_target_neu_cmss; // acceleration target in NEU cm/s/s - Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited + Vector3p _pos_estimate_neu_m; + Vector3p _pos_desired_neu_m; // desired location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_target minus offsets + Vector3p _pos_target_neu_m; // target location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_desired_neu_m plus offsets + Vector3f _vel_estimate_neu_ms; + Vector3f _vel_desired_neu_ms; // desired velocity in NEU cm/s + Vector3f _vel_target_neu_ms; // velocity target in NEU cm/s calculated by pos_to_rate step + Vector3f _accel_desired_neu_mss; // desired acceleration in NEU cm/s/s (feed forward) + Vector3f _accel_target_neu_mss; // acceleration target in NEU cm/s/s + // todo: seperate the limit vector into ne and u. ne is based on acceleration while u is set +-1 based on throttle saturation. Together they don't form a direction vector because the units are different. + Vector3f _limit_vector_neu; // the direction that the position controller is limited, zero when not limited // terrain handling variables - float _pos_terrain_target_u_cm; // position terrain target in cm relative to the EKF origin in NEU frame - float _pos_terrain_u_cm; // position terrain in cm from the EKF origin in NEU frame. this terrain moves towards _pos_terrain_target_u_cm - float _vel_terrain_u_cms; // velocity terrain in NEU cm/s calculated by pos_to_rate step. this terrain moves towards _vel_terrain_target - float _accel_terrain_u_cmss; // acceleration terrain in NEU cm/s/s + float _pos_terrain_target_u_m; // position terrain target in cm relative to the EKF origin in NEU frame + float _pos_terrain_u_m; // position terrain in cm from the EKF origin in NEU frame. this terrain moves towards _pos_terrain_target_u_m + float _vel_terrain_u_ms; // velocity terrain in NEU cm/s calculated by pos_to_rate step. this terrain moves towards _vel_terrain_target + float _accel_terrain_u_mss; // acceleration terrain in NEU cm/s/s // offset handling variables - Vector3p _pos_offset_target_neu_cm; // position offset target in cm relative to the EKF origin in NEU frame - Vector3p _pos_offset_neu_cm; // position offset in cm from the EKF origin in NEU frame. this offset moves towards _pos_offset_target_neu_cm - Vector3f _vel_offset_target_neu_cms; // velocity offset target in cm/s in NEU frame - Vector3f _vel_offset_neu_cms; // velocity offset in NEU cm/s calculated by pos_to_rate step. this offset moves towards _vel_offset_target_neu_cms - Vector3f _accel_offset_target_neu_cmss; // acceleration offset target in cm/s/s in NEU frame - Vector3f _accel_offset_neu_cmss; // acceleration offset in NEU cm/s/s + Vector3p _pos_offset_target_neu_m; // position offset target in cm relative to the EKF origin in NEU frame + Vector3p _pos_offset_neu_m; // position offset in cm from the EKF origin in NEU frame. this offset moves towards _pos_offset_target_neu_m + Vector3f _vel_offset_target_neu_ms; // velocity offset target in cm/s in NEU frame + Vector3f _vel_offset_neu_ms; // velocity offset in NEU cm/s calculated by pos_to_rate step. this offset moves towards _vel_offset_target_neu_ms + Vector3f _accel_offset_target_neu_mss; // acceleration offset target in cm/s/s in NEU frame + Vector3f _accel_offset_neu_mss; // acceleration offset in NEU cm/s/s uint32_t _posvelaccel_offset_target_ne_ms; // system time that pos, vel, accel targets were set (used to implement timeouts) uint32_t _posvelaccel_offset_target_u_ms; // system time that pos, vel, accel targets were set (used to implement timeouts) @@ -623,14 +694,14 @@ protected: private: // convenience method for writing PSCE, PSCN, and PSCD logs, to reduce code duplication - static void Write_PSCx(LogMessages ID, float pos_desired_cm, float pos_target_cm, float pos_cm, - float vel_desired_cms, float vel_target_cms, float vel_cms, - float accel_desired_cmss, float accel_target_cmss, float accel_cmss); + static void Write_PSCx(LogMessages ID, float pos_desired_m, float pos_target_m, float pos_m, + float vel_desired_ms, float vel_target_ms, float vel_ms, + float accel_desired_mss, float accel_target_mss, float accel_mss); // a convenience function for writing out the position controller offsets - static void Write_PSOx(LogMessages id, float pos_target_offset_cm, float pos_offset_cm, - float vel_target_offset_cms, float vel_offset_cms, - float accel_target_offset_cmss, float accel_offset_cmss); + static void Write_PSOx(LogMessages id, float pos_target_offset_m, float pos_offset_m, + float vel_target_offset_ms, float vel_offset_ms, + float accel_target_offset_mss, float accel_offset_mss); // singleton static AC_PosControl *_singleton; diff --git a/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp b/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp index f0f4e3c7a80..f35a41597be 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp @@ -8,83 +8,83 @@ #include "LogStructure.h" // a convenience function for writing out the position controller PIDs -void AC_PosControl::Write_PSCx(LogMessages id, float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss) +void AC_PosControl::Write_PSCx(LogMessages id, float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss) { const struct log_PSCx pkt{ LOG_PACKET_HEADER_INIT(id), time_us : AP_HAL::micros64(), - pos_desired : pos_desired_cm * 0.01f, - pos_target : pos_target_cm * 0.01f, - pos : pos_cm * 0.01f, - vel_desired : vel_desired_cms * 0.01f, - vel_target : vel_target_cms * 0.01f, - vel : vel_cms * 0.01f, - accel_desired : accel_desired_cmss * 0.01f, - accel_target : accel_target_cmss * 0.01f, - accel : accel_cmss * 0.01f + pos_desired : pos_desired_m, + pos_target : pos_target_m, + pos : pos_m, + vel_desired : vel_desired_ms, + vel_target : vel_target_ms, + vel : vel_ms, + accel_desired : accel_desired_mss, + accel_target : accel_target_mss, + accel : accel_mss }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -void AC_PosControl::Write_PSCN(float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss) +void AC_PosControl::Write_PSCN(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss) { - Write_PSCx(LOG_PSCN_MSG, pos_desired_cm, pos_target_cm, pos_cm, vel_desired_cms, vel_target_cms, vel_cms, accel_desired_cmss, accel_target_cmss, accel_cmss); + Write_PSCx(LOG_PSCN_MSG, pos_desired_m, pos_target_m, pos_m, vel_desired_ms, vel_target_ms, vel_ms, accel_desired_mss, accel_target_mss, accel_mss); } -void AC_PosControl::Write_PSCE(float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss) +void AC_PosControl::Write_PSCE(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss) { - Write_PSCx(LOG_PSCE_MSG, pos_desired_cm, pos_target_cm, pos_cm, vel_desired_cms, vel_target_cms, vel_cms, accel_desired_cmss, accel_target_cmss, accel_cmss); + Write_PSCx(LOG_PSCE_MSG, pos_desired_m, pos_target_m, pos_m, vel_desired_ms, vel_target_ms, vel_ms, accel_desired_mss, accel_target_mss, accel_mss); } -void AC_PosControl::Write_PSCD(float pos_desired_cm, float pos_target_cm, float pos_cm, float vel_desired_cms, float vel_target_cms, float vel_cms, float accel_desired_cmss, float accel_target_cmss, float accel_cmss) +void AC_PosControl::Write_PSCD(float pos_desired_m, float pos_target_m, float pos_m, float vel_desired_ms, float vel_target_ms, float vel_ms, float accel_desired_mss, float accel_target_mss, float accel_mss) { - Write_PSCx(LOG_PSCD_MSG, pos_desired_cm, pos_target_cm, pos_cm, vel_desired_cms, vel_target_cms, vel_cms, accel_desired_cmss, accel_target_cmss, accel_cmss); + Write_PSCx(LOG_PSCD_MSG, pos_desired_m, pos_target_m, pos_m, vel_desired_ms, vel_target_ms, vel_ms, accel_desired_mss, accel_target_mss, accel_mss); } // a convenience function for writing out the position controller offsets -void AC_PosControl::Write_PSOx(LogMessages id, float pos_target_offset_cm, float pos_offset_cm, - float vel_target_offset_cms, float vel_offset_cms, - float accel_target_offset_cmss, float accel_offset_cmss) +void AC_PosControl::Write_PSOx(LogMessages id, float pos_target_offset_m, float pos_offset_m, + float vel_target_offset_ms, float vel_offset_ms, + float accel_target_offset_mss, float accel_offset_mss) { const struct log_PSOx pkt{ LOG_PACKET_HEADER_INIT(id), time_us : AP_HAL::micros64(), - pos_target_offset : pos_target_offset_cm * 0.01f, - pos_offset : pos_offset_cm * 0.01f, - vel_target_offset : vel_target_offset_cms * 0.01f, - vel_offset : vel_offset_cms * 0.01f, - accel_target_offset : accel_target_offset_cmss * 0.01f, - accel_offset : accel_offset_cmss * 0.01f, + pos_target_offset : pos_target_offset_m, + pos_offset : pos_offset_m, + vel_target_offset : vel_target_offset_ms, + vel_offset : vel_offset_ms, + accel_target_offset : accel_target_offset_mss, + accel_offset : accel_offset_mss, }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -void AC_PosControl::Write_PSON(float pos_target_offset_cm, float pos_offset_cm, - float vel_target_offset_cms, float vel_offset_cms, - float accel_target_offset_cmss, float accel_offset_cmss) +void AC_PosControl::Write_PSON(float pos_target_offset_m, float pos_offset_m, + float vel_target_offset_ms, float vel_offset_ms, + float accel_target_offset_mss, float accel_offset_mss) { - Write_PSOx(LOG_PSON_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss); + Write_PSOx(LOG_PSON_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss); } -void AC_PosControl::Write_PSOE(float pos_target_offset_cm, float pos_offset_cm, - float vel_target_offset_cms, float vel_offset_cms, - float accel_target_offset_cmss, float accel_offset_cmss) +void AC_PosControl::Write_PSOE(float pos_target_offset_m, float pos_offset_m, + float vel_target_offset_ms, float vel_offset_ms, + float accel_target_offset_mss, float accel_offset_mss) { - Write_PSOx(LOG_PSOE_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss); + Write_PSOx(LOG_PSOE_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss); } -void AC_PosControl::Write_PSOD(float pos_target_offset_cm, float pos_offset_cm, - float vel_target_offset_cms, float vel_offset_cms, - float accel_target_offset_cmss, float accel_offset_cmss) +void AC_PosControl::Write_PSOD(float pos_target_offset_m, float pos_offset_m, + float vel_target_offset_ms, float vel_offset_ms, + float accel_target_offset_mss, float accel_offset_mss) { - Write_PSOx(LOG_PSOD_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss); + Write_PSOx(LOG_PSOD_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss); } -void AC_PosControl::Write_PSOT(float pos_target_offset_cm, float pos_offset_cm, - float vel_target_offset_cms, float vel_offset_cms, - float accel_target_offset_cmss, float accel_offset_cmss) +void AC_PosControl::Write_PSOT(float pos_target_offset_m, float pos_offset_m, + float vel_target_offset_ms, float vel_offset_ms, + float accel_target_offset_mss, float accel_offset_mss) { - Write_PSOx(LOG_PSOT_MSG, pos_target_offset_cm, pos_offset_cm, vel_target_offset_cms, vel_offset_cms, accel_target_offset_cmss, accel_offset_cmss); + Write_PSOx(LOG_PSOT_MSG, pos_target_offset_m, pos_offset_m, vel_target_offset_ms, vel_offset_ms, accel_target_offset_mss, accel_offset_mss); } #endif // HAL_LOGGING_ENABLED