78 lines
2.0 KiB
C
78 lines
2.0 KiB
C
// CONSTANTS:
|
|
#define MAX_CLIMB_SUM_ERR 10
|
|
#define MAX_CLIMB 1
|
|
|
|
#define CLOCK 16
|
|
#define MAX_PPRZ (CLOCK*600)
|
|
|
|
#define CLIMB_LEVEL_GAZ 0.31
|
|
#define CLIMB_GAZ_OF_CLIMB 0.75
|
|
#define CLIMB_PITCH_OF_VZ_PGAIN 0.05
|
|
#define CLIMB_PGAIN -0.03
|
|
#define CLIMB_IGAIN 0.1
|
|
|
|
const float pitch_of_vz_pgain=CLIMB_PITCH_OF_VZ_PGAIN;
|
|
const float climb_pgain=CLIMB_PGAIN;
|
|
const float climb_igain=CLIMB_IGAIN;
|
|
const float nav_pitch=0;
|
|
|
|
/** PID function INPUTS */
|
|
// The user input: target speed in vertical direction
|
|
float desired_climb;
|
|
// Vertical speed of the UAV detected by GPS sensor
|
|
float estimator_z_dot;
|
|
|
|
/** PID function OUTPUTS */
|
|
float desired_gaz;
|
|
float desired_pitch;
|
|
|
|
/** The state variable: accumulated error in the control */
|
|
float climb_sum_err=0;
|
|
|
|
/** Computes desired_gaz and desired_pitch */
|
|
void climb_pid_run()
|
|
{
|
|
|
|
float err=estimator_z_dot-desired_climb;
|
|
|
|
float fgaz=climb_pgain*(err+climb_igain*climb_sum_err)+CLIMB_LEVEL_GAZ+CLIMB_GAZ_OF_CLIMB*desired_climb;
|
|
|
|
float pprz=fgaz*MAX_PPRZ;
|
|
desired_gaz=((pprz>=0 && pprz<=MAX_PPRZ) ? pprz : (pprz>MAX_PPRZ ? MAX_PPRZ : 0));
|
|
|
|
/** pitch offset for climb */
|
|
float pitch_of_vz=(desired_climb>0) ? desired_climb*pitch_of_vz_pgain : 0;
|
|
desired_pitch=nav_pitch+pitch_of_vz;
|
|
|
|
climb_sum_err=err+climb_sum_err;
|
|
if (climb_sum_err>MAX_CLIMB_SUM_ERR) climb_sum_err=MAX_CLIMB_SUM_ERR;
|
|
if (climb_sum_err<-MAX_CLIMB_SUM_ERR) climb_sum_err=-MAX_CLIMB_SUM_ERR;
|
|
|
|
}
|
|
|
|
int main()
|
|
{
|
|
|
|
while(1)
|
|
{
|
|
/** Non-deterministic input values */
|
|
desired_climb=nondet_float();
|
|
estimator_z_dot=nondet_float();
|
|
|
|
/** Range of input values */
|
|
__CPROVER_assume(desired_climb>=-MAX_CLIMB && desired_climb<=MAX_CLIMB);
|
|
__CPROVER_assume(estimator_z_dot>=-MAX_CLIMB && estimator_z_dot<=MAX_CLIMB);
|
|
|
|
__CPROVER_input("desired_climb", desired_climb);
|
|
__CPROVER_input("estimator_z_dot", estimator_z_dot);
|
|
|
|
climb_pid_run();
|
|
|
|
__CPROVER_output("desired_gaz", desired_gaz);
|
|
__CPROVER_output("desired_pitch", desired_pitch);
|
|
|
|
}
|
|
|
|
return 0;
|
|
}
|