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

2-way SPI-Slave: Chimu receives config and corrections over SPI

Browse Source
This commit is contained in:
Christophe De Wagter
2011-04-06 11:57:14 +02:00
parent 19ee2fe45c
commit 5125399414
5 changed files with 74 additions and 10 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="60"/>
<event fun="parse_ins_msg()"/>
<makefile>
<define name="AHRS_TYPE_H" value="\\\"modules/ins/ins_module.h\\\""/>
sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.c
+16 -1
View File
@@ -38,6 +38,8 @@
/* High Speed SPI Slave Circular Buffer */
uint16_t spi_slave_hs_rx_insert_idx, spi_slave_hs_rx_extract_idx;
uint8_t spi_slave_hs_rx_buffer[SPI_SLAVE_HS_RX_BUFFER_SIZE];
uint16_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx;
uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE];
/* Prototypes */
// void spi_init( void ); // -> declared in spi.h
@@ -133,8 +135,21 @@ void spi_init(void) {
static void SSP_ISR(void) {
ISR_ENTRY();
LED_TOGGLE(3);
//LED_TOGGLE(3);
// If any TX bytes are pending
if (spi_slave_hs_tx_insert_idx != spi_slave_hs_tx_extract_idx)
{
uint8_t ret = spi_slave_hs_tx_buffer[spi_slave_hs_tx_extract_idx];
spi_slave_hs_tx_extract_idx = (spi_slave_hs_tx_extract_idx + 1)%SPI_SLAVE_HS_RX_BUFFER_SIZE;
SSP_Write(ret);
}
else
{
SSP_Write(0x00);
}
//do
{
uint16_t temp;
sw/airborne/arch/lpc21/mcu_periph/spi_slave_hs_arch.h
+13
View File
@@ -57,6 +57,19 @@ extern uint8_t spi_slave_hs_rx_buffer[SPI_SLAVE_HS_RX_BUFFER_SIZE];
ret; \
})
#define SPI_SLAVE_HS_TX_BUFFER_SIZE 128
extern uint16_t spi_slave_hs_tx_insert_idx, spi_slave_hs_tx_extract_idx;
extern uint8_t spi_slave_hs_tx_buffer[SPI_SLAVE_HS_TX_BUFFER_SIZE];
#define SpiSlaveTransmit(data) {\
uint16_t temp = (spi_slave_hs_tx_insert_idx + 1) % SPI_SLAVE_HS_TX_BUFFER_SIZE; \
if (temp != spi_slave_hs_tx_extract_idx) /* there is room left */ \
{ \
spi_slave_hs_tx_buffer[spi_slave_hs_tx_insert_idx] = (uint8_t)data; \
spi_slave_hs_tx_insert_idx = temp; \
} \
}
sw/airborne/modules/ins/ins_chimu_spi.c
+42 -7
View File
@@ -35,17 +35,44 @@ INS_FORMAT ins_pitch_neutral;
volatile uint8_t new_ins_attitude;
#define INS_LINK SpiSlave
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, 0x04, 0xff, 0x79, 0x00, 0x00, 0xab, 0xd3 }; // 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
new_ins_attitude = 0;
ins_roll_neutral = INS_ROLL_NEUTRAL_DEFAULT;
ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
new_ins_attitude= 0;
CHIMU_Init(&CHIMU_DATA);
// Quat Filter
for (int i=0;i<7;i++)
{
InsSend1(quaternions[i]);
}
// Wait a second
InsSend1(0);
InsSend1(0);
InsSend1(0);
InsSend1(0);
InsSend1(0);
// 50Hz data: attitude only
for (int i=0;i<12;i++)
{
InsSend1(rate[i]);
}
}
#define INS_LINK SpiSlave
//float tempang = 0;
void parse_ins_msg( void )
{
@@ -64,8 +91,9 @@ void parse_ins_msg( void )
{
CHIMU_DATA.m_attitude.euler.phi -= 2 * M_PI;
}
/*
if (CHIMU_DATA.m_attitude.euler.phi == tempang)
LED_TOGGLE(3);
/* if (CHIMU_DATA.m_attitude.euler.phi == tempang)
{
LED_ON(3);
}
@@ -74,12 +102,12 @@ void parse_ins_msg( void )
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);
//EstimatorSetRate(ins_p,ins_q);
//DOWNLINK_SEND_AHRS_EULER(DefaultChannel, &CHIMU_DATA.m_attitude.euler.phi, &CHIMU_DATA.m_attitude.euler.theta, &CHIMU_DATA.m_attitude.euler.psi);
}
}
}
@@ -89,6 +117,13 @@ void parse_ins_msg( void )
//Frequency defined in conf *.xml
void ins_periodic_task( void )
{
// Send Centripetal Corrections
uint8_t quaternions[7] = {0xae, 0xae, 0x01, 0xaa, 0x09, 0x01, 0x39 }; // 25Hz attitude only + SPI
for (int i=0;i<7;i++)
{
InsSend1(quaternions[i]);
}
// Downlink Send
}
sw/airborne/modules/ins/ins_module.h
+2 -1
View File
@@ -84,7 +84,8 @@ void parse_ins_buffer( uint8_t );
#define InsBuffer() InsLink(ChAvailable())
#define ReadInsBuffer() { while (InsLink(ChAvailable())&&!ins_msg_received) parse_ins_buffer(InsLink(Getch())); }
#define InsUartSend1(c) InsLink(Transmit(c))
#define InsSend1(c) InsLink(Transmit(c))
#define InsUartSend1(c) InsSend1(c)
#define InsUartInitParam(_a,_b,_c) InsLink(InitParam(_a,_b,_c))
#define InsUartRunning InsLink(TxRunning)