inline static void h_ctl_pitch_loop( void ) {
static float last_err;
struct FloatEulers* att = stateGetNedToBodyEulers_f();
/* sanity check */
if (h_ctl_elevator_of_roll <0.)
h_ctl_elevator_of_roll = 0.;
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_elevator_of_roll / h_ctl_pitch_pgain * fabs(att->phi);
float err = 0;
#ifdef USE_AOA
switch (h_ctl_pitch_mode){
case H_CTL_PITCH_MODE_THETA:
err = att->theta - h_ctl_pitch_loop_setpoint;
break;
case H_CTL_PITCH_MODE_AOA:
err = (*stateGetAngleOfAttack_f()) - h_ctl_pitch_loop_setpoint;
break;
default:
err = att->theta - h_ctl_pitch_loop_setpoint;
break;
}
#else //NO_AOA
err = att->theta - h_ctl_pitch_loop_setpoint;
#endif
float d_err = err - last_err;
last_err = err;
float cmd = -h_ctl_pitch_pgain * (err + h_ctl_pitch_dgain * d_err);
#ifdef LOITER_TRIM
cmd += loiter();
#endif
h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}
static void send_att(struct transport_tx *trans, struct link_device *dev)
{
struct FloatRates *body_rate = stateGetBodyRates_f();
struct FloatEulers *att = stateGetNedToBodyEulers_f();
pprz_msg_send_STAB_ATTITUDE_FLOAT(trans, dev, AC_ID,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err_quat.qx,
&stabilization_att_sum_err_quat.qy,
&stabilization_att_sum_err_quat.qz,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW],
&body_rate_d.p, &body_rate_d.q, &body_rate_d.r);
}
static void send_att(void) {
struct FloatRates* body_rate = stateGetBodyRates_f();
struct FloatEulers* att = stateGetNedToBodyEulers_f();
float foo = 0.0;
DOWNLINK_SEND_STAB_ATTITUDE_FLOAT(DefaultChannel, DefaultDevice,
&(body_rate->p), &(body_rate->q), &(body_rate->r),
&(att->phi), &(att->theta), &(att->psi),
&stab_att_sp_euler.phi,
&stab_att_sp_euler.theta,
&stab_att_sp_euler.psi,
&stabilization_att_sum_err.phi,
&stabilization_att_sum_err.theta,
&stabilization_att_sum_err.psi,
&stabilization_att_fb_cmd[COMMAND_ROLL],
&stabilization_att_fb_cmd[COMMAND_PITCH],
&stabilization_att_fb_cmd[COMMAND_YAW],
&stabilization_att_ff_cmd[COMMAND_ROLL],
&stabilization_att_ff_cmd[COMMAND_PITCH],
&stabilization_att_ff_cmd[COMMAND_YAW],
&stabilization_cmd[COMMAND_ROLL],
&stabilization_cmd[COMMAND_PITCH],
&stabilization_cmd[COMMAND_YAW],
&foo, &foo, &foo);
}
inline static void h_ctl_roll_loop( void ) {
static float cmd_fb = 0.;
#if USE_ANGLE_REF
// Update reference setpoints for roll
h_ctl_ref.roll_angle += h_ctl_ref.roll_rate * H_CTL_REF_DT;
h_ctl_ref.roll_rate += h_ctl_ref.roll_accel * H_CTL_REF_DT;
h_ctl_ref.roll_accel = H_CTL_REF_W_P*H_CTL_REF_W_P * (h_ctl_roll_setpoint - h_ctl_ref.roll_angle) - 2*H_CTL_REF_XI_P*H_CTL_REF_W_P * h_ctl_ref.roll_rate;
// Saturation on references
BoundAbs(h_ctl_ref.roll_accel, h_ctl_ref.max_p_dot);
if (h_ctl_ref.roll_rate > h_ctl_ref.max_p) {
h_ctl_ref.roll_rate = h_ctl_ref.max_p;
if (h_ctl_ref.roll_accel > 0.) h_ctl_ref.roll_accel = 0.;
}
else if (h_ctl_ref.roll_rate < - h_ctl_ref.max_p) {
h_ctl_ref.roll_rate = - h_ctl_ref.max_p;
if (h_ctl_ref.roll_accel < 0.) h_ctl_ref.roll_accel = 0.;
}
#else
h_ctl_ref.roll_angle = h_ctl_roll_setpoint;
h_ctl_ref.roll_rate = 0.;
h_ctl_ref.roll_accel = 0.;
#endif
#ifdef USE_KFF_UPDATE
// update Kff gains
h_ctl_roll_Kffa += KFFA_UPDATE * h_ctl_ref.roll_accel * cmd_fb / (h_ctl_ref.max_p_dot*h_ctl_ref.max_p_dot);
h_ctl_roll_Kffd += KFFD_UPDATE * h_ctl_ref.roll_rate * cmd_fb / (h_ctl_ref.max_p*h_ctl_ref.max_p);
#ifdef SITL
printf("%f %f %f\n", h_ctl_roll_Kffa, h_ctl_roll_Kffd, cmd_fb);
#endif
h_ctl_roll_Kffa = Max(h_ctl_roll_Kffa, 0);
h_ctl_roll_Kffd = Max(h_ctl_roll_Kffd, 0);
#endif
// Compute errors
float err = h_ctl_ref.roll_angle - stateGetNedToBodyEulers_f()->phi;
struct FloatRates* body_rate = stateGetBodyRates_f();
float d_err = h_ctl_ref.roll_rate - body_rate->p;
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_roll_sum_err = 0.;
}
else {
if (h_ctl_roll_igain > 0.) {
h_ctl_roll_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_roll_sum_err, H_CTL_ROLL_SUM_ERR_MAX / h_ctl_roll_igain);
} else {
h_ctl_roll_sum_err = 0.;
}
}
cmd_fb = h_ctl_roll_attitude_gain * err;
float cmd = - h_ctl_roll_Kffa * h_ctl_ref.roll_accel
- h_ctl_roll_Kffd * h_ctl_ref.roll_rate
- cmd_fb
- h_ctl_roll_rate_gain * d_err
- h_ctl_roll_igain * h_ctl_roll_sum_err
+ v_ctl_throttle_setpoint * h_ctl_aileron_of_throttle;
cmd /= airspeed_ratio2;
// Set aileron commands
h_ctl_aileron_setpoint = TRIM_PPRZ(cmd);
}
/**
* Poll lidar for data
* for altitude hold 50Hz-100Hz should be enough,
* in theory can go faster (see the datasheet for Lidar Lite v3
*/
void lidar_lite_periodic(void)
{
switch (lidar_lite.status) {
case LIDAR_LITE_INIT_RANGING:
if (lidar_lite.trans.status == I2CTransDone) {
// ask for i2c frame
lidar_lite.trans.buf[0] = LIDAR_LITE_REG_ADDR; // sets register pointer to (0x00)
lidar_lite.trans.buf[1] = LIDAR_LITE_REG_VAL; // take acquisition & correlation processing with DC correction
if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 2)){
// transaction OK, increment status
lidar_lite.status = LIDAR_LITE_REQ_READ;
}
}
break;
case LIDAR_LITE_REQ_READ:
if (lidar_lite.trans.status == I2CTransDone) {
lidar_lite.trans.buf[0] = LIDAR_LITE_READ_ADDR; // sets register pointer to results register
if (i2c_transmit(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 1)){
// transaction OK, increment status
lidar_lite.status = LIDAR_LITE_READ_DISTANCE;
}
}
break;
case LIDAR_LITE_READ_DISTANCE:
if (lidar_lite.trans.status == I2CTransDone) {
// clear buffer
lidar_lite.trans.buf[0] = 0;
lidar_lite.trans.buf[1] = 0;
if (i2c_receive(&LIDAR_LITE_I2C_DEV, &lidar_lite.trans, lidar_lite.addr, 2)){
// transaction OK, increment status
lidar_lite.status = LIDAR_LITE_PARSE;
}
}
break;
case LIDAR_LITE_PARSE: {
// filter data
uint32_t now_ts = get_sys_time_usec();
lidar_lite.distance_raw = update_median_filter_i(
&lidar_lite_filter,
(uint32_t)((lidar_lite.trans.buf[0] << 8) | lidar_lite.trans.buf[1]));
lidar_lite.distance = ((float)lidar_lite.distance_raw)/100.0;
// compensate AGL measurement for body rotation
if (lidar_lite.compensate_rotation) {
float phi = stateGetNedToBodyEulers_f()->phi;
float theta = stateGetNedToBodyEulers_f()->theta;
float gain = (float)fabs( (double) (cosf(phi) * cosf(theta)));
lidar_lite.distance = lidar_lite.distance / gain;
}
// send message (if requested)
if (lidar_lite.update_agl) {
AbiSendMsgAGL(AGL_LIDAR_LITE_ID, now_ts, lidar_lite.distance);
}
// increment status
lidar_lite.status = LIDAR_LITE_INIT_RANGING;
break;
}
default:
break;
}
}
static void send_attitude(struct transport_tx *trans, struct link_device *dev)
{
struct FloatEulers *att = stateGetNedToBodyEulers_f();
pprz_msg_send_ATTITUDE(trans, dev, AC_ID, &(att->phi), &(att->psi), &(att->theta));
};
void CN_vector_escape_velocity(void)
{
/////VECTOR METHOD VARIABLES//////
//Constants
//Parameters for Butterworth filter
float A_butter = -0.8541;//-0.7265;
float B_butter[2] = {0.0730 , 0.0730 };//{0.1367, 0.1367};
int8_t disp_count = 0;
//Initalize
//float fp_angle;
//float total_vel;
float Ca;
float Cv;
float angle_ver = 0;
float angle_hor = 0;
Repulsionforce_Kan.x = 0;
Repulsionforce_Kan.y = 0;
Repulsionforce_Kan.z = 0;
//struct FloatRMat T;
//struct FloatEulers current_attitude = {0,0,0};
struct FloatVect3 Total_Kan = {0, 0, 0};
//Tuning variables
//float force_max = 200;
/////ESCAPE METHOD VARIABLES//////
//Constants
int8_t min_disparity = 45;
int8_t number_of_buffer = 20;
float bias = 2 * M_PI;
vref_max = 0.1; //CHECK!
//init
float available_heading[size_matrix[0] * size_matrix[2]];
float diff_available_heading[size_matrix[0] * size_matrix[2]];
float new_heading_diff = 1000;
float new_heading = 1000;
int8_t i = 0;
int8_t i_buffer = 0;
float Distance_est;
float V_total = 0;
///////////////////////////////////
heading_goal_ref = heading_goal_f - stateGetNedToBodyEulers_f()->psi;
//FLOAT_ANGLE_NORMALIZE(heading_goal_ref);
//Calculate Attractforce_goal
Attractforce_goal.x = cos(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
Attractforce_goal.y = sin(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
Attractforce_goal.z = 0;
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
/ 2; //Check if bodyframe is correct with current_heading correction
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
angle_ver = 0.5 * stereo_fow[1] + stereo_fow[1] / size_matrix[1] - stereo_fow[1] / size_matrix[1] / 2;
for (int i2 = 0; i2 < 4; i2++) {
angle_ver = angle_ver - stereo_fow[1] / size_matrix[1];
Ca = cos((angle_hor - heading_goal_ref) * Cfreq) * erf(1 * sqrt(VECT2_NORM2(pos_diff)));
if (Ca < 0) {
Ca = 0;
}
//TODO make dependent on speed: total_vel/vref_max
Cv = F1 + F2 * Ca;
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
Distance_est = (baseline * (float)focal / ((float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
size_matrix[2] + i3]) - 18.0) / 1000;
disp_count++;
Repulsionforce_Kan.x = Repulsionforce_Kan.x - pow(Cv / (Distance_est + Dist_offset),
2) * cos(angle_hor) * cos(angle_ver);
Repulsionforce_Kan.y = Repulsionforce_Kan.y - pow(Cv / (Distance_est + Dist_offset),
2) * sin(angle_hor) * cos(angle_ver);
Repulsionforce_Kan.z = Repulsionforce_Kan.z - pow(Cv / (Distance_est + Dist_offset), 2) * sin(angle_ver);
}
}
}
}
//Normalize for ammount entries in Matrix
VECT3_SMUL(Repulsionforce_Kan, Repulsionforce_Kan, 1.0 / (float)disp_count);
if (repulsionforce_filter_flag == 1) {
Repulsionforce_Used.x = B_butter[0] * Repulsionforce_Kan.x + B_butter[1] * Repulsionforce_Kan_old.x - A_butter *
filter_repforce_old.x;
Repulsionforce_Used.y = B_butter[0] * Repulsionforce_Kan.y + B_butter[1] * Repulsionforce_Kan_old.y - A_butter *
filter_repforce_old.y;
Repulsionforce_Used.z = B_butter[0] * Repulsionforce_Kan.z + B_butter[1] * Repulsionforce_Kan_old.z - A_butter *
filter_repforce_old.z;
VECT3_COPY(Repulsionforce_Kan_old, Repulsionforce_Kan);
VECT3_COPY(filter_repforce_old, Repulsionforce_Used);
}
else {
VECT3_COPY(Repulsionforce_Used, Repulsionforce_Kan);
}
VECT3_SMUL(Repulsionforce_Used, Repulsionforce_Used, Ko);
VECT3_SMUL(Attractforce_goal, Attractforce_goal, Kg);
//Total force
VECT3_ADD(Total_Kan, Repulsionforce_Used);
VECT3_ADD(Total_Kan, Attractforce_goal);
//set variable for stabilization_opticflow
printf("RepulsionforceX: %f AttractX: %f \n", Repulsionforce_Used.x, Attractforce_goal.x);
printf("RepulsionforceY: %f AttractY: %f \n", Repulsionforce_Used.y, Attractforce_goal.y);
//set write values for logger
//Attractforce_goal_send.x = Attractforce_goal.x;
//Attractforce_goal_send.y = Attractforce_goal.y;
//Repulsionforce_Kan_send.x = Repulsionforce_Used.x;
//Repulsionforce_Kan_send.y = Repulsionforce_Used.y;
V_total = sqrt(pow(Total_Kan.x, 2) + pow(Total_Kan.y, 2));
if (V_total < v_min && sqrt(VECT2_NORM2(pos_diff)) > 1) {
escape_flag = 1;
}
else if (V_total > v_max && obstacle_flag == 0) {
escape_flag = 0;
}
printf("V_total: %f", V_total);
printf("escape_flag: %i, obstacle_flag: %i, set_bias: %i", escape_flag, obstacle_flag, set_bias);
printf("escape_flag: %i, obstacle_flag: %i, set_bias: %i", escape_flag, obstacle_flag, set_bias);
if (escape_flag == 0) {
ref_pitch = Total_Kan.x;
ref_roll = Total_Kan.y;
} else if (escape_flag == 1) {
heading_goal_ref = heading_goal_f - stateGetNedToBodyEulers_f()->psi;
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2];
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
available_heading[i] = angle_hor;
diff_available_heading[i] = heading_goal_ref - available_heading[i];
FLOAT_ANGLE_NORMALIZE(diff_available_heading[i]);
i++;
}
}
i = 0;
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
for (int i2 = 0; i2 < 4; i2++) {
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
//distance_est = (baseline*(float)focal/((float)READimageBuffer_offset[i1*size_matrix[1]+i2*size_matrix[0]*size_matrix[2] + i3])-18.0)/1000;
diff_available_heading[i] = 100;
for (int i_diff = -number_of_buffer; i_diff <= number_of_buffer + 1; i_diff++) {
i_buffer = i + i_diff;
if (i_buffer < 1) {
i_buffer = i_buffer + size_matrix[0] * size_matrix[2];
}
if (i_buffer > size_matrix[0]*size_matrix[2]) {
i_buffer = i_buffer - size_matrix[0] * size_matrix[2];
}
diff_available_heading[i_buffer] = 100;
}
}
}
i++;
}
}
//set bias
if (set_bias == 1) {
for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
if (diff_available_heading[i100] <= 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
diff_available_heading[i100] = diff_available_heading[i100] - bias;
}
}
} else if (set_bias == 2) {
for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
if (diff_available_heading[i100] > 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
diff_available_heading[i100] = diff_available_heading[i100] + bias;
}
}
}
for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
if (fabs(diff_available_heading[i100]) < fabs(new_heading_diff)) {
new_heading_diff = diff_available_heading[i100];
new_heading = available_heading[i100];
}
}
if (obstacle_flag == 0 && fabs(new_heading_diff) > (2 * M_PI / 36.0)) {
obstacle_flag = 1;
if (new_heading_diff > 0) {
set_bias = 1;
direction = -1;
} else if (new_heading_diff <= 0) {
set_bias = 2;
direction = 1;
}
}
if (fabs(new_heading_diff) <= (2 * M_PI / 36.0)) {
if (waypoint_rot == 0) {
obstacle_flag = 0;
set_bias = 0;
} else {
waypoint_rot = waypoint_rot - direction * 0.05 * M_PI;
}
}
if (fabs(new_heading_diff) >= 0.5 * M_PI) {
waypoint_rot = waypoint_rot + direction * 0.25 * M_PI;
}
//vref_max should be low
speed_pot = vref_max * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
if (speed_pot <= 0) {
speed_pot = 0;
}
new_heading_old = new_heading;
#if PRINT_STUFF
for (int i100 = 0; i100 < (size_matrix[0] * size_matrix[2]); i100++) {
printf("%i diff_available_heading: %f available_heading: %f \n", i100, diff_available_heading[i100],
available_heading[i100]);
}
printf("new_heading: %f current_heading: %f speed_pot: %f\n", new_heading, stateGetNedToBodyEulers_f()->psi,
speed_pot);
printf("set_bias: %i obstacle_flag: %i", set_bias, obstacle_flag);
#endif
ref_pitch = cos(new_heading) * speed_pot;
ref_roll = sin(new_heading) * speed_pot;
}
}
void CN_potential_velocity(void)
{
float OF_Result_Vy = 0;
float OF_Result_Vx = 0;
//Constants
float new_heading;
float alpha_fil = 0.2;
//Initialize
float potential_obst = 0; //define potential field variables
float potential_obst_integrated = 0;
float Distance_est;
float fp_angle;
float diff_angle;
float total_vel;
float current_heading;
float angle_hor = - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2] /
2; //calculate position angle of stereo
//Tune
//float dt = 0.5; //define delta t for integration! check response under 0.1 and 1
int8_t min_disparity = 40;
//Current state of system;
float r_old = stateGetBodyRates_f()->r;
current_heading = stateGetNedToBodyEulers_f()->psi; //check maybe cause of errors
//current_heading = 0;
total_vel = pow((OF_Result_Vy * OF_Result_Vy + OF_Result_Vx * OF_Result_Vx), 0.5);
if (total_vel > vmin) {
fp_angle = atan2(OF_Result_Vx, OF_Result_Vy);
} else {
fp_angle = 0;
}
heading_goal_ref = (fp_angle + current_heading) - heading_goal_f;
FLOAT_ANGLE_NORMALIZE(heading_goal_ref);
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
/ 2; //Check if bodyframe is correct with current_heading correction
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
FLOAT_ANGLE_NORMALIZE(angle_hor);
for (int i2 = 0; i2 < size_matrix[1]; i2++) {
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
Distance_est = ((baseline * (float)focal / (float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
size_matrix[2] + i3] - 18.0)) / 1000;
} else {
Distance_est = 2000;
}
diff_angle = fp_angle - angle_hor;
FLOAT_ANGLE_NORMALIZE(diff_angle);
potential_obst = potential_obst + K_obst * (diff_angle) * exp(-c3_oa * fabs(diff_angle)) * exp(
-c4_oa * Distance_est); //(tan(obst_width[i]+c5)-tan(c5));
potential_obst_integrated = potential_obst_integrated + K_obst * c3_oa * (fabs(diff_angle) + 1) / (c3_oa * c3_oa) * exp(
-c3_oa * fabs(diff_angle)) * exp(-c4_oa * Distance_est); // (tan(obst_width[i]+c5)-tan(c5));
}
}
}
//calculate angular accelaration from potential field
r_dot_new = -b_damp * r_old - K_goal * (heading_goal_ref) * (exp(-c1_oa * sqrt(VECT2_NORM2(
pos_diff))) + c2_oa) + potential_obst;
// Calculate velocity from potential
speed_pot = vref_max * exp(-kv * potential_obst_integrated) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff))) - epsilon;
if (speed_pot <= 0) {
speed_pot = 0;
}
//calculate new_ref_speeds
new_heading = fp_angle + alpha_fil * r_dot_new;
FLOAT_ANGLE_NORMALIZE(new_heading);
ref_pitch = new_heading;
}
void CN_escape_velocity(void)
{
//Constants
int8_t min_disparity = 45;
int8_t number_of_buffer = 20;
float bias = 2 * M_PI;
vref_max = 0.1; //CHECK!
//init variables
float angle_hor = 0;
float available_heading[size_matrix[0]*size_matrix[2]];
float diff_available_heading[size_matrix[0]*size_matrix[2]];
float new_heading_diff = 1000;
float new_heading = 1000;
int8_t i = 0;
int8_t i_buffer = 0;
float distance_est;
float distance_heading = 0;
//generate available_headings
heading_goal_ref = heading_goal_f - stateGetNedToBodyEulers_f()->psi;
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2];
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
available_heading[i] = angle_hor;
diff_available_heading[i] = heading_goal_ref - available_heading[i];
FLOAT_ANGLE_NORMALIZE(diff_available_heading[i]);
i++;
}
}
//set unavailable headings to 100;
i = 0;
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
for (int i2 = 0; i2 < 4; i2++) {
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
distance_est = (baseline * (float)focal / ((float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
size_matrix[2] + i3]) - 18.0) / 1000;
if (distance_est < distance_heading) {
distance_heading = distance_est;
}
diff_available_heading[i] = 100;
for (int i_diff = -number_of_buffer; i_diff <= number_of_buffer + 1; i_diff++) {
i_buffer = i + i_diff;
if (i_buffer < 1) {
i_buffer = i_buffer + size_matrix[0] * size_matrix[2];
}
if (i_buffer > size_matrix[0]*size_matrix[2]) {
i_buffer = i_buffer - size_matrix[0] * size_matrix[2];
}
diff_available_heading[i_buffer] = 100;
}
}
}
i++;
}
}
//set bias
if (set_bias == 1) {
for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
if (diff_available_heading[i100] <= 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
diff_available_heading[i100] = diff_available_heading[i100] - bias;
}
}
} else if (set_bias == 2) {
for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
if (diff_available_heading[i100] > 0 && (fabs(diff_available_heading[i100]) < M_PI - 5.0 / 360.0 * 2.0 * M_PI)) {
diff_available_heading[i100] = diff_available_heading[i100] + bias;
}
}
}
// select minimum available heading
for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
if (fabs(diff_available_heading[i100]) < fabs(new_heading_diff)) {
new_heading_diff = diff_available_heading[i100];
new_heading = available_heading[i100];
}
}
if (obstacle_flag == 0 && distance_heading > 2.0) {
new_heading = new_heading_old;
} else if (obstacle_flag == 0 && fabs(new_heading_diff) > (2 * M_PI / 36.0)) {
obstacle_flag = 1;
if (new_heading_diff > 0) {
set_bias = 1;
direction = -1;
} else if (new_heading_diff <= 0) {
set_bias = 2;
direction = 1;
}
}
// Rotate waypoint
if (fabs(new_heading_diff) >= 0.5 * M_PI) {
waypoint_rot = waypoint_rot + direction * 0.25 * M_PI;
}
if (fabs(new_heading_diff) <= (2 * M_PI / 36.0)) {
if (waypoint_rot == 0) {
obstacle_flag = 0;
set_bias = 0;
} else {
waypoint_rot = waypoint_rot - direction * 0.05 * M_PI;
}
}
//vref_max should be low
speed_pot = vref_max * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
if (speed_pot <= 0) {
speed_pot = 0;
}
ref_pitch = cos(new_heading) * speed_pot;
ref_roll = sin(new_heading) * speed_pot;
new_heading_old = new_heading;
#if PRINT_STUFF
for (int i100 = 0; i100 < (size_matrix[0]*size_matrix[2]); i100++) {
printf("%i diff_available_heading: %f available_heading: %f \n", i100, diff_available_heading[i100],
available_heading[i100]);
}
printf("new_heading: %f current_heading: %f speed_pot: %f\n", new_heading, stateGetNedToBodyEulers_f()->psi,
speed_pot);
printf("set_bias: %i obstacle_flag: %i", set_bias, obstacle_flag);
#endif
}
// Periodic
void takeoff_detect_periodic(void)
{
// Run detection state machine here
switch (takeoff_detect.state) {
case TO_DETECT_ARMED:
// test for "nose up" + AP in AUTO2, optionally AUTO1 not default since risky if one does not know what it does
if (stateGetNedToBodyEulers_f()->theta > TAKEOFF_DETECT_LAUNCH_PITCH)
{
#ifndef TAKEOFF_DETECT_ALSO_IN_AUTO1
if (autopilot_get_mode() != AP_MODE_AUTO2)
#else
if ((autopilot_get_mode() != AP_MODE_AUTO2) || (autopilot_get_mode() != AP_MODE_AUTO1))
#endif
{
takeoff_detect.timer++;
}
}
else {
// else reset timer
takeoff_detect.timer = 0;
}
// if timer is finished, start launching
if (takeoff_detect.timer > (int)(TAKEOFF_DETECT_PERIODIC_FREQ * TAKEOFF_DETECT_TIMER)) {
autopilot.launch = true;
takeoff_detect.state = TO_DETECT_LAUNCHING;
takeoff_detect.timer = 0;
}
break;
case TO_DETECT_LAUNCHING:
// abort if pitch goes below threshold while launching
if (stateGetNedToBodyEulers_f()->theta < TAKEOFF_DETECT_ABORT_PITCH)
{
#ifndef TAKEOFF_DETECT_ALSO_IN_AUTO1
if (autopilot_get_mode() != AP_MODE_AUTO2)
#else
if ((autopilot_get_mode() != AP_MODE_AUTO2) || (autopilot_get_mode() != AP_MODE_AUTO1))
#endif
{
// back to ARMED state
autopilot.launch = false;
takeoff_detect.state = TO_DETECT_ARMED;
}
}
// increment timer and disable detection after some time
takeoff_detect.timer++;
if (takeoff_detect.timer > (int)(TAKEOFF_DETECT_PERIODIC_FREQ * TAKEOFF_DETECT_DISABLE_TIMER)) {
takeoff_detect.state = TO_DETECT_DISABLED;
}
break;
case TO_DETECT_DISABLED:
// stop periodic call
takeoff_detect_takeoff_detect_periodic_status = MODULES_STOP;
break;
default:
// No kidding ?!
takeoff_detect.state = TO_DETECT_DISABLED;
break;
}
}
static void send_tune_roll(void) {
DOWNLINK_SEND_TUNE_ROLL(DefaultChannel, DefaultDevice,
&(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
}
// GENERIC TRAJECTORY CONTROLLER
void gvf_control_2D(float ke, float kn, float e,
struct gvf_grad *grad, struct gvf_Hess *hess)
{
struct FloatEulers *att = stateGetNedToBodyEulers_f();
float ground_speed = stateGetHorizontalSpeedNorm_f();
float course = stateGetHorizontalSpeedDir_f();
float px_dot = ground_speed * sinf(course);
float py_dot = ground_speed * cosf(course);
int s = gvf_control.s;
// gradient Phi
float nx = grad->nx;
float ny = grad->ny;
// tangent to Phi
float tx = s * grad->ny;
float ty = -s * grad->nx;
// Hessian
float H11 = hess->H11;
float H12 = hess->H12;
float H21 = hess->H21;
float H22 = hess->H22;
// Calculation of the desired angular velocity in the vector field
float pdx_dot = tx - ke * e * nx;
float pdy_dot = ty - ke * e * ny;
float norm_pd_dot = sqrtf(pdx_dot * pdx_dot + pdy_dot * pdy_dot);
float md_x = pdx_dot / norm_pd_dot;
float md_y = pdy_dot / norm_pd_dot;
float Apd_dot_dot_x = -ke * (nx * px_dot + ny * py_dot) * nx;
float Apd_dot_dot_y = -ke * (nx * px_dot + ny * py_dot) * ny;
float Bpd_dot_dot_x = ((-ke * e * H11) + s * H21) * px_dot
+ ((-ke * e * H12) + s * H22) * py_dot;
float Bpd_dot_dot_y = -(s * H11 + (ke * e * H21)) * px_dot
- (s * H12 + (ke * e * H22)) * py_dot;
float pd_dot_dot_x = Apd_dot_dot_x + Bpd_dot_dot_x;
float pd_dot_dot_y = Apd_dot_dot_y + Bpd_dot_dot_y;
float md_dot_const = -(md_x * pd_dot_dot_y - md_y * pd_dot_dot_x)
/ norm_pd_dot;
float md_dot_x = md_y * md_dot_const;
float md_dot_y = -md_x * md_dot_const;
float omega_d = -(md_dot_x * md_y - md_dot_y * md_x);
float mr_x = sinf(course);
float mr_y = cosf(course);
float omega = omega_d + kn * (mr_x * md_y - mr_y * md_x);
// Coordinated turn
if (autopilot_get_mode() == AP_MODE_AUTO2) {
h_ctl_roll_setpoint =
-atanf(omega * ground_speed / GVF_GRAVITY / cosf(att->theta));
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
lateral_mode = LATERAL_MODE_ROLL;
}
}
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool in_flight, bool in_carefree,
bool coordinated_turn)
{
/* last time this function was called, used to calculate yaw setpoint update */
static float last_ts = 0.f;
sp->phi = get_rc_roll_f();
sp->theta = get_rc_pitch_f();
if (in_flight) {
/* calculate dt for yaw integration */
float dt = get_sys_time_float() - last_ts;
/* make sure nothing drastically weird happens, bound dt to 0.5sec */
Bound(dt, 0, 0.5);
/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += get_rc_yaw_f() * dt;
FLOAT_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
float omega;
const float max_phi = RadOfDeg(60.0);
if (fabsf(sp->phi) < max_phi) {
omega = 9.81 / COORDINATED_TURN_AIRSPEED * tanf(sp->phi);
} else { //max 60 degrees roll
omega = 9.81 / COORDINATED_TURN_AIRSPEED * 1.72305 * ((sp->phi > 0) - (sp->phi < 0));
}
sp->psi += omega * dt;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
float heading = stabilization_attitude_get_heading_f();
float delta_psi = sp->psi - heading;
FLOAT_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
} else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT) {
sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
FLOAT_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
float cos_psi;
float sin_psi;
float temp_theta;
float care_free_delta_psi_f = sp->psi - care_free_heading;
FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
sin_psi = sinf(care_free_delta_psi_f);
cos_psi = cosf(care_free_delta_psi_f);
temp_theta = cos_psi * sp->theta - sin_psi * sp->phi;
sp->phi = cos_psi * sp->phi - sin_psi * sp->theta;
sp->theta = temp_theta;
}
} else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_f()->psi;
}
/* update timestamp for dt calculation */
last_ts = get_sys_time_float();
}
inline static void h_ctl_pitch_loop( void ) {
#if !USE_GYRO_PITCH_RATE
static float last_err;
#endif
/* sanity check */
if (h_ctl_pitch_of_roll <0.)
h_ctl_pitch_of_roll = 0.;
h_ctl_pitch_loop_setpoint = h_ctl_pitch_setpoint + h_ctl_pitch_of_roll * fabs(stateGetNedToBodyEulers_f()->phi);
#if USE_PITCH_TRIM
loiter();
#endif
#if USE_ANGLE_REF
// Update reference setpoints for pitch
h_ctl_ref.pitch_angle += h_ctl_ref.pitch_rate * H_CTL_REF_DT;
h_ctl_ref.pitch_rate += h_ctl_ref.pitch_accel * H_CTL_REF_DT;
h_ctl_ref.pitch_accel = H_CTL_REF_W_Q*H_CTL_REF_W_Q * (h_ctl_pitch_loop_setpoint - h_ctl_ref.pitch_angle) - 2*H_CTL_REF_XI_Q*H_CTL_REF_W_Q * h_ctl_ref.pitch_rate;
// Saturation on references
BoundAbs(h_ctl_ref.pitch_accel, h_ctl_ref.max_q_dot);
if (h_ctl_ref.pitch_rate > h_ctl_ref.max_q) {
h_ctl_ref.pitch_rate = h_ctl_ref.max_q;
if (h_ctl_ref.pitch_accel > 0.) h_ctl_ref.pitch_accel = 0.;
}
else if (h_ctl_ref.pitch_rate < - h_ctl_ref.max_q) {
h_ctl_ref.pitch_rate = - h_ctl_ref.max_q;
if (h_ctl_ref.pitch_accel < 0.) h_ctl_ref.pitch_accel = 0.;
}
#else
h_ctl_ref.pitch_angle = h_ctl_pitch_loop_setpoint;
h_ctl_ref.pitch_rate = 0.;
h_ctl_ref.pitch_accel = 0.;
#endif
// Compute errors
float err = h_ctl_ref.pitch_angle - stateGetNedToBodyEulers_f()->theta;
#if USE_GYRO_PITCH_RATE
float d_err = h_ctl_ref.pitch_rate - stateGetBodyRates_f()->q;
#else // soft derivation
float d_err = (err - last_err)/H_CTL_REF_DT - h_ctl_ref.pitch_rate;
last_err = err;
#endif
if (pprz_mode == PPRZ_MODE_MANUAL || launch == 0) {
h_ctl_pitch_sum_err = 0.;
}
else {
if (h_ctl_pitch_igain > 0.) {
h_ctl_pitch_sum_err += err * H_CTL_REF_DT;
BoundAbs(h_ctl_pitch_sum_err, H_CTL_PITCH_SUM_ERR_MAX / h_ctl_pitch_igain);
} else {
h_ctl_pitch_sum_err = 0.;
}
}
float cmd = - h_ctl_pitch_Kffa * h_ctl_ref.pitch_accel
- h_ctl_pitch_Kffd * h_ctl_ref.pitch_rate
+ h_ctl_pitch_pgain * err
+ h_ctl_pitch_dgain * d_err
+ h_ctl_pitch_igain * h_ctl_pitch_sum_err;
cmd /= airspeed_ratio2;
h_ctl_elevator_setpoint = TRIM_PPRZ(cmd);
}
void stabilization_attitude_set_failsafe_setpoint(void) {
stab_att_sp_euler.phi = 0.0;
stab_att_sp_euler.theta = 0.0;
stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi;
}
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
sp->phi = (radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ);
sp->theta = (radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ);
if (in_flight) {
/* do not advance yaw setpoint if within a small deadband around stick center or if throttle is zero */
if (YAW_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
sp->psi += (radio_control.values[RADIO_YAW] * STABILIZATION_ATTITUDE_SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
FLOAT_ANGLE_NORMALIZE(sp->psi);
}
if (coordinated_turn) {
//Coordinated turn
//feedforward estimate angular rotation omega = g*tan(phi)/v
//Take v = 9.81/1.3 m/s
float omega;
const float max_phi = RadOfDeg(85.0);
if(abs(sp->phi) < max_phi)
omega = 1.3*tanf(sp->phi);
else //max 60 degrees roll, then take constant omega
omega = 1.3*1.72305* ((sp->phi > 0) - (sp->phi < 0));
sp->psi += omega/RC_UPDATE_FREQ;
}
#ifdef STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT
// Make sure the yaw setpoint does not differ too much from the real yaw
// to prevent a sudden switch at 180 deg
float heading = stabilization_attitude_get_heading_f();
float delta_psi = sp->psi - heading;
FLOAT_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
sp->psi = heading + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
sp->psi = heading - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
FLOAT_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (in_carefree) {
//care_free_heading has been set to current psi when entering care free mode.
float cos_psi;
float sin_psi;
float temp_theta;
float care_free_delta_psi_f = sp->psi - care_free_heading;
FLOAT_ANGLE_NORMALIZE(care_free_delta_psi_f);
sin_psi = sinf(care_free_delta_psi_f);
cos_psi = cosf(care_free_delta_psi_f);
temp_theta = cos_psi*sp->theta - sin_psi*sp->phi;
sp->phi = cos_psi*sp->phi - sin_psi*sp->theta;
sp->theta = temp_theta;
}
}
else { /* if not flying, use current yaw as setpoint */
sp->psi = stateGetNedToBodyEulers_f()->psi;
}
}
void stabilization_attitude_run(bool_t in_flight) {
stabilization_attitude_ref_update();
/* Compute feedforward */
stabilization_att_ff_cmd[COMMAND_ROLL] =
stabilization_gains.dd.x * stab_att_ref_accel.p;
stabilization_att_ff_cmd[COMMAND_PITCH] =
stabilization_gains.dd.y * stab_att_ref_accel.q;
stabilization_att_ff_cmd[COMMAND_YAW] =
stabilization_gains.dd.z * stab_att_ref_accel.r;
/* Compute feedback */
/* attitude error */
struct FloatEulers *att_float = stateGetNedToBodyEulers_f();
struct FloatEulers att_err;
EULERS_DIFF(att_err, stab_att_ref_euler, *att_float);
FLOAT_ANGLE_NORMALIZE(att_err.psi);
if (in_flight) {
/* update integrator */
EULERS_ADD(stabilization_att_sum_err, att_err);
EULERS_BOUND_CUBE(stabilization_att_sum_err, -MAX_SUM_ERR, MAX_SUM_ERR);
}
else {
FLOAT_EULERS_ZERO(stabilization_att_sum_err);
}
/* rate error */
struct FloatRates* rate_float = stateGetBodyRates_f();
struct FloatRates rate_err;
RATES_DIFF(rate_err, stab_att_ref_rate, *rate_float);
/* PID */
stabilization_att_fb_cmd[COMMAND_ROLL] =
stabilization_gains.p.x * att_err.phi +
stabilization_gains.d.x * rate_err.p +
stabilization_gains.i.x * stabilization_att_sum_err.phi;
stabilization_att_fb_cmd[COMMAND_PITCH] =
stabilization_gains.p.y * att_err.theta +
stabilization_gains.d.y * rate_err.q +
stabilization_gains.i.y * stabilization_att_sum_err.theta;
stabilization_att_fb_cmd[COMMAND_YAW] =
stabilization_gains.p.z * att_err.psi +
stabilization_gains.d.z * rate_err.r +
stabilization_gains.i.z * stabilization_att_sum_err.psi;
stabilization_cmd[COMMAND_ROLL] =
(stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]);
stabilization_cmd[COMMAND_PITCH] =
(stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]);
stabilization_cmd[COMMAND_YAW] =
(stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]);
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}
/// reset the heading for care-free mode to current heading
void stabilization_attitude_reset_care_free_heading(void) {
care_free_heading = stateGetNedToBodyEulers_f()->psi;
}
void dc_periodic_4Hz(void)
{
static uint8_t dc_shutter_timer = 0;
switch (dc_autoshoot) {
case DC_AUTOSHOOT_PERIODIC:
if (dc_shutter_timer) {
dc_shutter_timer--;
} else {
dc_shutter_timer = dc_autoshoot_quartersec_period;
dc_send_command(DC_SHOOT);
}
break;
case DC_AUTOSHOOT_DISTANCE:
{
struct FloatVect2 cur_pos;
cur_pos.x = stateGetPositionEnu_f()->x;
cur_pos.y = stateGetPositionEnu_f()->y;
struct FloatVect2 delta_pos;
VECT2_DIFF(delta_pos, cur_pos, last_shot_pos);
float dist_to_last_shot = float_vect2_norm(&delta_pos);
if (dist_to_last_shot > dc_distance_interval) {
dc_gps_count++;
dc_send_command(DC_SHOOT);
VECT2_COPY(last_shot_pos, cur_pos);
}
}
break;
case DC_AUTOSHOOT_CIRCLE:
{
float course = DegOfRad(stateGetNedToBodyEulers_f()->psi) - dc_circle_start_angle;
if (course < 0.)
course += 360.;
float current_block = floorf(course/dc_circle_interval);
if (dim_mod(current_block, dc_circle_last_block, dc_circle_max_blocks) == 1) {
dc_gps_count++;
dc_circle_last_block = current_block;
dc_send_command(DC_SHOOT);
}
}
break;
case DC_AUTOSHOOT_SURVEY:
{
float dist_x = dc_gps_x - stateGetPositionEnu_f()->x;
float dist_y = dc_gps_y - stateGetPositionEnu_f()->y;
if (dist_x*dist_x + dist_y*dist_y >= dc_gps_next_dist*dc_gps_next_dist) {
dc_gps_next_dist += dc_survey_interval;
dc_gps_count++;
dc_send_command(DC_SHOOT);
}
}
break;
default:
dc_autoshoot = DC_AUTOSHOOT_STOP;
}
}
void CN_potential_heading(void)
{
//float OF_Result_Vy = 0;
//float OF_Result_Vx = 0
//Initialize
float potential_obst = 0; //define potential field variables
float potential_obst_temp = 0;
float potential_obst_integrated = 0;
float Distance_est;
float angle_hor = - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2] /
2; //calculate position angle of stereo
float angle_hor_abs;
float obst_count = 0.0;
//float total_vel;
//float current_heading;
//float fp_angle;
//Tune
//float dt = 0.5; //define delta t for integration! check response under 0.1 and 1
int8_t min_disparity = 40;
//Current state of system;
float r_old = stateGetBodyRates_f()->r;
//current_heading = stateGetNedToBodyEulers_f()->psi; //check maybe cause of errors
//current_heading = 0;
//check if side slip is zero
//total_vel = pow((OF_Result_Vy*OF_Result_Vy + OF_Result_Vx*OF_Result_Vx),0.5);
//if (total_vel>vmin){
// fp_angle = atan2(OF_Result_Vx,OF_Result_Vy);
//}
//else{
// fp_angle = 0;
//}
//fp_angle = 0;
heading_goal_ref = stateGetNedToBodyEulers_f()->psi - heading_goal_f;//
FLOAT_ANGLE_NORMALIZE(heading_goal_ref);
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
/ 2; //Check if bodyframe is correct with current_heading correction
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
FLOAT_ANGLE_NORMALIZE(angle_hor);
potential_obst_temp = 0.0;
obst_count = 0;
for (int i2 = 0; i2 < 4; i2++) {
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
Distance_est = ((baseline * (float)focal / (float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
size_matrix[2] + i3] - 18.0)) / 1000;
if (angle_hor < 0) {
angle_hor_abs = -1 * angle_hor;
} else {
angle_hor_abs = angle_hor;
}
potential_obst_temp = potential_obst_temp - K_obst * (angle_hor) * exp(-c3_oa * angle_hor_abs) * exp(
-c4_oa * Distance_est);//(tan(obst_width[i]+c5)-tan(c5));
potential_obst_integrated = potential_obst_integrated + K_obst * c3_oa * (fabs(angle_hor) + 1) / (c3_oa * c3_oa) * exp(
-c3_oa * fabs(angle_hor)) * exp(-c4_oa * Distance_est); // (tan(obst_width[i]+c5)-tan(c5));
//printf("angle_hor: %f, angle_hor_abs: %f, c3: %f, potential_obst: %f", angle_hor, angle_hor_abs,c3_oa,potential_obst);
obst_count++;
} else {
Distance_est = 2000;
}
//potential_obst = potential_obst + K_obst*(angle_hor);//*exp(-c3*abs(angle_hor))*exp(-c4*Distance_est);//(tan(obst_width[i]+c5)-tan(c5));
//potential_obst_write = potential_obst;
//potential_obst_integrated = potential_obst_integrated + K_obst*c3*(abs(angle_hor)+1)/(c3*c3)*exp(-c3*abs(angle_hor))*exp(-c4*Distance_est);// (tan(obst_width[i]+c5)-tan(c5));
}
if (obst_count != 0) {
potential_obst = potential_obst + potential_obst_temp / ((float)obst_count);
}
}
}
//printf("current_heading, %f heading_goal_f: %f",current_heading, heading_goal_ref);
//calculate angular accelaration from potential field
r_dot_new = -b_damp * r_old - K_goal * (heading_goal_ref) * (exp(-c1_oa * sqrt(VECT2_NORM2(
pos_diff))) + c2_oa) + potential_obst;
// Calculate velocity from potential
speed_pot = vref_max * exp(-kv * potential_obst_integrated) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff))) - epsilon;
if (speed_pot <= 0) {
speed_pot = 0;
}
//calculate new_heading(ref_pitch)
//ref_pitch = (current_state(3)+sideslip) + alpha_fil*r_dot_new;)
}
/**
* Update the controls based on a vision result
* @param[in] *result The opticflow calculation result used for control
*/
void OA_update()
{
float v_x = 0;
float v_y = 0;
if (opti_speed_flag == 1) {
//rotation_vector.psi = stateGetNedToBodyEulers_f()->psi;
//opti_speed = stateGetSpeedNed_f();
//Transform to body frame.
//float_rmat_of_eulers_312(&T, &rotation_vector)Attractforce_goal_send;
//MAT33_VECT3_MUL(*opti_speed, T, *opti_speed);
// Calculate the speed in body frame
struct FloatVect2 speed_cur;
float psi = stateGetNedToBodyEulers_f()->psi;
float s_psi = sin(psi);
float c_psi = cos(psi);
speed_cur.x = c_psi * stateGetSpeedNed_f()->x + s_psi * stateGetSpeedNed_f()->y;
speed_cur.y = -s_psi * stateGetSpeedNed_f()->x + c_psi * stateGetSpeedNed_f()->y;
opti_speed_read.x = speed_cur.x * 100;
opti_speed_read.y = speed_cur.y * 100;
//set result_vel
v_x = speed_cur.y * 100;
v_y = speed_cur.x * 100;
} else {
}
if (OA_method_flag == NO_OBSTACLE_AVOIDANCE) {
/* Calculate the error if we have enough flow */
opticflow_stab.desired_vx = 0;
opticflow_stab.desired_vy = 0;
err_vx = opticflow_stab.desired_vx - v_x;
err_vy = opticflow_stab.desired_vy - v_y;
/* Calculate the integrated errors (TODO: bound??) */
opticflow_stab.err_vx_int += err_vx / 100;
opticflow_stab.err_vy_int += err_vy / 100;
/* Calculate the commands */
opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
+ opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
+ opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
/* Bound the roll and pitch commands */
BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
}
if (OA_method_flag == PINGPONG) {
opticflow_stab.cmd.phi = ANGLE_BFP_OF_REAL(ref_roll);
opticflow_stab.cmd.theta = ANGLE_BFP_OF_REAL(ref_pitch);
}
if (OA_method_flag == 2) {
Total_Kan_x = r_dot_new;
Total_Kan_y = speed_pot;
alpha_fil = 0.1;
yaw_rate = (int32_t)(alpha_fil * ANGLE_BFP_OF_REAL(r_dot_new));
opticflow_stab.cmd.psi = stateGetNedToBodyEulers_i()->psi + yaw_rate;
INT32_ANGLE_NORMALIZE(opticflow_stab.cmd.psi);
opticflow_stab.desired_vx = 0;
opticflow_stab.desired_vy = speed_pot;
/* Calculate the error if we have enough flow */
err_vx = opticflow_stab.desired_vx - v_x;
err_vy = opticflow_stab.desired_vy - v_y;
/* Calculate the integrated errors (TODO: bound??) */
opticflow_stab.err_vx_int += err_vx / 100;
opticflow_stab.err_vy_int += err_vy / 100;
/* Calculate the commands */
opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
+ opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
+ opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
/* Bound the roll and pitch commands */
BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT);
}
if (OA_method_flag == POT_HEADING) {
new_heading = ref_pitch;
opticflow_stab.desired_vx = sin(new_heading) * speed_pot * 100;
opticflow_stab.desired_vy = cos(new_heading) * speed_pot * 100;
/* Calculate the error if we have enough flow */
err_vx = opticflow_stab.desired_vx - v_x;
err_vy = opticflow_stab.desired_vy - v_y;
/* Calculate the integrated errors (TODO: bound??) */
opticflow_stab.err_vx_int += err_vx / 100;
opticflow_stab.err_vy_int += err_vy / 100;
/* Calculate the commands */
opticflow_stab.cmd.phi = opticflow_stab.phi_pgain * err_vx / 100
+ opticflow_stab.phi_igain * opticflow_stab.err_vx_int;
opticflow_stab.cmd.theta = -(opticflow_stab.theta_pgain * err_vy / 100
+ opticflow_stab.theta_igain * opticflow_stab.err_vy_int);
/* Bound the roll and pitch commands */
BoundAbs(opticflow_stab.cmd.phi, CMD_OF_SAT);
BoundAbs(opticflow_stab.cmd.theta, CMD_OF_SAT)
}
void CN_vector_velocity(void)
{
//Constants
//Parameters for Butterworth filter
float A_butter = -0.8541;//-0.7265;
float B_butter[2] = {0.0730 , 0.0730 };//{0.1367, 0.1367};
//Initalize
int8_t disp_count = 0;
float escape_angle = 0;
float y_goal_frame;
//float total_vel;
float Distance_est;
float Ca;
float Cv;
float angle_ver = 0;
float angle_hor = 0;
//struct FloatVect3 Repulsionforce_Kan = {0,0,0};
Repulsionforce_Kan.x = 0;
Repulsionforce_Kan.y = 0;
Repulsionforce_Kan.z = 0;
//struct FloatVect3 Attractforce_goal = {0,0,0};
//Attractforce_goal.x = 0;
//Attractforce_goal.y = 0;
//Attractforce_goal.z = 0;
//struct FloatRMat T;
//struct FloatEulers current_attitude = {0,0,0};
struct FloatVect3 Total_Kan = {0, 0, 0};
//Tuning variables
//float force_max = 200;
int8_t min_disparity = 45;
//Flight plath angle calculation
// TODO make algorithm dependent on angle of obstacle.....
// total_vel = pow((OF_Result_Vy*OF_Result_Vy + OF_Result_Vx*OF_Result_Vx),0.5);
// if (total_vel>vmin){
// fp_angle = atan2(OF_Result_Vx,OF_Result_Vy);
// }
// else{
// fp_angle = 0;
// }
heading_goal_ref = heading_goal_f - stateGetNedToBodyEulers_f()->psi;
//FLOAT_ANGLE_NORMALIZE(heading_goal_ref);
//Calculate Attractforce_goal size = 1;
Attractforce_goal.x = cos(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
Attractforce_goal.y = sin(heading_goal_ref) * erf(0.5 * sqrt(VECT2_NORM2(pos_diff)));
Attractforce_goal.z = 0;
//printf("current_heading, %f heading_goal_f: %f, heading_goal_ref: %f",stateGetNedToBodyEulers_f()->psi, heading_goal_f, heading_goal_ref);
//Transform to body frame
//current_attitude.psi = stateGetNedToBodyEulers_f()->psi;Attractforce_already in body frame, check when using data form Roland
//float_rmat_of_eulers_312(&T, ¤t_attitude);
//MAT33_VECT3_MUL(Attractforce_goal, T, Attractforce_goal);
//Attractforce_goal_send.x = Attractforce_goal.x;
//Attractforce_goal_send.y = Attractforce_goal.y;
//Attractforce_goal_send.z = Attractforce_goal.z;
for (int i1 = 0; i1 < size_matrix[0]; i1++) {
angle_hor = angle_hor_board[i1] - 0.5 * stereo_fow[0] - stereo_fow[0] / size_matrix[2] + stereo_fow[0] / size_matrix[2]
/ 2; //Check if bodyframe is correct with current_heading correction
for (int i3 = 0; i3 < size_matrix[2]; i3++) {
angle_hor = angle_hor + stereo_fow[0] / size_matrix[2];
angle_ver = 0.5 * stereo_fow[1] + stereo_fow[1] / size_matrix[1] - stereo_fow[1] / size_matrix[1] / 2;
for (int i2 = 0; i2 < 4; i2++) {
angle_ver = angle_ver - stereo_fow[1] / size_matrix[1];
Ca = cos((angle_hor - heading_goal_ref) * Cfreq) * erf(1 * sqrt(VECT2_NORM2(pos_diff)));
if (Ca < 0) {
Ca = 0;
}
//TODO make dependent on speed: total_vel/vref_max
Cv = F1 + F2 * Ca;
if (stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3] > min_disparity) {
Distance_est = (baseline * (float)focal / ((float)stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0] *
size_matrix[2] + i3]) - 18.0) / 1000;
disp_count++;
Repulsionforce_Kan.x = Repulsionforce_Kan.x - pow(Cv / (Distance_est + Dist_offset),
2) * cos(angle_hor) * cos(angle_ver);
Repulsionforce_Kan.y = Repulsionforce_Kan.y - pow(Cv / (Distance_est + Dist_offset),
2) * sin(angle_hor) * cos(angle_ver);
Repulsionforce_Kan.z = Repulsionforce_Kan.z - pow(Cv / (Distance_est + Dist_offset), 2) * sin(angle_ver);
printf("rep.x %f index %d %d %d disp: %d cv: %f angle_hor: %f angle_ver: %f \n", Repulsionforce_Kan.x, i1, i2, i3,
stereocam_data.data[i1 * size_matrix[1] + i2 * size_matrix[0]*size_matrix[2] + i3], Cv, angle_hor, angle_ver);
}
// else {
// Distance_est = 2000;
// }
//
// if(pow(Cv/(READimageBuffer_offset[i1*size_matrix[1]+i2*size_matrix[0]*size_matrix[2] + i3]-0.2),2)<force_max){
// Repulsionforce_Kan.x = Repulsionforce_Kan.x - pow(Cv/(Distance_est+Dist_offset),2)*cos(angle_hor)*cos(angle_ver);
// Repulsionforce_Kan.y = Repulsionforce_Kan.y - pow(Cv/(Distance_est+Dist_offset),2)*sin(angle_hor)*cos(angle_ver);
// Repulsionforce_Kan.z = Repulsionforce_Kan.z - pow(Cv/(Distance_est+Dist_offset),2)*sin(angle_ver);
// }
// else{
// Repulsionforce_Kan.x = Repulsionforce_Kan.x - force_max*sin(angle_hor)*cos(angle_ver);
// Repulsionforce_Kan.y = Repulsionforce_Kan.y - force_max*cos(angle_hor)*cos(angle_ver);
// Repulsionforce_Kan.z = Repulsionforce_Kan.z - force_max*sin(angle_ver);
// }
}
}
}
//Normalize for ammount entries in Matrix
//Repulsionforce_Kan = Repulsionforce_Kan/(float)(size_matrix[1]*size_matrix[2]);
VECT3_SMUL(Repulsionforce_Kan, Repulsionforce_Kan, 1.0 / (float)disp_count);
printf("After multiplication: %f", Repulsionforce_Kan.x);
if (repulsionforce_filter_flag == 1) {
Repulsionforce_Used.x = B_butter[0] * Repulsionforce_Kan.x + B_butter[1] * Repulsionforce_Kan_old.x - A_butter *
filter_repforce_old.x;
Repulsionforce_Used.y = B_butter[0] * Repulsionforce_Kan.y + B_butter[1] * Repulsionforce_Kan_old.y - A_butter *
filter_repforce_old.y;
Repulsionforce_Used.z = B_butter[0] * Repulsionforce_Kan.z + B_butter[1] * Repulsionforce_Kan_old.z - A_butter *
filter_repforce_old.z;
VECT3_COPY(Repulsionforce_Kan_old, Repulsionforce_Kan);
VECT3_COPY(filter_repforce_old, Repulsionforce_Used);
}
else {
VECT3_COPY(Repulsionforce_Used, Repulsionforce_Kan);
}
VECT3_SMUL(Repulsionforce_Used, Repulsionforce_Used, Ko);
VECT3_SMUL(Attractforce_goal, Attractforce_goal, Kg);
//Total force
VECT3_ADD(Total_Kan, Repulsionforce_Used);
VECT3_ADD(Total_Kan, Attractforce_goal);
//set variable for stabilization_opticflow
//printf("RepulsionforceX: %f AttractX: %f \n",Repulsionforce_Used.x,Attractforce_goal.x);
//printf("RepulsionforceY: %f AttractY: %f \n",Repulsionforce_Used.y,Attractforce_goal.y);
//hysteris
if (sqrt(VECT2_NORM2(Total_Kan)) < V_hys_low && sqrt(VECT2_NORM2(pos_diff)) > 1) {
hysteris_flag = 1;
//x_goal_frame= cos() * Total_Kan.x + sin() * Total_Kan.y;
y_goal_frame = -sin(heading_goal_ref) * Total_Kan.x + cos(heading_goal_ref) * Total_Kan.y;
if (y_goal_frame >= 0) {
escape_angle = 0.5 * M_PI;
} else if (y_goal_frame < 0) {
escape_angle = -0.5 * M_PI;
}
}
if (sqrt(VECT2_NORM2(Total_Kan)) > V_hys_high || sqrt(VECT2_NORM2(pos_diff)) < 1) {
hysteris_flag = 0;
}
if (hysteris_flag == 1) {
Total_Kan.x = cos(heading_goal_ref + escape_angle) * V_hys_high;
Total_Kan.y = -sin(heading_goal_ref + escape_angle) * V_hys_high;
}
ref_pitch = Total_Kan.x;
ref_roll = Total_Kan.y;
printf("ref_pitch: %f ref_roll: %f disp_count: %d", ref_pitch, ref_roll, disp_count);
//set write values for logger
//Attractforce_goal_send.x = Attractforce_goal.x;
//Attractforce_goal_send.y = Attractforce_goal.y;
//Repulsionforce_Kan_send.x = Repulsionforce_Used.x;
//Repulsionforce_Kan_send.y = Repulsionforce_Used.y;
}
static void send_attitude(void) {
struct FloatEulers* att = stateGetNedToBodyEulers_f();
DOWNLINK_SEND_ATTITUDE(DefaultChannel, DefaultDevice,
&(att->phi), &(att->psi), &(att->theta));
};
/**
* \brief
*
*/
void h_ctl_course_loop(void)
{
static float last_err;
// Ground path error
float err = stateGetHorizontalSpeedDir_f() - h_ctl_course_setpoint;
NormRadAngle(err);
#ifdef STRONG_WIND
// Usefull path speed
const float reference_advance = (NOMINAL_AIRSPEED / 2.);
float advance = cos(err) * stateGetHorizontalSpeedNorm_f() / reference_advance;
if (
(advance < 1.) && // Path speed is small
(stateGetHorizontalSpeedNorm_f() < reference_advance) // Small path speed is due to wind (small groundspeed)
) {
/*
// rough crabangle approximation
float wind_mod = sqrt(wind_east*wind_east + wind_north*wind_north);
float wind_dir = atan2(wind_east,wind_north);
float wind_course = h_ctl_course_setpoint - wind_dir;
NormRadAngle(wind_course);
estimator_hspeed_dir = estimator_psi;
float crab = sin(wind_dir-estimator_psi) * atan2(wind_mod,NOMINAL_AIRSPEED);
//crab = estimator_hspeed_mod - estimator_psi;
NormRadAngle(crab);
*/
// Heading error
float herr = stateGetNedToBodyEulers_f()->psi - h_ctl_course_setpoint; //+crab);
NormRadAngle(herr);
if (advance < -0.5) { //<! moving in the wrong direction / big > 90 degree turn
err = herr;
} else if (advance < 0.) { //<!
err = (-advance) * 2. * herr;
} else {
err = advance * err;
}
// Reset differentiator when switching mode
//if (h_ctl_course_heading_mode == 0)
// last_err = err;
//h_ctl_course_heading_mode = 1;
}
/* else
{
// Reset differentiator when switching mode
if (h_ctl_course_heading_mode == 1)
last_err = err;
h_ctl_course_heading_mode = 0;
}
*/
#endif //STRONG_WIND
float d_err = err - last_err;
last_err = err;
NormRadAngle(d_err);
#ifdef H_CTL_COURSE_SLEW_INCREMENT
/* slew severe course changes (i.e. waypoint moves, block changes or perpendicular routes) */
static float h_ctl_course_slew_rate = 0.;
float nav_angle_saturation = h_ctl_roll_max_setpoint / h_ctl_course_pgain; /* heading error corresponding to max_roll */
float half_nav_angle_saturation = nav_angle_saturation / 2.;
if (autopilot.launch) { /* prevent accumulator run-up on the ground */
if (err > half_nav_angle_saturation) {
h_ctl_course_slew_rate = Max(h_ctl_course_slew_rate, 0.);
err = Min(err, (half_nav_angle_saturation + h_ctl_course_slew_rate));
h_ctl_course_slew_rate += h_ctl_course_slew_increment;
} else if (err < -half_nav_angle_saturation) {
h_ctl_course_slew_rate = Min(h_ctl_course_slew_rate, 0.);
err = Max(err, (-half_nav_angle_saturation + h_ctl_course_slew_rate));
h_ctl_course_slew_rate -= h_ctl_course_slew_increment;
} else {
h_ctl_course_slew_rate = 0.;
}
}
#endif
float speed_depend_nav = stateGetHorizontalSpeedNorm_f() / NOMINAL_AIRSPEED;
Bound(speed_depend_nav, 0.66, 1.5);
float cmd = -h_ctl_course_pgain * speed_depend_nav * (err + d_err * h_ctl_course_dgain);
#if defined(AGR_CLIMB) && !USE_AIRSPEED
/** limit navigation during extreme altitude changes */
if (AGR_BLEND_START > AGR_BLEND_END && AGR_BLEND_END > 0) { /* prevent divide by zero, reversed or negative values */
if (v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_AGRESSIVE ||
v_ctl_auto_throttle_submode == V_CTL_AUTO_THROTTLE_BLENDED) {
BoundAbs(cmd, h_ctl_roll_max_setpoint); /* bound cmd before NAV_RATIO and again after */
/* altitude: z-up is positive -> positive error -> too low */
if (v_ctl_altitude_error > 0) {
nav_ratio = AGR_CLIMB_NAV_RATIO + (1 - AGR_CLIMB_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
(AGR_BLEND_START - AGR_BLEND_END));
Bound(nav_ratio, AGR_CLIMB_NAV_RATIO, 1);
} else {
nav_ratio = AGR_DESCENT_NAV_RATIO + (1 - AGR_DESCENT_NAV_RATIO) * (1 - (fabs(v_ctl_altitude_error) - AGR_BLEND_END) /
(AGR_BLEND_START - AGR_BLEND_END));
Bound(nav_ratio, AGR_DESCENT_NAV_RATIO, 1);
}
cmd *= nav_ratio;
}
}
#endif
float roll_setpoint = cmd + h_ctl_course_pre_bank_correction * h_ctl_course_pre_bank;
#ifdef H_CTL_ROLL_SLEW
float diff_roll = roll_setpoint - h_ctl_roll_setpoint;
BoundAbs(diff_roll, h_ctl_roll_slew);
h_ctl_roll_setpoint += diff_roll;
#else
h_ctl_roll_setpoint = roll_setpoint;
#endif
BoundAbs(h_ctl_roll_setpoint, h_ctl_roll_max_setpoint);
}
static void send_tune_roll(struct transport_tx *trans, struct link_device *dev) {
pprz_msg_send_TUNE_ROLL(trans, dev, AC_ID,
&(stateGetBodyRates_f()->p), &(stateGetNedToBodyEulers_f()->phi), &h_ctl_roll_setpoint);
}
/**
* Auto-throttle inner loop
* \brief
*/
void v_ctl_climb_loop(void)
{
// Airspeed setpoint rate limiter:
// AIRSPEED_SETPOINT_SLEW in m/s/s - a change from 15m/s to 18m/s takes 3s with the default value of 1
float airspeed_incr = v_ctl_auto_airspeed_setpoint - v_ctl_auto_airspeed_setpoint_slew;
BoundAbs(airspeed_incr, AIRSPEED_SETPOINT_SLEW * dt_attidude);
v_ctl_auto_airspeed_setpoint_slew += airspeed_incr;
#ifdef V_CTL_AUTO_GROUNDSPEED_SETPOINT
// Ground speed control loop (input: groundspeed error, output: airspeed controlled)
float err_groundspeed = (v_ctl_auto_groundspeed_setpoint - stateGetHorizontalSpeedNorm_f());
v_ctl_auto_groundspeed_sum_err += err_groundspeed;
BoundAbs(v_ctl_auto_groundspeed_sum_err, V_CTL_AUTO_GROUNDSPEED_MAX_SUM_ERR);
v_ctl_auto_airspeed_controlled = (err_groundspeed + v_ctl_auto_groundspeed_sum_err * v_ctl_auto_groundspeed_igain) *
v_ctl_auto_groundspeed_pgain;
// Do not allow controlled airspeed below the setpoint
if (v_ctl_auto_airspeed_controlled < v_ctl_auto_airspeed_setpoint_slew) {
v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew;
// reset integrator of ground speed loop
v_ctl_auto_groundspeed_sum_err = v_ctl_auto_airspeed_controlled / (v_ctl_auto_groundspeed_pgain *
v_ctl_auto_groundspeed_igain);
}
#else
v_ctl_auto_airspeed_controlled = v_ctl_auto_airspeed_setpoint_slew;
#endif
// Airspeed outerloop: positive means we need to accelerate
float speed_error = v_ctl_auto_airspeed_controlled - stateGetAirspeed_f();
// Speed Controller to PseudoControl: gain 1 -> 5m/s error = 0.5g acceleration
v_ctl_desired_acceleration = speed_error * v_ctl_airspeed_pgain / 9.81f;
BoundAbs(v_ctl_desired_acceleration, v_ctl_max_acceleration);
// Actual Acceleration from IMU: attempt to reconstruct the actual kinematic acceleration
#ifndef SITL
struct Int32Vect3 accel_meas_body;
struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu);
int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel);
float vdot = ACCEL_FLOAT_OF_BFP(accel_meas_body.x) / 9.81f - sinf(stateGetNedToBodyEulers_f()->theta);
#else
float vdot = 0;
#endif
// Acceleration Error: positive means UAV needs to accelerate: needs extra energy
float vdot_err = low_pass_vdot(v_ctl_desired_acceleration - vdot);
// Flight Path Outerloop: positive means needs to climb more: needs extra energy
float gamma_err = (v_ctl_climb_setpoint - stateGetSpeedEnu_f()->z) / v_ctl_auto_airspeed_controlled;
// Total Energy Error: positive means energy should be added
float en_tot_err = gamma_err + vdot_err;
// Energy Distribution Error: positive means energy should go from overspeed to altitude = pitch up
float en_dis_err = gamma_err - vdot_err;
// Auto Cruise Throttle
if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) {
v_ctl_auto_throttle_nominal_cruise_throttle +=
v_ctl_auto_throttle_of_airspeed_igain * speed_error * dt_attidude
+ en_tot_err * v_ctl_energy_total_igain * dt_attidude;
Bound(v_ctl_auto_throttle_nominal_cruise_throttle, 0.0f, 1.0f);
}
// Total Controller
float controlled_throttle = v_ctl_auto_throttle_nominal_cruise_throttle
+ v_ctl_auto_throttle_climb_throttle_increment * v_ctl_climb_setpoint
+ v_ctl_auto_throttle_of_airspeed_pgain * speed_error
+ v_ctl_energy_total_pgain * en_tot_err;
if ((controlled_throttle >= 1.0f) || (controlled_throttle <= 0.0f) || (kill_throttle == 1)) {
// If your energy supply is not sufficient, then neglect the climb requirement
en_dis_err = -vdot_err;
// adjust climb_setpoint to maintain airspeed in case of an engine failure or an unrestriced climb
if (v_ctl_climb_setpoint > 0) { v_ctl_climb_setpoint += - 30. * dt_attidude; }
if (v_ctl_climb_setpoint < 0) { v_ctl_climb_setpoint += 30. * dt_attidude; }
}
/* pitch pre-command */
if (launch && (v_ctl_mode >= V_CTL_MODE_AUTO_CLIMB)) {
v_ctl_auto_throttle_nominal_cruise_pitch += v_ctl_auto_pitch_of_airspeed_igain * (-speed_error) * dt_attidude
+ v_ctl_energy_diff_igain * en_dis_err * dt_attidude;
Bound(v_ctl_auto_throttle_nominal_cruise_pitch, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT);
}
float v_ctl_pitch_of_vz =
+ (v_ctl_climb_setpoint /*+ d_err * v_ctl_auto_throttle_pitch_of_vz_dgain*/) * v_ctl_auto_throttle_pitch_of_vz_pgain
- v_ctl_auto_pitch_of_airspeed_pgain * speed_error
+ v_ctl_auto_pitch_of_airspeed_dgain * vdot
+ v_ctl_energy_diff_pgain * en_dis_err
+ v_ctl_auto_throttle_nominal_cruise_pitch;
if (kill_throttle) { v_ctl_pitch_of_vz = v_ctl_pitch_of_vz - 1 / V_CTL_GLIDE_RATIO; }
v_ctl_pitch_setpoint = v_ctl_pitch_of_vz + nav_pitch;
Bound(v_ctl_pitch_setpoint, H_CTL_PITCH_MIN_SETPOINT, H_CTL_PITCH_MAX_SETPOINT)
ac_char_update(controlled_throttle, v_ctl_pitch_of_vz, v_ctl_climb_setpoint, v_ctl_desired_acceleration);
v_ctl_throttle_setpoint = TRIM_UPPRZ(controlled_throttle * MAX_PPRZ);
}
static void reset_psi_ref_from_body(void) {
stab_att_ref_euler.psi = stateGetNedToBodyEulers_f()->psi;
stab_att_ref_rate.r = 0;
stab_att_ref_accel.r = 0;
}