mirror of https://github.com/ArduPilot/ardupilot
38 lines
2.0 KiB
C++
38 lines
2.0 KiB
C++
#pragma once
|
|
|
|
#include "AC_PosControl.h"
|
|
|
|
#define POSCONTROL_JERK_RATIO 1.0f // Defines the time it takes to reach the requested acceleration
|
|
|
|
class AC_PosControl_Sub : public AC_PosControl {
|
|
public:
|
|
AC_PosControl_Sub(AP_AHRS_View & ahrs, const AP_InertialNav& inav,
|
|
const AP_Motors& motors, AC_AttitudeControl& attitude_control, float dt);
|
|
|
|
/// set_alt_max - sets maximum altitude above the ekf origin in cm
|
|
/// only enforced when set_pos_target_z_from_climb_rate_cm is used
|
|
/// set to zero to disable limit
|
|
void set_alt_max(float alt) { _alt_max = alt; }
|
|
|
|
/// set_alt_min - sets the minimum altitude (maximum depth) in cm
|
|
/// only enforced when set_pos_target_z_from_climb_rate_cm is used
|
|
/// set to zero to disable limit
|
|
void set_alt_min(float alt) { _alt_min = alt; }
|
|
|
|
/// input_vel_accel_z - calculate a jerk limited path from the current position, velocity and acceleration to an input velocity.
|
|
/// 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 :
|
|
/// maximum velocity - vel_max,
|
|
/// maximum acceleration - accel_max,
|
|
/// time constant - tc.
|
|
/// The time constant defines the acceleration error decay in the kinematic path as the system approaches constant acceleration.
|
|
/// The time constant also defines the time taken to achieve the maximum acceleration.
|
|
/// The time constant must be positive.
|
|
/// The function alters the input velocity to be the velocity that the system could reach zero acceleration in the minimum time.
|
|
void input_vel_accel_z(Vector3f& vel, const Vector3f& accel, bool force_descend) override;
|
|
|
|
private:
|
|
float _alt_max; // max altitude - should be updated from the main code with altitude limit from fence
|
|
float _alt_min; // min altitude - should be updated from the main code with altitude limit from fence
|
|
};
|