void NodeList::append(Node* val){
Elem* bef(0);
Elem* af( d_first );
while( af ){
bef = af;
af = af->d_next;
}
if( bef ){
bef->d_next = new NodeList::Elem( val );
}
else {
d_first = new Elem( val );
d_current = d_first;
}
d_size++;
}
Node* NodeList::remove(){
Node* val = d_current->d_val;
d_size--;
Elem* bef(0);
Elem* af( d_first );
while( af != d_current){
bef = af;
af = af->d_next;
}
if( !bef ){
d_first = d_current->d_next;
}
else {
bef->d_next = d_current->d_next;
}
d_current = d_current->d_next;
af->d_next = 0;
delete af;// the old current - remove only it
if( !d_current ) d_current = d_first;
return val;
}
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
}
