Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-30 11:37:06 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[imu] Remove um6

Browse Source
This commit is contained in:
Freek van Tienen
2022-05-18 13:35:04 +02:00
parent 85d6056d37
commit dcca44e591
3 changed files with 0 additions and 528 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/imu_um6.xml
-34
View File
@@ -1,34 +0,0 @@
<!DOCTYPE module SYSTEM "module.dtd">
<module name="imu_um6" dir="imu" task="sensors">
<doc>
<description>
CHR-UM6 IMU.
</description>
<configure name="UM6_PORT" value="uart3" description="UART to use"/>
<configure name="UM6_BAUD" value="B115200" description="Baud rate"/>
</doc>
<dep>
<depends>uart,imu_common</depends>
<provides>imu,mag</provides>
</dep>
<autoload name="imu_nps"/>
<autoload name="imu_sim"/>
<header>
<file name="imu_um6.h"/>
</header>
<init fun="imu_um6_init()"/>
<periodic fun="imu_um6_periodic()"/>
<event fun="imu_um6_event()"/>
<makefile target="!sim|nps|fbw">
<configure name="UM6_PORT" default="uart3" case="lower|upper"/>
<configure name="UM6_BAUD" default="B115200"/>
<define name="USE_$(UM6_PORT_UPPER)"/>
<define name="$(UM6_PORT_UPPER)_BAUD" value="$(UM6_BAUD)"/>
<define name="UM6_LINK" value="$(UM6_PORT_LOWER)"/>
<define name="IMU_TYPE_H" value="imu/imu_um6.h" type="string"/>
<file name="imu_um6.c"/>
</makefile>
</module>
sw/airborne/modules/imu/imu_um6.c
-375
View File
@@ -1,375 +0,0 @@
/*
* 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 "modules/imu/imu_um6.h"
#include "modules/imu/imu.h"
#include "modules/core/abi.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 UM6_verify_chk(uint8_t packet_buffer[], uint8_t packet_length);
inline bool 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++) {
uart_put_byte(&(UM6_LINK), 0, 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_um6_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_um6_periodic(void)
{
/* 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;
}
}
/* no scaling */
void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
void imu_scale_mag(struct Imu *_imu __attribute__((unused))) {}
void imu_um6_publish(void)
{
uint32_t now_ts = get_sys_time_usec();
AbiSendMsgIMU_GYRO_INT32(IMU_UM6_ID, now_ts, &imu.gyro);
AbiSendMsgIMU_ACCEL_INT32(IMU_UM6_ID, now_ts, &imu.accel);
AbiSendMsgIMU_MAG_INT32(IMU_UM6_ID, now_ts, &imu.mag);
}
sw/airborne/modules/imu/imu_um6.h
-119
View File
@@ -1,119 +0,0 @@
/*
* 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 "modules/imu/imu.h"
#include "mcu_periph/uart.h"
#include "modules/ahrs/ahrs.h"
#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 void imu_um6_publish(void);
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 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
};
extern void imu_um6_init(void);
extern void imu_um6_periodic(void);
static inline void imu_um6_event(void)
{
if (uart_char_available(&(UM6_LINK))) {
while (uart_char_available(&(UM6_LINK)) && !UM6_packet.msg_available) {
UM6_packet_parse(uart_getch(&(UM6_LINK)));
}
if (UM6_packet.msg_available) {
UM6_packet.msg_available = false;
UM6_packet_read_message();
imu_um6_publish();
}
}
}
#endif /* IMU_UM6_H*/