mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-07 00:53:41 +08:00
[subsystems][imu][ahrs] Drivers for CH Robotic UM6 IMU/AHRS (see www.chrobotics.com).
Generic AHRS for all external AHRS systems (such as GX3 etc) added. closes #347
This commit is contained in:
podhrmic
Felix Ruess
@@ -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)
|
||||
@@ -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)
|
||||
@@ -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 <michal.podhradsky@aggiemail.usu.edu>
|
||||
*/
|
||||
#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) {
|
||||
}
|
||||
@@ -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 <michal.podhradsky@aggiemail.usu.edu>
|
||||
*/
|
||||
#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 */
|
||||
@@ -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 <michal.podhradsky@aggiemail.usu.edu>
|
||||
*/
|
||||
#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<packet_length; i++) {
|
||||
chk += packet_buffer[i];
|
||||
}
|
||||
return chk;
|
||||
}
|
||||
|
||||
inline void UM6_send_packet(uint8_t *packet_buffer, uint8_t packet_length) {
|
||||
for (int i=0; i<packet_length; i++) {
|
||||
UM6Link(Transmit(packet_buffer[i]));
|
||||
}
|
||||
}
|
||||
|
||||
void UM6_imu_align(void) {
|
||||
UM6_status = UM6Uninit;
|
||||
|
||||
// Acceleration vector realign
|
||||
buf_out[0] = 's';
|
||||
buf_out[1] = 'n';
|
||||
buf_out[2] = 'p';
|
||||
buf_out[3] = 0;
|
||||
buf_out[4] = IMU_UM6_SET_MAG_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);
|
||||
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
@@ -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 <michal.podhradsky@aggiemail.usu.edu>
|
||||
*/
|
||||
#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*/
|
||||
Reference in New Issue
Block a user