Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

CHIMU GPS speed feedback update

Browse Source
This commit is contained in:
Christophe De Wagter
2011-08-30 17:17:16 +02:00
parent f8d4f5f662
commit 71da5b05ee
5 changed files with 48 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/ins_chimu_spi.xml
+1 -1
View File
@@ -6,7 +6,7 @@
<file name="ins_module.h"/>
</header>
<init fun="ins_init()"/>
<periodic fun="ins_periodic_task()" freq="60"/>
<periodic fun="ins_periodic_task()" freq="8"/>
<event fun="parse_ins_msg()"/>
<makefile>
<define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_module.h\\\""/>
sw/airborne/modules/ins/imu_chimu.c
-28
View File
@@ -106,34 +106,6 @@ void CHIMU_Checksum(unsigned char *data, unsigned char buflen)
// Communication Definitions
#define CHIMU_COM_ID_HIGH 0x1F //Must set this to the max ID expected above
/***************************************************************************
* Endianness Swapping Functions
*/
#ifdef CHIMU_BIG_ENDIAN
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;
}
#else
#define FloatSwap(X) (X)
#endif
/*---------------------------------------------------------------------------
Name: CHIMU_Init
sw/airborne/modules/ins/imu_chimu.h
+29
View File
@@ -86,6 +86,35 @@
#define CHIMU_Msg_15_SFCheck 15
/***************************************************************************
* Endianness Swapping Functions
*/
#ifdef CHIMU_BIG_ENDIAN
static inline 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;
}
#else
#define FloatSwap(X) (X)
#endif
typedef struct {
float phi;
float theta;
sw/airborne/modules/ins/ins_chimu_spi.c
+14 -1
View File
@@ -27,6 +27,9 @@ C code to connect a CHIMU using uart
#include "ins_module.h"
#include "imu_chimu.h"
#include "subsystems/gps.h"
CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral;
@@ -101,7 +104,17 @@ void parse_ins_msg( void )
void ins_periodic_task( 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 };
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)
{
gps_speed = gps.speed_3d/100.;
}
gps_speed = FloatSwap(gps_speed);
memmove (&centripedal[6], &gps_speed, 4);
// Fill X-speed
sw/airborne/modules/ins/ins_xsens.c
+4
View File
@@ -306,6 +306,10 @@ void handle_ins_msg( void) {
hmsl /= 1000.0f;
EstimatorSetAlt(hmsl);
#ifndef ALT_KALMAN
#warning NO_VZ
#endif
// Horizontal speed
float fspeed = sqrt(ins_vx*ins_vx + ins_vy*ins_vy);
if (gps.fix != GPS_FIX_3D)