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);
}
