Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 04:46:51 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[ahrs] mostly update gx3

Browse Source
This commit is contained in:
Felix Ruess
2015-01-19 22:09:33 +01:00
parent b071a69ab0
commit 4da2e88786
4 changed files with 98 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/fixedwing/ahrs_gx3.makefile
-9
View File
@@ -4,17 +4,8 @@
GX3_PORT ?= UART3
GX3_BAUD ?= B921600
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DUSE_IMU
AHRS_CFLAGS += -DUSE_IMU_FLOAT
AHRS_CFLAGS += -DUSE_AHRS_ALIGNER
ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\"
AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
conf/firmwares/subsystems/rotorcraft/ahrs_gx3.makefile → conf/firmwares/subsystems/shared/ahrs_gx3.makefile
+1 -8
View File
@@ -1,18 +1,11 @@
# Rotorcraft AHRS module for GX3
# AHRS subsystem for GX3
# 2013, Utah State University, http://aggieair.usu.edu/
GX3_PORT ?= UART3
GX3_BAUD ?= B921600
AHRS_ALIGNER_LED ?= none
AHRS_CFLAGS = -DUSE_AHRS
AHRS_CFLAGS += -DAHRS_FLOAT
AHRS_CFLAGS += -DUSE_IMU
AHRS_CFLAGS += -DUSE_IMU_FLOAT
ifneq ($(AHRS_ALIGNER_LED),none)
AHRS_CFLAGS += -DAHRS_ALIGNER_LED=$(AHRS_ALIGNER_LED)
endif
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_gx3.h\"
AHRS_SRCS += $(SRC_SUBSYSTEMS)/ahrs.c
sw/airborne/subsystems/ahrs/ahrs_gx3.c
+76 -79
View File
@@ -19,6 +19,7 @@
* the Free Software Foundation, 59 Temple Place - Suite 330,
* Boston, MA 02111-1307, USA.
*/
/**
* @file ahrs_gx3.c
*
@@ -29,19 +30,21 @@
*
* @author Michal Podhradsky <michal.podhradsky@aggiemail.usu.edu>
*/
#include "subsystems/ahrs/ahrs_gx3.h"
// for ahrs_register_impl
#include "subsystems/ahrs.h"
#define GX3_CHKSM(_ubx_payload) (uint16_t)((uint16_t)(*((uint8_t*)_ubx_payload+66+1))|(uint16_t)(*((uint8_t*)_ubx_payload+66+0))<<8)
/*
/**
* Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
* Positive pitch : nose up
* Positive roll : right wing down
* Positive yaw : clockwise
*/
struct AhrsFloatQuat ahrs_impl;
struct AhrsAligner ahrs_aligner;
struct AhrsGX3 ahrs_gx3;
static inline bool_t gx3_verify_chk(volatile uint8_t *buff_add);
static inline float bef(volatile uint8_t *c);
@@ -69,9 +72,9 @@ static inline bool_t gx3_verify_chk(volatile uint8_t *buff_add)
return (chk_calc == ((((uint16_t) * buff_add) << 8) + (uint8_t) * (buff_add + 1)));
}
void ahrs_align(void)
void ahrs_gx3_align(void)
{
ahrs_impl.gx3_status = GX3Uninit;
ahrs_gx3.is_aligned = FALSE;
//make the gyros zero, takes 10s (specified in Byte 4 and 5)
uart_transmit(&GX3_PORT, 0xcd);
@@ -80,19 +83,19 @@ void ahrs_align(void)
uart_transmit(&GX3_PORT, 0x27);
uart_transmit(&GX3_PORT, 0x10);
ahrs_impl.gx3_status = GX3Running;
ahrs_gx3.is_aligned = TRUE;
}
#if PERIODIC_TELEMETRY
#include "subsystems/datalink/telemetry.h"
static send_gx3(struct transport_tx *trans, struct link_device *dev)
static void send_gx3(struct transport_tx *trans, struct link_device *dev)
{
pprz_msg_send_GX3_INFO(trans, dev, AC_ID,
&ahrs_impl.gx3_freq,
&ahrs_impl.gx3_packet.chksm_error,
&ahrs_impl.gx3_packet.hdr_error,
&ahrs_impl.gx3_chksm);
&ahrs_gx3.freq,
&ahrs_gx3.packet.chksm_error,
&ahrs_gx3.packet.hdr_error,
&ahrs_gx3.chksm);
}
#endif
@@ -103,14 +106,14 @@ static send_gx3(struct transport_tx *trans, struct link_device *dev)
void imu_impl_init(void)
{
// Initialize variables
ahrs_impl.gx3_status = GX3Uninit;
ahrs_gx3.is_aligned = FALSE;
// Initialize packet
ahrs_impl.gx3_packet.status = GX3PacketWaiting;
ahrs_impl.gx3_packet.msg_idx = 0;
ahrs_impl.gx3_packet.msg_available = FALSE;
ahrs_impl.gx3_packet.chksm_error = 0;
ahrs_impl.gx3_packet.hdr_error = 0;
ahrs_gx3.packet.status = GX3PacketWaiting;
ahrs_gx3.packet.msg_idx = 0;
ahrs_gx3.packet.msg_available = FALSE;
ahrs_gx3.packet.chksm_error = 0;
ahrs_gx3.packet.hdr_error = 0;
// It is necessary to wait for GX3 to power up for proper initialization
for (uint32_t startup_counter = 0; startup_counter < IMU_GX3_LONG_DELAY * 2; startup_counter++) {
@@ -203,9 +206,8 @@ void imu_impl_init(void)
uart_transmit(&GX3_PORT, 0xc8); // accel,gyro,R
// Reset gyros to zero
ahrs_align();
ahrs_gx3_align();
#endif
ahrs.status = AHRS_RUNNING;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, "GX3_INFO", send_gx3);
@@ -223,45 +225,48 @@ void imu_periodic(void)
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_gx3.accel.x = bef(&ahrs_gx3.packet.msg_buf[1]);
ahrs_gx3.accel.y = bef(&ahrs_gx3.packet.msg_buf[5]);
ahrs_gx3.accel.z = bef(&ahrs_gx3.packet.msg_buf[9]);
ahrs_gx3.rate.p = bef(&ahrs_gx3.packet.msg_buf[13]);
ahrs_gx3.rate.q = bef(&ahrs_gx3.packet.msg_buf[17]);
ahrs_gx3.rate.r = bef(&ahrs_gx3.packet.msg_buf[21]);
ahrs_gx3.rmat.m[0] = bef(&ahrs_gx3.packet.msg_buf[25]);
ahrs_gx3.rmat.m[1] = bef(&ahrs_gx3.packet.msg_buf[29]);
ahrs_gx3.rmat.m[2] = bef(&ahrs_gx3.packet.msg_buf[33]);
ahrs_gx3.rmat.m[3] = bef(&ahrs_gx3.packet.msg_buf[37]);
ahrs_gx3.rmat.m[4] = bef(&ahrs_gx3.packet.msg_buf[41]);
ahrs_gx3.rmat.m[5] = bef(&ahrs_gx3.packet.msg_buf[45]);
ahrs_gx3.rmat.m[6] = bef(&ahrs_gx3.packet.msg_buf[49]);
ahrs_gx3.rmat.m[7] = bef(&ahrs_gx3.packet.msg_buf[53]);
ahrs_gx3.rmat.m[8] = bef(&ahrs_gx3.packet.msg_buf[57]);
ahrs_gx3.time = (uint32_t)(ahrs_gx3.packet.msg_buf[61] << 24 |
ahrs_gx3.packet.msg_buf[62] << 16 |
ahrs_gx3.packet.msg_buf[63] << 8 |
ahrs_gx3.packet.msg_buf[64]);
ahrs_gx3.chksm = GX3_CHKSM(ahrs_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;
ahrs_gx3.freq = 62500.0 / (float)(ahrs_gx3.time - ahrs_gx3.ltime);
ahrs_gx3.ltime = ahrs_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;
VECT3_SMUL(ahrs_gx3.accel, ahrs_gx3.accel, 9.80665); // Convert g into m/s2
// for compatibility with fixed point interface
ACCELS_BFP_OF_REAL(imu.accel, ahrs_gx3.accel);
// Rates
// for compatibility with fixed point interface
RATES_BFP_OF_REAL(imu.gyro, ahrs_gx3.rate);
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);
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&imu.body_to_imu);
FLOAT_RMAT_TRANSP_RATEMULT(body_rate, *body_to_imu_rmat, ahrs_gx3.rate);
/* 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);
FLOAT_RMAT_COMP(ltp_to_body_rmat, ahrs_gx3.rmat, *body_to_imu_rmat);
#if AHRS_USE_GPS_HEADING && USE_GPS
struct FloatEulers ltp_to_body_eulers;
@@ -276,7 +281,7 @@ void gx3_packet_read_message(void)
#ifdef IMU_MAG_OFFSET
struct FloatEulers ltp_to_body_eulers;
float_eulers_of_rmat(&ltp_to_body_eulers, &ltp_to_body_rmat);
ltp_to_body_eulers.psi -= ahrs_impl.mag_offset;
ltp_to_body_eulers.psi -= ahrs_gx3.mag_offset;
stateSetNedToBodyEulers_f(&ltp_to_body_eulers);
#else
stateSetNedToBodyRMat_f(&ltp_to_body_rmat);
@@ -288,64 +293,56 @@ void gx3_packet_read_message(void)
/* GX3 Packet Collection */
void gx3_packet_parse(uint8_t c)
{
switch (ahrs_impl.gx3_packet.status) {
switch (ahrs_gx3.packet.status) {
case GX3PacketWaiting:
ahrs_impl.gx3_packet.msg_idx = 0;
ahrs_gx3.packet.msg_idx = 0;
if (c == GX3_HEADER) {
ahrs_impl.gx3_packet.status++;
ahrs_impl.gx3_packet.msg_buf[ahrs_impl.gx3_packet.msg_idx] = c;
ahrs_impl.gx3_packet.msg_idx++;
ahrs_gx3.packet.status++;
ahrs_gx3.packet.msg_buf[ahrs_gx3.packet.msg_idx] = c;
ahrs_gx3.packet.msg_idx++;
} else {
ahrs_impl.gx3_packet.hdr_error++;
ahrs_gx3.packet.hdr_error++;
}
break;
case GX3PacketReading:
ahrs_impl.gx3_packet.msg_buf[ahrs_impl.gx3_packet.msg_idx] = c;
ahrs_impl.gx3_packet.msg_idx++;
if (ahrs_impl.gx3_packet.msg_idx == GX3_MSG_LEN) {
if (gx3_verify_chk(ahrs_impl.gx3_packet.msg_buf)) {
ahrs_impl.gx3_packet.msg_available = TRUE;
ahrs_gx3.packet.msg_buf[ahrs_gx3.packet.msg_idx] = c;
ahrs_gx3.packet.msg_idx++;
if (ahrs_gx3.packet.msg_idx == GX3_MSG_LEN) {
if (gx3_verify_chk(ahrs_gx3.packet.msg_buf)) {
ahrs_gx3.packet.msg_available = TRUE;
} else {
ahrs_impl.gx3_packet.msg_available = FALSE;
ahrs_impl.gx3_packet.chksm_error++;
ahrs_gx3.packet.msg_available = FALSE;
ahrs_gx3.packet.chksm_error++;
}
ahrs_impl.gx3_packet.status = 0;
ahrs_gx3.packet.status = 0;
}
break;
default:
ahrs_impl.gx3_packet.status = 0;
ahrs_impl.gx3_packet.msg_idx = 0;
ahrs_gx3.packet.status = 0;
ahrs_gx3.packet.msg_idx = 0;
break;
}
}
void ahrs_init(void)
void ahrs_gx3_init(void)
{
ahrs.status = AHRS_UNINIT;
/* set ltp_to_imu so that body is zero */
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imuf.body_to_imu);
QUAT_COPY(ahrs_impl.ltp_to_imu_quat, *body_to_imu_quat);
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&imu.body_to_imu);
QUAT_COPY(ahrs_gx3.ltp_to_imu_quat, *body_to_imu_quat);
#ifdef IMU_MAG_OFFSET
ahrs_impl.mag_offset = IMU_MAG_OFFSET;
ahrs_gx3.mag_offset = IMU_MAG_OFFSET;
#else
ahrs_impl.mag_offset = 0.0;
ahrs_gx3.mag_offset = 0.0;
#endif
ahrs_aligner.status = AHRS_ALIGNER_LOCKED;
ahrs_gx3.is_aligned = FALSE;
}
void ahrs_aligner_run(void)
void ahrs_gx3_register(void)
{
#ifdef AHRS_ALIGNER_LED
LED_ON(AHRS_ALIGNER_LED);
#endif
ahrs.status = AHRS_RUNNING;
ahrs_register_impl(ahrs_gx3_init, NULL);
}
void ahrs_aligner_init(void)
{
}
/* no scaling */
void imu_scale_gyro(struct Imu *_imu __attribute__((unused))) {}
void imu_scale_accel(struct Imu *_imu __attribute__((unused))) {}
sw/airborne/subsystems/ahrs/ahrs_gx3.h
+21 -22
View File
@@ -34,11 +34,9 @@
#include "generated/airframe.h"
#include "subsystems/imu.h"
#include "subsystems/ahrs.h"
#include "subsystems/ins.h"
#include "subsystems/gps.h"
#include "mcu_periph/uart.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "state.h"
#include "led.h"
@@ -58,8 +56,8 @@ struct GX3Packet {
uint32_t chksm_error;
uint32_t hdr_error;
uint8_t msg_buf[GX3_MAX_PAYLOAD];
uint8_t status;
uint8_t msg_idx;
uint8_t status;
uint8_t msg_idx;
};
enum GX3PacketStatus {
@@ -67,32 +65,33 @@ enum GX3PacketStatus {
GX3PacketReading
};
enum GX3Status {
GX3Uninit,
GX3Running
};
//AHRS
struct AhrsFloatQuat {
struct AhrsGX3 {
struct FloatQuat ltp_to_imu_quat; ///< Rotation from LocalTangentPlane to IMU frame as quaternions
float mag_offset; ///< Difference between true and magnetic north
struct GX3Packet gx3_packet; ///< Packet struct
enum GX3Status gx3_status; ///< GX3 status
float gx3_freq; ///< data frequency
uint16_t gx3_chksm; ///< aux variable for checksum
uint32_t gx3_time; ///< GX3 time stamp
uint32_t gx3_ltime; ///< aux time stamp
struct FloatVect3 gx3_accel; ///< measured acceleration in IMU frame
struct FloatRates gx3_rate; ///< measured angular rates in IMU frame
struct FloatRMat gx3_rmat; ///< measured attitude in IMU frame (rotational matrix)
struct GX3Packet packet; ///< Packet struct
float freq; ///< data frequency
uint16_t chksm; ///< aux variable for checksum
uint32_t time; ///< GX3 time stamp
uint32_t ltime; ///< aux time stamp
struct FloatVect3 accel; ///< measured acceleration in IMU frame
struct FloatRates rate; ///< measured angular rates in IMU frame
struct FloatRMat rmat; ///< measured attitude in IMU frame (rotational matrix)
bool_t is_aligned;
};
extern struct AhrsFloatQuat ahrs_impl;
extern struct AhrsGX3 ahrs_gx3;
#define DefaultAhrsImpl ahrs_gx3
extern void ahrs_gx3_init(void);
extern void ahrs_gx3_align(void);
extern void ahrs_gx3_register(void);
static inline void ReadGX3Buffer(void)
{
while (uart_char_available(&GX3_PORT) && !ahrs_impl.gx3_packet.msg_available) {
while (uart_char_available(&GX3_PORT) && !ahrs_gx3.packet.msg_available) {
gx3_packet_parse(uart_getch(&GX3_PORT));
}
}
@@ -102,12 +101,12 @@ static inline void ImuEvent(void (* _gyro_handler)(void), void (* _accel_handler
if (uart_char_available(&GX3_PORT)) {
ReadGX3Buffer();
}
if (ahrs_impl.gx3_packet.msg_available) {
if (ahrs_gx3.packet.msg_available) {
gx3_packet_read_message();
_gyro_handler();
_accel_handler();
_mag_handler();
ahrs_impl.gx3_packet.msg_available = FALSE;
ahrs_gx3.packet.msg_available = FALSE;
}
}