39 #define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8)
70 chk_calc += (
uint8_t) * buff_add++;
72 return (chk_calc == ((((
uint16_t) * buff_add) << 8) + (
uint8_t) * (buff_add + 1)));
89 #if PERIODIC_TELEMETRY
92 static void send_gx3(
struct transport_tx *trans,
struct link_device *
dev)
94 pprz_msg_send_GX3_INFO(trans, dev, AC_ID,
123 #ifdef GX3_INITIALIZE_DURING_STARTUP
124 #pragma message "GX3 initializing"
141 #define ACC_FILT_DIV 2
142 #define MAG_FILT_DIV 30
143 #ifdef GX3_SAVE_SETTINGS
188 #ifdef GX3_SET_WAKEUP_MODE
212 #if PERIODIC_TELEMETRY
271 #if AHRS_USE_GPS_HEADING && USE_GPS
274 float course_f = (float)DegOfRad(
gps.
course / 1e7);
275 if (course_f > 180.0) {
278 ltp_to_body_eulers.
psi = (float)RadOfDeg(course_f);
280 #else // !AHRS_USE_GPS_HEADING
281 #ifdef IMU_MAG_OFFSET
288 #endif // IMU_MAG_OFFSET
289 #endif // !AHRS_USE_GPS_HEADING
332 #ifdef IMU_MAG_OFFSET
#define RATES_BFP_OF_REAL(_ri, _rf)
static void stateSetNedToBodyRMat_f(struct FloatRMat *ned_to_body_rmat)
Set vehicle body attitude from rotation matrix (float).
static void stateSetNedToBodyEulers_f(struct FloatEulers *ned_to_body_eulers)
Set vehicle body attitude from euler angles (float).
Dispatcher to register actual AHRS implementations.
void imu_scale_gyro(struct Imu *_imu)
uint8_t msg_buf[GX3_MAX_PAYLOAD]
Driver for Microstrain GX3 IMU/AHRS subsystem.
Periodic telemetry system header (includes downlink utility and generated code).
uint8_t uart_getch(struct uart_periph *p)
void imu_scale_accel(struct Imu *_imu)
void float_eulers_of_rmat(struct FloatEulers *e, struct FloatRMat *rm)
uint16_t uart_char_available(struct uart_periph *p)
Check UART for available chars in receive buffer.
static struct FloatRMat * orientationGetRMat_f(struct OrientationReps *orientation)
Get vehicle body attitude rotation matrix (float).
#define IMU_GX3_LONG_DELAY
struct AhrsGX3 ahrs_gx3
Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down...
struct Imu imu
global IMU state
static void send_gx3(struct transport_tx *trans, struct link_device *dev)
float mag_offset
Difference between true and magnetic north.
struct FloatQuat ltp_to_imu_quat
Rotation from LocalTangentPlane to IMU frame as quaternions.
#define ACCELS_BFP_OF_REAL(_ef, _ei)
#define QUAT_COPY(_qo, _qi)
struct FloatVect3 accel
measured acceleration in IMU frame
void float_rmat_comp(struct FloatRMat *m_a2c, struct FloatRMat *m_a2b, struct FloatRMat *m_b2c)
Composition (multiplication) of two rotation matrices.
struct Int32Vect3 accel
accelerometer measurements in m/s^2 in BFP with INT32_ACCEL_FRAC
void ahrs_register_impl(AhrsEnableOutput enable)
Register an AHRS implementation.
void ahrs_gx3_publish_imu(void)
void gx3_packet_parse(uint8_t c)
uint32_t time
GX3 time stamp.
#define DefaultPeriodic
Set default periodic telemetry.
#define FLOAT_RMAT_TRANSP_RATEMULT(_rb, _m_b2a, _ra)
void ahrs_gx3_register(void)
static void ReadGX3Buffer(void)
void ahrs_gx3_align(void)
struct Int32Vect3 mag
magnetometer measurements scaled to 1 in BFP with INT32_MAG_FRAC
struct OrientationReps body_to_imu
rotation from body to imu frame
static const struct usb_device_descriptor dev
void uart_put_byte(struct uart_periph *p, long fd, uint8_t data)
Uart transmit implementation.
#define GX3_CHKSM(_ubx_payload)
#define VECT3_SMUL(_vo, _vi, _s)
void imu_scale_mag(struct Imu *_imu)
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
struct FloatRMat rmat
measured attitude in IMU frame (rotational matrix)
void imu_gx3_periodic(void)
struct GX3Packet packet
Packet struct.
uint16_t chksm
aux variable for checksum
void gx3_packet_read_message(void)
abstract IMU interface providing fixed point interface
static float bef(volatile uint8_t *c)
struct FloatRates rate
measured angular rates in IMU frame
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static void stateSetBodyRates_f(struct FloatRates *body_rate)
Set vehicle body angular rate (float).
static bool gx3_verify_chk(volatile uint8_t *buff_add)
struct GpsState gps
global GPS state
uint32_t ltime
aux time stamp
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
static struct FloatQuat * orientationGetQuat_f(struct OrientationReps *orientation)
Get vehicle body attitude quaternion (float).
struct Int32Rates gyro
gyroscope measurements in rad/s in BFP with INT32_RATE_FRAC