Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_PosControl: support 3D pos, vel, accel offsets 

Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Leonard Hall 2024-08-02 13:45:17 +09:00 committed by Randy Mackay
parent e40ae8e649
commit 9fb8a0f1ac
4 changed files with 575 additions and 158 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

439
libraries/AC_AttitudeControl/AC_PosControl.cpp
View File

@ -74,6 +74,9 @@ extern const AP_HAL::HAL& hal;
#define POSCONTROL_VIBE_COMP_P_GAIN 0.250f #define POSCONTROL_VIBE_COMP_P_GAIN 0.250f
#define POSCONTROL_VIBE_COMP_I_GAIN 0.125f #define POSCONTROL_VIBE_COMP_I_GAIN 0.125f
// velocity offset targets timeout if not updated within 3 seconds
#define POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS 3000
const AP_Param::GroupInfo AC_PosControl::var_info[] = { const AP_Param::GroupInfo AC_PosControl::var_info[] = {
// 0 was used for HOVER // 0 was used for HOVER
@ -358,30 +361,26 @@ AC_PosControl::AC_PosControl(AP_AHRS_View& ahrs, const AP_InertialNav& inav,
/// The jerk limit defines the acceleration error decay in the kinematic path as the system approaches constant acceleration. /// 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 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. /// 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_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer) void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_terrain_target, float terrain_buffer)
{ {
// Terrain following velocity scalar must be calculated before we remove the position offset // Terrain following velocity scalar must be calculated before we remove the position offset
const float offset_z_scaler = pos_offset_z_scaler(pos_offset_z, pos_offset_z_buffer); const float offset_z_scaler = pos_offset_z_scaler(pos_terrain_target, terrain_buffer);
set_pos_terrain_target_cm(pos_terrain_target);
// remove terrain offsets for flat earth assumption
_pos_target.z -= _pos_offset_z;
_vel_desired.z -= _vel_offset_z;
_accel_desired.z -= _accel_offset_z;
// calculated increased maximum acceleration and jerk if over speed // calculated increased maximum acceleration and jerk if over speed
const float overspeed_gain = calculate_overspeed_gain(); const float overspeed_gain = calculate_overspeed_gain();
const float accel_max_z_cmss = _accel_max_z_cmss * overspeed_gain; const float accel_max_z_cmss = _accel_max_z_cmss * overspeed_gain;
const float jerk_max_z_cmsss = _jerk_max_z_cmsss * overspeed_gain; const float jerk_max_z_cmsss = _jerk_max_z_cmsss * overspeed_gain;
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error()); update_pos_vel_accel_xy(_pos_desired.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
// adjust desired altitude if motors have not hit their limits // adjust desired altitude if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error()); update_pos_vel_accel(_pos_desired.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
// calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos // calculate the horizontal and vertical velocity limits to travel directly to the destination defined by pos
float vel_max_xy_cms = 0.0f; float vel_max_xy_cms = 0.0f;
float vel_max_z_cms = 0.0f; float vel_max_z_cms = 0.0f;
Vector3f dest_vector = (pos - _pos_target).tofloat(); Vector3f dest_vector = (pos - _pos_desired).tofloat();
if (is_positive(dest_vector.length_squared()) ) { if (is_positive(dest_vector.length_squared()) ) {
dest_vector.normalize(); dest_vector.normalize();
float dest_vector_xy_length = dest_vector.xy().length(); float dest_vector_xy_length = dest_vector.xy().length();
@ -396,23 +395,15 @@ void AC_PosControl::input_pos_xyz(const Vector3p& pos, float pos_offset_z, float
Vector2f vel; Vector2f vel;
Vector2f accel; Vector2f accel;
shape_pos_vel_accel_xy(pos.xy(), vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), shape_pos_vel_accel_xy(pos.xy(), vel, accel, _pos_desired.xy(), _vel_desired.xy(), _accel_desired.xy(),
vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, false); vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, false);
float posz = pos.z; float posz = pos.z;
shape_pos_vel_accel(posz, 0, 0, shape_pos_vel_accel(posz, 0, 0,
_pos_target.z, _vel_desired.z, _accel_desired.z, _pos_desired.z, _vel_desired.z, _accel_desired.z,
-vel_max_z_cms, vel_max_z_cms, -vel_max_z_cms, vel_max_z_cms,
-constrain_float(accel_max_z_cmss, 0.0f, 750.0f), accel_max_z_cmss, -constrain_float(accel_max_z_cmss, 0.0f, 750.0f), accel_max_z_cmss,
jerk_max_z_cmsss, _dt, false); jerk_max_z_cmsss, _dt, false);
// update the vertical position, velocity and acceleration offsets
update_pos_offset_z(pos_offset_z);
// add terrain offsets
_pos_target.z += _pos_offset_z;
_vel_desired.z += _vel_offset_z;
_accel_desired.z += _accel_offset_z;
} }
@ -422,7 +413,7 @@ float AC_PosControl::pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_
if (is_zero(pos_offset_z_buffer)) { if (is_zero(pos_offset_z_buffer)) {
return 1.0; return 1.0;
} }
float pos_offset_error_z = _inav.get_position_z_up_cm() - (_pos_target.z - _pos_offset_z + pos_offset_z); float pos_offset_error_z = _inav.get_position_z_up_cm() - (_pos_target.z + (pos_offset_z - _pos_terrain));
return constrain_float((1.0 - (fabsf(pos_offset_error_z) - 0.5 * pos_offset_z_buffer) / (0.5 * pos_offset_z_buffer)), 0.01, 1.0); return constrain_float((1.0 - (fabsf(pos_offset_error_z) - 0.5 * pos_offset_z_buffer) / (0.5 * pos_offset_z_buffer)), 0.01, 1.0);
} }
@ -466,12 +457,13 @@ void AC_PosControl::set_correction_speed_accel_xy(float speed_cms, float accel_c
/// init_xy_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration. /// init_xy_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. /// 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_cm() after calling this function. /// The starting position can be retrieved by getting the position target using get_pos_desired_cm() after calling this function.
void AC_PosControl::init_xy_controller_stopping_point() void AC_PosControl::init_xy_controller_stopping_point()
{ {
init_xy_controller(); init_xy_controller();
get_stopping_point_xy_cm(_pos_target.xy()); get_stopping_point_xy_cm(_pos_desired.xy());
_pos_target.xy() = _pos_desired.xy() + _pos_offset.xy();
_vel_desired.xy().zero(); _vel_desired.xy().zero();
_accel_desired.xy().zero(); _accel_desired.xy().zero();
} }
@ -496,6 +488,7 @@ void AC_PosControl::soften_for_landing_xy()
// decay position error to zero // decay position error to zero
if (is_positive(_dt)) { if (is_positive(_dt)) {
_pos_target.xy() += (_inav.get_position_xy_cm().topostype() - _pos_target.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC)); _pos_target.xy() += (_inav.get_position_xy_cm().topostype() - _pos_target.xy()) * (_dt / (_dt + POSCONTROL_RELAX_TC));
_pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
} }
// Prevent I term build up in xy velocity controller. // Prevent I term build up in xy velocity controller.
@ -507,6 +500,9 @@ void AC_PosControl::soften_for_landing_xy()
/// This function is the default initialisation for any position control that provides position, velocity and acceleration. /// This function is the default initialisation for any position control that provides position, velocity and acceleration.
void AC_PosControl::init_xy_controller() void AC_PosControl::init_xy_controller()
{ {
// initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
init_offsets_xy();
// set roll, pitch lean angle targets to current attitude // set roll, pitch lean angle targets to current attitude
const Vector3f &att_target_euler_cd = _attitude_control.get_att_target_euler_cd(); const Vector3f &att_target_euler_cd = _attitude_control.get_att_target_euler_cd();
_roll_target = att_target_euler_cd.x; _roll_target = att_target_euler_cd.x;
@ -516,10 +512,10 @@ void AC_PosControl::init_xy_controller()
_angle_max_override_cd = 0.0; _angle_max_override_cd = 0.0;
_pos_target.xy() = _inav.get_position_xy_cm().topostype(); _pos_target.xy() = _inav.get_position_xy_cm().topostype();
_pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
const Vector2f &curr_vel = _inav.get_velocity_xy_cms(); _vel_target.xy() = _inav.get_velocity_xy_cms();
_vel_desired.xy() = curr_vel; _vel_desired.xy() = _vel_target.xy() - _vel_offset.xy();
_vel_target.xy() = curr_vel;
// Set desired accel to zero because raw acceleration is prone to noise // Set desired accel to zero because raw acceleration is prone to noise
_accel_desired.xy().zero(); _accel_desired.xy().zero();
@ -553,11 +549,8 @@ void AC_PosControl::init_xy_controller()
/// The jerk limit also defines the time taken to achieve the maximum acceleration. /// The jerk limit also defines the time taken to achieve the maximum acceleration.
void AC_PosControl::input_accel_xy(const Vector3f& accel) void AC_PosControl::input_accel_xy(const Vector3f& accel)
{ {
// check for ekf xy position reset update_pos_vel_accel_xy(_pos_desired.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
handle_ekf_xy_reset(); shape_accel_xy(accel.xy(), _accel_desired.xy(), _jerk_max_xy_cmsss, _dt);
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
shape_accel_xy(accel, _accel_desired, _jerk_max_xy_cmsss, _dt);
} }
/// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration. /// input_vel_accel_xy - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity and acceleration.
@ -567,7 +560,7 @@ void AC_PosControl::input_accel_xy(const Vector3f& accel)
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. /// 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_xy(Vector2f& vel, const Vector2f& accel, bool limit_output) void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, bool limit_output)
{ {
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error()); update_pos_vel_accel_xy(_pos_desired.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
shape_vel_accel_xy(vel, accel, _vel_desired.xy(), _accel_desired.xy(), shape_vel_accel_xy(vel, accel, _vel_desired.xy(), _accel_desired.xy(),
_accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output); _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output);
@ -582,29 +575,51 @@ void AC_PosControl::input_vel_accel_xy(Vector2f& vel, const Vector2f& accel, boo
/// The parameter limit_output specifies if the velocity and acceleration limits are applied to the sum of commanded and correction values or just correction. /// 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_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output) void AC_PosControl::input_pos_vel_accel_xy(Vector2p& pos, Vector2f& vel, const Vector2f& accel, bool limit_output)
{ {
update_pos_vel_accel_xy(_pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error()); update_pos_vel_accel_xy(_pos_desired.xy(), _vel_desired.xy(), _accel_desired.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
shape_pos_vel_accel_xy(pos, vel, accel, _pos_target.xy(), _vel_desired.xy(), _accel_desired.xy(), shape_pos_vel_accel_xy(pos, vel, accel, _pos_desired.xy(), _vel_desired.xy(), _accel_desired.xy(),
_vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output); _vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, limit_output);
update_pos_vel_accel_xy(pos, vel, accel, _dt, Vector2f(), Vector2f(), Vector2f()); update_pos_vel_accel_xy(pos, vel, accel, _dt, Vector2f(), Vector2f(), Vector2f());
} }
/// update the horizontal position and velocity offsets
/// this moves the offsets (e.g _pos_offset, _vel_offset, _accel_offset) towards the targets (e.g. _pos_offset_target, _vel_offset_target, _accel_offset_target)
void AC_PosControl::update_offsets_xy()
{
// check for offset target timeout
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _posvelaccel_offset_target_xy_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
_pos_offset_target.xy().zero();
_vel_offset_target.xy().zero();
_accel_offset_target.xy().zero();
}
// update position, velocity, accel offsets for this iteration
update_pos_vel_accel_xy(_pos_offset_target.xy(), _vel_offset_target.xy(), _accel_offset_target.xy(), _dt, Vector2f(), Vector2f(), Vector2f());
update_pos_vel_accel_xy(_pos_offset.xy(), _vel_offset.xy(), _accel_offset.xy(), _dt, _limit_vector.xy(), _p_pos_xy.get_error(), _pid_vel_xy.get_error());
// input shape horizontal position, velocity and acceleration offsets
shape_pos_vel_accel_xy(_pos_offset_target.xy(), _vel_offset_target.xy(), _accel_offset_target.xy(),
_pos_offset.xy(), _vel_offset.xy(), _accel_offset.xy(),
_vel_max_xy_cms, _accel_max_xy_cmss, _jerk_max_xy_cmsss, _dt, false);
}
/// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system /// stop_pos_xy_stabilisation - sets the target to the current position to remove any position corrections from the system
void AC_PosControl::stop_pos_xy_stabilisation() void AC_PosControl::stop_pos_xy_stabilisation()
{ {
_pos_target.xy() = _inav.get_position_xy_cm().topostype(); _pos_target.xy() = _inav.get_position_xy_cm().topostype();
_pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
} }
/// stop_vel_xy_stabilisation - sets the target to the current position and velocity to the current velocity to remove any position and velocity corrections from the system /// stop_vel_xy_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_xy_stabilisation() void AC_PosControl::stop_vel_xy_stabilisation()
{ {
_pos_target.xy() = _inav.get_position_xy_cm().topostype(); _pos_target.xy() = _inav.get_position_xy_cm().topostype();
_pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
const Vector2f &curr_vel = _inav.get_velocity_xy_cms(); _vel_target.xy() = _inav.get_velocity_xy_cms();;
_vel_desired.xy() = curr_vel; _vel_desired.xy() = _vel_target.xy() - _vel_offset.xy();
// with zero position error _vel_target = _vel_desired
_vel_target.xy() = curr_vel;
// initialise I terms from lean angles // initialise I terms from lean angles
_pid_vel_xy.reset_filter(); _pid_vel_xy.reset_filter();
@ -640,37 +655,44 @@ void AC_PosControl::update_xy_controller()
float ahrsGndSpdLimit, ahrsControlScaleXY; float ahrsGndSpdLimit, ahrsControlScaleXY;
AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY); AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY);
// update the position, velocity and acceleration offsets
update_offsets_xy();
// Position Controller // Position Controller
const Vector3f &curr_pos = _inav.get_position_neu_cm(); _pos_target.xy() = _pos_desired.xy() + _pos_offset.xy();
// determine the combined position of the actual position and the disturbance from system ID mode // determine the combined position of the actual position and the disturbance from system ID mode
const Vector3f &curr_pos = _inav.get_position_neu_cm();
Vector3f comb_pos = curr_pos; Vector3f comb_pos = curr_pos;
comb_pos.xy() += _disturb_pos; comb_pos.xy() += _disturb_pos;
Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, comb_pos);
// add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, comb_pos);
vel_target *= ahrsControlScaleXY; _pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
_vel_target.xy() = vel_target;
_vel_target.xy() += _vel_desired.xy();
// Velocity Controller // Velocity Controller
const Vector2f &curr_vel = _inav.get_velocity_xy_cms(); // add velocity feed-forward scaled to compensate for optical flow measurement induced EKF noise
vel_target *= ahrsControlScaleXY;
_vel_target.xy() = vel_target;
_vel_target.xy() += _vel_desired.xy() + _vel_offset.xy();
// determine the combined velocity of the actual velocity and the disturbance from system ID mode // determine the combined velocity of the actual velocity and the disturbance from system ID mode
const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
Vector2f comb_vel = curr_vel; Vector2f comb_vel = curr_vel;
comb_vel += _disturb_vel; comb_vel += _disturb_vel;
Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), comb_vel, _dt, _limit_vector.xy()); Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), comb_vel, _dt, _limit_vector.xy());
// Acceleration Controller
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target *= ahrsControlScaleXY; accel_target *= ahrsControlScaleXY;
// pass the correction acceleration to the target acceleration output // pass the correction acceleration to the target acceleration output
_accel_target.xy() = accel_target; _accel_target.xy() = accel_target;
_accel_target.xy() += _accel_desired.xy() + _accel_offset.xy();
// Add feed forward into the target acceleration output
_accel_target.xy() += _accel_desired.xy();
// Acceleration Controller
// limit acceleration using maximum lean angles // limit acceleration using maximum lean angles
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd()); float angle_max = MIN(_attitude_control.get_althold_lean_angle_max_cd(), get_lean_angle_max_cd());
@ -693,6 +715,7 @@ void AC_PosControl::update_xy_controller()
accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target); accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
calculate_yaw_and_rate_yaw(); calculate_yaw_and_rate_yaw();
// reset the disturbance from system ID mode to zero
_disturb_pos.zero(); _disturb_pos.zero();
_disturb_vel.zero(); _disturb_vel.zero();
} }
@ -751,10 +774,14 @@ void AC_PosControl::init_z_controller_no_descent()
init_z_controller(); init_z_controller();
// remove all descent if present // remove all descent if present
_vel_desired.z = MAX(0.0, _vel_desired.z);
_vel_target.z = MAX(0.0, _vel_target.z); _vel_target.z = MAX(0.0, _vel_target.z);
_accel_desired.z = MAX(0.0, _accel_desired.z); _vel_desired.z = MAX(0.0, _vel_desired.z);
_vel_terrain = MAX(0.0, _vel_terrain);
_vel_offset.z = MAX(0.0, _vel_offset.z);
_accel_target.z = MAX(0.0, _accel_target.z); _accel_target.z = MAX(0.0, _accel_target.z);
_accel_desired.z = MAX(0.0, _accel_desired.z);
_accel_terrain = MAX(0.0, _accel_terrain);
_accel_offset.z = MAX(0.0, _accel_offset.z);
} }
/// init_z_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration. /// init_z_controller_stopping_point - initialise the position controller to the stopping point with zero velocity and acceleration.
@ -765,7 +792,8 @@ void AC_PosControl::init_z_controller_stopping_point()
// Initialise the position controller to the current throttle, position, velocity and acceleration. // Initialise the position controller to the current throttle, position, velocity and acceleration.
init_z_controller(); init_z_controller();
get_stopping_point_z_cm(_pos_target.z); get_stopping_point_z_cm(_pos_desired.z);
_pos_target.z = _pos_desired.z + _pos_offset.z;
_vel_desired.z = 0.0f; _vel_desired.z = 0.0f;
_accel_desired.z = 0.0f; _accel_desired.z = 0.0f;
} }
@ -787,28 +815,26 @@ void AC_PosControl::relax_z_controller(float throttle_setting)
/// This function is private and contains all the shared z axis initialisation functions /// This function is private and contains all the shared z axis initialisation functions
void AC_PosControl::init_z_controller() void AC_PosControl::init_z_controller()
{ {
_pos_target.z = _inav.get_position_z_up_cm(); // initialise terrain targets and offsets to zero
init_terrain();
const float curr_vel_z = _inav.get_velocity_z_up_cms(); // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
_vel_desired.z = curr_vel_z; init_offsets_z();
// with zero position error _vel_target = _vel_desired
_vel_target.z = curr_vel_z; _pos_target.z = _inav.get_position_z_up_cm();
_pos_desired.z = _pos_target.z - _pos_offset.z;
_vel_target.z = _inav.get_velocity_z_up_cms();
_vel_desired.z = _vel_target.z - _vel_offset.z;
// Reset I term of velocity PID // Reset I term of velocity PID
_pid_vel_z.reset_filter(); _pid_vel_z.reset_filter();
_pid_vel_z.set_integrator(0.0f); _pid_vel_z.set_integrator(0.0f);
_accel_desired.z = constrain_float(get_z_accel_cmss(), -_accel_max_z_cmss, _accel_max_z_cmss); _accel_target.z = constrain_float(get_z_accel_cmss(), -_accel_max_z_cmss, _accel_max_z_cmss);
// with zero position error _accel_target = _accel_desired _accel_desired.z = _accel_target.z - (_accel_offset.z + _accel_terrain);
_accel_target.z = _accel_desired.z;
_pid_accel_z.reset_filter(); _pid_accel_z.reset_filter();
// initialise vertical offsets
_pos_offset_target_z = 0.0;
_pos_offset_z = 0.0;
_vel_offset_z = 0.0;
_accel_offset_z = 0.0;
// Set accel PID I term based on the current throttle // Set accel PID I term based on the current throttle
// Remove the expected P term due to _accel_desired.z being constrained to _accel_max_z_cmss // Remove the expected P term due to _accel_desired.z being constrained to _accel_max_z_cmss
// Remove the expected FF term due to non-zero _accel_target.z // Remove the expected FF term due to non-zero _accel_target.z
@ -831,7 +857,7 @@ void AC_PosControl::input_accel_z(float accel)
float jerk_max_z_cmsss = _jerk_max_z_cmsss * calculate_overspeed_gain(); float jerk_max_z_cmsss = _jerk_max_z_cmsss * calculate_overspeed_gain();
// adjust desired alt if motors have not hit their limits // adjust desired alt if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error()); update_pos_vel_accel(_pos_desired.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
shape_accel(accel, _accel_desired.z, jerk_max_z_cmsss, _dt); shape_accel(accel, _accel_desired.z, jerk_max_z_cmsss, _dt);
} }
@ -848,7 +874,7 @@ void AC_PosControl::input_vel_accel_z(float &vel, float accel, bool limit_output
const float jerk_max_z_cmsss = _jerk_max_z_cmsss * overspeed_gain; const float jerk_max_z_cmsss = _jerk_max_z_cmsss * overspeed_gain;
// adjust desired alt if motors have not hit their limits // adjust desired alt if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error()); update_pos_vel_accel(_pos_desired.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
shape_vel_accel(vel, accel, shape_vel_accel(vel, accel,
_vel_desired.z, _accel_desired.z, _vel_desired.z, _accel_desired.z,
@ -863,21 +889,7 @@ void AC_PosControl::input_vel_accel_z(float &vel, float accel, bool limit_output
/// The zero target altitude is varied to follow pos_offset_z /// The zero target altitude is varied to follow pos_offset_z
void AC_PosControl::set_pos_target_z_from_climb_rate_cm(float vel) void AC_PosControl::set_pos_target_z_from_climb_rate_cm(float vel)
{ {
// remove terrain offsets for flat earth assumption input_vel_accel_z(vel, 0.0);
_pos_target.z -= _pos_offset_z;
_vel_desired.z -= _vel_offset_z;
_accel_desired.z -= _accel_offset_z;
float vel_temp = vel;
input_vel_accel_z(vel_temp, 0.0);
// update the vertical position, velocity and acceleration offsets
update_pos_offset_z(_pos_offset_target_z);
// add terrain offsets
_pos_target.z += _pos_offset_z;
_vel_desired.z += _vel_offset_z;
_accel_desired.z += _accel_offset_z;
} }
/// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s /// land_at_climb_rate_cm - adjusts target up or down using a commanded climb rate in cm/s
@ -906,10 +918,10 @@ void AC_PosControl::input_pos_vel_accel_z(float &pos, float &vel, float accel, b
const float jerk_max_z_cmsss = _jerk_max_z_cmsss * overspeed_gain; const float jerk_max_z_cmsss = _jerk_max_z_cmsss * overspeed_gain;
// adjust desired altitude if motors have not hit their limits // adjust desired altitude if motors have not hit their limits
update_pos_vel_accel(_pos_target.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error()); update_pos_vel_accel(_pos_desired.z, _vel_desired.z, _accel_desired.z, _dt, _limit_vector.z, _p_pos_z.get_error(), _pid_vel_z.get_error());
shape_pos_vel_accel(pos, vel, accel, shape_pos_vel_accel(pos, vel, accel,
_pos_target.z, _vel_desired.z, _accel_desired.z, _pos_desired.z, _vel_desired.z, _accel_desired.z,
_vel_max_down_cms, _vel_max_up_cms, _vel_max_down_cms, _vel_max_up_cms,
-constrain_float(accel_max_z_cmss, 0.0f, 750.0f), accel_max_z_cmss, -constrain_float(accel_max_z_cmss, 0.0f, 750.0f), accel_max_z_cmss,
jerk_max_z_cmsss, _dt, limit_output); jerk_max_z_cmsss, _dt, limit_output);
@ -927,19 +939,32 @@ void AC_PosControl::set_alt_target_with_slew(float pos)
input_pos_vel_accel_z(pos, zero, 0); input_pos_vel_accel_z(pos, zero, 0);
} }
/// update_pos_offset_z - updates the vertical offsets used by terrain following /// update_offsets_z - updates the vertical offsets used by terrain following
void AC_PosControl::update_pos_offset_z(float pos_offset_z) void AC_PosControl::update_offsets_z()
{ {
postype_t p_offset_z = _pos_offset_z; // check for offset target timeout
update_pos_vel_accel(p_offset_z, _vel_offset_z, _accel_offset_z, _dt, MIN(_limit_vector.z, 0.0f), _p_pos_z.get_error(), _pid_vel_z.get_error()); uint32_t now_ms = AP_HAL::millis();
_pos_offset_z = p_offset_z; if (now_ms - _posvelaccel_offset_target_z_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
_pos_offset_target.z = 0.0;
_vel_offset_target.z = 0.0;
_accel_offset_target.z = 0.0;
}
// input shape the terrain offset // update position, velocity, accel offsets for this iteration
shape_pos_vel_accel(pos_offset_z, 0.0f, 0.0f, postype_t p_offset_z = _pos_offset.z;
_pos_offset_z, _vel_offset_z, _accel_offset_z, update_pos_vel_accel(p_offset_z, _vel_offset.z, _accel_offset.z, _dt, MIN(_limit_vector.z, 0.0f), _p_pos_z.get_error(), _pid_vel_z.get_error());
_pos_offset.z = p_offset_z;
// input shape vertical position, velocity and acceleration offsets
shape_pos_vel_accel(_pos_offset_target.z, _vel_offset_target.z, _accel_offset_target.z,
_pos_offset.z, _vel_offset.z, _accel_offset.z,
get_max_speed_down_cms(), get_max_speed_up_cms(), get_max_speed_down_cms(), get_max_speed_up_cms(),
-get_max_accel_z_cmss(), get_max_accel_z_cmss(), -get_max_accel_z_cmss(), get_max_accel_z_cmss(),
_jerk_max_z_cmsss, _dt, false); _jerk_max_z_cmsss, _dt, false);
p_offset_z = _pos_offset_target.z;
update_pos_vel_accel(p_offset_z, _vel_offset_target.z, _accel_offset_target.z, _dt, 0.0, 0.0, 0.0);
_pos_offset_target.z = p_offset_z;
} }
// is_active_z - returns true if the z position controller has been run in the previous loop // is_active_z - returns true if the z position controller has been run in the previous loop
@ -968,6 +993,11 @@ void AC_PosControl::update_z_controller()
} }
_last_update_z_ticks = AP::scheduler().ticks32(); _last_update_z_ticks = AP::scheduler().ticks32();
// update the position, velocity and acceleration offsets
update_offsets_z();
update_terrain();
_pos_target.z = _pos_desired.z + _pos_offset.z + _pos_terrain;
// calculate the target velocity correction // calculate the target velocity correction
float pos_target_zf = _pos_target.z; float pos_target_zf = _pos_target.z;
@ -975,9 +1005,10 @@ void AC_PosControl::update_z_controller()
_vel_target.z *= AP::ahrs().getControlScaleZ(); _vel_target.z *= AP::ahrs().getControlScaleZ();
_pos_target.z = pos_target_zf; _pos_target.z = pos_target_zf;
_pos_desired.z = _pos_target.z - (_pos_offset.z + _pos_terrain);
// add feed forward component // add feed forward component
_vel_target.z += _vel_desired.z; _vel_target.z += _vel_desired.z + _vel_offset.z + _vel_terrain;
// Velocity Controller // Velocity Controller
@ -986,7 +1017,7 @@ void AC_PosControl::update_z_controller()
_accel_target.z *= AP::ahrs().getControlScaleZ(); _accel_target.z *= AP::ahrs().getControlScaleZ();
// add feed forward component // add feed forward component
_accel_target.z += _accel_desired.z; _accel_target.z += _accel_desired.z + _accel_offset.z + _accel_terrain;
// Acceleration Controller // Acceleration Controller
@ -1045,18 +1076,18 @@ float AC_PosControl::get_lean_angle_max_cd() const
return _lean_angle_max * 100.0f; return _lean_angle_max * 100.0f;
} }
/// set position, velocity and acceleration targets /// set the desired position, velocity and acceleration targets
void AC_PosControl::set_pos_vel_accel(const Vector3p& pos, const Vector3f& vel, const Vector3f& accel) void AC_PosControl::set_pos_vel_accel(const Vector3p& pos, const Vector3f& vel, const Vector3f& accel)
{ {
_pos_target = pos; _pos_desired = pos;
_vel_desired = vel; _vel_desired = vel;
_accel_desired = accel; _accel_desired = accel;
} }
/// set position, velocity and acceleration targets /// set the desired position, velocity and acceleration targets
void AC_PosControl::set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& vel, const Vector2f& accel) void AC_PosControl::set_pos_vel_accel_xy(const Vector2p& pos, const Vector2f& vel, const Vector2f& accel)
{ {
_pos_target.xy() = pos; _pos_desired.xy() = pos;
_vel_desired.xy() = vel; _vel_desired.xy() = vel;
_accel_desired.xy() = accel; _accel_desired.xy() = accel;
} }
@ -1079,6 +1110,136 @@ Vector3f AC_PosControl::lean_angles_to_accel(const Vector3f& att_target_euler) c
}; };
} }
/// Terrain
/// set the terrain position, velocity and acceleration in cm, cms and cm/s/s from EKF origin in NE frame
/// this is used to initiate the offsets when initialise the position controller or do an offset reset
void AC_PosControl::init_terrain()
{
// set terrain position and target to zero
_pos_terrain_target = 0.0;
_pos_terrain = 0.0;
// set velocity offset to zero
_vel_terrain = 0.0;
// set acceleration offset to zero
_accel_terrain = 0.0;
}
// init_pos_terrain_cm - initialises the current terrain altitude and target altitude to pos_offset_terrain_cm
void AC_PosControl::init_pos_terrain_cm(float pos_terrain_cm)
{
_pos_desired.z -= (pos_terrain_cm - _pos_terrain);
_pos_terrain_target = pos_terrain_cm;
_pos_terrain = pos_terrain_cm;
}
/// Offsets
/// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
/// this is used to initiate the offsets when initialise the position controller or do an offset reset
void AC_PosControl::init_offsets_xy()
{
// check for offset target timeout
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _posvelaccel_offset_target_xy_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
_pos_offset_target.xy().zero();
_vel_offset_target.xy().zero();
_accel_offset_target.xy().zero();
}
// set position offset to target
_pos_offset.xy() = _pos_offset_target.xy();
// set velocity offset to target
_vel_offset.xy() = _vel_offset_target.xy();
// set acceleration offset to target
_accel_offset.xy() = _accel_offset_target.xy();
}
/// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
/// this is used to initiate the offsets when initialise the position controller or do an offset reset
void AC_PosControl::init_offsets_z()
{
// check for offset target timeout
uint32_t now_ms = AP_HAL::millis();
if (now_ms - _posvelaccel_offset_target_z_ms > POSCONTROL_POSVELACCEL_OFFSET_TARGET_TIMEOUT_MS) {
_pos_offset_target.z = 0.0;
_vel_offset_target.z = 0.0;
_accel_offset_target.z = 0.0;
}
// set position offset to target
_pos_offset.z = _pos_offset_target.z;
// set velocity offset to target
_vel_offset.z = _vel_offset_target.z;
// set acceleration offset to target
_accel_offset.z = _accel_offset_target.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)
{
set_posvelaccel_offset_target_xy_cm(pos_offset_NED.topostype().xy() * 100.0, vel_offset_NED.xy() * 100.0, accel_offset_NED.xy() * 100.0);
set_posvelaccel_offset_target_z_cm(-pos_offset_NED.topostype().z * 100.0, -vel_offset_NED.z * 100, -accel_offset_NED.z * 100.0);
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)
{
pos_offset_NED.xy() = _pos_offset_target.xy().tofloat() * 0.01;
pos_offset_NED.z = -_pos_offset_target.z * 0.01;
vel_offset_NED.xy() = _vel_offset_target.xy() * 0.01;
vel_offset_NED.z = -_vel_offset_target.z * 0.01;
accel_offset_NED.xy() = _accel_offset_target.xy() * 0.01;
accel_offset_NED.z = -_accel_offset_target.z * 0.01;
return true;
}
#endif
/// 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_xy_cm(const Vector2p& pos_offset_target_xy_cm, const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss)
{
// set position offset target
_pos_offset_target.xy() = pos_offset_target_xy_cm;
// set velocity offset target
_vel_offset_target.xy() = vel_offset_target_xy_cms;
// set acceleration offset target
_accel_offset_target.xy() = accel_offset_target_xy_cmss;
// record time of update so we can detect timeouts
_posvelaccel_offset_target_xy_ms = AP_HAL::millis();
}
/// 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_z_cm(float pos_offset_target_z_cm, float vel_offset_target_z_cms, const float accel_offset_target_z_cmss)
{
// set position offset target
_pos_offset_target.z = pos_offset_target_z_cm;
// set velocity offset target
_vel_offset_target.z = vel_offset_target_z_cms;
// set acceleration offset target
_accel_offset_target.z = accel_offset_target_z_cmss;
// record time of update so we can detect timeouts
_posvelaccel_offset_target_z_ms = AP_HAL::millis();
}
// returns the NED target acceleration vector for attitude control // returns the NED target acceleration vector for attitude control
Vector3f AC_PosControl::get_thrust_vector() const Vector3f AC_PosControl::get_thrust_vector() const
{ {
@ -1091,10 +1252,12 @@ Vector3f AC_PosControl::get_thrust_vector() const
/// function does not change the z axis /// function does not change the z axis
void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
{ {
// todo: we should use the current target position and velocity if we are currently running the position controller
stopping_point = _inav.get_position_xy_cm().topostype(); stopping_point = _inav.get_position_xy_cm().topostype();
float kP = _p_pos_xy.kP(); stopping_point -= _pos_offset.xy();
Vector2f curr_vel = _inav.get_velocity_xy_cms(); Vector2f curr_vel = _inav.get_velocity_xy_cms();
curr_vel -= _vel_offset.xy();
// calculate current velocity // calculate current velocity
float vel_total = curr_vel.length(); float vel_total = curr_vel.length();
@ -1103,6 +1266,7 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
return; return;
} }
float kP = _p_pos_xy.kP();
const float stopping_dist = stopping_distance(constrain_float(vel_total, 0.0, _vel_max_xy_cms), kP, _accel_max_xy_cmss); const float stopping_dist = stopping_distance(constrain_float(vel_total, 0.0, _vel_max_xy_cms), kP, _accel_max_xy_cmss);
if (!is_positive(stopping_dist)) { if (!is_positive(stopping_dist)) {
return; return;
@ -1116,7 +1280,11 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void AC_PosControl::get_stopping_point_z_cm(postype_t &stopping_point) const void AC_PosControl::get_stopping_point_z_cm(postype_t &stopping_point) const
{ {
const float curr_pos_z = _inav.get_position_z_up_cm(); float curr_pos_z = _inav.get_position_z_up_cm();
curr_pos_z -= _pos_offset.z;
float curr_vel_z = _inav.get_velocity_z_up_cms();
curr_vel_z -= _vel_offset.z;
// avoid divide by zero by using current position if kP is very low or acceleration is zero // avoid divide by zero by using current position if kP is very low or acceleration is zero
if (!is_positive(_p_pos_z.kP()) || !is_positive(_accel_max_z_cmss)) { if (!is_positive(_p_pos_z.kP()) || !is_positive(_accel_max_z_cmss)) {
@ -1124,7 +1292,7 @@ void AC_PosControl::get_stopping_point_z_cm(postype_t &stopping_point) const
return; return;
} }
stopping_point = curr_pos_z + constrain_float(stopping_distance(_inav.get_velocity_z_up_cms(), _p_pos_z.kP(), _accel_max_z_cmss), - POSCONTROL_STOPPING_DIST_DOWN_MAX, POSCONTROL_STOPPING_DIST_UP_MAX); stopping_point = curr_pos_z + constrain_float(stopping_distance(curr_vel_z, _p_pos_z.kP(), _accel_max_z_cmss), - POSCONTROL_STOPPING_DIST_DOWN_MAX, POSCONTROL_STOPPING_DIST_UP_MAX);
} }
/// get_bearing_to_target_cd - get bearing to target position in centi-degrees /// get_bearing_to_target_cd - get bearing to target position in centi-degrees
@ -1176,18 +1344,32 @@ void AC_PosControl::write_log()
if (is_active_xy()) { if (is_active_xy()) {
float accel_x, accel_y; float accel_x, accel_y;
lean_angles_to_accel_xy(accel_x, accel_y); lean_angles_to_accel_xy(accel_x, accel_y);
Write_PSCN(get_pos_target_cm().x, _inav.get_position_neu_cm().x, Write_PSCN(_pos_desired.x, _pos_target.x, _inav.get_position_neu_cm().x,
get_vel_desired_cms().x, get_vel_target_cms().x, _inav.get_velocity_neu_cms().x, _vel_desired.x, _vel_target.x, _inav.get_velocity_neu_cms().x,
_accel_desired.x, get_accel_target_cmss().x, accel_x); _accel_desired.x, _accel_target.x, accel_x);
Write_PSCE(get_pos_target_cm().y, _inav.get_position_neu_cm().y, Write_PSCE(_pos_desired.y, _pos_target.y, _inav.get_position_neu_cm().y,
get_vel_desired_cms().y, get_vel_target_cms().y, _inav.get_velocity_neu_cms().y, _vel_desired.y, _vel_target.y, _inav.get_velocity_neu_cms().y,
_accel_desired.y, get_accel_target_cmss().y, accel_y); _accel_desired.y, _accel_target.y, accel_y);
// log offsets if they are being used
if (!_pos_offset.xy().is_zero()) {
Write_PSON(_pos_offset_target.x, _pos_offset.x, _vel_offset_target.x, _vel_offset.x, _accel_offset_target.x, _accel_offset.x);
Write_PSOE(_pos_offset_target.y, _pos_offset.y, _vel_offset_target.y, _vel_offset.y, _accel_offset_target.y, _accel_offset.y);
}
} }
if (is_active_z()) { if (is_active_z()) {
Write_PSCD(-get_pos_target_cm().z, -_inav.get_position_z_up_cm(), Write_PSCD(-_pos_desired.z, -_pos_target.z, -_inav.get_position_z_up_cm(),
-get_vel_desired_cms().z, -get_vel_target_cms().z, -_inav.get_velocity_z_up_cms(), -_vel_desired.z, -_vel_target.z, -_inav.get_velocity_z_up_cms(),
-_accel_desired.z, -get_accel_target_cmss().z, -get_z_accel_cmss()); -_accel_desired.z, -_accel_target.z, -get_z_accel_cmss());
// log down and terrain offsets if they are being used
if (!is_zero(_pos_offset.z)) {
Write_PSOD(-_pos_offset_target.z, -_pos_offset.z, -_vel_offset_target.z, -_vel_offset.z, -_accel_offset_target.z, -_accel_offset.z);
}
if (!is_zero(_pos_terrain)) {
Write_PSOT(-_pos_terrain_target, -_pos_terrain, 0, -_vel_terrain, 0, -_accel_terrain);
}
} }
} }
#endif // HAL_LOGGING_ENABLED #endif // HAL_LOGGING_ENABLED
@ -1213,6 +1395,27 @@ float AC_PosControl::crosstrack_error() const
/// private methods /// private methods
/// ///
/// Terrain
/// update_z_offsets - updates the vertical offsets used by terrain following
void AC_PosControl::update_terrain()
{
// update position, velocity, accel offsets for this iteration
postype_t pos_terrain = _pos_terrain;
update_pos_vel_accel(pos_terrain, _vel_terrain, _accel_terrain, _dt, MIN(_limit_vector.z, 0.0f), _p_pos_z.get_error(), _pid_vel_z.get_error());
_pos_terrain = pos_terrain;
// input shape horizontal position, velocity and acceleration offsets
shape_pos_vel_accel(_pos_terrain_target, 0.0, 0.0,
_pos_terrain, _vel_terrain, _accel_terrain,
get_max_speed_down_cms(), get_max_speed_up_cms(),
-get_max_accel_z_cmss(), get_max_accel_z_cmss(),
_jerk_max_z_cmsss, _dt, false);
// we do not have to update _pos_terrain_target 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 here
}
// get_lean_angles_to_accel - convert roll, pitch lean angles to NE frame accelerations in cm/s/s // get_lean_angles_to_accel - convert roll, pitch lean angles to NE frame accelerations in cm/s/s
void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
{ {
@ -1294,8 +1497,14 @@ void AC_PosControl::handle_ekf_xy_reset()
uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift); uint32_t reset_ms = _ahrs.getLastPosNorthEastReset(pos_shift);
if (reset_ms != _ekf_xy_reset_ms) { if (reset_ms != _ekf_xy_reset_ms) {
// ToDo: move EKF steps into the offsets for modes setting absolute position and velocity
// 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.xy() = (_inav.get_position_xy_cm() + _p_pos_xy.get_error()).topostype(); _pos_target.xy() = (_inav.get_position_xy_cm() + _p_pos_xy.get_error()).topostype();
_pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
_vel_target.xy() = _inav.get_velocity_xy_cms() + _pid_vel_xy.get_error(); _vel_target.xy() = _inav.get_velocity_xy_cms() + _pid_vel_xy.get_error();
_vel_desired.xy() = _vel_target.xy() - _vel_offset.xy();
_ekf_xy_reset_ms = reset_ms; _ekf_xy_reset_ms = reset_ms;
} }
@ -1316,8 +1525,14 @@ void AC_PosControl::handle_ekf_z_reset()
uint32_t reset_ms = _ahrs.getLastPosDownReset(alt_shift); uint32_t reset_ms = _ahrs.getLastPosDownReset(alt_shift);
if (reset_ms != 0 && reset_ms != _ekf_z_reset_ms) { if (reset_ms != 0 && reset_ms != _ekf_z_reset_ms) {
// ToDo: move EKF steps into the offsets for modes setting absolute position and velocity
// 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.z = _inav.get_position_z_up_cm() + _p_pos_z.get_error(); _pos_target.z = _inav.get_position_z_up_cm() + _p_pos_z.get_error();
_pos_desired.z = _pos_target.z - (_pos_offset.z + _pos_terrain);
_vel_target.z = _inav.get_velocity_z_up_cms() + _pid_vel_z.get_error(); _vel_target.z = _inav.get_velocity_z_up_cms() + _pid_vel_z.get_error();
_vel_desired.z = _vel_target.z - (_vel_offset.z + _vel_terrain);
_ekf_z_reset_ms = reset_ms; _ekf_z_reset_ms = reset_ms;
} }

143
libraries/AC_AttitudeControl/AC_PosControl.h
View File

@ -11,6 +11,7 @@
#include <AC_PID/AC_PID_Basic.h> // PID library (1-axis) #include <AC_PID/AC_PID_Basic.h> // PID library (1-axis)
#include <AC_PID/AC_PID_2D.h> // PID library (2-axis) #include <AC_PID/AC_PID_2D.h> // PID library (2-axis)
#include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library #include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
#include <AP_Scripting/AP_Scripting_config.h>
#include "AC_AttitudeControl.h" // Attitude control library #include "AC_AttitudeControl.h" // Attitude control library
#include <AP_Logger/LogStructure.h> #include <AP_Logger/LogStructure.h>
@ -62,7 +63,7 @@ public:
/// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position. /// input_pos_xyz - calculate a jerk limited path from the current position, velocity and acceleration to an input position.
/// 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 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_xy. /// The kinematic path is constrained by the maximum acceleration and jerk set using the function set_max_speed_accel_xy.
void input_pos_xyz(const Vector3p& pos, float pos_offset_z, float pos_offset_z_buffer); void input_pos_xyz(const Vector3p& pos, float pos_terrain_target, float terrain_buffer);
/// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range /// pos_offset_z_scaler - calculates a multiplier used to reduce the horizontal velocity to allow the z position controller to stay within the provided buffer range
float pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer) const; float pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_buffer) const;
@ -225,9 +226,6 @@ public:
/// using the default position control kinematic path. /// using the default position control kinematic path.
void set_alt_target_with_slew(float pos); void set_alt_target_with_slew(float pos);
/// update_pos_offset_z - updates the vertical offsets used by terrain following
void update_pos_offset_z(float pos_offset);
// is_active_z - returns true if the z position controller has been run in the previous 5 loop times // is_active_z - returns true if the z position controller has been run in the previous 5 loop times
bool is_active_z() const; bool is_active_z() const;
@ -250,32 +248,44 @@ public:
/// Position /// Position
/// set_pos_target_xy_cm - sets the position target, frame NEU in cm relative to the EKF origin
void set_pos_target_xy_cm(float pos_x, float pos_y) { _pos_target.x = pos_x; _pos_target.y = pos_y; }
/// get_pos_target_cm - returns the position target, frame NEU in cm relative to the EKF origin /// get_pos_target_cm - returns the position target, frame NEU in cm relative to the EKF origin
const Vector3p& get_pos_target_cm() const { return _pos_target; } const Vector3p& get_pos_target_cm() const { return _pos_target; }
/// set_pos_target_z_cm - set altitude target in cm above the EKF origin /// set_pos_desired_xy_cm - sets the position target, frame NEU in cm relative to the EKF origin
void set_pos_target_z_cm(float pos_target) { _pos_target.z = pos_target; } void set_pos_desired_xy_cm(const Vector2f& pos) { _pos_desired.xy() = pos.topostype(); }
/// get_pos_desired_cm - returns the position desired, frame NEU in cm relative to the EKF origin
const Vector3p& get_pos_desired_cm() const { return _pos_desired; }
/// get_pos_target_z_cm - get target altitude (in cm above the EKF origin) /// get_pos_target_z_cm - get target altitude (in cm above the EKF origin)
float get_pos_target_z_cm() const { return _pos_target.z; } float get_pos_target_z_cm() const { return _pos_target.z; }
/// set_pos_desired_z_cm - set altitude target in cm above the EKF origin
void set_pos_desired_z_cm(float pos_z) { _pos_desired.z = pos_z; }
/// get_pos_desired_z_cm - get target altitude (in cm above the EKF origin)
float get_pos_desired_z_cm() const { return _pos_desired.z; }
/// Stopping Point
/// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration /// get_stopping_point_xy_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void get_stopping_point_xy_cm(Vector2p &stopping_point) const; void get_stopping_point_xy_cm(Vector2p &stopping_point) const;
/// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
void get_stopping_point_z_cm(postype_t &stopping_point) const; void get_stopping_point_z_cm(postype_t &stopping_point) const;
/// Position Error
/// get_pos_error_cm - get position error vector between the current and target position /// get_pos_error_cm - get position error vector between the current and target position
const Vector3f get_pos_error_cm() const { return (_pos_target - _inav.get_position_neu_cm().topostype()).tofloat(); } const Vector3f get_pos_error_cm() const { return Vector3f(_p_pos_xy.get_error().x, _p_pos_xy.get_error().y, _p_pos_z.get_error()); }
/// get_pos_error_xy_cm - get the length of the position error vector in the xy plane /// get_pos_error_xy_cm - get the length of the position error vector in the xy plane
float get_pos_error_xy_cm() const { return get_horizontal_distance_cm(_inav.get_position_xy_cm().topostype(), _pos_target.xy()); } float get_pos_error_xy_cm() const { return _p_pos_xy.get_error().length(); }
/// get_pos_error_z_cm - returns altitude error in cm /// get_pos_error_z_cm - returns altitude error in cm
float get_pos_error_z_cm() const { return (_pos_target.z - _inav.get_position_z_up_cm()); } float get_pos_error_z_cm() const { return _p_pos_z.get_error(); }
/// Velocity /// Velocity
@ -286,7 +296,7 @@ public:
/// set_vel_desired_xy_cms - sets horizontal desired velocity in NEU cm/s /// set_vel_desired_xy_cms - sets horizontal desired velocity in NEU cm/s
void set_vel_desired_xy_cms(const Vector2f &vel) {_vel_desired.xy() = vel; } void set_vel_desired_xy_cms(const Vector2f &vel) {_vel_desired.xy() = vel; }
/// get_vel_desired_cms - returns desired velocity (i.e. feed forward) in cm/s in NEU /// get_vel_desired_cms - returns desired velocity in cm/s in NEU
const Vector3f& get_vel_desired_cms() { return _vel_desired; } const Vector3f& get_vel_desired_cms() { return _vel_desired; }
// get_vel_target_cms - returns the target velocity in NEU cm/s // get_vel_target_cms - returns the target velocity in NEU cm/s
@ -301,30 +311,56 @@ public:
/// Acceleration /// Acceleration
// set_accel_desired_xy_cmss set desired acceleration in cm/s in xy axis // set_accel_desired_xy_cmss - set desired acceleration in cm/s in xy axis
void set_accel_desired_xy_cmss(const Vector2f &accel_cms) { _accel_desired.xy() = accel_cms; } void set_accel_desired_xy_cmss(const Vector2f &accel_cms) { _accel_desired.xy() = accel_cms; }
// get_accel_target_cmss - returns the target acceleration in NEU cm/s/s // get_accel_target_cmss - returns the target acceleration in NEU cm/s/s
const Vector3f& get_accel_target_cmss() const { return _accel_target; } const Vector3f& get_accel_target_cmss() const { return _accel_target; }
/// Terrain
// set_pos_terrain_target_cm - set target terrain altitude in cm
void set_pos_terrain_target_cm(float pos_terrain_target) {_pos_terrain_target = pos_terrain_target;}
// init_pos_terrain_cm - initialises the current terrain altitude and target altitude to pos_offset_terrain_cm
void init_pos_terrain_cm(float pos_offset_terrain_cm);
// get_pos_terrain_cm - returns the current terrain altitude in cm
float get_pos_terrain_cm() { return _pos_terrain; }
/// Offset /// Offset
/// set_pos_offset_target_z_cm - set altitude offset target in cm above the EKF origin #if AP_SCRIPTING_ENABLED
void set_pos_offset_target_z_cm(float pos_offset_target_z) { _pos_offset_target_z = pos_offset_target_z; } // 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);
#endif
/// 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 set_posvelaccel_offset_target_xy_cm(const Vector2p& pos_offset_target_xy_cm, const Vector2f& vel_offset_target_xy_cms, const Vector2f& accel_offset_target_xy_cmss);
void set_posvelaccel_offset_target_z_cm(float pos_offset_target_z_cm, float vel_offset_target_z_cms, float accel_offset_target_z_cmss);
/// get the position, velocity or acceleration offets in cm from EKF origin in NEU frame
const Vector3p& get_pos_offset_cm() const { return _pos_offset; }
const Vector3f& get_vel_offset_cms() const { return _vel_offset; }
const Vector3f& get_accel_offset_cmss() const { return _accel_offset; }
/// set_pos_offset_z_cm - set altitude offset in cm above the EKF origin /// set_pos_offset_z_cm - set altitude offset in cm above the EKF origin
void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset_z = pos_offset_z; } void set_pos_offset_z_cm(float pos_offset_z) { _pos_offset.z = pos_offset_z; }
/// get_pos_offset_z_cm - returns altitude offset in cm above the EKF origin /// get_pos_offset_z_cm - returns altitude offset in cm above the EKF origin
float get_pos_offset_z_cm() const { return _pos_offset_z; } float get_pos_offset_z_cm() const { return _pos_offset.z; }
/// get_vel_offset_z_cm - returns current vertical offset speed in cm/s /// get_vel_offset_z_cm - returns current vertical offset speed in cm/s
float get_vel_offset_z_cms() const { return _vel_offset_z; } float get_vel_offset_z_cms() const { return _vel_offset.z; }
/// get_accel_offset_z_cm - returns current vertical offset acceleration in cm/s/s /// get_accel_offset_z_cm - returns current vertical offset acceleration in cm/s/s
float get_accel_offset_z_cmss() const { return _accel_offset_z; } float get_accel_offset_z_cmss() const { return _accel_offset.z; }
/// Outputs /// Outputs
@ -407,9 +443,13 @@ public:
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
static void Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); static void Write_PSCN(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
static void Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); static void Write_PSCE(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
static void Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); static void Write_PSCD(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
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);
protected: protected:
@ -428,6 +468,32 @@ protected:
// calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected // calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected
float calculate_overspeed_gain(); float calculate_overspeed_gain();
/// Terrain Following
/// set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
/// this is used to initiate the offsets when initialise the position controller or do an offset reset
/// note that this sets the actual offsets, not the offset targets
void init_terrain();
/// update_terrain - updates the terrain position, velocity and acceleration estimation
/// this moves the estimated terrain position _pos_terrain towards the target _pos_terrain_target
void update_terrain();
/// Offsets
/// init_offsets - set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame
/// this is used to initiate the offsets when initialise the position controller or do an offset reset
/// note that this sets the actual offsets, not the offset targets
void init_offsets_xy();
void init_offsets_z();
/// update_offsets - update the position and velocity offsets
/// this moves the offsets (e.g _pos_offset, _vel_offset, _accel_offset) towards the targets (e.g. _pos_offset_target or _vel_offset_target)
void update_offsets_xy();
void update_offsets_z();
/// initialise and check for ekf position resets /// initialise and check for ekf position resets
void init_ekf_xy_reset(); void init_ekf_xy_reset();
void handle_ekf_xy_reset(); void handle_ekf_xy_reset();
@ -472,7 +538,8 @@ protected:
float _yaw_rate_target; // desired yaw rate in centi-degrees per second calculated by position controller float _yaw_rate_target; // desired yaw rate in centi-degrees per second calculated by position controller
// position controller internal variables // position controller internal variables
Vector3p _pos_target; // target location, frame NEU in cm relative to the EKF origin Vector3p _pos_desired; // desired location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_target minus offsets
Vector3p _pos_target; // target location, frame NEU in cm relative to the EKF origin. This is equal to the _pos_desired plus offsets
Vector3f _vel_desired; // desired velocity in NEU cm/s Vector3f _vel_desired; // desired velocity in NEU cm/s
Vector3f _vel_target; // velocity target in NEU cm/s calculated by pos_to_rate step Vector3f _vel_target; // velocity target in NEU cm/s calculated by pos_to_rate step
Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward) Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward)
@ -481,10 +548,21 @@ protected:
bool _fwd_pitch_is_limited; // true when the forward pitch demand is being limited to meet acceleration limits bool _fwd_pitch_is_limited; // true when the forward pitch demand is being limited to meet acceleration limits
float _pos_offset_target_z; // vertical position offset target, frame NEU in cm relative to the EKF origin // terrain handling variables
float _pos_offset_z; // vertical position offset, frame NEU in cm relative to the EKF origin float _pos_terrain_target; // position terrain target in cm relative to the EKF origin in NEU frame
float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step float _pos_terrain; // position terrain in cm from the EKF origin in NEU frame. this terrain moves towards _pos_terrain_target
float _accel_offset_z; // vertical acceleration offset in NEU cm/s/s float _vel_terrain; // velocity terrain in NEU cm/s calculated by pos_to_rate step. this terrain moves towards _vel_terrain_target
float _accel_terrain; // acceleration terrain in NEU cm/s/s
// offset handling variables
Vector3p _pos_offset_target; // position offset target in cm relative to the EKF origin in NEU frame
Vector3p _pos_offset; // position offset in cm from the EKF origin in NEU frame. this offset moves towards _pos_offset_target
Vector3f _vel_offset_target; // velocity offset target in cm/s in NEU frame
Vector3f _vel_offset; // velocity offset in NEU cm/s calculated by pos_to_rate step. this offset moves towards _vel_offset_target
Vector3f _accel_offset_target; // acceleration offset target in cm/s/s in NEU frame
Vector3f _accel_offset; // acceleration offset in NEU cm/s/s
uint32_t _posvelaccel_offset_target_xy_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)
uint32_t _posvelaccel_offset_target_z_ms; // system time that pos, vel, accel targets were set (used to implement timeouts)
// ekf reset handling // ekf reset handling
uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset
@ -502,6 +580,11 @@ protected:
private: private:
// convenience method for writing out the identical PSCE, PSCN, PSCD - and // convenience method for writing out the identical PSCE, PSCN, PSCD - and
// to save bytes // to save bytes
static void Write_PSCx(LogMessages ID, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel); static void Write_PSCx(LogMessages ID, float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel);
// 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);
}; };

61
libraries/AC_AttitudeControl/AC_PosControl_Logging.cpp
View File

@ -8,11 +8,12 @@
#include "LogStructure.h" #include "LogStructure.h"
// a convenience function for writing out the position controller PIDs // a convenience function for writing out the position controller PIDs
void AC_PosControl::Write_PSCx(LogMessages id, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel) void AC_PosControl::Write_PSCx(LogMessages id, float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{ {
const struct log_PSCx pkt{ const struct log_PSCx pkt{
LOG_PACKET_HEADER_INIT(id), LOG_PACKET_HEADER_INIT(id),
time_us : AP_HAL::micros64(), time_us : AP_HAL::micros64(),
pos_desired : pos_desired * 0.01f,
pos_target : pos_target * 0.01f, pos_target : pos_target * 0.01f,
pos : pos * 0.01f, pos : pos * 0.01f,
vel_desired : vel_desired * 0.01f, vel_desired : vel_desired * 0.01f,
@ -25,19 +26,65 @@ void AC_PosControl::Write_PSCx(LogMessages id, float pos_target, float pos, floa
AP::logger().WriteBlock(&pkt, sizeof(pkt)); AP::logger().WriteBlock(&pkt, sizeof(pkt));
} }
void AC_PosControl::Write_PSCN(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel) void AC_PosControl::Write_PSCN(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{ {
Write_PSCx(LOG_PSCN_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel); Write_PSCx(LOG_PSCN_MSG, pos_desired, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
} }
void AC_PosControl::Write_PSCE(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel) void AC_PosControl::Write_PSCE(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{ {
Write_PSCx(LOG_PSCE_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel); Write_PSCx(LOG_PSCE_MSG, pos_desired, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
} }
void AC_PosControl::Write_PSCD(float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel) void AC_PosControl::Write_PSCD(float pos_desired, float pos_target, float pos, float vel_desired, float vel_target, float vel, float accel_desired, float accel_target, float accel)
{ {
Write_PSCx(LOG_PSCD_MSG, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel); Write_PSCx(LOG_PSCD_MSG, pos_desired, pos_target, pos, vel_desired, vel_target, vel, accel_desired, accel_target, accel);
}
// 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)
{
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,
};
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)
{
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);
}
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)
{
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);
}
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)
{
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);
}
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)
{
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);
} }
#endif // HAL_LOGGING_ENABLED #endif // HAL_LOGGING_ENABLED

84
libraries/AC_AttitudeControl/LogStructure.h
View File

@ -6,11 +6,16 @@
LOG_PSCN_MSG, \ LOG_PSCN_MSG, \
LOG_PSCE_MSG, \ LOG_PSCE_MSG, \
LOG_PSCD_MSG, \ LOG_PSCD_MSG, \
LOG_PSON_MSG, \
LOG_PSOE_MSG, \
LOG_PSOD_MSG, \
LOG_PSOT_MSG, \
LOG_ANG_MSG LOG_ANG_MSG
// @LoggerMessage: PSCN // @LoggerMessage: PSCN
// @Description: Position Control North // @Description: Position Control North
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
// @Field: DPN: Desired position relative to EKF origin
// @Field: TPN: Target position relative to EKF origin // @Field: TPN: Target position relative to EKF origin
// @Field: PN: Position relative to EKF origin // @Field: PN: Position relative to EKF origin
// @Field: DVN: Desired velocity North // @Field: DVN: Desired velocity North
@ -23,6 +28,7 @@
// @LoggerMessage: PSCE // @LoggerMessage: PSCE
// @Description: Position Control East // @Description: Position Control East
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
// @Field: DPE: Desired position relative to EKF origin + Offsets
// @Field: TPE: Target position relative to EKF origin // @Field: TPE: Target position relative to EKF origin
// @Field: PE: Position relative to EKF origin // @Field: PE: Position relative to EKF origin
// @Field: DVE: Desired velocity East // @Field: DVE: Desired velocity East
@ -35,6 +41,7 @@
// @LoggerMessage: PSCD // @LoggerMessage: PSCD
// @Description: Position Control Down // @Description: Position Control Down
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
// @Field: DPD: Desired position relative to EKF origin + Offsets
// @Field: TPD: Target position relative to EKF origin // @Field: TPD: Target position relative to EKF origin
// @Field: PD: Position relative to EKF origin // @Field: PD: Position relative to EKF origin
// @Field: DVD: Desired velocity Down // @Field: DVD: Desired velocity Down
@ -49,6 +56,7 @@
struct PACKED log_PSCx { struct PACKED log_PSCx {
LOG_PACKET_HEADER; LOG_PACKET_HEADER;
uint64_t time_us; uint64_t time_us;
float pos_desired;
float pos_target; float pos_target;
float pos; float pos;
float vel_desired; float vel_desired;
@ -59,6 +67,58 @@ struct PACKED log_PSCx {
float accel; float accel;
}; };
// @LoggerMessage: PSON
// @Description: Position Control Offsets North
// @Field: TimeUS: Time since system startup
// @Field: TPON: Target position offset North
// @Field: PON: Position offset North
// @Field: TVON: Target velocity offset North
// @Field: VON: Velocity offset North
// @Field: TAON: Target acceleration offset North
// @Field: AON: Acceleration offset North
// @LoggerMessage: PSOE
// @Description: Position Control Offsets East
// @Field: TimeUS: Time since system startup
// @Field: TPOE: Target position offset East
// @Field: POE: Position offset East
// @Field: TVOE: Target velocity offset East
// @Field: VOE: Velocity offset East
// @Field: TAOE: Target acceleration offset East
// @Field: AOE: Acceleration offset East
// @LoggerMessage: PSOD
// @Description: Position Control Offsets Down
// @Field: TimeUS: Time since system startup
// @Field: TPOD: Target position offset Down
// @Field: POD: Position offset Down
// @Field: TVOD: Target velocity offset Down
// @Field: VOD: Velocity offset Down
// @Field: TAOD: Target acceleration offset Down
// @Field: AOD: Acceleration offset Down
// @LoggerMessage: PSOT
// @Description: Position Control Offsets Terrain (Down)
// @Field: TimeUS: Time since system startup
// @Field: TPOT: Target position offset Terrain
// @Field: POT: Position offset Terrain
// @Field: TVOT: Target velocity offset Terrain
// @Field: VOT: Velocity offset Terrain
// @Field: TAOT: Target acceleration offset Terrain
// @Field: AOT: Acceleration offset Terrain
// position controller per-axis offset logging
struct PACKED log_PSOx {
LOG_PACKET_HEADER;
uint64_t time_us;
float pos_target_offset;
float pos_offset;
float vel_target_offset;
float vel_offset;
float accel_target_offset;
float accel_offset;
};
// @LoggerMessage: RATE // @LoggerMessage: RATE
// @Description: Desired and achieved vehicle attitude rates. Not logged in Fixed Wing Plane modes. // @Description: Desired and achieved vehicle attitude rates. Not logged in Fixed Wing Plane modes.
// @Field: TimeUS: Time since system startup // @Field: TimeUS: Time since system startup
@ -115,17 +175,29 @@ struct PACKED log_ANG {
float sensor_dt; float sensor_dt;
}; };
#define PSCx_FMT "Qffffffff" #define PSCx_FMT "Qfffffffff"
#define PSCx_UNITS "smmnnnooo" #define PSCx_UNITS "smmmnnnooo"
#define PSCx_MULTS "F00000000" #define PSCx_MULTS "F000000000"
#define PSOx_FMT "Qffffff"
#define PSOx_UNITS "smmnnoo"
#define PSOx_MULTS "F000000"
#define LOG_STRUCTURE_FROM_AC_ATTITUDECONTROL \ #define LOG_STRUCTURE_FROM_AC_ATTITUDECONTROL \
{ LOG_PSCN_MSG, sizeof(log_PSCx), \ { LOG_PSCN_MSG, sizeof(log_PSCx), \
"PSCN", PSCx_FMT, "TimeUS,TPN,PN,DVN,TVN,VN,DAN,TAN,AN", PSCx_UNITS, PSCx_MULTS }, \ "PSCN", PSCx_FMT, "TimeUS,DPN,TPN,PN,DVN,TVN,VN,DAN,TAN,AN", PSCx_UNITS, PSCx_MULTS }, \
{ LOG_PSCE_MSG, sizeof(log_PSCx), \ { LOG_PSCE_MSG, sizeof(log_PSCx), \
"PSCE", PSCx_FMT, "TimeUS,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", PSCx_UNITS, PSCx_MULTS }, \ "PSCE", PSCx_FMT, "TimeUS,DPE,TPE,PE,DVE,TVE,VE,DAE,TAE,AE", PSCx_UNITS, PSCx_MULTS }, \
{ LOG_PSCD_MSG, sizeof(log_PSCx), \ { LOG_PSCD_MSG, sizeof(log_PSCx), \
"PSCD", PSCx_FMT, "TimeUS,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }, \ "PSCD", PSCx_FMT, "TimeUS,DPD,TPD,PD,DVD,TVD,VD,DAD,TAD,AD", PSCx_UNITS, PSCx_MULTS }, \
{ LOG_PSON_MSG, sizeof(log_PSOx), \
"PSON", PSOx_FMT, "TimeUS,TPON,PON,TVON,VON,TAON,AON", PSOx_UNITS, PSOx_MULTS }, \
{ LOG_PSOE_MSG, sizeof(log_PSOx), \
"PSOE", PSOx_FMT, "TimeUS,TPOE,POE,TVOE,VOE,TAOE,AOE", PSOx_UNITS, PSOx_MULTS }, \
{ LOG_PSOD_MSG, sizeof(log_PSOx), \
"PSOD", PSOx_FMT, "TimeUS,TPOD,POD,TVOD,VOD,TAOD,AOD", PSOx_UNITS, PSOx_MULTS }, \
{ LOG_PSOT_MSG, sizeof(log_PSOx), \
"PSOT", PSOx_FMT, "TimeUS,TPOT,POT,TVOT,VOT,TAOT,AOT", PSOx_UNITS, PSOx_MULTS }, \
{ LOG_RATE_MSG, sizeof(log_Rate), \ { LOG_RATE_MSG, sizeof(log_Rate), \
"RATE", "Qfffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true }, \ "RATE", "Qfffffffffffff", "TimeUS,RDes,R,ROut,PDes,P,POut,YDes,Y,YOut,ADes,A,AOut,AOutSlew", "skk-kk-kk-oo--", "F?????????BB--" , true }, \
{ LOG_ANG_MSG, sizeof(log_ANG),\ { LOG_ANG_MSG, sizeof(log_ANG),\