Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 16:58:48 +08:00
Code Issues Actions 24 Packages Projects Releases Wiki Activity

Checksum + Swap Func included

Browse Source
This commit is contained in:
Christophe De Wagter
2011-04-05 13:31:45 +02:00
parent 382d6a1c1b
commit 3c9e00c424
7 changed files with 75 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/AirborneCodeReorg/ChimuTinyFw.xml
+1 -3
View File
@@ -46,8 +46,6 @@
</section>
<command_laws>
<!-- Brake Rate Limiter -->
<let var="brake_value" value="LIMIT(-@BRAKE, 0, MAX_PPRZ)"/>
<!-- Differential Aileron Depending on Brake Value -->
<set servo="AILERON_LEFT" value="@ROLL"/>
<set servo="AILERON_RIGHT" value="@ROLL"/>
@@ -161,7 +159,7 @@
<modules>
<load name="ins_chimu.xml">
<load name="ins_chimu_uart.xml">
<configure name="CHIMU_UART_NR" value="0"/>
</load>
conf/modules/ins_chimu.xml → conf/modules/ins_chimu_uart.xml
-1
View File
@@ -16,6 +16,5 @@
<define name="UART$(CHIMU_UART_NR)_BAUD" value="B115200"/>
<file name="ins_chimu_uart.c"/>
<file name="imu_chimu.c"/>
<file name="endian_functions.c"/>
</makefile>
</module>
sw/airborne/modules/gps_i2c/gps_i2c.c
+2 -2
View File
@@ -49,8 +49,8 @@ bool_t gps_i2c_done, gps_i2c_data_ready_to_transmit;
}
static uint8_t gps_i2c_status;
static uint16_t gps_i2c_nb_avail_bytes; /* size buffer =~ 12k */
static uint8_t data_buf_len;
//static uint16_t gps_i2c_nb_avail_bytes; /* size buffer =~ 12k */
//static uint8_t data_buf_len;
void gps_i2c_init(void) {
gps_i2c_status = GPS_I2C_STATUS_IDLE;
sw/airborne/modules/ins/endian_functions.c
-74
View File
@@ -1,74 +0,0 @@
/**********************************************************************************************
* Ryan Mechatronics firmware (C) 2007 - All Rights Reserved
* CONFIDENTIAL: NO PART OF THIS CODE MAY BE RELEASED WITHOUT WRITTEN PERMISSION
* ---------------------------------------------------------------------------------------------
*
* Module:
* Endian Functions - Handles various low level endian swap functions
*
***********************************************************************************************/
//-----------------------------------------------------------------------------------
// Includes
//-----------------------------------------------------------------------------------
#include "endian_functions.h"
//#include <globals.h>
short int ShortSwap( short int s )
{
unsigned char b1, b2;
b1 = s & 255;
b2 = (s >> 8) & 255;
return (b1 << 8) + b2;
}
short int ShortNoSwap( short int s )
{
return s;
}
int LongSwap (int i)
{
unsigned char b1, b2, b3, b4;
b1 = i & 255;
b2 = ( i >> 8 ) & 255;
b3 = ( i>>16 ) & 255;
b4 = ( i>>24 ) & 255;
return ((int)b1 << 24) + ((int)b2 << 16) + ((int)b3 << 8) + b4;
}
int LongNoSwap( int i )
{
return i;
}
float FloatSwap( float f )
{
union
{
float f;
unsigned char b[4];
} dat1, dat2;
dat1.f = f;
dat2.b[0] = dat1.b[3];
dat2.b[1] = dat1.b[2];
dat2.b[2] = dat1.b[1];
dat2.b[3] = dat1.b[0];
return dat2.f;
}
float FloatNoSwap( float f )
{
return f;
}
sw/airborne/modules/ins/endian_functions.h
-26
View File
@@ -1,26 +0,0 @@
/**********************************************************************************************
* Ryan Mechatronics firmware (C) 2007 - All Rights Reserved
* CONFIDENTIAL: NO PART OF THIS CODE MAY BE RELEASED WITHOUT WRITTEN PERMISSION
* ---------------------------------------------------------------------------------------------
*
* Module:
* Endian Functions - Handles various low level endian swap functions
*
***********************************************************************************************/
#ifndef ENDIAN_H
#define ENDIAN_H
short int ShortSwap( short int s );
short int ShortNoSwap( short int s );
int LongSwap (int i);
int LongNoSwap( int i );
float FloatSwap( float f );
float FloatNoSwap( float f );
#endif
sw/airborne/modules/ins/imu_chimu.c
+71 -30
View File
@@ -29,7 +29,6 @@
Public Functions:
CHIMU_Init Create component instance
CHIMU_Done Free component instance
CHIMU_Parse Parse the RX byte stream message
Applicable Documents:
@@ -41,14 +40,75 @@
#include "imu_chimu.h"
#include "string.h"
//#include "crc.h"
#include "endian_functions.h"
//#include "util.h"
#include "math.h"
/***************************************************************************
* Endianness Swapping Functions
*/
//---[Defines]------------------------------------------------------
static float FloatSwap( float f )
{
union
{
float f;
unsigned char b[4];
} dat1, dat2;
dat1.f = f;
dat2.b[0] = dat1.b[3];
dat2.b[1] = dat1.b[2];
dat2.b[2] = dat1.b[1];
dat2.b[3] = dat1.b[0];
return dat2.f;
}
/***************************************************************************
* Cyclic Redundancy Checksum
*/
static unsigned long UpdateCRC(unsigned long CRC_acc, void *data, unsigned long data_len)
{
unsigned long i; // loop counter
#define POLY 0xEDB88320 // bit-reversed version of the poly 0x04C11DB7
// Create the CRC "dividend" for polynomial arithmetic (binary arithmetic
// with no carries)
unsigned char *CRC_input = (unsigned char*)data;
for (unsigned long j = data_len; j; --j)
{
CRC_acc = CRC_acc ^ *CRC_input++;
// "Divide" the poly into the dividend using CRC XOR subtraction
// CRC_acc holds the "remainder" of each divide
//
// Only complete this division for 8 bits since input is 1 byte
for (i = 0; i < 8; i++)
{
// Check if the MSB is set (if MSB is 1, then the POLY can "divide"
// into the "dividend")
if ((CRC_acc & 0x00000001) == 0x00000001)
{
// if so, shift the CRC value, and XOR "subtract" the poly
CRC_acc = CRC_acc >> 1;
CRC_acc ^= POLY;
}
else
{
// if not, just shift the CRC value
CRC_acc = CRC_acc >> 1;
}
}
}
// Return the final remainder (CRC value)
return CRC_acc;
}
/***************************************************************************
* CHIMU Protocol Definition
*/
// Lowlevel Protocol Decoding
#define CHIMU_STATE_MACHINE_START 0
#define CHIMU_STATE_MACHINE_HEADER2 1
#define CHIMU_STATE_MACHINE_LEN 2
@@ -57,9 +117,7 @@
#define CHIMU_STATE_MACHINE_PAYLOAD 5
#define CHIMU_STATE_MACHINE_XSUM 6
//---[DEFINES for Message List]---------------------------------------
//Message ID's that go TO the CHIMU
// Message ID's that go TO the CHIMU
#define MSG00_PING 0x00
#define MSG01_BIAS 0x01
#define MSG02_DACMODE 0x02
@@ -79,8 +137,7 @@
#define MSG10_UARTSETTINGS 0x10
#define MSG11_SERIALNUMBER 0x11
//Output message identifiers from the CHIMU unit
//---[Defines]------------------------------------------------------
// Output message identifiers from the CHIMU unit
#define CHIMU_Msg_0_Ping 0
#define CHIMU_Msg_1_IMU_Raw 1
#define CHIMU_Msg_2_IMU_FP 2
@@ -98,20 +155,9 @@
#define CHIMU_Msg_14_RefVector 14
#define CHIMU_Msg_15_SFCheck 15
//---[COM] defines
#define CHIMU_COM_ID_LOW 0x00
// Communication Definitions
#define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above
#define NP_MAX_CMD_LEN 8 // maximum command length (CHIMU address)
#define NP_MAX_DATA_LEN 256 // maximum data length
#define NP_MAX_CHAN 36 // maximum number of channels
#define NP_WAYPOINT_ID_LEN 32 // waypoint max string len
#define NP_XSUM_LEN 3 // chars in checksum string
#define CHIMU_STANDARD 0x00
/*---------------------------------------------------------------------------
Name: CHIMU_Init
@@ -155,11 +201,9 @@ void CHIMU_Init(CHIMU_PARSER_DATA *pstData)
pstData->m_DeviceID = 0x01; //look at this later
}
/*---------------------------------------------------------------------------
Name: CHIMU_Parse
Abstract: Parse message, returns TRUE if new data.
Note: A typical sentence is constructed as:
---------------------------------------------------------------------------*/
unsigned char CHIMU_Parse(
@@ -168,7 +212,6 @@ unsigned char CHIMU_Parse(
CHIMU_PARSER_DATA *pstData) /* resulting data */
{
//long int i;
char bUpdate = FALSE;
switch (pstData->m_State) {
@@ -215,7 +258,7 @@ unsigned char CHIMU_Parse(
break;
case CHIMU_STATE_MACHINE_ID: // Get ID
pstData->m_MsgID = btData; // might be invalid, chgeck it out here:
if ( (pstData->m_MsgID<CHIMU_COM_ID_LOW) || (pstData->m_MsgID>CHIMU_COM_ID_HIGH))
if ( pstData->m_MsgID>CHIMU_COM_ID_HIGH)
{
pstData->m_State = CHIMU_STATE_MACHINE_START;
//BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL);
@@ -230,8 +273,7 @@ unsigned char CHIMU_Parse(
pstData->m_FullMessage[pstData->m_Index++]=btData;
if ((pstData->m_Index) >= (pstData->m_MsgLen + 5)) //Now we have the payload. Verify XSUM and then parse it next
{
// TODO Redo Checksum
// pstData->m_Checksum = (unsigned char) ((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage , (unsigned long) (pstData->m_MsgLen)+5)) & 0xFF);
pstData->m_Checksum = (unsigned char) ((UpdateCRC(0xFFFFFFFF , pstData->m_FullMessage , (unsigned long) (pstData->m_MsgLen)+5)) & 0xFF);
pstData->m_State = CHIMU_STATE_MACHINE_XSUM;
} else {
return FALSE;
@@ -240,8 +282,7 @@ unsigned char CHIMU_Parse(
case CHIMU_STATE_MACHINE_XSUM: // Verify
pstData->m_ReceivedChecksum = btData;
pstData->m_FullMessage[pstData->m_Index++]=btData;
// if (pstData->m_Checksum!=pstData->m_ReceivedChecksum)
if (FALSE)
if (pstData->m_Checksum!=pstData->m_ReceivedChecksum)
{
bUpdate = FALSE;
//BuiltInTest(BIT_COM_UART_RECEIPTFAIL, BIT_FAIL);
sw/airborne/modules/ins/ins_chimu_uart.c
+1 -1
View File
@@ -49,7 +49,7 @@ void parse_ins_msg( void )
{
if(CHIMU_DATA.m_MsgID==0x03)
{
RunOnceEvery(25, LED_TOGGLE(3) );
//RunOnceEvery(25, LED_TOGGLE(3) );
if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;