void ahrs_realign_heading(float heading) {
FLOAT_ANGLE_NORMALIZE(heading);
/* quaternion representing only the heading rotation from ltp to body */
struct FloatQuat q_h_new;
q_h_new.qx = 0.0;
q_h_new.qy = 0.0;
q_h_new.qz = sinf(heading/2.0);
q_h_new.qi = cosf(heading/2.0);
struct FloatQuat* ltp_to_body_quat = stateGetNedToBodyQuat_f();
/* quaternion representing current heading only */
struct FloatQuat q_h;
QUAT_COPY(q_h, *ltp_to_body_quat);
q_h.qx = 0.;
q_h.qy = 0.;
FLOAT_QUAT_NORMALIZE(q_h);
/* quaternion representing rotation from current to new heading */
struct FloatQuat q_c;
FLOAT_QUAT_INV_COMP_NORM_SHORTEST(q_c, q_h, q_h_new);
/* correct current heading in body frame */
struct FloatQuat q;
FLOAT_QUAT_COMP_NORM_SHORTEST(q, q_c, *ltp_to_body_quat);
/* compute ltp to imu rotation quaternion and matrix */
FLOAT_QUAT_COMP(ahrs_impl.ltp_to_imu_quat, q, ahrs_impl.body_to_imu_quat);
FLOAT_RMAT_OF_QUAT(ahrs_impl.ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_quat);
/* set state */
stateSetNedToBodyQuat_f(&q);
ahrs_impl.heading_aligned = TRUE;
}
void ahrs_fc_realign_heading(float heading)
{
FLOAT_ANGLE_NORMALIZE(heading);
/* quaternion representing only the heading rotation from ltp to body */
struct FloatQuat q_h_new;
q_h_new.qx = 0.0;
q_h_new.qy = 0.0;
q_h_new.qz = sinf(heading / 2.0);
q_h_new.qi = cosf(heading / 2.0);
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
/* quaternion representing current heading only */
struct FloatQuat q_h;
QUAT_COPY(q_h, ltp_to_body_quat);
q_h.qx = 0.;
q_h.qy = 0.;
float_quat_normalize(&q_h);
/* quaternion representing rotation from current to new heading */
struct FloatQuat q_c;
float_quat_inv_comp_norm_shortest(&q_c, &q_h, &q_h_new);
/* correct current heading in body frame */
struct FloatQuat q;
float_quat_comp_norm_shortest(&q, &q_c, <p_to_body_quat);
/* compute ltp to imu rotation quaternion and matrix */
float_quat_comp(&ahrs_fc.ltp_to_imu_quat, &q, body_to_imu_quat);
float_rmat_of_quat(&ahrs_fc.ltp_to_imu_rmat, &ahrs_fc.ltp_to_imu_quat);
ahrs_fc.heading_aligned = TRUE;
}
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 / 32.;
stabilization_att_ff_cmd[COMMAND_PITCH] =
stabilization_gains.dd.y * stab_att_ref_accel.q / 32.;
stabilization_att_ff_cmd[COMMAND_YAW] =
stabilization_gains.dd.z * stab_att_ref_accel.r / 32.;
/* 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 / 1024.;
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 / 1024.;
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 / 1024.;
stabilization_cmd[COMMAND_ROLL] =
(stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
stabilization_cmd[COMMAND_PITCH] =
(stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
stabilization_cmd[COMMAND_YAW] =
(stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW])/16.;
}
void stabilization_attitude_read_rc_setpoint_eulers_f(struct FloatEulers *sp, bool_t in_flight) {
sp->phi = (radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ);
sp->theta = (radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ);
if (in_flight) {
if (YAW_DEADBAND_EXCEEDED()) {
sp->psi += (radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ);
FLOAT_ANGLE_NORMALIZE(sp->psi);
}
#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 delta_psi = sp->psi - stateGetNedToBodyEulers_f()->psi;
FLOAT_ANGLE_NORMALIZE(delta_psi);
if (delta_psi > STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
sp->psi = stateGetNedToBodyEulers_f()->psi + STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
else if (delta_psi < -STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT){
sp->psi = stateGetNedToBodyEulers_f()->psi - STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT;
}
FLOAT_ANGLE_NORMALIZE(sp->psi);
#endif
//Care Free mode
if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) {
//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 = sin(care_free_delta_psi_f);
cos_psi = cos(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_ref_update()
{
#if USE_REF
/* dumb integrate reference attitude */
struct FloatRates delta_rate;
RATES_SMUL(delta_rate, stab_att_ref_rate, DT_UPDATE);
struct FloatEulers delta_angle;
EULERS_ASSIGN(delta_angle, delta_rate.p, delta_rate.q, delta_rate.r);
EULERS_ADD(stab_att_ref_euler, delta_angle);
FLOAT_ANGLE_NORMALIZE(stab_att_ref_euler.psi);
/* integrate reference rotational speeds */
struct FloatRates delta_accel;
RATES_SMUL(delta_accel, stab_att_ref_accel, DT_UPDATE);
RATES_ADD(stab_att_ref_rate, delta_accel);
/* compute reference attitude error */
struct FloatEulers ref_err;
EULERS_DIFF(ref_err, stab_att_ref_euler, stab_att_sp_euler);
/* wrap it in the shortest direction */
FLOAT_ANGLE_NORMALIZE(ref_err.psi);
/* compute reference angular accelerations */
stab_att_ref_accel.p = -2.*ZETA_P * OMEGA_P * stab_att_ref_rate.p - OMEGA_P * OMEGA_P * ref_err.phi;
stab_att_ref_accel.q = -2.*ZETA_Q * OMEGA_P * stab_att_ref_rate.q - OMEGA_Q * OMEGA_Q * ref_err.theta;
stab_att_ref_accel.r = -2.*ZETA_R * OMEGA_P * stab_att_ref_rate.r - OMEGA_R * OMEGA_R * ref_err.psi;
/* saturate acceleration */
const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R };
const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R };
RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL);
/* saturate speed and trim accel accordingly */
SATURATE_SPEED_TRIM_ACCEL();
#else /* !USE_REF */
EULERS_COPY(stab_att_ref_euler, stabilization_att_sp);
FLOAT_RATES_ZERO(stab_att_ref_rate);
FLOAT_RATES_ZERO(stab_att_ref_accel);
#endif /* USE_REF */
}
void ahrs_fc_update_heading(float heading)
{
FLOAT_ANGLE_NORMALIZE(heading);
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_fc.body_to_imu);
struct FloatQuat ltp_to_body_quat;
float_quat_comp_inv(<p_to_body_quat, &ahrs_fc.ltp_to_imu_quat, body_to_imu_quat);
struct FloatRMat ltp_to_body_rmat;
float_rmat_of_quat(<p_to_body_rmat, <p_to_body_quat);
// row 0 of ltp_to_body_rmat = body x-axis in ltp frame
// we only consider x and y
struct FloatVect2 expected_ltp = {
RMAT_ELMT(ltp_to_body_rmat, 0, 0),
RMAT_ELMT(ltp_to_body_rmat, 0, 1)
};
// expected_heading cross measured_heading
struct FloatVect3 residual_ltp = {
0,
0,
expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)
};
struct FloatVect3 residual_imu;
float_rmat_vmult(&residual_imu, &ahrs_fc.ltp_to_imu_rmat, &residual_ltp);
const float heading_rate_update_gain = 2.5;
RATES_ADD_SCALED_VECT(ahrs_fc.rate_correction, residual_imu, heading_rate_update_gain);
float heading_bias_update_gain;
/* crude attempt to only update bias if deviation is small
* e.g. needed when you only have gps providing heading
* and the inital heading is totally different from
* the gps course information you get once you have a gps fix.
* Otherwise the bias will be falsely "corrected".
*/
if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.))) {
heading_bias_update_gain = -2.5e-4;
} else {
heading_bias_update_gain = 0.0;
}
RATES_ADD_SCALED_VECT(ahrs_fc.gyro_bias, residual_imu, heading_bias_update_gain);
}
void ahrs_update_heading(float heading) {
FLOAT_ANGLE_NORMALIZE(heading);
struct FloatRMat* ltp_to_body_rmat = stateGetNedToBodyRMat_f();
// row 0 of ltp_to_body_rmat = body x-axis in ltp frame
// we only consider x and y
struct FloatVect2 expected_ltp =
{ RMAT_ELMT(*ltp_to_body_rmat, 0, 0),
RMAT_ELMT(*ltp_to_body_rmat, 0, 1)
};
// expected_heading cross measured_heading
struct FloatVect3 residual_ltp =
{ 0,
0,
expected_ltp.x * sinf(heading) - expected_ltp.y * cosf(heading)
};
struct FloatVect3 residual_imu;
FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_impl.ltp_to_imu_rmat, residual_ltp);
const float heading_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, heading_rate_update_gain);
float heading_bias_update_gain;
/* crude attempt to only update bias if deviation is small
* e.g. needed when you only have gps providing heading
* and the inital heading is totally different from
* the gps course information you get once you have a gps fix.
* Otherwise the bias will be falsely "corrected".
*/
if (fabs(residual_ltp.z) < sinf(RadOfDeg(5.)))
heading_bias_update_gain = -2.5e-4;
else
heading_bias_update_gain = 0.0;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, heading_bias_update_gain);
}
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_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;)
}
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
}
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_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();
}