diff --git a/conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile b/conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile new file mode 100644 index 0000000000..9a2087c811 --- /dev/null +++ b/conf/firmwares/subsystems/rotorcraft/ahrs_extern_quat.makefile @@ -0,0 +1,17 @@ +# +# AHRS wrapper for AHRS devices, such as GX3 or UM6 +# 2013, Utah State University, http://aggieair.usu.edu/ + +AHRS_CFLAGS = -DUSE_AHRS + +ifneq ($(AHRS_ALIGNER_LED),none) + AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED) +endif + +AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_extern_quat.h\" +AHRS_SRCS += subsystems/ahrs.c +AHRS_SRCS += subsystems/ahrs/ahrs_aligner.c +AHRS_SRCS += subsystems/ahrs/ahrs_extern_quat.c + +ap.CFLAGS += $(AHRS_CFLAGS) +ap.srcs += $(AHRS_SRCS) diff --git a/conf/firmwares/subsystems/shared/imu_um6.makefile b/conf/firmwares/subsystems/shared/imu_um6.makefile new file mode 100644 index 0000000000..e91c8c6220 --- /dev/null +++ b/conf/firmwares/subsystems/shared/imu_um6.makefile @@ -0,0 +1,20 @@ +# Attitude estimation for fixedwings and rotorcrafts via CHR-UM6 +# 2012, Utah State University, http://aggieair.usu.edu/ + +ifndef UM6_PORT + UM6_PORT=UART3 +endif +ifndef UM6_BAUD + UM6_BAUD=B115200 +endif + +IMU_UM6_CFLAGS += -DUSE_IMU +IMU_UM6_CFLAGS += -DIMU_TYPE_H=\"imu/imu_um6.h\" +IMU_UM6_SRCS += $(SRC_SUBSYSTEMS)/imu.c +IMU_UM6_SRCS += $(SRC_SUBSYSTEMS)/imu/imu_um6.c + +IMU_UM6_CFLAGS += -DUSE_$(UM6_PORT) -D$(UM6_PORT)_BAUD=$(UM6_BAUD) +IMU_UM6_CFLAGS += -DUM6_LINK=$(UM6_PORT) + +ap.CFLAGS += $(IMU_UM6_CFLAGS) +ap.srcs += $(IMU_UM6_SRCS) diff --git a/sw/airborne/subsystems/ahrs/ahrs_extern_quat.c b/sw/airborne/subsystems/ahrs/ahrs_extern_quat.c new file mode 100644 index 0000000000..b778af0233 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_extern_quat.c @@ -0,0 +1,85 @@ +/* + * Copyright (C) 2013 Michal Podhradsky + * Utah State University, http://aggieair.usu.edu/ + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + /** + * @file subsystems/ahrs/ahrs_extern_quat.c + * + * AHRS interface for multiple IMU/AHRS subsystems, such as GX3, UM6 etc. + * + * Propagates the estimated attitude and rates from IMU to body states. Quaternion + * calculation is used. + * + * @author Michal Podhradsky + */ +#include "ahrs_extern_quat.h" +#include "mcu_periph/sys_time.h" +#include "led.h" + +struct AhrsIntExternQuat ahrs_impl; + +void ahrs_init(void) { + ahrs.status = AHRS_UNINIT; + + /* set ltp_to_imu so that body is zero */ + QUAT_COPY(ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); + INT_RATES_ZERO(ahrs_impl.imu_rate); + + #ifdef IMU_MAG_OFFSET + ahrs_impl.mag_offset = IMU_MAG_OFFSET; + #else + ahrs_impl.mag_offset = 0.; + #endif + + //Needed to set orientations + ahrs.status = AHRS_RUNNING; + + #ifdef AHRS_ALIGNER_LED + LED_ON(AHRS_ALIGNER_LED); + #endif +} + +void ahrs_propagate(void) { + /* Compute LTP to BODY quaternion */ + struct Int32Quat ltp_to_body_quat; + INT32_QUAT_COMP_INV(ltp_to_body_quat, ahrs_impl.ltp_to_imu_quat, imu.body_to_imu_quat); + stateSetNedToBodyQuat_i(<p_to_body_quat); + + // TODO: compensate for magnetic offset + + struct Int32Rates body_rate; + ahrs_impl.imu_rate = imu.gyro; + /* compute body rates */ + INT32_RMAT_TRANSP_RATEMULT(body_rate, imu.body_to_imu_rmat, ahrs_impl.imu_rate); + /* Set state */ + stateSetBodyRates_i(&body_rate); +} + +void ahrs_align(void) { +} + +void ahrs_update_accel(void) { +} + +void ahrs_update_mag(void) { +} + +void ahrs_update_gps(void) { +} diff --git a/sw/airborne/subsystems/ahrs/ahrs_extern_quat.h b/sw/airborne/subsystems/ahrs/ahrs_extern_quat.h new file mode 100644 index 0000000000..74a415f387 --- /dev/null +++ b/sw/airborne/subsystems/ahrs/ahrs_extern_quat.h @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2013 Michal Podhradsky + * Utah State University, http://aggieair.usu.edu/ + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + /** + * @file subsystems/ahrs/ahrs_extern_quat.h + * + * AHRS interface for multiple IMU/AHRS subsystems, such as GX3, UM6 etc. + * + * Propagates the estimated attitude and rates from IMU to body states. Quaternion + * calculation is used. + * + * @author Michal Podhradsky + */ +#ifndef AHRS_EXTERN_QUAT_H +#define AHRS_EXTERN_QUAT_H + +#include "state.h" +#include "subsystems/ahrs.h" +#include "subsystems/imu.h" + +struct AhrsIntExternQuat { + struct Int32Eulers ltp_to_imu_euler; ///< Rotation from LocalTangentPlane to IMU frame as Euler angles + struct Int32Quat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as quaternions + struct Int32Rates imu_rate; ///< Rotational velocity in IMU frame + float mag_offset; +}; + +extern struct AhrsIntExternQuat ahrs_impl; + +#endif /* AHRS_EXTERN_QUAT_H */ diff --git a/sw/airborne/subsystems/imu/imu_um6.c b/sw/airborne/subsystems/imu/imu_um6.c new file mode 100644 index 0000000000..00b3ba017a --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_um6.c @@ -0,0 +1,358 @@ +/* + * Copyright (C) 2013 Michal Podhradsky + * Utah State University, http://aggieair.usu.edu/ + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + /** + * @file imu_um6.c + * + * Driver for CH Robotics UM6 IMU/AHRS subsystem + * + * Takes care of configuration of the IMU, communication and parsing + * the received packets. See UM6 datasheet for configuration options. + * Should be used with @ahrs_extern_euler AHRS subsystem. + * + * @author Michal Podhradsky + */ +#include "subsystems/imu/imu_um6.h" +#include "subsystems/imu.h" +#include "mcu_periph/sys_time.h" + +struct UM6Packet UM6_packet; +uint8_t buf_out[IMU_UM6_BUFFER_LENGTH]; +uint16_t data_chk; + +uint8_t PacketLength; +uint8_t PacketType; +uint8_t PacketAddr; +enum UM6Status UM6_status; + +uint16_t chk_calc; +uint16_t chk_rec; + +struct FloatVect3 UM6_accel; +struct FloatRates UM6_rate; +struct FloatVect3 UM6_mag; +struct FloatEulers UM6_eulers; +struct FloatQuat UM6_quat; + +inline void UM6_imu_align(void); +inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length); +inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length); +inline bool_t UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length); + +inline bool_t UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length) { + chk_rec = (packet_buffer[packet_length-2] << 8) | packet_buffer[packet_length-1]; + chk_calc = UM6_calculate_checksum(packet_buffer, packet_length-2); + return (chk_calc == chk_rec); +} + +inline uint16_t UM6_calculate_checksum(uint8_t packet_buffer[], uint8_t packet_length) { + uint16_t chk = 0; + for (int i=0; i> 8); + buf_out[6] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 7); + + // Magnetic realign + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0; + buf_out[4] = IMU_UM6_SET_ACCEL_REF; + data_chk = UM6_calculate_checksum(buf_out, 5); + buf_out[5] = (uint8_t)(data_chk >> 8); + buf_out[6] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 7); + + // Zero gyros 0xAC, takes around 3s + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0; + buf_out[4] = IMU_UM6_ZERO_GYROS_CMD; + data_chk = UM6_calculate_checksum(buf_out, 5); + buf_out[5] = (uint8_t)(data_chk >> 8); + buf_out[6] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 7); + + // Reset EKF 0xAD + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0; + buf_out[4] = IMU_UM6_RESET_EKF_CMD; + data_chk = UM6_calculate_checksum(buf_out, 5); + buf_out[5] = (uint8_t)(data_chk >> 8); + buf_out[6] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 7); + + UM6_status = UM6Running; +} + +void imu_impl_init(void) { + // Initialize variables + UM6_status = UM6Uninit; + + // Initialize packet + UM6_packet.status = UM6PacketWaiting; + UM6_packet.msg_idx = 0; + UM6_packet.msg_available = FALSE; + UM6_packet.chksm_error = 0; + UM6_packet.hdr_error = 0; + + // Communication register 0x00 + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0x80; // has data, zeros + buf_out[4] = IMU_UM6_COMMUNICATION_REG; // communication register address + buf_out[5] = 0b01000110; // B3 - broadcast enabled, processed data enabled (acc, gyro, mag) + buf_out[6] = 0b10000000; // B2 - no euler angles, quaternions enabled + buf_out[7] = 0b00101101; // B1 - baud 115200 + buf_out[8] = 100; // B0 - broadcast rate, 1=20Hz, 100=130Hz, 255=300Hz + data_chk = UM6_calculate_checksum(buf_out, 9); + buf_out[9] = (uint8_t)(data_chk >> 8); + buf_out[10] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 11); + + // Config register 0x01 + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0x80; // has data, zeros + buf_out[4] = IMU_UM6_MISC_CONFIG_REG; // config register address + buf_out[5] = 0b11110000; // B3 - mag, accel updates enabled, use quaternions + buf_out[6] = 0; // B2 - RES + buf_out[7] = 0; // B1 - RES + buf_out[8] = 0; // B0 - RES + data_chk = UM6_calculate_checksum(buf_out, 9); + buf_out[9] = (uint8_t)(data_chk >> 8); + buf_out[10] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 11); + + /* + // Get firmware + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0; + buf_out[4] = IMU_UM6_GET_FIRMWARE_CMD; + data_chk = UM6_calculate_checksum(buf_out, 5); + buf_out[5] = (uint8_t)(data_chk >> 8); + buf_out[6] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 7); + */ + + UM6_imu_align(); +} + + + +void imu_periodic(void) { + if (ins.vf_realign == TRUE) { + UM6_imu_align(); + } + + /* We would request for data here - optional + //GET_DATA command 0xAE + buf_out[0] = 's'; + buf_out[1] = 'n'; + buf_out[2] = 'p'; + buf_out[3] = 0; // zeros + buf_out[4] = 0xAE; // GET_DATA command + data_chk = UM6_calculate_checksum(buf_out, 5); + buf_out[5] = (uint8_t)(data_chk >> 8); + buf_out[6] = (uint8_t)data_chk; + UM6_send_packet(buf_out, 7); + */ +} + +void UM6_packet_read_message(void) { + if ((UM6_status == UM6Running) && PacketLength > 11) { + switch (PacketAddr) { + case IMU_UM6_GYRO_PROC: + UM6_rate.p = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+0]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+1])))*0.0610352; + UM6_rate.q = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+2]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+3])))*0.0610352; + UM6_rate.r = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+4]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+5])))*0.0610352; + RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec + RATES_BFP_OF_REAL(imu.gyro, UM6_rate); + break; + case IMU_UM6_ACCEL_PROC: + UM6_accel.x = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+0]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+1])))*0.000183105; + UM6_accel.y = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+2]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+3])))*0.000183105; + UM6_accel.z = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+4]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+5])))*0.000183105; + VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2 + ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int + break; + case IMU_UM6_MAG_PROC: + UM6_mag.x = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+0]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+1])))*0.000305176; + UM6_mag.y = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+2]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+3])))*0.000305176; + UM6_mag.z = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+4]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+5])))*0.000305176; + // Assume the same units for magnetic field + // Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss). + MAGS_BFP_OF_REAL(imu.mag, UM6_mag); + break; + case IMU_UM6_EULER: + UM6_eulers.phi = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+0]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+1])))*0.0109863; + UM6_eulers.theta = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+2]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+3])))*0.0109863; + UM6_eulers.psi = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+4]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+5])))*0.0109863; + EULERS_SMUL(UM6_eulers,UM6_eulers, 0.0174532925); //Convert deg to rad + EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers); + break; + case IMU_UM6_QUAT: + UM6_quat.qi = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+0]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+1])))*0.0000335693; + UM6_quat.qx = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+2]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+3])))*0.0000335693; + UM6_quat.qy = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+4]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+5])))*0.0000335693; + UM6_quat.qz = + ((float)((int16_t) + ((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+6]<<8)|UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET+7])))*0.0000335693; + QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat); + break; + default: + break; + } + } +} + +/* UM6 Packet Collection */ +void UM6_packet_parse( uint8_t c ) { + switch (UM6_packet.status) { + case UM6PacketWaiting: + UM6_packet.msg_idx = 0; + if (c == 's') { + UM6_packet.status = UM6PacketReadingS; + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + } else { + UM6_packet.hdr_error++; + UM6_packet.status = UM6PacketWaiting; + } + break; + case UM6PacketReadingS: + if (c == 'n') { + UM6_packet.status = UM6PacketReadingN; + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + } else { + UM6_packet.hdr_error++; + UM6_packet.status = UM6PacketWaiting; + } + break; + case UM6PacketReadingN: + if (c == 'p') { + UM6_packet.status = UM6PacketReadingPT; + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + } else { + UM6_packet.hdr_error++; + UM6_packet.status = UM6PacketWaiting; + } + break; + case UM6PacketReadingPT: + PacketType = c; + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + UM6_packet.status = UM6PacketReadingAddr; + if ((PacketType & 0xC0) == 0xC0) { + PacketLength = 4*((PacketType >>2) & 0xF) + 7; // Batch, has 4*BatchLength bytes of data + } + else if ((PacketType & 0xC0) == 0x80) { + PacketLength = 11; // Not batch, has 4 bytes of data + } + else if ((PacketType & 0xC0) == 0x00) { + PacketLength = 7; // Not batch, no data + } + break; + case UM6PacketReadingAddr: + PacketAddr = c; + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + UM6_packet.status = UM6PacketReadingData; + break; + case UM6PacketReadingData: + UM6_packet.msg_buf[UM6_packet.msg_idx] = c; + UM6_packet.msg_idx++; + if (UM6_packet.msg_idx == PacketLength) { + if (UM6_verify_chk(UM6_packet.msg_buf, PacketLength)) { + UM6_packet.msg_available = TRUE; + } else { + UM6_packet.msg_available = FALSE; + UM6_packet.chksm_error++; + } + UM6_packet.status = UM6PacketWaiting; + } + break; + default: + UM6_packet.status = UM6PacketWaiting; + UM6_packet.msg_idx = 0; + break; + } +} diff --git a/sw/airborne/subsystems/imu/imu_um6.h b/sw/airborne/subsystems/imu/imu_um6.h new file mode 100644 index 0000000000..849d3fd7c5 --- /dev/null +++ b/sw/airborne/subsystems/imu/imu_um6.h @@ -0,0 +1,145 @@ +/* + * Copyright (C) 2013 Michal Podhradsky + * Utah State University, http://aggieair.usu.edu/ + * + * This file is part of paparazzi. + * + * paparazzi is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2, or (at your option) + * any later version. + * + * paparazzi is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + /** + * @file imu_um6.h + * + * Driver for CH Robotics UM6 IMU/AHRS subsystem + * + * Takes care of configuration of the IMU, communication and parsing + * the received packets. See UM6 datasheet for configuration options. + * Should be used with @ahrs_extern_euler AHRS subsystem. + * + * @author Michal Podhradsky + */ +#ifndef IMU_UM6_H +#define IMU_UM6_H + +#include "generated/airframe.h" +#include "subsystems/imu.h" +#include "mcu_periph/uart.h" +#include "subsystems/ahrs.h" +#include "subsystems/ins.h" + +#define __UM6Link(dev, _x) dev##_x +#define _UM6Link(dev, _x) __UM6Link(dev, _x) +#define UM6Link(_x) _UM6Link(UM6_LINK, _x) + +#define UM6Buffer() UM6Link(ChAvailable()) + +#ifdef ImuScaleGyro +#undef ImuScaleGyro +#endif +#define ImuScaleGyro(_imu) {} + +#ifdef ImuScaleAccel +#undef ImuScaleAccel +#endif +#define ImuScaleAccel(_imu) {} + +#ifdef ImuScaleMag +#undef ImuScaleMag +#endif +#define ImuScaleMag(_imu) {} + +#define IMU_UM6_BUFFER_LENGTH 32 +#define IMU_UM6_DATA_OFFSET 5 +#define IMU_UM6_LONG_DELAY 4000000 + +#define IMU_UM6_COMMUNICATION_REG 0x00 +#define IMU_UM6_MISC_CONFIG_REG 0x01 +#define IMU_UM6_GET_FIRMWARE_CMD 0xAA +#define IMU_UM6_ZERO_GYROS_CMD 0xAC +#define IMU_UM6_RESET_EKF_CMD 0xAD +#define IMU_UM6_GET_DATA 0xAE +#define IMU_UM6_SET_ACCEL_REF 0xAF +#define IMU_UM6_SET_MAG_REF 0xB0 + +#define IMU_UM6_GYRO_PROC 0x5C +#define IMU_UM6_ACCEL_PROC 0x5E +#define IMU_UM6_MAG_PROC 0x60 +#define IMU_UM6_EULER 0x62 +#define IMU_UM6_QUAT 0x64 + +extern void UM6_packet_read_message(void); +extern void UM6_packet_parse(uint8_t c); + +extern struct UM6Packet UM6_packet; + +extern uint8_t PacketLength; +extern uint8_t PacketType; +extern uint8_t PacketAddr; + +extern uint16_t chk_calc; +extern uint16_t chk_rec; + +extern enum UM6Status UM6_status; +extern volatile uint8_t UM6_imu_available; + +extern struct FloatEulers UM6_eulers; +extern struct FloatQuat UM6_quat; + +struct UM6Packet { + bool_t msg_available; + uint32_t chksm_error; + uint32_t hdr_error; + uint8_t msg_buf[IMU_UM6_BUFFER_LENGTH]; + uint8_t status; + uint8_t msg_idx; +}; + +enum UM6PacketStatus { + UM6PacketWaiting, + UM6PacketReadingS, + UM6PacketReadingN, + UM6PacketReadingPT, + UM6PacketReadingAddr, + UM6PacketReadingData +}; + +enum UM6Status { + UM6Uninit, + UM6Running +}; + +#define imu_um6_event(_callback1, _callback2, _callback3) { \ + if (UM6Buffer()) { \ + ReadUM6Buffer(); \ + } \ + if (UM6_packet.msg_available) { \ + UM6_packet.msg_available = FALSE; \ + UM6_packet_read_message(); \ + _callback1(); \ + _callback2(); \ + _callback3(); \ + } \ +} + +#define ReadUM6Buffer() { \ + while (UM6Link(ChAvailable())&&!UM6_packet.msg_available) \ + UM6_packet_parse(UM6Link(Getch())); \ + } + +#define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) { \ + imu_um6_event(_gyro_handler, _accel_handler, _mag_handler); \ +} + +#endif /* IMU_UM6_H*/