void ahrs_update_infrared(void) {
float phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
float theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
if (theta < -M_PI_2) theta += M_PI;
else if (theta > M_PI_2) theta -= M_PI;
if (phi >= 0) phi *= infrared.correction_right;
else phi *= infrared.correction_left;
if (theta >= 0) theta *= infrared.correction_up;
else theta *= infrared.correction_down;
struct FloatEulers att = { phi, theta, heading };
stateSetNedToBodyEulers_f(&att);
}
void ahrs_infrared_periodic(void)
{
float phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
float theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
if (theta < -M_PI_2) { theta += M_PI; }
else if (theta > M_PI_2) { theta -= M_PI; }
if (phi >= 0) { phi *= infrared.correction_right; }
else { phi *= infrared.correction_left; }
if (theta >= 0) { theta *= infrared.correction_up; }
else { theta *= infrared.correction_down; }
struct FloatEulers att = { phi, theta, heading };
stateSetNedToBodyEulers_f(&att);
}
void parse_ins_msg(void)
{
struct link_device *dev = InsLinkDevice;
while (dev->char_available(dev->periph)) {
uint8_t ch = dev->get_byte(dev->periph);
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
RunOnceEvery(25, LED_TOGGLE(3));
if (CHIMU_DATA.m_MsgID == CHIMU_Msg_3_IMU_Attitude) {
// TODO: remove this flag as it doesn't work with multiple AHRS
new_ins_attitude = 1;
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
//FIXME
ahrs_chimu.is_aligned = true;
if (ahrs_chimu.is_enabled) {
struct FloatEulers att = {
CHIMU_DATA.m_attitude.euler.phi,
CHIMU_DATA.m_attitude.euler.theta,
CHIMU_DATA.m_attitude.euler.psi
};
stateSetNedToBodyEulers_f(&att);
struct FloatRates rates = {
CHIMU_DATA.m_sensor.rate[0],
CHIMU_DATA.m_attrates.euler.theta,
0.
}; // FIXME rate r
stateSetBodyRates_f(&rates);
}
} else if (CHIMU_DATA.m_MsgID == 0x02) {
#if CHIMU_DOWNLINK_IMMEDIATE
RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0],
&CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
#endif
}
}
}
}
/*
* Compute body orientation and rates from imu orientation and rates
*/
static inline void set_body_orientation_and_rates(void) {
struct FloatRates body_rate;
FLOAT_RMAT_TRANSP_RATEMULT(body_rate, ahrs_impl.body_to_imu_rmat, ahrs_impl.imu_rate);
stateSetBodyRates_f(&body_rate);
struct FloatRMat ltp_to_imu_rmat, ltp_to_body_rmat;
FLOAT_RMAT_OF_EULERS(ltp_to_imu_rmat, ahrs_impl.ltp_to_imu_euler);
FLOAT_RMAT_COMP_INV(ltp_to_body_rmat, ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
// Some stupid lines of code for neutrals
struct FloatEulers ltp_to_body_euler;
FLOAT_EULERS_OF_RMAT(ltp_to_body_euler, ltp_to_body_rmat);
ltp_to_body_euler.phi -= ins_roll_neutral;
ltp_to_body_euler.theta -= ins_pitch_neutral;
stateSetNedToBodyEulers_f(<p_to_body_euler);
// should be replaced at the end by:
// stateSetNedToBodyRMat_f(<p_to_body_rmat);
}
void parse_ins_msg(void)
{
while (InsLink(ChAvailable())) {
uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
RunOnceEvery(25, LED_TOGGLE(3));
if (CHIMU_DATA.m_MsgID == CHIMU_Msg_3_IMU_Attitude) {
new_ins_attitude = 1;
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
struct FloatEulers att = {
CHIMU_DATA.m_attitude.euler.phi,
CHIMU_DATA.m_attitude.euler.theta,
CHIMU_DATA.m_attitude.euler.psi
};
stateSetNedToBodyEulers_f(&att);
struct FloatRates rates = {
CHIMU_DATA.m_sensor.rate[0],
CHIMU_DATA.m_attrates.euler.theta,
0.
}; // FIXME rate r
stateSetBodyRates_f(&rates);
//FIXME
ahrs_chimu.is_aligned = TRUE;
} else if (CHIMU_DATA.m_MsgID == 0x02) {
#if CHIMU_DOWNLINK_IMMEDIATE
RunOnceEvery(25, DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0],
&CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
#endif
}
}
}
}
void vn100_event_task(void)
{
if (vn100_trans.status == SPITransSuccess) {
parse_ins_msg();
#ifndef INS_VN100_READ_ONLY
// Update estimator
// FIXME Use a proper rotation matrix here
struct FloatEulers att = {
ins_eulers.phi - ins_roll_neutral,
ins_eulers.theta - ins_pitch_neutral,
ins_eulers.psi
};
stateSetNedToBodyEulers_f(&att);
stateSetBodyRates_f(&ins_rates);
#endif
//uint8_t s = 4+VN100_REG_QMR_SIZE;
//DOWNLINK_SEND_DEBUG(DefaultChannel, DefaultDevice,s,spi_buffer_input);
vn100_trans.status = SPITransDone;
}
if (vn100_trans.status == SPITransFailed) {
vn100_trans.status = SPITransDone;
// FIXME retry config if not done ?
}
}
void ahrs_propagate(void) {
struct NedCoor_f accel;
struct FloatRates body_rates;
struct FloatEulers eulers;
// 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;
INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.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, imu.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
FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat);
#if INS_UPDATE_FW_ESTIMATOR
// Some stupid lines of code for neutrals
eulers.phi -= ins_roll_neutral;
eulers.theta -= ins_pitch_neutral;
stateSetNedToBodyEulers_f(&eulers);
#else
stateSetNedToBodyQuat_f(&ins_impl.state.quat);
#endif
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
FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel);
FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as));
FLOAT_VECT3_ADD(accel, A);
stateSetAccelNed_f(&accel);
//------------------------------------------------------------//
RunOnceEvery(3,{
DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice,
&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)
});
#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
}
void ahrs_ardrone2_propagate(void)
{
int l;
//Recieve the main packet
l = at_com_recieve_navdata(buffer);
navdata_t *main_packet = (navdata_t *) &buffer;
#ifdef ARDRONE2_DEBUG
if (l < 0) {
printf("errno = %d\n", errno);
}
#endif
//When this isn't a valid packet return
if (l < 0 || main_packet->header != NAVDATA_HEADER) {
return;
}
#ifdef ARDRONE2_DEBUG
printf("Read %d\n", l);
dump(buffer, l);
#endif
//Set the state
ahrs_ardrone2.state = main_packet->ardrone_state;
//Init the option
navdata_option_t *navdata_option = (navdata_option_t *) & (main_packet->options[0]);
bool_t full_read = FALSE;
//The possible packets
navdata_demo_t *navdata_demo;
navdata_gps_t *navdata_gps;
navdata_phys_measures_t *navdata_phys_measures;
//Read the navdata until packet is fully readed
while (!full_read && navdata_option->size > 0) {
#ifdef ARDRONE2_DEBUG
printf("tag = %d\n", navdata_option->tag);
#endif
//Check the tag for the right option
switch (navdata_option->tag) {
case 0: //NAVDATA_DEMO
navdata_demo = (navdata_demo_t *) navdata_option;
//Set the AHRS state
ahrs_ardrone2.control_state = navdata_demo->ctrl_state >> 16;
ahrs_ardrone2.eulers.phi = navdata_demo->phi;
ahrs_ardrone2.eulers.theta = navdata_demo->theta;
ahrs_ardrone2.eulers.psi = navdata_demo->psi;
ahrs_ardrone2.speed.x = navdata_demo->vx / 1000;
ahrs_ardrone2.speed.y = navdata_demo->vy / 1000;
ahrs_ardrone2.speed.z = navdata_demo->vz / 1000;
ahrs_ardrone2.altitude = navdata_demo->altitude / 10;
ahrs_ardrone2.battery = navdata_demo->vbat_flying_percentage;
//Set the ned to body eulers
struct FloatEulers angles;
angles.theta = navdata_demo->theta / 180000.*M_PI;
angles.psi = navdata_demo->psi / 180000.*M_PI;
angles.phi = navdata_demo->phi / 180000.*M_PI;
stateSetNedToBodyEulers_f(&angles);
//Update the electrical supply
electrical.vsupply = navdata_demo->vbat_flying_percentage;
break;
case 3: //NAVDATA_PHYS_MEASURES
navdata_phys_measures = (navdata_phys_measures_t *) navdata_option;
//Set the AHRS accel state
INT32_VECT3_SCALE_2(ahrs_ardrone2.accel, navdata_phys_measures->phys_accs, 9.81, 1000)
break;
#ifdef USE_GPS_ARDRONE2
case 27: //NAVDATA_GPS
# ifdef ARDRONE2_DEBUG
dump(navdata_option, navdata_option->size);
# endif
navdata_gps = (navdata_gps_t *) navdata_option;
// Send the data to the gps parser
gps_ardrone2_parse(navdata_gps);
break;
#endif
case 0xFFFF: //CHECKSUM
//TODO: Check the checksum
full_read = TRUE;
break;
default:
#ifdef ARDRONE2_DEBUG
printf("NAVDATA UNKNOWN TAG: %d %d\n", navdata_option->tag, navdata_option->size);
#endif
break;
}
navdata_option = (navdata_option_t *)((uint32_t)navdata_option + navdata_option->size);
}
}
void gx3_packet_read_message(void) {
ahrs_impl.gx3_accel.x = bef(&ahrs_impl.gx3_packet.msg_buf[1]);
ahrs_impl.gx3_accel.y = bef(&ahrs_impl.gx3_packet.msg_buf[5]);
ahrs_impl.gx3_accel.z = bef(&ahrs_impl.gx3_packet.msg_buf[9]);
ahrs_impl.gx3_rate.p = bef(&ahrs_impl.gx3_packet.msg_buf[13]);
ahrs_impl.gx3_rate.q = bef(&ahrs_impl.gx3_packet.msg_buf[17]);
ahrs_impl.gx3_rate.r = bef(&ahrs_impl.gx3_packet.msg_buf[21]);
ahrs_impl.gx3_rmat.m[0] = bef(&ahrs_impl.gx3_packet.msg_buf[25]);
ahrs_impl.gx3_rmat.m[1] = bef(&ahrs_impl.gx3_packet.msg_buf[29]);
ahrs_impl.gx3_rmat.m[2] = bef(&ahrs_impl.gx3_packet.msg_buf[33]);
ahrs_impl.gx3_rmat.m[3] = bef(&ahrs_impl.gx3_packet.msg_buf[37]);
ahrs_impl.gx3_rmat.m[4] = bef(&ahrs_impl.gx3_packet.msg_buf[41]);
ahrs_impl.gx3_rmat.m[5] = bef(&ahrs_impl.gx3_packet.msg_buf[45]);
ahrs_impl.gx3_rmat.m[6] = bef(&ahrs_impl.gx3_packet.msg_buf[49]);
ahrs_impl.gx3_rmat.m[7] = bef(&ahrs_impl.gx3_packet.msg_buf[53]);
ahrs_impl.gx3_rmat.m[8] = bef(&ahrs_impl.gx3_packet.msg_buf[57]);
ahrs_impl.gx3_time = (uint32_t)(ahrs_impl.gx3_packet.msg_buf[61] << 24 |
ahrs_impl.gx3_packet.msg_buf[62] << 16 | ahrs_impl.gx3_packet.msg_buf[63] << 8 | ahrs_impl.gx3_packet.msg_buf[64]);
ahrs_impl.gx3_chksm = GX3_CHKSM(ahrs_impl.gx3_packet.msg_buf);
ahrs_impl.gx3_freq = 62500.0 / (float)(ahrs_impl.gx3_time - ahrs_impl.gx3_ltime);
ahrs_impl.gx3_ltime = ahrs_impl.gx3_time;
// Acceleration
VECT3_SMUL(ahrs_impl.gx3_accel, ahrs_impl.gx3_accel, 9.80665); // Convert g into m/s2
ACCELS_BFP_OF_REAL(imu.accel, ahrs_impl.gx3_accel); // for backwards compatibility with fixed point interface
imuf.accel = ahrs_impl.gx3_accel;
// Rates
struct FloatRates body_rate;
imuf.gyro = ahrs_impl.gx3_rate;
/* compute body rates */
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imuf.body_to_imu);
FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, imuf.gyro);
/* Set state */
stateSetBodyRates_f(&body_rate);
// Attitude
struct FloatRMat ltp_to_body_rmat;
FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_impl.gx3_rmat, *body_to_imu_rmat);
#if AHRS_USE_GPS_HEADING && USE_GPS
struct FloatEulers ltp_to_body_eulers;
float_eulers_of_rmat(<p_to_body_eulers, <p_to_body_rmat);
float course_f = (float)DegOfRad(gps.course / 1e7);
if (course_f > 180.0) {
course_f -= 360.0;
}
ltp_to_body_eulers.psi = (float)RadOfDeg(course_f);
stateSetNedToBodyEulers_f(<p_to_body_eulers);
#else // !AHRS_USE_GPS_HEADING
#ifdef IMU_MAG_OFFSET
struct FloatEulers ltp_to_body_eulers;
float_eulers_of_rmat(<p_to_body_eulers, <p_to_body_rmat);
ltp_to_body_eulers.psi -= ahrs_impl.mag_offset;
stateSetNedToBodyEulers_f(<p_to_body_eulers);
#else
stateSetNedToBodyRMat_f(<p_to_body_rmat);
#endif // IMU_MAG_OFFSET
#endif // !AHRS_USE_GPS_HEADING
}