mirror of https://github.com/ArduPilot/ardupilot
AC_PosControl: support 3D pos, vel, accel offsets
Co-authored-by: Randy Mackay <rmackay9@yahoo.com>
This commit is contained in:
Leonard Hall
Randy Mackay
parent
e40ae8e649
commit
9fb8a0f1ac
|
|
@ -74,6 +74,9 @@ extern const AP_HAL::HAL& hal;
|
|||
#define POSCONTROL_VIBE_COMP_P_GAIN 0.250f
|
||||
#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[] = {
|
||||
// 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 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_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
|
||||
const float offset_z_scaler = pos_offset_z_scaler(pos_offset_z, pos_offset_z_buffer);
|
||||
|
||||
// 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;
|
||||
const float offset_z_scaler = pos_offset_z_scaler(pos_terrain_target, terrain_buffer);
|
||||
set_pos_terrain_target_cm(pos_terrain_target);
|
||||
|
||||
// calculated increased maximum acceleration and jerk if over speed
|
||||
const float overspeed_gain = calculate_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;
|
||||
|
||||
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
|
||||
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
|
||||
float vel_max_xy_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()) ) {
|
||||
dest_vector.normalize();
|
||||
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 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);
|
||||
|
||||
float posz = pos.z;
|
||||
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,
|
||||
-constrain_float(accel_max_z_cmss, 0.0f, 750.0f), accel_max_z_cmss,
|
||||
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)) {
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -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.
|
||||
/// 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()
|
||||
{
|
||||
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();
|
||||
_accel_desired.xy().zero();
|
||||
}
|
||||
|
|
@ -496,6 +488,7 @@ void AC_PosControl::soften_for_landing_xy()
|
|||
// decay position error to zero
|
||||
if (is_positive(_dt)) {
|
||||
_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.
|
||||
|
|
@ -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.
|
||||
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
|
||||
const Vector3f &att_target_euler_cd = _attitude_control.get_att_target_euler_cd();
|
||||
_roll_target = att_target_euler_cd.x;
|
||||
|
|
@ -516,10 +512,10 @@ void AC_PosControl::init_xy_controller()
|
|||
_angle_max_override_cd = 0.0;
|
||||
|
||||
_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_desired.xy() = curr_vel;
|
||||
_vel_target.xy() = curr_vel;
|
||||
_vel_target.xy() = _inav.get_velocity_xy_cms();
|
||||
_vel_desired.xy() = _vel_target.xy() - _vel_offset.xy();
|
||||
|
||||
// Set desired accel to zero because raw acceleration is prone to noise
|
||||
_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.
|
||||
void AC_PosControl::input_accel_xy(const Vector3f& accel)
|
||||
{
|
||||
// check for ekf xy position reset
|
||||
handle_ekf_xy_reset();
|
||||
|
||||
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);
|
||||
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_accel_xy(accel.xy(), _accel_desired.xy(), _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.
|
||||
|
|
@ -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.
|
||||
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(),
|
||||
_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.
|
||||
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);
|
||||
|
||||
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
|
||||
void AC_PosControl::stop_pos_xy_stabilisation()
|
||||
{
|
||||
_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
|
||||
void AC_PosControl::stop_vel_xy_stabilisation()
|
||||
{
|
||||
_pos_target.xy() = _inav.get_position_xy_cm().topostype();
|
||||
|
||||
const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
|
||||
_vel_desired.xy() = curr_vel;
|
||||
// with zero position error _vel_target = _vel_desired
|
||||
_vel_target.xy() = curr_vel;
|
||||
_pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
|
||||
|
||||
_vel_target.xy() = _inav.get_velocity_xy_cms();;
|
||||
_vel_desired.xy() = _vel_target.xy() - _vel_offset.xy();
|
||||
|
||||
// initialise I terms from lean angles
|
||||
_pid_vel_xy.reset_filter();
|
||||
|
|
@ -640,37 +655,44 @@ void AC_PosControl::update_xy_controller()
|
|||
float ahrsGndSpdLimit, ahrsControlScaleXY;
|
||||
AP::ahrs().getControlLimits(ahrsGndSpdLimit, ahrsControlScaleXY);
|
||||
|
||||
// update the position, velocity and acceleration offsets
|
||||
update_offsets_xy();
|
||||
|
||||
// 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
|
||||
const Vector3f &curr_pos = _inav.get_position_neu_cm();
|
||||
Vector3f comb_pos = curr_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
|
||||
vel_target *= ahrsControlScaleXY;
|
||||
_vel_target.xy() = vel_target;
|
||||
_vel_target.xy() += _vel_desired.xy();
|
||||
Vector2f vel_target = _p_pos_xy.update_all(_pos_target.x, _pos_target.y, comb_pos);
|
||||
_pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
|
||||
|
||||
// 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
|
||||
const Vector2f &curr_vel = _inav.get_velocity_xy_cms();
|
||||
Vector2f comb_vel = curr_vel;
|
||||
comb_vel += _disturb_vel;
|
||||
|
||||
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
|
||||
accel_target *= ahrsControlScaleXY;
|
||||
|
||||
// pass the correction acceleration to the target acceleration output
|
||||
_accel_target.xy() = accel_target;
|
||||
|
||||
// Add feed forward into the target acceleration output
|
||||
_accel_target.xy() += _accel_desired.xy();
|
||||
|
||||
// Acceleration Controller
|
||||
_accel_target.xy() += _accel_desired.xy() + _accel_offset.xy();
|
||||
|
||||
// limit acceleration using maximum lean angles
|
||||
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);
|
||||
calculate_yaw_and_rate_yaw();
|
||||
|
||||
// reset the disturbance from system ID mode to zero
|
||||
_disturb_pos.zero();
|
||||
_disturb_vel.zero();
|
||||
}
|
||||
|
|
@ -751,10 +774,14 @@ void AC_PosControl::init_z_controller_no_descent()
|
|||
init_z_controller();
|
||||
|
||||
// remove all descent if present
|
||||
_vel_desired.z = MAX(0.0, _vel_desired.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_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.
|
||||
|
|
@ -765,7 +792,8 @@ void AC_PosControl::init_z_controller_stopping_point()
|
|||
// Initialise the position controller to the current throttle, position, velocity and acceleration.
|
||||
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;
|
||||
_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
|
||||
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();
|
||||
_vel_desired.z = curr_vel_z;
|
||||
// with zero position error _vel_target = _vel_desired
|
||||
_vel_target.z = curr_vel_z;
|
||||
// initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
|
||||
init_offsets_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
|
||||
_pid_vel_z.reset_filter();
|
||||
_pid_vel_z.set_integrator(0.0f);
|
||||
|
||||
_accel_desired.z = constrain_float(get_z_accel_cmss(), -_accel_max_z_cmss, _accel_max_z_cmss);
|
||||
// with zero position error _accel_target = _accel_desired
|
||||
_accel_target.z = _accel_desired.z;
|
||||
_accel_target.z = constrain_float(get_z_accel_cmss(), -_accel_max_z_cmss, _accel_max_z_cmss);
|
||||
_accel_desired.z = _accel_target.z - (_accel_offset.z + _accel_terrain);
|
||||
_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
|
||||
// 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
|
||||
|
|
@ -831,7 +857,7 @@ void AC_PosControl::input_accel_z(float accel)
|
|||
float jerk_max_z_cmsss = _jerk_max_z_cmsss * calculate_overspeed_gain();
|
||||
|
||||
// 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);
|
||||
}
|
||||
|
|
@ -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;
|
||||
|
||||
// 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,
|
||||
_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
|
||||
void AC_PosControl::set_pos_target_z_from_climb_rate_cm(float vel)
|
||||
{
|
||||
// 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;
|
||||
|
||||
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;
|
||||
input_vel_accel_z(vel, 0.0);
|
||||
}
|
||||
|
||||
/// 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;
|
||||
|
||||
// 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,
|
||||
_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,
|
||||
-constrain_float(accel_max_z_cmss, 0.0f, 750.0f), accel_max_z_cmss,
|
||||
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);
|
||||
}
|
||||
|
||||
/// update_pos_offset_z - updates the vertical offsets used by terrain following
|
||||
void AC_PosControl::update_pos_offset_z(float pos_offset_z)
|
||||
/// update_offsets_z - updates the vertical offsets used by terrain following
|
||||
void AC_PosControl::update_offsets_z()
|
||||
{
|
||||
postype_t p_offset_z = _pos_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;
|
||||
// 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;
|
||||
}
|
||||
|
||||
// input shape the terrain offset
|
||||
shape_pos_vel_accel(pos_offset_z, 0.0f, 0.0f,
|
||||
_pos_offset_z, _vel_offset_z, _accel_offset_z,
|
||||
// update position, velocity, accel offsets for this iteration
|
||||
postype_t p_offset_z = _pos_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_accel_z_cmss(), get_max_accel_z_cmss(),
|
||||
_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
|
||||
|
|
@ -968,6 +993,11 @@ void AC_PosControl::update_z_controller()
|
|||
}
|
||||
_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
|
||||
float pos_target_zf = _pos_target.z;
|
||||
|
||||
|
|
@ -975,9 +1005,10 @@ void AC_PosControl::update_z_controller()
|
|||
_vel_target.z *= AP::ahrs().getControlScaleZ();
|
||||
|
||||
_pos_target.z = pos_target_zf;
|
||||
_pos_desired.z = _pos_target.z - (_pos_offset.z + _pos_terrain);
|
||||
|
||||
// add feed forward component
|
||||
_vel_target.z += _vel_desired.z;
|
||||
_vel_target.z += _vel_desired.z + _vel_offset.z + _vel_terrain;
|
||||
|
||||
// Velocity Controller
|
||||
|
||||
|
|
@ -986,7 +1017,7 @@ void AC_PosControl::update_z_controller()
|
|||
_accel_target.z *= AP::ahrs().getControlScaleZ();
|
||||
|
||||
// add feed forward component
|
||||
_accel_target.z += _accel_desired.z;
|
||||
_accel_target.z += _accel_desired.z + _accel_offset.z + _accel_terrain;
|
||||
|
||||
// Acceleration Controller
|
||||
|
||||
|
|
@ -1045,18 +1076,18 @@ float AC_PosControl::get_lean_angle_max_cd() const
|
|||
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)
|
||||
{
|
||||
_pos_target = pos;
|
||||
_pos_desired = pos;
|
||||
_vel_desired = vel;
|
||||
_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)
|
||||
{
|
||||
_pos_target.xy() = pos;
|
||||
_pos_desired.xy() = pos;
|
||||
_vel_desired.xy() = vel;
|
||||
_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
|
||||
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
|
||||
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();
|
||||
float kP = _p_pos_xy.kP();
|
||||
stopping_point -= _pos_offset.xy();
|
||||
|
||||
Vector2f curr_vel = _inav.get_velocity_xy_cms();
|
||||
curr_vel -= _vel_offset.xy();
|
||||
|
||||
// calculate current velocity
|
||||
float vel_total = curr_vel.length();
|
||||
|
|
@ -1102,7 +1265,8 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
|
|||
if (!is_positive(vel_total)) {
|
||||
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);
|
||||
if (!is_positive(stopping_dist)) {
|
||||
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
|
||||
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
|
||||
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;
|
||||
}
|
||||
|
||||
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
|
||||
|
|
@ -1176,18 +1344,32 @@ void AC_PosControl::write_log()
|
|||
if (is_active_xy()) {
|
||||
float 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,
|
||||
get_vel_desired_cms().x, get_vel_target_cms().x, _inav.get_velocity_neu_cms().x,
|
||||
_accel_desired.x, get_accel_target_cmss().x, accel_x);
|
||||
Write_PSCE(get_pos_target_cm().y, _inav.get_position_neu_cm().y,
|
||||
get_vel_desired_cms().y, get_vel_target_cms().y, _inav.get_velocity_neu_cms().y,
|
||||
_accel_desired.y, get_accel_target_cmss().y, accel_y);
|
||||
Write_PSCN(_pos_desired.x, _pos_target.x, _inav.get_position_neu_cm().x,
|
||||
_vel_desired.x, _vel_target.x, _inav.get_velocity_neu_cms().x,
|
||||
_accel_desired.x, _accel_target.x, accel_x);
|
||||
Write_PSCE(_pos_desired.y, _pos_target.y, _inav.get_position_neu_cm().y,
|
||||
_vel_desired.y, _vel_target.y, _inav.get_velocity_neu_cms().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()) {
|
||||
Write_PSCD(-get_pos_target_cm().z, -_inav.get_position_z_up_cm(),
|
||||
-get_vel_desired_cms().z, -get_vel_target_cms().z, -_inav.get_velocity_z_up_cms(),
|
||||
-_accel_desired.z, -get_accel_target_cmss().z, -get_z_accel_cmss());
|
||||
Write_PSCD(-_pos_desired.z, -_pos_target.z, -_inav.get_position_z_up_cm(),
|
||||
-_vel_desired.z, -_vel_target.z, -_inav.get_velocity_z_up_cms(),
|
||||
-_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
|
||||
|
|
@ -1213,6 +1395,27 @@ float AC_PosControl::crosstrack_error() const
|
|||
/// 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
|
||||
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);
|
||||
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_desired.xy() = _pos_target.xy() - _pos_offset.xy();
|
||||
_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;
|
||||
}
|
||||
|
|
@ -1316,8 +1525,14 @@ void AC_PosControl::handle_ekf_z_reset()
|
|||
uint32_t reset_ms = _ahrs.getLastPosDownReset(alt_shift);
|
||||
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_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_desired.z = _vel_target.z - (_vel_offset.z + _vel_terrain);
|
||||
|
||||
_ekf_z_reset_ms = reset_ms;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -11,6 +11,7 @@
|
|||
#include <AC_PID/AC_PID_Basic.h> // PID library (1-axis)
|
||||
#include <AC_PID/AC_PID_2D.h> // PID library (2-axis)
|
||||
#include <AP_InertialNav/AP_InertialNav.h> // Inertial Navigation library
|
||||
#include <AP_Scripting/AP_Scripting_config.h>
|
||||
#include "AC_AttitudeControl.h" // Attitude control library
|
||||
|
||||
#include <AP_Logger/LogStructure.h>
|
||||
|
|
@ -52,7 +53,7 @@ public:
|
|||
float get_dt() const { return _dt; }
|
||||
|
||||
/// get_shaping_jerk_xy_cmsss - gets the jerk limit of the xy kinematic path generation in cm/s/s/s
|
||||
float get_shaping_jerk_xy_cmsss() const { return _shaping_jerk_xy*100.0; }
|
||||
float get_shaping_jerk_xy_cmsss() const { return _shaping_jerk_xy * 100.0; }
|
||||
|
||||
|
||||
///
|
||||
|
|
@ -62,7 +63,7 @@ public:
|
|||
/// 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 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
|
||||
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.
|
||||
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
|
||||
bool is_active_z() const;
|
||||
|
||||
|
|
@ -250,32 +248,44 @@ public:
|
|||
|
||||
/// 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
|
||||
const Vector3p& get_pos_target_cm() const { return _pos_target; }
|
||||
|
||||
/// set_pos_target_z_cm - set altitude target in cm above the EKF origin
|
||||
void set_pos_target_z_cm(float pos_target) { _pos_target.z = pos_target; }
|
||||
/// set_pos_desired_xy_cm - sets the position target, frame NEU in cm relative to the EKF origin
|
||||
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)
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
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
|
||||
|
|
@ -286,7 +296,7 @@ public:
|
|||
/// 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; }
|
||||
|
||||
/// 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; }
|
||||
|
||||
// get_vel_target_cms - returns the target velocity in NEU cm/s
|
||||
|
|
@ -301,30 +311,56 @@ public:
|
|||
|
||||
/// 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; }
|
||||
|
||||
// get_accel_target_cmss - returns the target acceleration in NEU cm/s/s
|
||||
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
|
||||
|
||||
/// set_pos_offset_target_z_cm - set altitude offset target in cm above the EKF origin
|
||||
void set_pos_offset_target_z_cm(float pos_offset_target_z) { _pos_offset_target_z = pos_offset_target_z; }
|
||||
#if AP_SCRIPTING_ENABLED
|
||||
// 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
|
||||
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
|
||||
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
|
||||
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
|
||||
float get_accel_offset_z_cmss() const { return _accel_offset_z; }
|
||||
|
||||
float get_accel_offset_z_cmss() const { return _accel_offset.z; }
|
||||
|
||||
/// Outputs
|
||||
|
||||
|
|
@ -407,9 +443,13 @@ public:
|
|||
|
||||
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_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_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_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_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_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:
|
||||
|
||||
|
|
@ -428,6 +468,32 @@ protected:
|
|||
// calculate_overspeed_gain - calculated increased maximum acceleration and jerk if over speed condition is detected
|
||||
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
|
||||
void init_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
|
||||
|
||||
// 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_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)
|
||||
|
|
@ -481,10 +548,21 @@ protected:
|
|||
|
||||
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
|
||||
float _pos_offset_z; // vertical position offset, frame NEU in cm relative to the EKF origin
|
||||
float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step
|
||||
float _accel_offset_z; // vertical acceleration offset in NEU cm/s/s
|
||||
// terrain handling variables
|
||||
float _pos_terrain_target; // position terrain target in cm relative to the EKF origin in NEU frame
|
||||
float _pos_terrain; // position terrain in cm from the EKF origin in NEU frame. this terrain moves towards _pos_terrain_target
|
||||
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
|
||||
uint32_t _ekf_xy_reset_ms; // system time of last recorded ekf xy position reset
|
||||
|
|
@ -502,6 +580,11 @@ protected:
|
|||
private:
|
||||
// convenience method for writing out the identical PSCE, PSCN, PSCD - and
|
||||
// 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);
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -8,11 +8,12 @@
|
|||
#include "LogStructure.h"
|
||||
|
||||
// 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{
|
||||
LOG_PACKET_HEADER_INIT(id),
|
||||
time_us : AP_HAL::micros64(),
|
||||
pos_desired : pos_desired * 0.01f,
|
||||
pos_target : pos_target * 0.01f,
|
||||
pos : pos * 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));
|
||||
}
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -6,11 +6,16 @@
|
|||
LOG_PSCN_MSG, \
|
||||
LOG_PSCE_MSG, \
|
||||
LOG_PSCD_MSG, \
|
||||
LOG_PSON_MSG, \
|
||||
LOG_PSOE_MSG, \
|
||||
LOG_PSOD_MSG, \
|
||||
LOG_PSOT_MSG, \
|
||||
LOG_ANG_MSG
|
||||
|
||||
// @LoggerMessage: PSCN
|
||||
// @Description: Position Control North
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: DPN: Desired position relative to EKF origin
|
||||
// @Field: TPN: Target position relative to EKF origin
|
||||
// @Field: PN: Position relative to EKF origin
|
||||
// @Field: DVN: Desired velocity North
|
||||
|
|
@ -23,6 +28,7 @@
|
|||
// @LoggerMessage: PSCE
|
||||
// @Description: Position Control East
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: DPE: Desired position relative to EKF origin + Offsets
|
||||
// @Field: TPE: Target position relative to EKF origin
|
||||
// @Field: PE: Position relative to EKF origin
|
||||
// @Field: DVE: Desired velocity East
|
||||
|
|
@ -35,6 +41,7 @@
|
|||
// @LoggerMessage: PSCD
|
||||
// @Description: Position Control Down
|
||||
// @Field: TimeUS: Time since system startup
|
||||
// @Field: DPD: Desired position relative to EKF origin + Offsets
|
||||
// @Field: TPD: Target position relative to EKF origin
|
||||
// @Field: PD: Position relative to EKF origin
|
||||
// @Field: DVD: Desired velocity Down
|
||||
|
|
@ -49,6 +56,7 @@
|
|||
struct PACKED log_PSCx {
|
||||
LOG_PACKET_HEADER;
|
||||
uint64_t time_us;
|
||||
float pos_desired;
|
||||
float pos_target;
|
||||
float pos;
|
||||
float vel_desired;
|
||||
|
|
@ -59,6 +67,58 @@ struct PACKED log_PSCx {
|
|||
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
|
||||
// @Description: Desired and achieved vehicle attitude rates. Not logged in Fixed Wing Plane modes.
|
||||
// @Field: TimeUS: Time since system startup
|
||||
|
|
@ -115,17 +175,29 @@ struct PACKED log_ANG {
|
|||
float sensor_dt;
|
||||
};
|
||||
|
||||
#define PSCx_FMT "Qffffffff"
|
||||
#define PSCx_UNITS "smmnnnooo"
|
||||
#define PSCx_MULTS "F00000000"
|
||||
#define PSCx_FMT "Qfffffffff"
|
||||
#define PSCx_UNITS "smmmnnnooo"
|
||||
#define PSCx_MULTS "F000000000"
|
||||
|
||||
#define PSOx_FMT "Qffffff"
|
||||
#define PSOx_UNITS "smmnnoo"
|
||||
#define PSOx_MULTS "F000000"
|
||||
|
||||
#define LOG_STRUCTURE_FROM_AC_ATTITUDECONTROL \
|
||||
{ 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), \
|
||||
"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), \
|
||||
"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), \
|
||||
"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),\
|
||||
|
|
|
|||
Loading…
Reference in New Issue