Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 16:30:07 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Merge pull request #125 from paparazzi/chimu

Browse Source 
temporary fix for ins_chimu modules
This commit is contained in:
Felix Ruess
2012-02-10 12:24:35 -08:00
parent 3cbc4ffb02 0cbd944de8
commit 98232352b4
7 changed files with 350 additions and 363 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/ins_chimu_spi.xml
+1 -1
View File
@@ -4,9 +4,9 @@
<!-- <depend conflict="ins" -->
<header>
<file name="ins_module.h"/>
<file name="ins_chimu.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic_task()" freq="8"/>
<event fun="parse_ins_msg()"/>
<makefile>
<define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_module.h\\\""/>
conf/modules/ins_chimu_uart.xml
+1
View File
@@ -10,6 +10,7 @@ For older CHIMU v1.0 you should define CHIMU_BIG_ENDIAN
<!-- <depend conflict="ins" -->
<header>
<file name="ins_module.h"/>
<file name="ins_chimu.h"/>
</header>
<init fun="ins_init()"/>
<!--<periodic fun="ins_periodic_task()" freq="60"/>-->
sw/airborne/modules/ins/imu_chimu.c
+248 -264
View File
File diff suppressed because it is too large Load Diff
sw/airborne/modules/ins/imu_chimu.h
+53 -54
View File
@@ -20,23 +20,23 @@
*/
/*---------------------------------------------------------------------------
Copyright (c) Ryan Mechatronics 2008. All Rights Reserved.
Copyright (c) Ryan Mechatronics 2008. All Rights Reserved.
File: *.c
File: *.c
Description: CHIMU Protocol Parser
Description: CHIMU Protocol Parser
Public Functions:
CHIMU_Init Create component instance
CHIMU_Done Free component instance
CHIMU_Parse Parse the RX byte stream message
Public Functions:
CHIMU_Init Create component instance
CHIMU_Done Free component instance
CHIMU_Parse Parse the RX byte stream message
Applicable Documents:
CHIMU parsing documentation
Applicable Documents:
CHIMU parsing documentation
---------------------------------------------------------------------------*/
---------------------------------------------------------------------------*/
#include "paparazzi.h"
@@ -116,25 +116,25 @@ static inline float FloatSwap( float f )
typedef struct {
float phi;
float theta;
float psi;
float phi;
float theta;
float psi;
} CHIMU_Euler;
typedef struct {
float x;
float y;
float z;
float x;
float y;
float z;
} CHIMU_Vector;
typedef struct {
float s;
CHIMU_Vector v;
float s;
CHIMU_Vector v;
} CHIMU_Quaternion;
typedef struct {
CHIMU_Euler euler;
CHIMU_Quaternion q;
CHIMU_Euler euler;
CHIMU_Quaternion q;
} CHIMU_attitude_data;
#ifndef FALSE
@@ -145,53 +145,53 @@ typedef struct {
#endif
typedef struct {
float cputemp;
float acc[3];
float rate[3];
float mag[3];
float spare1;
float cputemp;
float acc[3];
float rate[3];
float mag[3];
float spare1;
} CHIMU_sensor_data;
#define CHIMU_RX_BUFFERSIZE 128
typedef struct {
unsigned char m_State; // Current state protocol parser is in
unsigned char m_Checksum; // Calculated CHIMU sentence checksum
unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists)
unsigned char m_Index; // Index used for command and data
unsigned char m_PayloadIndex;
unsigned char m_MsgID;
unsigned char m_MsgLen;
unsigned char m_TempDeviceID;
unsigned char m_DeviceID;
unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data
unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data
CHIMU_attitude_data m_attitude;
CHIMU_attitude_data m_attrates;
CHIMU_sensor_data m_sensor;
unsigned char m_State; // Current state protocol parser is in
unsigned char m_Checksum; // Calculated CHIMU sentence checksum
unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists)
unsigned char m_Index; // Index used for command and data
unsigned char m_PayloadIndex;
unsigned char m_MsgID;
unsigned char m_MsgLen;
unsigned char m_TempDeviceID;
unsigned char m_DeviceID;
unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data
unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data
CHIMU_attitude_data m_attitude;
CHIMU_attitude_data m_attrates;
CHIMU_sensor_data m_sensor;
// Ping data
uint8_t gCHIMU_SW_Exclaim;
uint8_t gCHIMU_SW_Major;
uint8_t gCHIMU_SW_Minor;
uint16_t gCHIMU_SW_SerialNumber;
// Ping data
uint8_t gCHIMU_SW_Exclaim;
uint8_t gCHIMU_SW_Major;
uint8_t gCHIMU_SW_Minor;
uint16_t gCHIMU_SW_SerialNumber;
// Config
uint8_t gCalStatus;
uint8_t gCHIMU_BIT;
uint8_t gConfigInfo;
// Config
uint8_t gCalStatus;
uint8_t gCHIMU_BIT;
uint8_t gConfigInfo;
} CHIMU_PARSER_DATA;
/*---------------------------------------------------------------------------
Name: CHIMU_Init
---------------------------------------------------------------------------*/
Name: CHIMU_Init
---------------------------------------------------------------------------*/
void CHIMU_Init(CHIMU_PARSER_DATA *pstData);
/*---------------------------------------------------------------------------
Name: CHIMU_Parse
Abstract: Parse message input test mode, returns TRUE if new data.
---------------------------------------------------------------------------*/
Name: CHIMU_Parse
Abstract: Parse message input test mode, returns TRUE if new data.
---------------------------------------------------------------------------*/
unsigned char CHIMU_Parse(unsigned char btData, unsigned char bInputType, CHIMU_PARSER_DATA *pstData);
unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloadData, CHIMU_PARSER_DATA *pstData);
@@ -199,4 +199,3 @@ unsigned char CHIMU_ProcessMessage(unsigned char *pMsgID, unsigned char *pPayloa
void CHIMU_Checksum(unsigned char *data, unsigned char buflen);
#endif // CHIMU_DEFINED
sw/airborne/modules/ins/ins_chimu.h
+6
View File
@@ -0,0 +1,6 @@
#ifndef INS_CHIMU_H
#define INS_CHIMU_H
extern void ahrs_update_gps( void );
#endif
sw/airborne/modules/ins/ins_chimu_spi.c
+21 -22
View File
@@ -1,5 +1,5 @@
/*
C code to connect a CHIMU using uart
C code to connect a CHIMU using uart
*/
@@ -71,28 +71,23 @@ void ins_init( void )
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
{
while (InsLink(ChAvailable())) {
uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
{
RunOnceEvery(25, LED_TOGGLE(3) );
if(CHIMU_DATA.m_MsgID==CHIMU_Msg_3_IMU_Attitude)
{
new_ins_attitude = 1;
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
RunOnceEvery(25, LED_TOGGLE(3) );
if(CHIMU_DATA.m_MsgID==CHIMU_Msg_3_IMU_Attitude) {
new_ins_attitude = 1;
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta,0.); // FIXME rate r
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta,0.); // FIXME rate r
}
else if(CHIMU_DATA.m_MsgID==0x02)
{
else if(CHIMU_DATA.m_MsgID==0x02) {
RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
RunOnceEvery(25,DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_sensor.rate[0], &CHIMU_DATA.m_sensor.rate[1], &CHIMU_DATA.m_sensor.rate[2]));
}
}
@@ -100,16 +95,14 @@ void parse_ins_msg( void )
}
//Frequency defined in conf *.xml
void ins_periodic_task( void )
void ahrs_update_gps( void )
{
// Send SW Centripetal Corrections
uint8_t centripedal[19] = {0xae, 0xae, 0x0d, 0xaa, 0x0b, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xc2 };
float gps_speed = 0;
if (gps.fix == GPS_FIX_3D)
{
if (gps.fix == GPS_FIX_3D) {
gps_speed = gps.speed_3d/100.;
}
gps_speed = FloatSwap(gps_speed);
@@ -123,3 +116,9 @@ void ins_periodic_task( void )
// Downlink Send
}
//Frequency defined in conf *.xml
void ins_periodic_task( void )
{
}
sw/airborne/modules/ins/ins_chimu_uart.c
+20 -22
View File
@@ -1,5 +1,5 @@
/*
C code to connect a CHIMU using uart
C code to connect a CHIMU using uart
*/
@@ -39,8 +39,8 @@ void ins_init( void )
uint8_t ping[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG00_PING, 0x00, 0xE6 };
uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
uint8_t quaternions[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x01, 0x39 }; // 25Hz attitude only + SPI
// uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI
// uint8_t rate[12] = {CHIMU_STX, CHIMU_STX, 0x06, CHIMU_BROADCAST, MSG10_UARTSETTINGS, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI
new_ins_attitude = 0;
@@ -51,15 +51,13 @@ void ins_init( void )
CHIMU_Init(&CHIMU_DATA);
// Request Software version
for (int i=0;i<7;i++)
{
for (int i=0;i<7;i++) {
InsUartSend1(ping[i]);
}
// Quat Filter
for (int i=0;i<7;i++)
{
for (int i=0;i<7;i++) {
InsUartSend1(quaternions[i]);
}
@@ -72,25 +70,21 @@ void ins_init( void )
void parse_ins_msg( void )
{
while (InsLink(ChAvailable()))
{
while (InsLink(ChAvailable())) {
uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA))
{
if(CHIMU_DATA.m_MsgID==0x03)
{
new_ins_attitude = 1;
RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
if(CHIMU_DATA.m_MsgID==0x03) {
new_ins_attitude = 1;
RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
//EstimatorSetRate(ins_p,ins_q,ins_r);
EstimatorSetAtt(CHIMU_DATA.m_attitude.euler.phi, CHIMU_DATA.m_attitude.euler.psi, CHIMU_DATA.m_attitude.euler.theta);
//EstimatorSetRate(ins_p,ins_q,ins_r);
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
DOWNLINK_SEND_AHRS_EULER(DefaultChannel, DefaultDevice, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
}
}
@@ -104,3 +98,7 @@ void ins_periodic_task( void )
// Downlink Send
}
void ahrs_update_gps( void )
{
}