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); #ifndef DISABLE_MAG_UPDATE 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); #endif aos.heading_meas = aos.ltp_to_imu_euler.psi + get_gaussian_noise() * aos.heading_noise; #ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN #if AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel); ahrs_impl.ltp_vel_norm_valid = true; #endif #if AHRS_TYPE == AHRS_TYPE_FCR2 ahrs_impl.ltp_vel_norm = float_vect3_norm(&aos.ltp_vel); ahrs_impl.ltp_vel_norm_valid = true; #endif #if AHRS_TYPE == AHRS_TYPE_FCR ahrs_impl.gps_speed = float_vect3_norm(&aos.ltp_vel); ahrs_impl.gps_age = 0; ahrs_update_gps(); //RunOnceEvery(100,printf("# gps accel: %f\n", ahrs_impl.gps_acceleration)); #endif #if AHRS_TYPE == AHRS_TYPE_ICQ ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(float_vect3_norm(&aos.ltp_vel)); ahrs_impl.ltp_vel_norm_valid = true; #endif #endif }
float test_quat(void) { struct FloatVect3 u = { 1., 2., 3.}; FLOAT_VECT3_NORMALIZE(u); float angle = RadOfDeg(30.); struct FloatQuat q; FLOAT_QUAT_OF_AXIS_ANGLE(q, u, angle); DISPLAY_FLOAT_QUAT("q ", q); struct FloatQuat iq; FLOAT_QUAT_INVERT(iq, q); DISPLAY_FLOAT_QUAT("iq", iq); struct FloatQuat q1; float_quat_comp(&q1, &q, &iq); DISPLAY_FLOAT_QUAT("q1", q1); struct FloatQuat q2; float_quat_comp(&q2, &q, &iq); DISPLAY_FLOAT_QUAT("q2", q2); struct FloatQuat qe; QUAT_EXPLEMENTARY(qe, q); DISPLAY_FLOAT_QUAT("qe", qe); struct FloatVect3 a = { 2., 1., 3.}; DISPLAY_FLOAT_VECT3("a ", a); struct FloatVect3 a1; float_quat_vmult(&a1, &q, &a); DISPLAY_FLOAT_VECT3("a1", a1); struct FloatVect3 a2; float_quat_vmult(&a2, &qe, &a); DISPLAY_FLOAT_VECT3("a2", a2); return 0.; }
static void test_2(void) { struct Int32Vect3 v1 = { 5000, 5000, 5000 }; DISPLAY_INT32_VECT3("v1", v1); struct FloatEulers euler_f = { RadOfDeg(45.), RadOfDeg(0.), RadOfDeg(0.)}; DISPLAY_FLOAT_EULERS("euler_f", euler_f); struct Int32Eulers euler_i; EULERS_BFP_OF_REAL(euler_i, euler_f); DISPLAY_INT32_EULERS("euler_i", euler_i); struct Int32Quat quat_i; int32_quat_of_eulers(&quat_i, &euler_i); DISPLAY_INT32_QUAT("quat_i", quat_i); int32_quat_normalize(&quat_i); DISPLAY_INT32_QUAT("quat_i_n", quat_i); struct Int32Vect3 v2; int32_quat_vmult(&v2, &quat_i, &v1); DISPLAY_INT32_VECT3("v2", v2); struct Int32RMat rmat_i; int32_rmat_of_quat(&rmat_i, &quat_i); DISPLAY_INT32_RMAT("rmat_i", rmat_i); struct Int32Vect3 v3; INT32_RMAT_VMULT(v3, rmat_i, v1); DISPLAY_INT32_VECT3("v3", v3); struct Int32RMat rmat_i2; int32_rmat_of_eulers(&rmat_i2, &euler_i); DISPLAY_INT32_RMAT("rmat_i2", rmat_i2); struct Int32Vect3 v4; INT32_RMAT_VMULT(v4, rmat_i2, v1); DISPLAY_INT32_VECT3("v4", v4); struct FloatQuat quat_f; float_quat_of_eulers(&quat_f, &euler_f); DISPLAY_FLOAT_QUAT("quat_f", quat_f); struct FloatVect3 v5; VECT3_COPY(v5, v1); DISPLAY_FLOAT_VECT3("v5", v5); struct FloatVect3 v6; float_quat_vmult(&v6, &quat_f, &v5); DISPLAY_FLOAT_VECT3("v6", v6); }
static void traj_bungee_takeoff_update(void) { const float initial_bungee_accel = 20.0; // in m/s^2 const float start = 5; const float duration = 2; struct FloatVect3 accel = {0, 0, 0}; //acceleration in imu x-direction in m/s^2 if (aos.time > start && aos.time < start + duration) { accel.x = initial_bungee_accel * (1 - (aos.time - start) / duration); } else { accel.x = 0; } struct FloatQuat imu2ltp; QUAT_INVERT(imu2ltp, aos.ltp_to_imu_quat); float_quat_vmult(&aos.ltp_accel, &imu2ltp, accel); float_vect3_integrate_fi(&aos.ltp_vel, &aos.ltp_accel, aos.dt); float_vect3_integrate_fi(&aos.ltp_pos, &aos.ltp_vel, aos.dt); }
/** * Run the optical flow on a new image frame * @param[in] *opticflow The opticalflow structure that keeps track of previous images * @param[in] *state The state of the drone * @param[in] *img The image frame to calculate the optical flow from * @param[out] *result The optical flow result */ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_t *state, struct image_t *img, struct opticflow_result_t *result) { // A switch counter that checks in the loop if the current method is similar, // to the previous (for reinitializing structs) static int8_t switch_counter = -1; if (switch_counter != opticflow->method) { opticflow->just_switched_method = true; switch_counter = opticflow->method; } else { opticflow->just_switched_method = false; } // Switch between methods (0 = fast9/lukas-kanade, 1 = EdgeFlow) if (opticflow->method == 0) { calc_fast9_lukas_kanade(opticflow, state, img, result); } else if (opticflow->method == 1) { calc_edgeflow_tot(opticflow, state, img, result); } /* Rotate velocities from camera frame coordinates to body coordinates for control * IMPORTANT!!! This frame to body orientation should be the case for the Parrot * ARdrone and Bebop, however this can be different for other quadcopters * ALWAYS double check! */ result->vel_body_x = result->vel_y; result->vel_body_y = - result->vel_x; // KALMAN filter on velocity float measurement_noise[2] = {result->noise_measurement, 1.0f}; static bool reinitialize_kalman = true; static uint8_t wait_filter_counter = 0; // When starting up the opticalflow module, or switching between methods, wait for a bit to prevent bias if (opticflow->kalman_filter == true) { if (opticflow->just_switched_method == true) { wait_filter_counter = 0; reinitialize_kalman = true; } if (wait_filter_counter > 50) { // Get accelerometer values rotated to body axis // TODO: use acceleration from the state ? struct FloatVect3 accel_imu_f; ACCELS_FLOAT_OF_BFP(accel_imu_f, state->accel_imu_meas); struct FloatVect3 accel_meas_body; float_quat_vmult(&accel_meas_body, &state->imu_to_body_quat, &accel_imu_f); float acceleration_measurement[2]; acceleration_measurement[0] = accel_meas_body.x; acceleration_measurement[1] = accel_meas_body.y; kalman_filter_opticflow_velocity(&result->vel_body_x, &result->vel_body_y, acceleration_measurement, result->fps, measurement_noise, opticflow->kalman_filter_process_noise, reinitialize_kalman); if (reinitialize_kalman) { reinitialize_kalman = false; } } else { wait_filter_counter++; } } else { reinitialize_kalman = true; } }
/** * 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 /* convert last imu accel measurement to float */ struct FloatVect3 accel_imu_f; ACCELS_FLOAT_OF_BFP(accel_imu_f, accel_imu_meas); /* rotate from imu to body frame */ struct FloatVect3 accel_meas_body; float_quat_vmult(&accel_meas_body, &imu_to_body_quat, &accel_imu_f); float vdot = 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 (autopilot.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) || (autopilot_throttle_killed() == 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 (autopilot.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 (autopilot_throttle_killed()) { 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); }
void ahrs_propagate(float dt) { struct FloatVect3 accel; struct FloatRates body_rates; // realign all the filter if needed // a complete init cycle is required if (ins_impl.reset) { ins_impl.reset = FALSE; ins.status = INS_UNINIT; ahrs.status = AHRS_UNINIT; init_invariant_state(); } // fill command vector struct Int32Rates gyro_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, &imu.gyro); RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); // update correction gains error_output(&ins_impl); // propagate model struct inv_state new_state; runge_kutta_4_float((float *)&new_state, (float *)&ins_impl.state, INV_STATE_DIM, (float *)&ins_impl.cmd, INV_COMMAND_DIM, invariant_model, dt); ins_impl.state = new_state; // normalize quaternion FLOAT_QUAT_NORMALIZE(ins_impl.state.quat); // set global state stateSetNedToBodyQuat_f(&ins_impl.state.quat); RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias); stateSetBodyRates_f(&body_rates); stateSetPositionNed_f(&ins_impl.state.pos); stateSetSpeedNed_f(&ins_impl.state.speed); // untilt accel and remove gravity struct FloatQuat q_b2n; float_quat_invert(&q_b2n, &ins_impl.state.quat); float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel); VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as)); VECT3_ADD(accel, A); stateSetAccelNed_f((struct NedCoor_f *)&accel); //------------------------------------------------------------// #if SEND_INVARIANT_FILTER struct FloatEulers eulers; FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); RunOnceEvery(3, { pprz_msg_send_INV_FILTER(trans, dev, AC_ID, &ins_impl.state.quat.qi, &eulers.phi, &eulers.theta, &eulers.psi, &ins_impl.state.speed.x, &ins_impl.state.speed.y, &ins_impl.state.speed.z, &ins_impl.state.pos.x, &ins_impl.state.pos.y, &ins_impl.state.pos.z, &ins_impl.state.bias.p, &ins_impl.state.bias.q, &ins_impl.state.bias.r, &ins_impl.state.as, &ins_impl.state.hb, &ins_impl.meas.baro_alt, &ins_impl.meas.pos_gps.z) }); #endif #if LOG_INVARIANT_FILTER if (pprzLogFile.fs != NULL) { if (!log_started) { // log file header sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n"); log_started = TRUE; } else { sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", ins_impl.cmd.rates.p, ins_impl.cmd.rates.q, ins_impl.cmd.rates.r, ins_impl.cmd.accel.x, ins_impl.cmd.accel.y, ins_impl.cmd.accel.z, ins_impl.meas.pos_gps.x, ins_impl.meas.pos_gps.y, ins_impl.meas.pos_gps.z, ins_impl.meas.speed_gps.x, ins_impl.meas.speed_gps.y, ins_impl.meas.speed_gps.z, ins_impl.meas.mag.x, ins_impl.meas.mag.y, ins_impl.meas.mag.z, ins_impl.meas.baro_alt, ins_impl.state.quat.qi, ins_impl.state.quat.qx, ins_impl.state.quat.qy, ins_impl.state.quat.qz, ins_impl.state.bias.p, ins_impl.state.bias.q, ins_impl.state.bias.r, ins_impl.state.speed.x, ins_impl.state.speed.y, ins_impl.state.speed.z, ins_impl.state.pos.x, ins_impl.state.pos.y, ins_impl.state.pos.z, ins_impl.state.hb, ins_impl.state.as); } } #endif }