Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

cleanup whitespaces, indentation

Browse Source
This commit is contained in:
Felix Ruess
2012-02-02 11:30:15 +01:00
parent f07b05e6da
commit def7def1ef
4 changed files with 333 additions and 362 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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: Public Functions:
CHIMU_Init Create component instance CHIMU_Init Create component instance
CHIMU_Done Free component instance CHIMU_Done Free component instance
CHIMU_Parse Parse the RX byte stream message CHIMU_Parse Parse the RX byte stream message
Applicable Documents: Applicable Documents:
CHIMU parsing documentation CHIMU parsing documentation
---------------------------------------------------------------------------*/ ---------------------------------------------------------------------------*/
#include "paparazzi.h" #include "paparazzi.h"
@@ -116,25 +116,25 @@ static inline float FloatSwap( float f )
typedef struct { typedef struct {
float phi; float phi;
float theta; float theta;
float psi; float psi;
} CHIMU_Euler; } CHIMU_Euler;
typedef struct { typedef struct {
float x; float x;
float y; float y;
float z; float z;
} CHIMU_Vector; } CHIMU_Vector;
typedef struct { typedef struct {
float s; float s;
CHIMU_Vector v; CHIMU_Vector v;
} CHIMU_Quaternion; } CHIMU_Quaternion;
typedef struct { typedef struct {
CHIMU_Euler euler; CHIMU_Euler euler;
CHIMU_Quaternion q; CHIMU_Quaternion q;
} CHIMU_attitude_data; } CHIMU_attitude_data;
#ifndef FALSE #ifndef FALSE
@@ -145,53 +145,53 @@ typedef struct {
#endif #endif
typedef struct { typedef struct {
float cputemp; float cputemp;
float acc[3]; float acc[3];
float rate[3]; float rate[3];
float mag[3]; float mag[3];
float spare1; float spare1;
} CHIMU_sensor_data; } CHIMU_sensor_data;
#define CHIMU_RX_BUFFERSIZE 128 #define CHIMU_RX_BUFFERSIZE 128
typedef struct { typedef struct {
unsigned char m_State; // Current state protocol parser is in unsigned char m_State; // Current state protocol parser is in
unsigned char m_Checksum; // Calculated CHIMU sentence checksum unsigned char m_Checksum; // Calculated CHIMU sentence checksum
unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists) unsigned char m_ReceivedChecksum; // Received CHIMU sentence checksum (if exists)
unsigned char m_Index; // Index used for command and data unsigned char m_Index; // Index used for command and data
unsigned char m_PayloadIndex; unsigned char m_PayloadIndex;
unsigned char m_MsgID; unsigned char m_MsgID;
unsigned char m_MsgLen; unsigned char m_MsgLen;
unsigned char m_TempDeviceID; unsigned char m_TempDeviceID;
unsigned char m_DeviceID; unsigned char m_DeviceID;
unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data unsigned char m_Payload[CHIMU_RX_BUFFERSIZE]; // CHIMU data
unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data unsigned char m_FullMessage[CHIMU_RX_BUFFERSIZE]; // CHIMU data
CHIMU_attitude_data m_attitude; CHIMU_attitude_data m_attitude;
CHIMU_attitude_data m_attrates; CHIMU_attitude_data m_attrates;
CHIMU_sensor_data m_sensor; CHIMU_sensor_data m_sensor;
// Ping data // Ping data
uint8_t gCHIMU_SW_Exclaim; uint8_t gCHIMU_SW_Exclaim;
uint8_t gCHIMU_SW_Major; uint8_t gCHIMU_SW_Major;
uint8_t gCHIMU_SW_Minor; uint8_t gCHIMU_SW_Minor;
uint16_t gCHIMU_SW_SerialNumber; uint16_t gCHIMU_SW_SerialNumber;
// Config // Config
uint8_t gCalStatus; uint8_t gCalStatus;
uint8_t gCHIMU_BIT; uint8_t gCHIMU_BIT;
uint8_t gConfigInfo; uint8_t gConfigInfo;
} CHIMU_PARSER_DATA; } CHIMU_PARSER_DATA;
/*--------------------------------------------------------------------------- /*---------------------------------------------------------------------------
Name: CHIMU_Init Name: CHIMU_Init
---------------------------------------------------------------------------*/ ---------------------------------------------------------------------------*/
void CHIMU_Init(CHIMU_PARSER_DATA *pstData); void CHIMU_Init(CHIMU_PARSER_DATA *pstData);
/*--------------------------------------------------------------------------- /*---------------------------------------------------------------------------
Name: CHIMU_Parse Name: CHIMU_Parse
Abstract: Parse message input test mode, returns TRUE if new data. 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_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); 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); void CHIMU_Checksum(unsigned char *data, unsigned char buflen);
#endif // CHIMU_DEFINED #endif // CHIMU_DEFINED
sw/airborne/modules/ins/ins_chimu_spi.c
+16 -21
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 ) void parse_ins_msg( void )
{ {
while (InsLink(ChAvailable())) while (InsLink(ChAvailable())) {
{
uint8_t ch = InsLink(Getch()); uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
{ RunOnceEvery(25, LED_TOGGLE(3) );
RunOnceEvery(25, LED_TOGGLE(3) ); if(CHIMU_DATA.m_MsgID==CHIMU_Msg_3_IMU_Attitude) {
if(CHIMU_DATA.m_MsgID==CHIMU_Msg_3_IMU_Attitude) new_ins_attitude = 1;
{ if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
new_ins_attitude = 1; CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
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); 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 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]));
} }
} }
@@ -109,9 +104,9 @@ void ins_periodic_task( void )
float gps_speed = 0; float gps_speed = 0;
if (gps.fix == GPS_FIX_3D) if (gps.fix == GPS_FIX_3D)
{ {
gps_speed = gps.speed_3d/100.; gps_speed = gps.speed_3d/100.;
} }
gps_speed = FloatSwap(gps_speed); gps_speed = FloatSwap(gps_speed);
memmove (&centripedal[6], &gps_speed, 4); memmove (&centripedal[6], &gps_speed, 4);
sw/airborne/modules/ins/ins_chimu_uart.c
+16 -23
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 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 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 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 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 euler[7] = {CHIMU_STX, CHIMU_STX, 0x01, CHIMU_BROADCAST, MSG09_ESTIMATOR, 0x00, 0xaf }; // 25Hz attitude only + SPI
new_ins_attitude = 0; new_ins_attitude = 0;
@@ -51,15 +51,13 @@ void ins_init( void )
CHIMU_Init(&CHIMU_DATA); CHIMU_Init(&CHIMU_DATA);
// Request Software version // Request Software version
for (int i=0;i<7;i++) for (int i=0;i<7;i++) {
{
InsUartSend1(ping[i]); InsUartSend1(ping[i]);
} }
// Quat Filter // Quat Filter
for (int i=0;i<7;i++) for (int i=0;i<7;i++) {
{
InsUartSend1(quaternions[i]); InsUartSend1(quaternions[i]);
} }
@@ -72,25 +70,21 @@ void ins_init( void )
void parse_ins_msg( void ) void parse_ins_msg( void )
{ {
while (InsLink(ChAvailable())) while (InsLink(ChAvailable())) {
{
uint8_t ch = InsLink(Getch()); uint8_t ch = InsLink(Getch());
if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) if (CHIMU_Parse(ch, 0, &CHIMU_DATA)) {
{ if(CHIMU_DATA.m_MsgID==0x03) {
if(CHIMU_DATA.m_MsgID==0x03) new_ins_attitude = 1;
{ RunOnceEvery(25, LED_TOGGLE(3) );
new_ins_attitude = 1; if (CHIMU_DATA.m_attitude.euler.phi > M_PI) {
RunOnceEvery(25, LED_TOGGLE(3) ); CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
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); 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); //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);
} }
} }
@@ -103,4 +97,3 @@ void ins_periodic_task( void )
{ {
// Downlink Send // Downlink Send
} }