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

Chimu Cleanup

Browse Source
This commit is contained in:
Christophe De Wagter
2011-04-22 08:40:11 +02:00
parent 73d7af9e49 223e331050
commit bcf40abd95
2 changed files with 21 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/ins/ins_chimu_spi.c
+4 -25
View File
@@ -27,7 +27,6 @@ C code to connect a CHIMU using uart
#include "ins_module.h" #include "ins_module.h"
#include "imu_chimu.h" #include "imu_chimu.h"
CHIMU_PARSER_DATA CHIMU_DATA; CHIMU_PARSER_DATA CHIMU_DATA;
INS_FORMAT ins_roll_neutral; INS_FORMAT ins_roll_neutral;
@@ -37,15 +36,13 @@ volatile uint8_t new_ins_attitude;
void ins_init( void ) void ins_init( void )
{ {
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI // uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI // uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
CHIMU_Checksum(rate,12); CHIMU_Checksum(rate,12);
new_ins_attitude = 0; new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
@@ -58,6 +55,7 @@ void ins_init( void )
{ {
InsSend1(quaternions[i]); InsSend1(quaternions[i]);
} }
// Wait a bit (SPI send zero) // Wait a bit (SPI send zero)
InsSend1(0); InsSend1(0);
InsSend1(0); InsSend1(0);
@@ -73,9 +71,6 @@ void ins_init( void )
} }
//float tempang = 0;
void parse_ins_msg( void ) void parse_ins_msg( void )
{ {
while (InsLink(ChAvailable())) while (InsLink(ChAvailable()))
@@ -87,28 +82,12 @@ void parse_ins_msg( void )
if(CHIMU_DATA.m_MsgID==0x03) if(CHIMU_DATA.m_MsgID==0x03)
{ {
new_ins_attitude = 1; new_ins_attitude = 1;
// RunOnceEvery(25, LED_TOGGLE(3) ); RunOnceEvery(25, LED_TOGGLE(3) );
// LED_TOGGLE(3);
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{ {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
} }
if (CHIMU_DATA.m_attrates.euler.phi > M_PI)
{
CHIMU_DATA.m_attrates.euler.phi -= 2 * M_PI;
}
LED_TOGGLE(3);
/* if (CHIMU_DATA.m_attitude.euler.phi == tempang)
{
LED_ON(3);
}
else
{
LED_OFF(3);
}
tempang = CHIMU_DATA.m_attitude.euler.phi;
*/
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); EstimatorSetRate(CHIMU_DATA.m_sensor.rate[0],CHIMU_DATA.m_attrates.euler.theta);
} }
sw/airborne/modules/ins/ins_chimu_uart.c
+17 -17
View File
@@ -4,8 +4,11 @@ C code to connect a CHIMU using uart
#include <stdbool.h> #include <stdbool.h>
//#include "modules/ins/ins_chimu_uart.h" //#include "modules/ins/ins_chimu_uart.h"
// Output // Output
#include "estimator.h" #include "estimator.h"
@@ -33,11 +36,13 @@ volatile uint8_t new_ins_attitude;
void ins_init( void ) void ins_init( void )
{ {
uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0xab, 0x76 }; // 50Hz attitude only + SPI uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x05, 0xff, 0x79, 0x00, 0x00, 0x01, 0x76 }; // 50Hz attitude only + SPI
// uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 25Hz attitude only + SPI // uint8_t rate[12] = {0xae, 0xae, 0x06, 0xaa, 0x10, 0x04, 0xff, 0x79, 0x00, 0x00, 0x01, 0xd3 }; // 25Hz attitude only + SPI
// uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI // uint8_t euler[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x00, 0xaf }; // 25Hz attitude only + SPI
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
CHIMU_Checksum(rate,12);
new_ins_attitude = 0; new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT; ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
@@ -50,6 +55,14 @@ void ins_init( void )
{ {
InsUartSend1(quaternions[i]); InsUartSend1(quaternions[i]);
} }
// 50Hz // 50Hz
for (int i=0;i<12;i++) for (int i=0;i<12;i++)
{ {
@@ -58,8 +71,6 @@ void ins_init( void )
} }
float tempang = 0;
void parse_ins_msg( void ) void parse_ins_msg( void )
{ {
while (InsLink(ChAvailable())) while (InsLink(ChAvailable()))
@@ -71,23 +82,12 @@ void parse_ins_msg( void )
if(CHIMU_DATA.m_MsgID==0x03) if(CHIMU_DATA.m_MsgID==0x03)
{ {
new_ins_attitude = 1; new_ins_attitude = 1;
// RunOnceEvery(25, LED_TOGGLE(3) ); RunOnceEvery(25, LED_TOGGLE(3) );
// LED_TOGGLE(3);
if (CHIMU_DATA.m_attitude.euler.phi > M_PI) if (CHIMU_DATA.m_attitude.euler.phi > M_PI)
{ {
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI; CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
} }
if (CHIMU_DATA.m_attitude.euler.phi == tempang)
{
LED_ON(3);
}
else
{
LED_OFF(3);
}
tempang = CHIMU_DATA.m_attitude.euler.phi;
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); //EstimatorSetRate(ins_p,ins_q);