void imu_init(void) { /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); /* Compute quaternion and rotation matrix for conversions between body and imu frame */ #if defined IMU_BODY_TO_IMU_PHI && defined IMU_BODY_TO_IMU_THETA & defined IMU_BODY_TO_IMU_PSI struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALISE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); #else INT32_QUAT_ZERO(imu.body_to_imu_quat); INT32_RMAT_ZERO(imu.body_to_imu_rmat); #endif imu_impl_init(); }
// reset to "hover" setpoint static void reset_sp_quat(int32_t _psi, int32_t _theta, struct Int32Quat *initial) { int32_t pitch_rotation_angle; struct Int32Quat pitch_axis_quat; struct Int32Quat pitch_rotated_quat, pitch_rotated_quat2; struct Int32Vect3 y_axis = { 0, 1, 0 }; struct Int32Eulers rotated_eulers; // compose rotation about Y axis (pitch axis) from hover pitch_rotation_angle = ANGLE_BFP_OF_REAL(-QUAT_SETPOINT_HOVER_PITCH); INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle); INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat, *initial, pitch_axis_quat); INT32_EULERS_OF_QUAT(rotated_eulers, pitch_rotated_quat); // reset euler angles rotated_eulers.theta = _theta; rotated_eulers.phi = _psi; INT32_QUAT_OF_EULERS(pitch_rotated_quat, rotated_eulers); // compose rotation about Y axis (pitch axis) to hover pitch_rotation_angle = ANGLE_BFP_OF_REAL(QUAT_SETPOINT_HOVER_PITCH); INT32_QUAT_OF_AXIS_ANGLE(pitch_axis_quat, y_axis, pitch_rotation_angle); INT32_QUAT_COMP_NORM_SHORTEST(pitch_rotated_quat2, pitch_rotated_quat, pitch_axis_quat); // store result into setpoint QUAT_COPY(stab_att_sp_quat, pitch_rotated_quat2); }
void imu_init(void) { /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); #if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); #else #if USE_MAGNETOMETER #pragma message "Info: Magnetomter neutrals are set to zero!" #endif INT_VECT3_ZERO(imu.mag_neutral); #endif /* Compute quaternion and rotation matrix for conversions between body and imu frame */ struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALIZE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); imu_impl_init(); }
void imu_init(void) { #ifdef IMU_POWER_GPIO gpio_setup_output(IMU_POWER_GPIO); IMU_POWER_GPIO_ON(IMU_POWER_GPIO); #endif /* initialises neutrals */ RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL); VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL); #if defined IMU_MAG_X_NEUTRAL && defined IMU_MAG_Y_NEUTRAL && defined IMU_MAG_Z_NEUTRAL VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL); #else #if USE_MAGNETOMETER INFO("Magnetometer neutrals are set to zero, you should calibrate!") #endif INT_VECT3_ZERO(imu.mag_neutral); #endif /* Compute quaternion and rotation matrix for conversions between body and imu frame */ struct Int32Eulers body_to_imu_eulers = { ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PHI), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_THETA), ANGLE_BFP_OF_REAL(IMU_BODY_TO_IMU_PSI) }; INT32_QUAT_OF_EULERS(imu.body_to_imu_quat, body_to_imu_eulers); INT32_QUAT_NORMALIZE(imu.body_to_imu_quat); INT32_RMAT_OF_EULERS(imu.body_to_imu_rmat, body_to_imu_eulers); #if PERIODIC_TELEMETRY register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); #if USE_IMU_FLOAT #else // !USE_IMU_FLOAT register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_RAW", send_accel_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL_SCALED", send_accel_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_ACCEL", send_accel); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_RAW", send_gyro_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO_SCALED", send_gyro_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_GYRO", send_gyro); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_RAW", send_mag_raw); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG_SCALED", send_mag_scaled); register_periodic_telemetry(DefaultPeriodic, "IMU_MAG", send_mag); #endif // !USE_IMU_FLOAT #endif // DOWNLINK imu_impl_init(); }
/** Read attitude setpoint from RC as euler angles. * @param[in] in_flight true if in flight * @param[out] sp attitude setpoint as euler angles */ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight) { sp->phi = ((int32_t) radio_control.values[RADIO_ROLL] * SP_MAX_PHI / MAX_PPRZ); sp->theta = ((int32_t) radio_control.values[RADIO_PITCH] * SP_MAX_THETA / MAX_PPRZ); if (in_flight) { if (YAW_DEADBAND_EXCEEDED()) { sp->psi += ((int32_t) radio_control.values[RADIO_YAW] * SP_MAX_R / MAX_PPRZ / RC_UPDATE_FREQ); INT32_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 int32_t delta_psi = sp->psi - stateGetNedToBodyEulers_i()->psi; int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT); INT32_ANGLE_NORMALIZE(delta_psi); if (delta_psi > delta_limit){ sp->psi = stateGetNedToBodyEulers_i()->psi + delta_limit; } else if (delta_psi < -delta_limit){ sp->psi = stateGetNedToBodyEulers_i()->psi - delta_limit; } INT32_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. int32_t cos_psi; int32_t sin_psi; int32_t temp_theta; int32_t care_free_delta_psi_i; care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading); INT32_ANGLE_NORMALIZE(care_free_delta_psi_i); PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i); PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i); temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC); sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC); sp->theta = temp_theta; } } else { /* if not flying, use current yaw as setpoint */ sp->psi = stateGetNedToBodyEulers_i()->psi; } }
static int32_t get_rc_yaw(void) { const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R); int32_t yaw = radio_control.values[RADIO_YAW]; DeadBand(yaw, STABILIZATION_ATTITUDE_DEADBAND_R); return yaw * max_rc_r / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_R); }
static int32_t get_rc_pitch(void) { const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA); int32_t pitch = radio_control.values[RADIO_PITCH]; #if STABILIZATION_ATTITUDE_DEADBAND_E DeadBand(pitch, STABILIZATION_ATTITUDE_DEADBAND_E); return pitch * max_rc_theta / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_E); #else return pitch * max_rc_theta / MAX_PPRZ; #endif }
static int32_t get_rc_roll(void) { const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI); int32_t roll = radio_control.values[RADIO_ROLL]; #if STABILIZATION_ATTITUDE_DEADBAND_A DeadBand(roll, STABILIZATION_ATTITUDE_DEADBAND_A); return roll * max_rc_phi / (MAX_PPRZ - STABILIZATION_ATTITUDE_DEADBAND_A); #else return roll * max_rc_phi / MAX_PPRZ; #endif }
static void test_4_int(void) { printf("euler to quat to euler - int\n"); /* initial euler angles */ struct Int32Eulers _e; EULERS_ASSIGN(_e, ANGLE_BFP_OF_REAL(RadOfDeg(-10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("euler orig ", _e); /* transform to quaternion */ struct Int32Quat _q; INT32_QUAT_OF_EULERS(_q, _e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("quat1 ", _q); // INT32_QUAT_NORMALIZE(_q); // DISPLAY_INT32_QUAT_2("_q_n", _q); /* back to eulers */ struct Int32Eulers _e2; INT32_EULERS_OF_QUAT(_e2, _q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("back to euler ", _e2); }
void aos_run(void) { aos_compute_state(); aos_compute_sensors(); #ifndef DISABLE_ALIGNEMENT if (ahrs.status == AHRS_UNINIT) { ahrs_aligner_run(); if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED) { ahrs_align(); } } else { #endif /* DISABLE_ALIGNEMENT */ ahrs_propagate(aos.dt); ahrs_update_accel(); #ifndef DISABLE_MAG_UPDATE ahrs_update_mag(); #endif #if AHRS_USE_GPS_HEADING #if AHRS_TYPE == AHRS_TYPE_ICQ int32_t heading = ANGLE_BFP_OF_REAL(aos.heading_meas); #endif #if AHRS_TYPE == AHRS_TYPE_FCQ float heading = aos.heading_meas; #endif #if AHRS_TYPE == AHRS_TYPE_FCR ahrs_impl.gps_course = aos.heading_meas; ahrs_impl.gps_course_valid = true; #else if (aos.time > 10) { if (!ahrs_impl.heading_aligned) { ahrs_realign_heading(heading); } else { RunOnceEvery(100, ahrs_update_heading(heading)); } } #endif #endif // AHRS_USE_GPS_HEADING #ifndef DISABLE_ALIGNEMENT } #endif }
void camera_mount_run(void) { // floating point version of DCM00 const float dcm22 = TRIG_FLOAT_OF_BFP(ahrs.ltp_to_body_rmat.m[8]); // hover "pitch", derived from ltp to body rmat, rotated by +90 about Y axis int32_t hover_pitch = ANGLE_BFP_OF_REAL(-asinf(dcm22)); commands[COMMAND_CAMERA] = CAMERA_MOUNT_HOVER + (hover_pitch * CAMERA_MOUNT_GAIN_NUM / CAMERA_MOUNT_GAIN_DEN); switch (guidance_h_mode) { case GUIDANCE_H_MODE_TOYTRONICS_FORWARD: case GUIDANCE_H_MODE_TOYTRONICS_AEROBATIC: commands[COMMAND_CAMERA] = CAMERA_MOUNT_FORWARD; break; default: break; // other modes do nothing, leave previous command in place } // add in user setpoint from transmitter commands[COMMAND_CAMERA] += radio_control.values[CAMERA_RADIO_CHANNEL]; Bound(commands[COMMAND_CAMERA], MIN_PPRZ, MAX_PPRZ); }
void guidance_flip_run(void) { uint32_t timer; int32_t phi; static uint32_t timer_save = 0; timer = (flip_counter++ << 12) / PERIODIC_FREQUENCY; phi = stateGetNedToBodyEulers_i()->phi; switch (flip_state) { case 0: flip_cmd_earth.x = 0; flip_cmd_earth.y = 0; stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth, heading_save); stabilization_attitude_run(autopilot_in_flight); stabilization_cmd[COMMAND_THRUST] = 8000; //Thrust to go up first timer_save = 0; if (timer > BFP_OF_REAL(FIRST_THRUST_DURATION, 12)) { flip_state++; } break; case 1: stabilization_cmd[COMMAND_ROLL] = 9000; // Rolling command stabilization_cmd[COMMAND_PITCH] = 0; stabilization_cmd[COMMAND_YAW] = 0; stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust? if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) { flip_state++; } break; case 2: stabilization_cmd[COMMAND_ROLL] = 0; stabilization_cmd[COMMAND_PITCH] = 0; stabilization_cmd[COMMAND_YAW] = 0; stabilization_cmd[COMMAND_THRUST] = 1000; //Min thrust? if (phi > ANGLE_BFP_OF_REAL(RadOfDeg(-110.0)) && phi < ANGLE_BFP_OF_REAL(RadOfDeg(STOP_ROLL_CMD_ANGLE))) { timer_save = timer; flip_state++; } break; case 3: flip_cmd_earth.x = 0; flip_cmd_earth.y = 0; stabilization_attitude_set_earth_cmd_i(&flip_cmd_earth, heading_save); stabilization_attitude_run(autopilot_in_flight); stabilization_cmd[COMMAND_THRUST] = FINAL_THRUST_LEVEL; //Thrust to stop falling if ((timer - timer_save) > BFP_OF_REAL(0.5, 12)) { flip_state++; } break; default: autopilot_mode_auto2 = autopilot_mode_old; autopilot_set_mode(autopilot_mode_old); stab_att_sp_euler.psi = heading_save; flip_rollout = false; flip_counter = 0; timer_save = 0; flip_state = 0; stabilization_cmd[COMMAND_ROLL] = 0; stabilization_cmd[COMMAND_PITCH] = 0; stabilization_cmd[COMMAND_YAW] = 0; stabilization_cmd[COMMAND_THRUST] = 8000; //Some thrust to come out of the roll? break; } }
/** Read attitude setpoint from RC as euler angles * @param[in] coordinated_turn true if in horizontal mode forward * @param[in] in_carefree true if in carefree mode * @param[in] in_flight true if in flight * @param[out] sp attitude setpoint as euler angles */ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { const int32_t max_rc_phi = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI); const int32_t max_rc_theta = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA); const int32_t max_rc_r = (int32_t) ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_R); sp->phi = (int32_t) ((radio_control.values[RADIO_ROLL] * max_rc_phi) / MAX_PPRZ); sp->theta = (int32_t) ((radio_control.values[RADIO_PITCH] * max_rc_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 += (int32_t) ((radio_control.values[RADIO_YAW] * max_rc_r) / MAX_PPRZ / RC_UPDATE_FREQ); INT32_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 int32_t omega; const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(85.0)); if(abs(sp->phi) < max_phi) omega = ANGLE_BFP_OF_REAL(1.3*tanf(ANGLE_FLOAT_OF_BFP(sp->phi))); else //max 60 degrees roll, then take constant omega omega = ANGLE_BFP_OF_REAL(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 const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT); int32_t heading = stabilization_attitude_get_heading_i(); int32_t delta_psi = sp->psi - heading; INT32_ANGLE_NORMALIZE(delta_psi); if (delta_psi > delta_limit){ sp->psi = heading + delta_limit; } else if (delta_psi < -delta_limit){ sp->psi = heading - delta_limit; } INT32_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. int32_t cos_psi; int32_t sin_psi; int32_t temp_theta; int32_t care_free_delta_psi_i; care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading); INT32_ANGLE_NORMALIZE(care_free_delta_psi_i); PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i); PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i); temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC); sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC); sp->theta = temp_theta; } } else { /* if not flying, use current yaw as setpoint */ sp->psi = stateGetNedToBodyEulers_i()->psi; } }
/** * 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) }
static void test_3(void) { /* Compute BODY to IMU eulers */ struct Int32Eulers b2i_e; EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e); /* Compute BODY to IMU quaternion */ struct Int32Quat b2i_q; INT32_QUAT_OF_EULERS(b2i_q, b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); // INT32_QUAT_NORMALIZE(b2i_q); // DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q); /* Compute BODY to IMU rotation matrix */ struct Int32RMat b2i_r; INT32_RMAT_OF_EULERS(b2i_r, b2i_e); // DISPLAY_INT32_RMAT("b2i_r", b2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r); /* Compute LTP to IMU eulers */ struct Int32Eulers l2i_e; EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e); /* Compute LTP to IMU quaternion */ struct Int32Quat l2i_q; INT32_QUAT_OF_EULERS(l2i_q, l2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q); /* Compute LTP to IMU rotation matrix */ struct Int32RMat l2i_r; INT32_RMAT_OF_EULERS(l2i_r, l2i_e); // DISPLAY_INT32_RMAT("l2i_r", l2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r); /* again but from quaternion */ struct Int32RMat l2i_r2; INT32_RMAT_OF_QUAT(l2i_r2, l2i_q); // DISPLAY_INT32_RMAT("l2i_r2", l2i_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2); /* Compute LTP to BODY quaternion */ struct Int32Quat l2b_q; INT32_QUAT_COMP_INV(l2b_q, b2i_q, l2i_q); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q); /* Compute LTP to BODY rotation matrix */ struct Int32RMat l2b_r; INT32_RMAT_COMP_INV(l2b_r, l2i_r, b2i_r); // DISPLAY_INT32_RMAT("l2b_r", l2b_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r); /* again but from quaternion */ struct Int32RMat l2b_r2; INT32_RMAT_OF_QUAT(l2b_r2, l2b_q); // DISPLAY_INT32_RMAT("l2b_r2", l2b_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2); /* compute LTP to BODY eulers */ struct Int32Eulers l2b_e; INT32_EULERS_OF_RMAT(l2b_e, l2b_r); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e); /* again but from quaternion */ struct Int32Eulers l2b_e2; INT32_EULERS_OF_QUAT(l2b_e2, l2b_q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2); }
/** Read attitude setpoint from RC as euler angles * @param[in] coordinated_turn true if in horizontal mode forward * @param[in] in_carefree true if in carefree mode * @param[in] in_flight true if in flight * @param[out] sp attitude setpoint as euler angles */ void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eulers *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(); sp->theta = get_rc_pitch(); 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() * dt; INT32_ANGLE_NORMALIZE(sp->psi); } if (coordinated_turn) { //Coordinated turn //feedforward estimate angular rotation omega = g*tan(phi)/v int32_t omega; const int32_t max_phi = ANGLE_BFP_OF_REAL(RadOfDeg(60.0)); if (abs(sp->phi) < max_phi) { omega = ANGLE_BFP_OF_REAL(9.81 / COORDINATED_TURN_AIRSPEED * tanf(ANGLE_FLOAT_OF_BFP(sp->phi))); } else { //max 60 degrees roll omega = ANGLE_BFP_OF_REAL(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 const int32_t delta_limit = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_PSI_DELTA_LIMIT); int32_t heading = stabilization_attitude_get_heading_i(); int32_t delta_psi = sp->psi - heading; INT32_ANGLE_NORMALIZE(delta_psi); if (delta_psi > delta_limit) { sp->psi = heading + delta_limit; } else if (delta_psi < -delta_limit) { sp->psi = heading - delta_limit; } INT32_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. int32_t cos_psi; int32_t sin_psi; int32_t temp_theta; int32_t care_free_delta_psi_i; care_free_delta_psi_i = sp->psi - ANGLE_BFP_OF_REAL(care_free_heading); INT32_ANGLE_NORMALIZE(care_free_delta_psi_i); PPRZ_ITRIG_SIN(sin_psi, care_free_delta_psi_i); PPRZ_ITRIG_COS(cos_psi, care_free_delta_psi_i); temp_theta = INT_MULT_RSHIFT(cos_psi, sp->theta, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->phi, INT32_ANGLE_FRAC); sp->phi = INT_MULT_RSHIFT(cos_psi, sp->phi, INT32_ANGLE_FRAC) - INT_MULT_RSHIFT(sin_psi, sp->theta, INT32_ANGLE_FRAC); sp->theta = temp_theta; } } else { /* if not flying, use current yaw as setpoint */ sp->psi = stateGetNedToBodyEulers_i()->psi; } /* update timestamp for dt calculation */ last_ts = get_sys_time_float(); }
static void test_3(void) { /* Compute BODY to IMU eulers */ struct Int32Eulers b2i_e; EULERS_ASSIGN(b2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(10.66)), ANGLE_BFP_OF_REAL(RadOfDeg(-0.7)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("b2i_e", b2i_e); /* Compute BODY to IMU quaternion */ struct Int32Quat b2i_q; int32_quat_of_eulers(&b2i_q, &b2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q", b2i_q); // int32_quat_normalize(&b2i_q); // DISPLAY_INT32_QUAT_AS_EULERS_DEG("b2i_q_n", b2i_q); /* Compute BODY to IMU rotation matrix */ struct Int32RMat b2i_r; int32_rmat_of_eulers(&b2i_r, &b2i_e); // DISPLAY_INT32_RMAT("b2i_r", b2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("b2i_r", b2i_r); /* Compute LTP to IMU eulers */ struct Int32Eulers l2i_e; EULERS_ASSIGN(l2i_e, ANGLE_BFP_OF_REAL(RadOfDeg(0.)), ANGLE_BFP_OF_REAL(RadOfDeg(20.)), ANGLE_BFP_OF_REAL(RadOfDeg(0.))); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2i_e", l2i_e); /* Compute LTP to IMU quaternion */ struct Int32Quat l2i_q; int32_quat_of_eulers(&l2i_q, &l2i_e); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2i_q", l2i_q); /* Compute LTP to IMU rotation matrix */ struct Int32RMat l2i_r; int32_rmat_of_eulers(&l2i_r, &l2i_e); // DISPLAY_INT32_RMAT("l2i_r", l2i_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r", l2i_r); /* again but from quaternion */ struct Int32RMat l2i_r2; int32_rmat_of_quat(&l2i_r2, &l2i_q); // DISPLAY_INT32_RMAT("l2i_r2", l2i_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2i_r2", l2i_r2); /* Compute LTP to BODY quaternion */ struct Int32Quat l2b_q; int32_quat_comp_inv(&l2b_q, &b2i_q, &l2i_q); DISPLAY_INT32_QUAT_AS_EULERS_DEG("l2b_q", l2b_q); /* Compute LTP to BODY rotation matrix */ struct Int32RMat l2b_r; int32_rmat_comp_inv(&l2b_r, &l2i_r, &b2i_r); // DISPLAY_INT32_RMAT("l2b_r", l2b_r); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r); /* again but from quaternion */ struct Int32RMat l2b_r2; int32_rmat_of_quat(&l2b_r2, &l2b_q); // DISPLAY_INT32_RMAT("l2b_r2", l2b_r2); DISPLAY_INT32_RMAT_AS_EULERS_DEG("l2b_r2", l2b_r2); /* compute LTP to BODY eulers */ struct Int32Eulers l2b_e; int32_eulers_of_rmat(&l2b_e, &l2b_r); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e", l2b_e); /* again but from quaternion */ struct Int32Eulers l2b_e2; int32_eulers_of_quat(&l2b_e2, &l2b_q); DISPLAY_INT32_EULERS_AS_FLOAT_DEG("l2b_e2", l2b_e2); }