static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *gains, struct Int32Rates *ref_accel)
{
/* Compute feedforward based on reference acceleration */
ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x * RATE_FLOAT_OF_BFP(ref_accel->p) / (1 << 7);
ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y * RATE_FLOAT_OF_BFP(ref_accel->q) / (1 << 7);
ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z * RATE_FLOAT_OF_BFP(ref_accel->r) / (1 << 7);
}
static void passthrough_up_parse(struct AutopilotMessagePTUp *msg_up)
{
if (msg_up->valid.vane && vane_callback)
vane_callback(0, msg_up->vane_angle1, msg_up->vane_angle2);
// Fill pressure data
if (msg_up->valid.pressure_absolute && pressure_absolute_callback)
pressure_absolute_callback(0, msg_up->pressure_absolute);
if (msg_up->valid.pressure_differential && pressure_differential_callback)
pressure_differential_callback(0, (32768 + msg_up->pressure_differential));
if (msg_up->valid.adc) {
if(adc_callback) {
adc_callback(msg_up->adc.channels);
}
}
// Fill radio data
if (msg_up->valid.rc && radio_control_callback) {
radio_control.values[RADIO_ROLL] = msg_up->rc_roll;
radio_control.values[RADIO_PITCH] = msg_up->rc_pitch;
radio_control.values[RADIO_YAW] = msg_up->rc_yaw;
radio_control.values[RADIO_THROTTLE] = msg_up->rc_thrust;
radio_control.values[RADIO_MODE] = msg_up->rc_mode;
radio_control.values[RADIO_KILL] = msg_up->rc_kill;
radio_control.values[RADIO_GEAR] = msg_up->rc_gear;
radio_control.values[RADIO_AUX2] = msg_up->rc_aux2;
radio_control.values[RADIO_AUX3] = msg_up->rc_aux3;
radio_control_callback();
}
// always fill status, it may change even when in the case when there is no new data
radio_control.status = msg_up->rc_status;
// Fill IMU data
imuFloat.gyro.p = RATE_FLOAT_OF_BFP(msg_up->gyro.p);
imuFloat.gyro.q = RATE_FLOAT_OF_BFP(msg_up->gyro.q);
imuFloat.gyro.r = RATE_FLOAT_OF_BFP(msg_up->gyro.r);
imuFloat.accel.x = ACCEL_FLOAT_OF_BFP(msg_up->accel.x);
imuFloat.accel.y = ACCEL_FLOAT_OF_BFP(msg_up->accel.y);
imuFloat.accel.z = ACCEL_FLOAT_OF_BFP(msg_up->accel.z);
imuFloat.mag.x = MAG_FLOAT_OF_BFP(msg_up->mag.x);
imuFloat.mag.y = MAG_FLOAT_OF_BFP(msg_up->mag.y);
imuFloat.mag.z = MAG_FLOAT_OF_BFP(msg_up->mag.z);
imuFloat.sample_count = msg_up->imu_tick;
if (msg_up->valid.imu)
rdyb_booz_imu_update(&imuFloat);
}
void ahrs_propagate(void) {
struct FloatRates body_rate = { 0., 0., 0. };
#ifdef ADC_CHANNEL_GYRO_P
body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
#endif
#ifdef ADC_CHANNEL_GYRO_Q
body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
#endif
#ifdef ADC_CHANNEL_GYRO_R
body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
#endif
stateSetBodyRates_f(&body_rate);
}
void ArduIMU_event(void)
{
// Handle INS I2C event
if (ardu_ins_trans.status == I2CTransSuccess) {
// received data from I2C transaction
recievedData[0] = (ardu_ins_trans.buf[1] << 8) | ardu_ins_trans.buf[0];
recievedData[1] = (ardu_ins_trans.buf[3] << 8) | ardu_ins_trans.buf[2];
recievedData[2] = (ardu_ins_trans.buf[5] << 8) | ardu_ins_trans.buf[4];
recievedData[3] = (ardu_ins_trans.buf[7] << 8) | ardu_ins_trans.buf[6];
recievedData[4] = (ardu_ins_trans.buf[9] << 8) | ardu_ins_trans.buf[8];
recievedData[5] = (ardu_ins_trans.buf[11] << 8) | ardu_ins_trans.buf[10];
recievedData[6] = (ardu_ins_trans.buf[13] << 8) | ardu_ins_trans.buf[12];
recievedData[7] = (ardu_ins_trans.buf[15] << 8) | ardu_ins_trans.buf[14];
recievedData[8] = (ardu_ins_trans.buf[17] << 8) | ardu_ins_trans.buf[16];
// Update ArduIMU data
arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]) - ins_roll_neutral;
arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]) - ins_pitch_neutral;
arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]);
arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]);
arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]);
arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]);
arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]);
arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]);
arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]);
// Update estimator
stateSetNedToBodyEulers_f(&arduimu_eulers);
stateSetBodyRates_f(&arduimu_rates);
stateSetAccelNed_f(&((struct NedCoor_f)arduimu_accel));
ardu_ins_trans.status = I2CTransDone;
#ifdef ARDUIMU_SYNC_SEND
// uint8_t arduimu_id = 102;
//RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi, &arduimu_id));
RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, DefaultDevice, &arduimu_rates.p, &arduimu_rates.q,
&arduimu_rates.r));
RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, DefaultDevice, &arduimu_accel.x, &arduimu_accel.y,
&arduimu_accel.z));
#endif
} else if (ardu_ins_trans.status == I2CTransFailed) {
ardu_ins_trans.status = I2CTransDone;
}
// Handle GPS I2C event
if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) {
ardu_gps_trans.status = I2CTransDone;
}
}
void ArduIMU_event( void ) {
// Handle INS I2C event
if (ardu_ins_trans.status == I2CTransSuccess) {
// received data from I2C transaction
recievedData[0] = (ardu_ins_trans.buf[1]<<8) | ardu_ins_trans.buf[0];
recievedData[1] = (ardu_ins_trans.buf[3]<<8) | ardu_ins_trans.buf[2];
recievedData[2] = (ardu_ins_trans.buf[5]<<8) | ardu_ins_trans.buf[4];
recievedData[3] = (ardu_ins_trans.buf[7]<<8) | ardu_ins_trans.buf[6];
recievedData[4] = (ardu_ins_trans.buf[9]<<8) | ardu_ins_trans.buf[8];
recievedData[5] = (ardu_ins_trans.buf[11]<<8) | ardu_ins_trans.buf[10];
recievedData[6] = (ardu_ins_trans.buf[13]<<8) | ardu_ins_trans.buf[12];
recievedData[7] = (ardu_ins_trans.buf[15]<<8) | ardu_ins_trans.buf[14];
recievedData[8] = (ardu_ins_trans.buf[17]<<8) | ardu_ins_trans.buf[16];
// Update ArduIMU data
arduimu_eulers.phi = ANGLE_FLOAT_OF_BFP(recievedData[0]);
arduimu_eulers.theta = ANGLE_FLOAT_OF_BFP(recievedData[1]);
arduimu_eulers.psi = ANGLE_FLOAT_OF_BFP(recievedData[2]);
arduimu_rates.p = RATE_FLOAT_OF_BFP(recievedData[3]);
arduimu_rates.q = RATE_FLOAT_OF_BFP(recievedData[4]);
arduimu_rates.r = RATE_FLOAT_OF_BFP(recievedData[5]);
arduimu_accel.x = ACCEL_FLOAT_OF_BFP(recievedData[6]);
arduimu_accel.y = ACCEL_FLOAT_OF_BFP(recievedData[7]);
arduimu_accel.z = ACCEL_FLOAT_OF_BFP(recievedData[8]);
// Update estimator
estimator_phi = arduimu_eulers.phi - ins_roll_neutral;
estimator_theta = arduimu_eulers.theta - ins_pitch_neutral;
estimator_p = arduimu_rates.p;
ardu_ins_trans.status = I2CTransDone;
//RunOnceEvery(15, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &arduimu_eulers.phi, &arduimu_eulers.theta, &arduimu_eulers.psi));
RunOnceEvery(15, DOWNLINK_SEND_IMU_GYRO(DefaultChannel, &arduimu_rates.p, &arduimu_rates.q, &arduimu_rates.r));
RunOnceEvery(15, DOWNLINK_SEND_IMU_ACCEL(DefaultChannel, &arduimu_accel.x, &arduimu_accel.y, &arduimu_accel.z));
}
else if (ardu_ins_trans.status == I2CTransFailed) {
ardu_ins_trans.status = I2CTransDone;
}
// Handle GPS I2C event
if (ardu_gps_trans.status == I2CTransSuccess || ardu_gps_trans.status == I2CTransFailed) {
ardu_gps_trans.status = I2CTransDone;
}
}
static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err,
struct Int32Rates *rate_err, struct Int32Quat *sum_err)
{
/* PID feedback */
fb_commands[COMMAND_ROLL] =
GAIN_PRESCALER_P * gains->p.x * QUAT1_FLOAT_OF_BFP(att_err->qx) +
GAIN_PRESCALER_D * gains->d.x * RATE_FLOAT_OF_BFP(rate_err->p) +
GAIN_PRESCALER_I * gains->i.x * QUAT1_FLOAT_OF_BFP(sum_err->qx);
fb_commands[COMMAND_PITCH] =
GAIN_PRESCALER_P * gains->p.y * QUAT1_FLOAT_OF_BFP(att_err->qy) +
GAIN_PRESCALER_D * gains->d.y * RATE_FLOAT_OF_BFP(rate_err->q) +
GAIN_PRESCALER_I * gains->i.y * QUAT1_FLOAT_OF_BFP(sum_err->qy);
fb_commands[COMMAND_YAW] =
GAIN_PRESCALER_P * gains->p.z * QUAT1_FLOAT_OF_BFP(att_err->qz) +
GAIN_PRESCALER_D * gains->d.z * RATE_FLOAT_OF_BFP(rate_err->r) +
GAIN_PRESCALER_I * gains->i.z * QUAT1_FLOAT_OF_BFP(sum_err->qz);
}
