2017-02-07 20:49:55 -04:00
|
|
|
#pragma once
|
|
|
|
|
|
|
|
#include "AC_PosControl.h"
|
|
|
|
|
2021-04-16 12:53:14 -03:00
|
|
|
#define POSCONTROL_JERK_RATIO 1.0f // Defines the time it takes to reach the requested acceleration
|
|
|
|
|
2017-02-07 20:49:55 -04:00
|
|
|
class AC_PosControl_Sub : public AC_PosControl {
|
|
|
|
public:
|
2019-12-10 05:31:08 -04:00
|
|
|
AC_PosControl_Sub(AP_AHRS_View & ahrs, const AP_InertialNav& inav,
|
2021-04-16 12:53:14 -03:00
|
|
|
const AP_Motors& motors, AC_AttitudeControl& attitude_control, float dt);
|
2017-02-07 20:49:55 -04:00
|
|
|
|
|
|
|
/// set_alt_max - sets maximum altitude above the ekf origin in cm
|
2021-04-16 12:53:14 -03:00
|
|
|
/// only enforced when set_pos_target_z_from_climb_rate_cm is used
|
2017-02-07 20:49:55 -04:00
|
|
|
/// 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
|
2021-04-16 12:53:14 -03:00
|
|
|
/// only enforced when set_pos_target_z_from_climb_rate_cm is used
|
2017-02-07 20:49:55 -04:00
|
|
|
/// set to zero to disable limit
|
|
|
|
void set_alt_min(float alt) { _alt_min = alt; }
|
|
|
|
|
|
|
|
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
|
|
|
|
};
|