bool_t nav_spiral_setup(uint8_t center_wp, uint8_t edge_wp, float radius_start, float radius_inc, float segments) { VECT2_COPY(nav_spiral.center, waypoints[center_wp]); // center of the helix nav_spiral.center.z = waypoints[center_wp].a; nav_spiral.radius_start = radius_start; // start radius of the helix nav_spiral.segments = segments; nav_spiral.radius_min = NAV_SPIRAL_MIN_CIRCLE_RADIUS; if (nav_spiral.radius_start < nav_spiral.radius_min) nav_spiral.radius_start = nav_spiral.radius_min; nav_spiral.radius_increment = radius_inc; // multiplier for increasing the spiral struct FloatVect2 edge; VECT2_DIFF(edge, waypoints[edge_wp], nav_spiral.center); FLOAT_VECT2_NORM(nav_spiral.radius, edge); // get a copy of the current position struct EnuCoor_f pos_enu; memcpy(&pos_enu, stateGetPositionEnu_f(), sizeof(struct EnuCoor_f)); VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center); nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z; nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current); // nav_spiral.alpha_limit denotes angle, where the radius will be increased nav_spiral.alpha_limit = 2*M_PI / nav_spiral.segments; //current position nav_spiral.fly_from.x = stateGetPositionEnu_f()->x; nav_spiral.fly_from.y = stateGetPositionEnu_f()->y; if(nav_spiral.dist_from_center > nav_spiral.radius) nav_spiral.status = SpiralOutside; return FALSE; }
void ahrs_update_accel(void) { /* last column of roation matrix = ltp z-axis in imu-frame */ struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2) }; struct FloatVect3 imu_accel_float; ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel); struct FloatVect3 residual; if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame * a_c_body = omega x (omega x r) * (omega x r) = tangential velocity in body frame * a_c_body = omega x vel_tangential_body * assumption: tangential velocity only along body x-axis */ const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0}; struct FloatVect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body); /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravitiy vector */ struct FloatVect3 corrected_gravity; VECT3_DIFF(corrected_gravity, imu_accel_float, acc_c_imu); /* compute the residual of gravity vector in imu frame */ FLOAT_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2); } else { FLOAT_VECT3_CROSS_PRODUCT(residual, imu_accel_float, c2); } #ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC /* heuristic on acceleration norm */ const float acc_norm = FLOAT_VECT3_NORM(imu_accel_float); const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.); #else const float weight = 1.; #endif /* compute correction */ const float gravity_rate_update_gain = -5e-2; // -5e-2 FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain); const float gravity_bias_update_gain = 1e-5; // -5e-6 FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain); /* FIXME: saturate bias */ }
void ahrs_update_accel(void) { struct FloatVect3 imu_g; ACCELS_FLOAT_OF_BFP(imu_g, imu.accel); const float alpha = 0.92; ahrs_impl.lp_accel = alpha * ahrs_impl.lp_accel + (1. - alpha) *(FLOAT_VECT3_NORM(imu_g) - 9.81); const struct FloatVect3 earth_g = {0., 0., -9.81 }; const float dn = 250*fabs( ahrs_impl.lp_accel ); struct FloatVect3 g_noise = {1.+dn, 1.+dn, 1.+dn}; update_state(&earth_g, &imu_g, &g_noise); reset_state(); }
void ahrs_update_accel(void) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN float v = FLOAT_VECT3_NORM(ahrs_impl.est_ltp_speed); accel_float.y -= v * ahrs_float.imu_rate.r; accel_float.z -= -v * ahrs_float.imu_rate.q; #endif struct FloatVect3 c2 = { RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,2), RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,2), RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)}; struct FloatVect3 residual; FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, c2); #ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC /* heuristic on acceleration norm */ const float acc_norm = FLOAT_VECT3_NORM(accel_float); const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.); #else const float weight = 1.; #endif /* compute correction */ const float gravity_rate_update_gain = -5e-2; // -5e-2 FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain); #if 1 const float gravity_bias_update_gain = 1e-5; // -5e-6 FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain); #else const float alpha = 5e-4; FLOAT_RATES_SCALE(ahrs_impl.gyro_bias, 1.-alpha); FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, alpha); #endif /* FIXME: saturate bias */ }
void ahrs_update_accel(void) { struct FloatVect3 accel_float; ACCELS_FLOAT_OF_BFP(accel_float, imu.accel); struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0)); struct FloatVect3 residual; FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, (*r2)); /* heuristic on acceleration norm */ const float acc_norm = FLOAT_VECT3_NORM(accel_float); const float weight = Chop(1.-2*fabs(1-acc_norm/9.81), 0., 1.); //const float weight = 1.; /* compute correction */ const float gravity_rate_update_gain = 5e-2; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain); const float gravity_bias_update_gain = -5e-6; FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain); /* FIXME: saturate bias */ }
void aos_compute_sensors(void) { struct FloatRates gyro; RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias); // printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r)); float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise); RATES_BFP_OF_REAL(imu.gyro, gyro); RATES_BFP_OF_REAL(imu.gyro_prev, gyro); struct FloatVect3 g_ltp = {0., 0., 9.81}; struct FloatVect3 accelero_ltp; VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp); struct FloatVect3 accelero_imu; FLOAT_QUAT_VMULT(accelero_imu, aos.ltp_to_imu_quat, accelero_ltp); float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise); ACCELS_BFP_OF_REAL(imu.accel, accelero_imu); struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z}; struct FloatVect3 h_imu; FLOAT_QUAT_VMULT(h_imu, aos.ltp_to_imu_quat, h_earth); MAGS_BFP_OF_REAL(imu.mag, h_imu); #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN #if AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ VECT3_COPY(ahrs_impl.est_ltp_speed, aos.ltp_vel); #endif #if AHRS_TYPE == AHRS_TYPE_ICQ ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel)); #endif #endif }
bool nav_spiral_run(void) { struct EnuCoor_f pos_enu = *stateGetPositionEnu_f(); VECT2_DIFF(nav_spiral.trans_current, pos_enu, nav_spiral.center); nav_spiral.dist_from_center = FLOAT_VECT3_NORM(nav_spiral.trans_current); float DistanceStartEstim; float CircleAlpha; switch (nav_spiral.status) { case SpiralOutside: //flys until center of the helix is reached an start helix nav_route_xy(nav_spiral.fly_from.x, nav_spiral.fly_from.y, nav_spiral.center.x, nav_spiral.center.y); // center reached? if (nav_approaching_xy(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.fly_from.x, nav_spiral.fly_from.y, 0)) { // nadir image #ifdef DIGITAL_CAM dc_send_command(DC_SHOOT); #endif nav_spiral.status = SpiralStartCircle; } break; case SpiralStartCircle: // Starts helix // storage of current coordinates // calculation needed, State switch to SpiralCircle nav_circle_XY(nav_spiral.center.y, nav_spiral.center.y, nav_spiral.radius_start); if (nav_spiral.dist_from_center >= nav_spiral.radius_start) { VECT2_COPY(nav_spiral.last_circle, pos_enu); nav_spiral.status = SpiralCircle; // Start helix #ifdef DIGITAL_CAM dc_Circle(360 / nav_spiral.segments); #endif } break; case SpiralCircle: { nav_circle_XY(nav_spiral.center.x, nav_spiral.center.y, nav_spiral.radius_start); // Trigonometrische Berechnung des bereits geflogenen Winkels alpha // equation: // alpha = 2 * asin ( |Starting position angular - current positon| / (2* nav_spiral.radius_start) // if alphamax already reached, increase radius. //DistanceStartEstim = |Starting position angular - current positon| struct FloatVect2 pos_diff; VECT2_DIFF(pos_diff, nav_spiral.last_circle, pos_enu); DistanceStartEstim = float_vect2_norm(&pos_diff); CircleAlpha = (2.0 * asin(DistanceStartEstim / (2 * nav_spiral.radius_start))); if (CircleAlpha >= nav_spiral.alpha_limit) { VECT2_COPY(nav_spiral.last_circle, pos_enu); nav_spiral.status = SpiralInc; } break; } case SpiralInc: // increasing circle radius as long as it is smaller than max helix radius if (nav_spiral.radius_start + nav_spiral.radius_increment < nav_spiral.radius) { nav_spiral.radius_start = nav_spiral.radius_start + nav_spiral.radius_increment; #ifdef DIGITAL_CAM if (dc_cam_tracing) { // calculating Cam angle for camera alignment nav_spiral.trans_current.z = stateGetPositionUtm_f()->alt - nav_spiral.center.z; dc_cam_angle = atan(nav_spiral.radius_start / nav_spiral.trans_current.z) * 180 / M_PI; } #endif } else { nav_spiral.radius_start = nav_spiral.radius; #ifdef DIGITAL_CAM // Stopps DC dc_stop(); #endif } nav_spiral.status = SpiralCircle; break; default: break; } NavVerticalAutoThrottleMode(0.); /* No pitch */ NavVerticalAltitudeMode(nav_spiral.center.z, 0.); /* No preclimb */ return true; }
void ahrs_update_accel(void) { /* last column of roation matrix = ltp z-axis in imu-frame */ struct FloatVect3 c2 = { RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 0,2), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 1,2), RMAT_ELMT(ahrs_impl.ltp_to_imu_rmat, 2,2)}; struct FloatVect3 imu_accel_float; ACCELS_FLOAT_OF_BFP(imu_accel_float, imu.accel); struct FloatVect3 residual; struct FloatVect3 pseudo_gravity_measurement; if (ahrs_impl.correct_gravity && ahrs_impl.ltp_vel_norm_valid) { /* * centrifugal acceleration in body frame * a_c_body = omega x (omega x r) * (omega x r) = tangential velocity in body frame * a_c_body = omega x vel_tangential_body * assumption: tangential velocity only along body x-axis */ const struct FloatVect3 vel_tangential_body = {ahrs_impl.ltp_vel_norm, 0.0, 0.0}; struct FloatVect3 acc_c_body; VECT3_RATES_CROSS_VECT3(acc_c_body, *stateGetBodyRates_f(), vel_tangential_body); /* convert centrifugal acceleration from body to imu frame */ struct FloatVect3 acc_c_imu; FLOAT_RMAT_VECT3_MUL(acc_c_imu, ahrs_impl.body_to_imu_rmat, acc_c_body); /* and subtract it from imu measurement to get a corrected measurement of the gravity vector */ VECT3_DIFF(pseudo_gravity_measurement, imu_accel_float, acc_c_imu); } else { VECT3_COPY(pseudo_gravity_measurement, imu_accel_float); } FLOAT_VECT3_CROSS_PRODUCT(residual, pseudo_gravity_measurement, c2); /* FIR filtered pseudo_gravity_measurement */ #define FIR_FILTER_SIZE 8 static struct FloatVect3 filtered_gravity_measurement = {0., 0., 0.}; VECT3_SMUL(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE-1); VECT3_ADD(filtered_gravity_measurement, pseudo_gravity_measurement); VECT3_SDIV(filtered_gravity_measurement, filtered_gravity_measurement, FIR_FILTER_SIZE); if (ahrs_impl.gravity_heuristic_factor) { /* heuristic on acceleration (gravity estimate) norm */ /* Factor how strongly to change the weight. * e.g. for gravity_heuristic_factor 30: * <0.66G = 0, 1G = 1.0, >1.33G = 0 */ const float g_meas_norm = FLOAT_VECT3_NORM(filtered_gravity_measurement) / 9.81; ahrs_impl.weight = 1.0 - ahrs_impl.gravity_heuristic_factor * fabs(1.0 - g_meas_norm) / 10.0; Bound(ahrs_impl.weight, 0.15, 1.0); } else { ahrs_impl.weight = 1.0; } /* Complementary filter proportional gain. * Kp = 2 * zeta * omega * weight * AHRS_PROPAGATE_FREQUENCY / AHRS_CORRECT_FREQUENCY */ const float gravity_rate_update_gain = -2 * ahrs_impl.accel_zeta * ahrs_impl.accel_omega * ahrs_impl.weight * AHRS_PROPAGATE_FREQUENCY / (AHRS_CORRECT_FREQUENCY * 9.81); FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, gravity_rate_update_gain); /* Complementary filter integral gain * Correct the gyro bias. * Ki = (omega*weight)^2/AHRS_CORRECT_FREQUENCY */ const float gravity_bias_update_gain = ahrs_impl.accel_omega * ahrs_impl.accel_omega * ahrs_impl.weight * ahrs_impl.weight / (AHRS_CORRECT_FREQUENCY * 9.81); FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, gravity_bias_update_gain); /* FIXME: saturate bias */ }
void quat_from_earth_cmd_f(struct FloatQuat *quat, struct FloatVect2 *cmd, float heading) { /* cmd_x is positive to north = negative pitch * cmd_y is positive to east = positive roll * * orientation vector describing simultaneous rotation of roll/pitch */ const struct FloatVect3 ov = {cmd->y, -cmd->x, 0.0}; /* quaternion from that orientation vector */ struct FloatQuat q_rp; float_quat_of_orientation_vect(&q_rp, &ov); /* as rotation matrix */ struct FloatRMat R_rp; float_rmat_of_quat(&R_rp, &q_rp); /* body x-axis (before heading command) is first column */ struct FloatVect3 b_x; VECT3_ASSIGN(b_x, R_rp.m[0], R_rp.m[3], R_rp.m[6]); /* body z-axis (thrust vect) is last column */ struct FloatVect3 thrust_vect; VECT3_ASSIGN(thrust_vect, R_rp.m[2], R_rp.m[5], R_rp.m[8]); /// @todo optimize yaw angle calculation /* * Instead of using the psi setpoint angle to rotate around the body z-axis, * calculate the real angle needed to align the projection of the body x-axis * onto the horizontal plane with the psi setpoint. * * angle between two vectors a and b: * angle = atan2(norm(cross(a,b)), dot(a,b)) * sign(dot(cross(a,b), n)) * where the normal n is the thrust vector (i.e. both a and b lie in that plane) */ // desired heading vect in earth x-y plane const struct FloatVect3 psi_vect = {cosf(heading), sinf(heading), 0.0}; /* projection of desired heading onto body x-y plane * b = v - dot(v,n)*n */ float dot = VECT3_DOT_PRODUCT(psi_vect, thrust_vect); struct FloatVect3 dotn; VECT3_SMUL(dotn, thrust_vect, dot); // b = v - dot(v,n)*n struct FloatVect3 b; VECT3_DIFF(b, psi_vect, dotn); dot = VECT3_DOT_PRODUCT(b_x, b); struct FloatVect3 cross; VECT3_CROSS_PRODUCT(cross, b_x, b); // norm of the cross product float nc = FLOAT_VECT3_NORM(cross); // angle = atan2(norm(cross(a,b)), dot(a,b)) float yaw2 = atan2(nc, dot) / 2.0; // negative angle if needed // sign(dot(cross(a,b), n) float dot_cross_ab = VECT3_DOT_PRODUCT(cross, thrust_vect); if (dot_cross_ab < 0) { yaw2 = -yaw2; } /* quaternion with yaw command */ struct FloatQuat q_yaw; QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2)); /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */ float_quat_comp(quat, &q_rp, &q_yaw); float_quat_normalize(quat); float_quat_wrap_shortest(quat); }