aleks/hoverboard-sideboard-hack-GD
1
0
Fork 0
mirror of https://github.com/EFeru/hoverboard-sideboard-hack-GD.git synced 2025-08-17 00:56:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Added Platformio support

Browse Source 
- firmware can now be built in Platformio too
- minor bug fixes
- added LED board picture
This commit is contained in:
EmanuelFeru
2020-02-13 18:18:02 +01:00
parent b69942e80e
commit 882d4b0115
31 changed files with 801 additions and 3602 deletions
Download Patch File Download Diff File Expand all files Collapse all files

260
Src/i2c_it.c
View File

@@ -24,43 +24,52 @@
#include "systick.h"
#include "config.h"
void I2C0_EventIRQ_Handler(void)
{
uint16_t k;
if (i2c_ReadWriteCmd == WRITE) { // check for WRITE command
// ======================================== WRITE ========================================
// --------------------------------------------------------------------
// | Master | S | AD+W | | RA | | DATA | | DATA | | P |
// | Slave | | | ACK | | ACK | | ACK | | ACK | |
// --------------------------------------------------------------------
if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C0, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_nRABytes > 0) { // check if the Register Address has been sent
i2c_data_transmit(I2C0, i2c_regAddress); // the master sends the Register Address byte
i2c_nRABytes--;
} else {
if (i2c_nDABytes > 0) {
i2c_data_transmit(I2C0, *i2c_txbuffer++); // the master sends a data byte
i2c_nDABytes--;
for(k=0; k<500; k++); // make some clock cycles delay (otherwise DMP writing will fail!! Reason unknown yet.. could be that writing to MPU6050 memory takes a bit more time)
} else {
i2c_stop_on_bus(I2C0); // the master sends a stop condition to I2C bus
i2c_status = 0; // 0 = Success
i2c_interrupt_disable(I2C0, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV); // disable the I2C0 interrupt
}
}
if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
}
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C0, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_nRABytes > 0) { // check if the Register Address has been sent
i2c_data_transmit(I2C0, i2c_regAddress); // the master sends the Register Address byte
i2c_nRABytes--;
} else {
if (i2c_nDABytes > 0) {
i2c_data_transmit(I2C0, *i2c_txbuffer++); // the master sends a data byte
i2c_nDABytes--;
if (0 == i2c_nDABytes) {
i2c_status = 0; // 0 = Success
}
for(k=0; k<500; k++) {
#ifdef __GNUC__
asm volatile ("nop"); // unoptimizable NOP loop, 500 times to make some clock cycles delay (otherwise DMP writing will fail!! Reason unknown yet.. could be that writing to MPU6050 memory takes a bit more time)
#else
__asm volatile ("nop");
#endif
}
} else {
i2c_stop_on_bus(I2C0); // the master sends a stop condition to I2C bus
i2c_interrupt_disable(I2C0, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV); // disable the I2C0 interrupt
}
}
}
} else if (i2c_ReadWriteCmd == READ) { // check for READ command
@@ -74,55 +83,55 @@ void I2C0_EventIRQ_Handler(void)
// Phase 1 - send the Register Address
if (i2c_nRABytes >= 0) {
if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C0, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
i2c_interrupt_flag_clear(I2C0, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_nRABytes > 0) { // check RABytes
i2c_data_transmit(I2C0, i2c_regAddress); // the master sends the Register Address byte
} else {
i2c_start_on_bus(I2C0); // send start condition
}
i2c_nRABytes--;
} else if (i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_nRABytes > 0) { // check RABytes
i2c_data_transmit(I2C0, i2c_regAddress); // the master sends the Register Address byte
} else {
i2c_start_on_bus(I2C0); // send start condition
}
i2c_nRABytes--;
}
// Phase 2 - read Data
} else {
if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)){ // check if start condition is sent out in master mode
if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_SBSEND)){ // check if start condition is sent out in master mode
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_RECEIVER); // sends slave address with Receive Request
i2c_master_addressing(I2C0, i2c_slaveAddress, I2C_RECEIVER); // sends slave address with Receive Request
}else if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)){ // check if address is sent in master mode
}else if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_ADDSEND)){ // check if address is sent in master mode
if((1 == i2c_nDABytes) || (2 == i2c_nDABytes)){
i2c_ack_config(I2C0, I2C_ACK_DISABLE); // clear the ACKEN before the ADDSEND is cleared
i2c_interrupt_flag_clear(I2C0,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}else{
i2c_interrupt_flag_clear(I2C0,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}
if((1 == i2c_nDABytes) || (2 == i2c_nDABytes)){
i2c_ack_config(I2C0, I2C_ACK_DISABLE); // clear the ACKEN before the ADDSEND is cleared
i2c_interrupt_flag_clear(I2C0,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}else{
i2c_interrupt_flag_clear(I2C0,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}
}else if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_RBNE)){ // check if I2C_DATA is not Empty (Received Byte Not Empty)
if(i2c_nDABytes > 0){
if(3 == i2c_nDABytes){
while(!i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_BTC)); // wait until the second last data byte is received into the shift register
i2c_ack_config(I2C0, I2C_ACK_DISABLE); // send a NACK for the last data byte
}
*i2c_rxbuffer++ = i2c_data_receive(I2C0); // read a data byte from I2C_DATA
i2c_nDABytes--;
if(0 == i2c_nDABytes){
i2c_stop_on_bus(I2C0); // send a stop condition
i2c_status = 0; // 0 = Success
i2c_ack_config(I2C0, I2C_ACK_ENABLE);
i2c_ackpos_config(I2C0, I2C_ACKPOS_CURRENT);
i2c_interrupt_disable(I2C0, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
}
}
}else if(i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_RBNE)){ // check if I2C_DATA is not Empty (Received Byte Not Empty)
if(i2c_nDABytes > 0){
if(3 == i2c_nDABytes){
while(!i2c_interrupt_flag_get(I2C0, I2C_INT_FLAG_BTC)); // wait until the second last data byte is received into the shift register
i2c_ack_config(I2C0, I2C_ACK_DISABLE); // send a NACK for the last data byte
}
*i2c_rxbuffer++ = i2c_data_receive(I2C0); // read a data byte from I2C_DATA
i2c_nDABytes--;
if(0 == i2c_nDABytes){
i2c_stop_on_bus(I2C0); // send a stop condition
i2c_status = 0; // 0 = Success
i2c_ack_config(I2C0, I2C_ACK_ENABLE);
i2c_ackpos_config(I2C0, I2C_ACKPOS_CURRENT);
i2c_interrupt_disable(I2C0, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
}
}
}
@@ -192,32 +201,39 @@ void I2C1_EventIRQ_Handler(void)
// | Slave | | | ACK | | ACK | | ACK | | ACK | |
// --------------------------------------------------------------------
if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C1, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_aux_nRABytes > 0) { // check if the Register Address has been sent
i2c_data_transmit(I2C1, i2c_aux_regAddress); // the master sends the Register Address byte
i2c_aux_nRABytes--;
} else {
if (i2c_aux_nDABytes > 0) {
i2c_data_transmit(I2C1, *i2c_aux_txbuffer++); // the master sends a data byte
i2c_aux_nDABytes--;
for(k=0; k<500; k++); // make some clock cycles delay (otherwise DMP writing will fail!! Reason unknown yet.. could be that writing to MPU6050 memory takes a bit more time)
} else {
i2c_stop_on_bus(I2C1); // the master sends a stop condition to I2C bus
i2c_aux_status = 0; // 0 = Success
i2c_interrupt_disable(I2C1, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV); // disable the I2C0 interrupt
}
}
if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
}
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C1, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_aux_nRABytes > 0) { // check if the Register Address has been sent
i2c_data_transmit(I2C1, i2c_aux_regAddress); // the master sends the Register Address byte
i2c_aux_nRABytes--;
} else {
if (i2c_aux_nDABytes > 0) {
i2c_data_transmit(I2C1, *i2c_aux_txbuffer++); // the master sends a data byte
i2c_aux_nDABytes--;
for(k=0; k<500; k++);
if (0 == i2c_aux_nDABytes) {
i2c_aux_status = 0; // 0 = Success
}
#ifdef __GNUC__
asm volatile ("nop"); // unoptimizable NOP loop, 500 times to make some clock cycles delay (otherwise DMP writing will fail!! Reason unknown yet.. could be that writing to MPU6050 memory takes a bit more time)
#else
__asm volatile ("nop");
#endif
} else {
i2c_stop_on_bus(I2C1); // the master sends a stop condition to I2C bus
i2c_interrupt_disable(I2C1, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV); // disable the I2C0 interrupt
}
}
}
} else if (i2c_aux_ReadWriteCmd == READ) { // check for READ command
@@ -231,55 +247,55 @@ void I2C1_EventIRQ_Handler(void)
// Phase 1 - send the Register Address
if (i2c_aux_nRABytes >= 0) {
if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)) { // check if start condition is sent out in master mode
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_TRANSMITTER); // send slave address with Transmit request
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)) { // check if address is sent in master mode
i2c_interrupt_flag_clear(I2C1, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
i2c_interrupt_flag_clear(I2C1, I2C_INT_FLAG_ADDSEND); // clear ADDSEND bit
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_aux_nRABytes > 0) { // check RABytes
i2c_data_transmit(I2C1, i2c_aux_regAddress); // the master sends the Register Address byte
} else {
i2c_start_on_bus(I2C1); // send start condition
}
i2c_aux_nRABytes--;
} else if (i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_TBE)) { // check if I2C_DATA is empty (Transmitted Byte Empty)
if (i2c_aux_nRABytes > 0) { // check RABytes
i2c_data_transmit(I2C1, i2c_aux_regAddress); // the master sends the Register Address byte
} else {
i2c_start_on_bus(I2C1); // send start condition
}
i2c_aux_nRABytes--;
}
// Phase 2 - read Data
} else {
if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)){ // check if start condition is sent out in master mode
if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_SBSEND)){ // check if start condition is sent out in master mode
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_RECEIVER); // sends slave address with Receive Request
i2c_master_addressing(I2C1, i2c_aux_slaveAddress, I2C_RECEIVER); // sends slave address with Receive Request
}else if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)){ // check if address is sent in master mode
}else if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_ADDSEND)){ // check if address is sent in master mode
if((1 == i2c_aux_nDABytes) || (2 == i2c_aux_nDABytes)){
i2c_ack_config(I2C1, I2C_ACK_DISABLE); // clear the ACKEN before the ADDSEND is cleared
i2c_interrupt_flag_clear(I2C1,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}else{
i2c_interrupt_flag_clear(I2C1,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}
if((1 == i2c_aux_nDABytes) || (2 == i2c_aux_nDABytes)){
i2c_ack_config(I2C1, I2C_ACK_DISABLE); // clear the ACKEN before the ADDSEND is cleared
i2c_interrupt_flag_clear(I2C1,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}else{
i2c_interrupt_flag_clear(I2C1,I2C_INT_FLAG_ADDSEND); // clear the ADDSEND bit
}
}else if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_RBNE)){ // check if I2C_DATA is not Empty (Received Byte Not Empty)
if(i2c_aux_nDABytes > 0){
if(3 == i2c_aux_nDABytes){
while(!i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_BTC)); // wait until the second last data byte is received into the shift register
i2c_ack_config(I2C1, I2C_ACK_DISABLE); // send a NACK for the last data byte
}
*i2c_aux_rxbuffer++ = i2c_data_receive(I2C1); // read a data byte from I2C_DATA
i2c_aux_nDABytes--;
if(0 == i2c_aux_nDABytes){
i2c_stop_on_bus(I2C1); // send a stop condition
i2c_aux_status = 0; // 0 = Success
i2c_ack_config(I2C1, I2C_ACK_ENABLE);
i2c_ackpos_config(I2C1, I2C_ACKPOS_CURRENT);
i2c_interrupt_disable(I2C1, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
}
}else if(i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_RBNE)){ // check if I2C_DATA is not Empty (Received Byte Not Empty)
if(i2c_aux_nDABytes > 0){
if(3 == i2c_aux_nDABytes){
while(!i2c_interrupt_flag_get(I2C1, I2C_INT_FLAG_BTC)); // wait until the second last data byte is received into the shift register
i2c_ack_config(I2C1, I2C_ACK_DISABLE); // send a NACK for the last data byte
}
*i2c_aux_rxbuffer++ = i2c_data_receive(I2C1); // read a data byte from I2C_DATA
i2c_aux_nDABytes--;
if(0 == i2c_aux_nDABytes){
i2c_stop_on_bus(I2C1); // send a stop condition
i2c_aux_status = 0; // 0 = Success
i2c_ack_config(I2C1, I2C_ACK_ENABLE);
i2c_ackpos_config(I2C1, I2C_ACKPOS_CURRENT);
i2c_interrupt_disable(I2C1, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
}
}
}

176
Src/main.c
View File

@@ -31,28 +31,29 @@
#ifdef SERIAL_CONTROL
typedef struct{
uint16_t start;
int16_t roll;
int16_t pitch;
uint16_t start;
int16_t roll;
int16_t pitch;
int16_t yaw;
uint16_t sensors;
uint16_t checksum;
uint16_t sensors;
uint16_t checksum;
} SerialSideboard;
SerialSideboard Sideboard;
#endif
#ifdef SERIAL_FEEDBACK
typedef struct{
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedR;
int16_t speedL;
int16_t speedR_meas;
int16_t speedL_meas;
int16_t batVoltage;
int16_t boardTemp;
int16_t checksum;
uint16_t start;
int16_t cmd1;
int16_t cmd2;
int16_t speedR;
int16_t speedL;
int16_t speedR_meas;
int16_t speedL_meas;
int16_t batVoltage;
int16_t boardTemp;
uint16_t cmdLed;
uint16_t checksum;
} SerialFeedback;
SerialFeedback Feedback;
SerialFeedback NewFeedback;
@@ -61,22 +62,23 @@ static int16_t timeoutCntSerial = 0; // Timeout counter for Rx Serial com
static uint8_t timeoutFlagSerial = 0; // Timeout Flag for Rx Serial command: 0 = OK, 1 = Problem detected (line disconnected or wrong Rx data)
#endif
extern MPU_Data mpu; // holds the MPU-6050 data
ErrStatus mpuStatus = SUCCESS; // holds the MPU-6050 status: SUCCESS or ERROR
extern MPU_Data mpu; // holds the MPU-6050 data
ErrStatus mpuStatus = SUCCESS; // holds the MPU-6050 status: SUCCESS or ERROR
uint8_t userCommand; // holds the user command input
uint8_t sensor1, sensor2; // holds the sensor1 and sensor 2 values
uint8_t userCommand; // holds the user command input
FlagStatus sensor1, sensor2; // holds the sensor1 and sensor 2 values
FlagStatus sensor1_read, sensor2_read; // holds the instantaneous Read for sensor1 and sensor 2
static uint32_t main_loop_counter; // main loop counter to perform task squeduling inside main()
static uint32_t main_loop_counter; // main loop counter to perform task squeduling inside main()
int main(void)
{
systick_config(); // SysTick config
gpio_config(); // GPIO config
usart_config(USART_MAIN, USART_MAIN_BAUD); // USART config
i2c_config(); // I2C config
i2c_nvic_config(); // NVIC peripheral config
systick_config(); // SysTick config
gpio_config(); // GPIO config
usart_config(USART_MAIN, USART_MAIN_BAUD); // USART config
i2c_config(); // I2C config
i2c_nvic_config(); // NVIC peripheral config
#ifdef SERIAL_CONTROL
usart_Tx_DMA_config(USART_MAIN, (uint8_t *)&Sideboard, sizeof(Sideboard));
@@ -85,30 +87,39 @@ int main(void)
usart_Rx_DMA_config(USART_MAIN, (uint8_t *)&NewFeedback, sizeof(NewFeedback));
#endif
introDemoLED(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to initialize.
if(mpu_config()) { // IMU MPU-6050 config
intro_demo_led(100); // Short LEDs intro demo with 100 ms delay. This also gives some time for the MPU-6050 to power-up.
if(mpu_config()) { // IMU MPU-6050 config
mpuStatus = ERROR;
gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on RED LED
}
mpu_handle_input('h'); // Print the User Help commands to serial
else {
gpio_bit_set(LED2_GPIO_Port, LED2_Pin); // Turn on GREEN LED
}
mpu_handle_input('h'); // Print the User Help commands to serial
while(1){
while(1) {
delay_1ms(DELAY_IN_MAIN_LOOP);
// ==================================== LEDs Handling ====================================
// gpio_bit_write(LED4_GPIO_Port, LED4_Pin, (bit_status)(1-gpio_input_bit_get(LED4_GPIO_Port, LED4_Pin))); // Toggle BLUE1 LED
if (SUCCESS == mpuStatus) {
gpio_bit_set(LED2_GPIO_Port, LED2_Pin); // Turn on GREEN LED
} else {
gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on RED LED
}
// toggle_led(LED4_GPIO_Port, LED4_Pin); // Toggle BLUE1 LED
#ifdef SERIAL_FEEDBACK
if (!timeoutFlagSerial) {
if (Feedback.cmdLed & LED1_SET) { gpio_bit_set(LED1_GPIO_Port, LED1_Pin); } else { gpio_bit_reset(LED1_GPIO_Port, LED1_Pin); }
if (Feedback.cmdLed & LED2_SET) { gpio_bit_set(LED2_GPIO_Port, LED2_Pin); } else { gpio_bit_reset(LED2_GPIO_Port, LED2_Pin); }
if (Feedback.cmdLed & LED3_SET) { gpio_bit_set(LED3_GPIO_Port, LED3_Pin); } else { gpio_bit_reset(LED3_GPIO_Port, LED3_Pin); }
if (Feedback.cmdLed & LED4_SET) { gpio_bit_set(LED4_GPIO_Port, LED4_Pin); } else { gpio_bit_reset(LED4_GPIO_Port, LED4_Pin); }
if (Feedback.cmdLed & LED5_SET) { gpio_bit_set(LED5_GPIO_Port, LED5_Pin); } else { gpio_bit_reset(LED5_GPIO_Port, LED5_Pin); }
}
#endif
// ==================================== USER Handling ====================================
#ifdef SERIAL_DEBUG
// Get the user Input as one character from Serial
if(SET == usart_flag_get(USART_MAIN, USART_FLAG_RBNE)) { // Check if Read Buffer Not Empty meanind Serial data is available
if(SET == usart_flag_get(USART_MAIN, USART_FLAG_RBNE)) { // Check if Read Buffer Not Empty meanind Serial data is available
userCommand = usart_data_receive(USART_MAIN);
if (userCommand != 10 && userCommand != 13) { // Do not accept 'new line' (ascii 10) and 'carriage return' (ascii 13) commands
if (userCommand != 10 && userCommand != 13) { // Do not accept 'new line' (ascii 10) and 'carriage return' (ascii 13) commands
log_i("Command = %c\n", userCommand);
mpu_handle_input(userCommand);
}
@@ -122,35 +133,42 @@ int main(void)
mpu_get_data();
}
// Print MPU data to Console
if (main_loop_counter % 50 == 0 && SUCCESS == mpuStatus) {
if (main_loop_counter % 50 == 0) {
mpu_print_to_console();
}
// ==================================== SENSORS Handling ====================================
sensor1_read = gpio_input_bit_get(SENSOR1_GPIO_Port, SENSOR1_Pin);
sensor2_read = gpio_input_bit_get(SENSOR2_GPIO_Port, SENSOR2_Pin);
// SENSOR1
if (gpio_input_bit_get(SENSOR1_GPIO_Port, SENSOR1_Pin)) {
sensor1 = 1;
// Sensor ACTIVE: Do something here
if (sensor1 == RESET && sensor1_read == SET) {
sensor1 = SET;
// Sensor ACTIVE: Do something here (one time task on activation)
gpio_bit_set(LED4_GPIO_Port, LED4_Pin);
consoleLog("-- SENSOR 1 Active --\n");
delay_1ms(50);
} else {
sensor1 = 0;
consoleLog("-- SENSOR 1 Active --\n");
} else if(sensor1 == SET && sensor1_read == RESET) {
sensor1 = RESET;
gpio_bit_reset(LED4_GPIO_Port, LED4_Pin);
}
// SENSOR2
if (gpio_input_bit_get(SENSOR2_GPIO_Port, SENSOR2_Pin)) {
sensor2 = 1;
// Sensor ACTIVE: Do something here
if (sensor2 == RESET && sensor2_read == SET) {
sensor2 = SET;
// Sensor ACTIVE: Do something here (one time task on activation)
gpio_bit_set(LED5_GPIO_Port, LED5_Pin);
consoleLog("-- SENSOR 2 Active --\n");
delay_1ms(50);
} else {
sensor2 = 0;
} else if (sensor2 == SET && sensor2_read == RESET) {
sensor2 = RESET;
gpio_bit_reset(LED5_GPIO_Port, LED5_Pin);
}
if (sensor1 == SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
if (sensor2 == SET) {
// Sensor ACTIVE: Do something here (continuous task)
}
// ==================================== SERIAL Tx/Rx Handling ====================================
@@ -161,8 +179,8 @@ int main(void)
Sideboard.start = (uint16_t)SERIAL_START_FRAME;
Sideboard.roll = (int16_t)mpu.euler.roll;
Sideboard.pitch = (int16_t)mpu.euler.pitch;
Sideboard.yaw = (int16_t)mpu.euler.yaw;
Sideboard.sensors = (uint16_t)(sensor1 | (sensor2 << 1));
Sideboard.yaw = (int16_t)mpu.euler.yaw;
Sideboard.sensors = (uint16_t)(sensor1 | (sensor2 << 1));
Sideboard.checksum = (uint16_t)(Sideboard.start ^ Sideboard.roll ^ Sideboard.pitch ^ Sideboard.yaw ^ Sideboard.sensors);
dma_channel_disable(DMA_CH3);
@@ -175,33 +193,33 @@ int main(void)
#ifdef SERIAL_FEEDBACK
uint16_t checksum;
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp);
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.cmdLed);
if (NewFeedback.start == SERIAL_START_FRAME && NewFeedback.checksum == checksum) {
if (timeoutFlagSerial) { // Check for previous timeout flag
if (timeoutCntSerial-- <= 0) // Timeout de-qualification
timeoutFlagSerial = 0; // Timeout flag cleared
} else {
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); // Copy the new data
NewFeedback.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle
timeoutCntSerial = 0; // Reset the timeout counter
}
} else {
if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial = 1; // Timeout detected
timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value
}
// Check periodically the received Start Frame. If it is NOT OK, most probably we are out-of-sync. Try to re-sync by reseting the DMA
if (main_loop_counter % 50 == 0 && NewFeedback.start != SERIAL_START_FRAME && NewFeedback.start != 0xFFFF) {
dma_channel_disable(DMA_CH4);
usart_Rx_DMA_config(USART_MAIN, (uint8_t *)&NewFeedback, sizeof(NewFeedback));
}
if (timeoutFlagSerial) { // Check for previous timeout flag
if (timeoutCntSerial-- <= 0) // Timeout de-qualification
timeoutFlagSerial = 0; // Timeout flag cleared
} else {
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); // Copy the new data
NewFeedback.start = 0xFFFF; // Change the Start Frame for timeout detection in the next cycle
timeoutCntSerial = 0; // Reset the timeout counter
}
} else {
if (timeoutCntSerial++ >= SERIAL_TIMEOUT) { // Timeout qualification
timeoutFlagSerial = 1; // Timeout detected
timeoutCntSerial = SERIAL_TIMEOUT; // Limit timout counter value
}
if (timeoutFlagSerial) { // In case of timeout bring the system to a Safe State and indicate error if desired
gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on Red LED
} else {
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin); // Follow the Normal behavior
}
// Check periodically the received Start Frame. If it is NOT OK, most probably we are out-of-sync. Try to re-sync by reseting the DMA
if (main_loop_counter % 50 == 0 && NewFeedback.start != SERIAL_START_FRAME && NewFeedback.start != 0xFFFF) {
dma_channel_disable(DMA_CH4);
usart_Rx_DMA_config(USART_MAIN, (uint8_t *)&NewFeedback, sizeof(NewFeedback));
}
}
if (timeoutFlagSerial) { // In case of timeout bring the system to a Safe State and indicate error if desired
gpio_bit_set(LED1_GPIO_Port, LED1_Pin); // Turn on Red LED
} else {
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin); // Follow the Normal behavior
}
#endif

352
Src/mpu6050.c
View File

@@ -522,13 +522,13 @@ const struct hw_s hw = {
const struct test_s test = {
.gyro_sens = 32768/250,
.accel_sens = 32768/2, // FSR = +-2G = 16384 LSB/G
.accel_sens = 32768/2, // FSR = +-2G = 16384 LSB/G
.reg_rate_div = 0, // 1kHz.
.reg_lpf = 2, // 92Hz low pass filter
.reg_gyro_fsr = 0, // 250dps.
.reg_accel_fsr = 0x0, // Accel FSR setting = 2g.
.wait_ms = 200, // 200ms stabilization time
.packet_thresh = 200, // 200 samples
.packet_thresh = 200, // 200 samples
.min_dps = 20.f, // 20 dps for Gyro Criteria C
.max_dps = 60.f, // Must exceed 60 dps threshold for Gyro Criteria B
.max_gyro_var = .5f, // Must exceed +50% variation for Gyro Criteria A
@@ -2183,17 +2183,17 @@ static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
for (i = 0; i < 3; i++) {
st_shift_cust[i] = bias_st[i] - bias_regular[i];
if(debug) {
log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n",
st_shift_cust[i]/1.f, bias_regular[i]/1.f,
bias_st[i]/1.f);
log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f);
log_i("Bias_Shift=%ld, Bias_Reg=%ld, Bias_HWST=%ld\r\n",
(long)st_shift_cust[i], bias_regular[i],
bias_st[i]);
log_i("OTP value: %ld\r\n", (long)ct_shift_prod[i]);
}
st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i] - 1.f;
if(debug)
log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f,
test.max_accel_var/1.f);
log_i("ratio=%ld, threshold=%ld\r\n", (long)st_shift_ratio[i],
(long)test.max_accel_var);
if (fabs(st_shift_ratio[i]) > test.max_accel_var) {
if(debug)
@@ -2209,15 +2209,15 @@ static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
if(debug) {
log_i("ACCEL:CRITERIA B\r\n");
log_i("Min MG: %7.4f\r\n", accel_st_al_min/1.f);
log_i("Max MG: %7.4f\r\n", accel_st_al_max/1.f);
log_i("Min MG: %ld\r\n", (long)accel_st_al_min);
log_i("Max MG: %ld\r\n", (long)accel_st_al_max);
}
for (i = 0; i < 3; i++) {
st_shift_cust[i] = bias_st[i] - bias_regular[i];
if(debug)
log_i("Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\n", st_shift_cust[i]/1.f, bias_st[i]/1.f, bias_regular[i]/1.f);
log_i("Bias_shift=%ld, st=%ld, reg=%ld\n", (long)st_shift_cust[i], bias_st[i], bias_regular[i]);
if(st_shift_cust[i] < accel_st_al_min || st_shift_cust[i] > accel_st_al_max) {
if(debug)
log_i("Accel FAIL axis:%d <= 225mg or >= 675mg\n", i);
@@ -2230,7 +2230,7 @@ static int accel_6500_self_test(long *bias_regular, long *bias_st, int debug)
/* Self Test Pass/Fail Criteria C */
accel_offset_max = test.max_g_offset * 65536.f;
if(debug)
log_i("Accel:CRITERIA C: bias less than %7.4f\n", accel_offset_max/1.f);
log_i("Accel:CRITERIA C: bias less than %ld\n", (long)accel_offset_max);
for (i = 0; i < 3; i++) {
if(fabs(bias_regular[i]) > accel_offset_max) {
if(debug)
@@ -2279,17 +2279,17 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
st_shift_cust[i] = bias_st[i] - bias_regular[i];
if(debug) {
log_i("Bias_Shift=%7.4f, Bias_Reg=%7.4f, Bias_HWST=%7.4f\r\n",
st_shift_cust[i]/1.f, bias_regular[i]/1.f,
bias_st[i]/1.f);
log_i("OTP value: %7.4f\r\n", ct_shift_prod[i]/1.f);
log_i("Bias_Shift=%ld, Bias_Reg=%ld, Bias_HWST=%ld\r\n",
(long)st_shift_cust[i], bias_regular[i],
bias_st[i]);
log_i("OTP value: %ld\r\n", (long)ct_shift_prod[i]);
}
st_shift_ratio[i] = st_shift_cust[i] / ct_shift_prod[i];
if(debug)
log_i("ratio=%7.4f, threshold=%7.4f\r\n", st_shift_ratio[i]/1.f,
test.max_gyro_var/1.f);
log_i("ratio=%ld, threshold=%ld\r\n", (long)st_shift_ratio[i],
(long)test.max_gyro_var);
if (fabs(st_shift_ratio[i]) < test.max_gyro_var) {
if(debug)
@@ -2304,14 +2304,14 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
if(debug) {
log_i("GYRO:CRITERIA B\r\n");
log_i("Max DPS: %7.4f\r\n", gyro_st_al_max/1.f);
log_i("Max DPS: %ld\r\n", (long)gyro_st_al_max);
}
for (i = 0; i < 3; i++) {
st_shift_cust[i] = bias_st[i] - bias_regular[i];
if(debug)
log_i("Bias_shift=%7.4f, st=%7.4f, reg=%7.4f\n", st_shift_cust[i]/1.f, bias_st[i]/1.f, bias_regular[i]/1.f);
log_i("Bias_shift=%ld, st=%ld, reg=%ld\n", (long)st_shift_cust[i], bias_st[i], bias_regular[i]);
if(st_shift_cust[i] < gyro_st_al_max) {
if(debug)
log_i("GYRO FAIL axis:%d greater than 60dps\n", i);
@@ -2324,7 +2324,7 @@ static int gyro_6500_self_test(long *bias_regular, long *bias_st, int debug)
/* Self Test Pass/Fail Criteria C */
gyro_offset_max = test.min_dps * 65536.f;
if(debug)
log_i("Gyro:CRITERIA C: bias less than %7.4f\n", gyro_offset_max/1.f);
log_i("Gyro:CRITERIA C: bias less than %ld\n", (long)gyro_offset_max);
for (i = 0; i < 3; i++) {
if(fabs(bias_regular[i]) > gyro_offset_max) {
if(debug)
@@ -2456,8 +2456,8 @@ static int get_st_6500_biases(long *gyro, long *accel, unsigned char hw_test, in
if(debug) {
log_i("Accel offset data HWST bit=%d: %7.4f %7.4f %7.4f\r\n", hw_test, accel[0]/65536.f, accel[1]/65536.f, accel[2]/65536.f);
log_i("Gyro offset data HWST bit=%d: %7.4f %7.4f %7.4f\r\n", hw_test, gyro[0]/65536.f, gyro[1]/65536.f, gyro[2]/65536.f);
log_i("Accel offset data HWST bit=%d: %ld %ld %ld\r\n", hw_test, accel[0], accel[1], accel[2]);
log_i("Gyro offset data HWST bit=%d: %ld %ld %ld\r\n", hw_test, gyro[0], gyro[1], gyro[2]);
}
return 0;
@@ -2872,9 +2872,9 @@ static int setup_compass(void)
if (akm_addr > 0x0F) {
/* TODO: Handle this case in all compass-related functions. */
#ifdef SERIAL_DEBUG
log_i("Compass not found.\n");
#endif
#ifdef SERIAL_DEBUG
log_i("Compass not found.\n");
#endif
return -1;
}
@@ -3227,18 +3227,18 @@ void mpu_start_self_test(void)
result = mpu_run_self_test(gyro, accel);
#endif
if (result == 0x7) {
#ifdef SERIAL_DEBUG
consoleLog("Passed!\n");
log_i("accel: %7.4f %7.4f %7.4f\n",
accel[0]/65536.f,
accel[1]/65536.f,
accel[2]/65536.f);
log_i("gyro: %7.4f %7.4f %7.4f\n",
gyro[0]/65536.f,
gyro[1]/65536.f,
gyro[2]/65536.f);
/* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/
#endif
#ifdef SERIAL_DEBUG
consoleLog("Passed!\n");
log_i("accel: %ld %ld %ld\n",
accel[0],
accel[1],
accel[2]);
log_i("gyro: %ld %ld %ld\n",
gyro[0],
gyro[1],
gyro[2]);
/* Test passed. We can trust the gyro data here, so now we need to update calibrated data*/
#endif
#ifdef USE_CAL_HW_REGISTERS
/*
@@ -3302,17 +3302,17 @@ void mpu_setup_gyro(void)
unsigned char mask = 0, lp_accel_was_on = 0;
if (hal.sensors & ACCEL_ON) {
mask |= INV_XYZ_ACCEL;
consoleLog("Accel sensor On.\n");
consoleLog("Accel sensor On.\n");
} else {
consoleLog("Accel sensor Off.\n");
}
consoleLog("Accel sensor Off.\n");
}
if (hal.sensors & GYRO_ON) {
mask |= INV_XYZ_GYRO;
lp_accel_was_on |= hal.lp_accel_mode;
consoleLog("Gyro sensor On.\n");
consoleLog("Gyro sensor On.\n");
} else {
consoleLog("Gyro sensor Off.\n");
}
consoleLog("Gyro sensor Off.\n");
}
#ifdef COMPASS_ENABLED
if (hal.sensors & COMPASS_ON) {
mask |= INV_XYZ_COMPASS;
@@ -3338,35 +3338,35 @@ void mpu_setup_gyro(void)
/* =========================== MPU-6050 Configuration =========================== */
int mpu_config(void)
{
consoleLog("-- Configuring MPU6050... ");
if(mpu_init()) {
consoleLog("FAIL (MPU).\n");
return -1;
}
consoleLog("-- Configuring MPU6050... ");
if(mpu_init()) {
consoleLog("FAIL (MPU).\n");
return -1;
}
/* Get/set hardware configuration. Start gyro. */
/* Get/set hardware configuration. Start gyro. */
/* Wake up all sensors. */
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sensors(INV_XYZ_GYRO | INV_XYZ_ACCEL);
/* Push both gyro and accel data into the FIFO. */
/* Push both gyro and accel data into the FIFO. */
mpu_configure_fifo(INV_XYZ_GYRO | INV_XYZ_ACCEL);
mpu_set_sample_rate(MPU_DEFAULT_HZ);
/* Read back configuration in case it was set improperly. */
/* Read back configuration in case it was set improperly. */
// mpu_get_sample_rate(&gyro_rate);
// mpu_get_gyro_fsr(&gyro_fsr);
// mpu_get_accel_fsr(&accel_fsr);
/* Initialize HAL state variables. */
hal.sensors = ACCEL_ON | GYRO_ON;
hal.dmp_on = 0;
hal.report = 0;
/* Initialize HAL state variables. */
hal.sensors = ACCEL_ON | GYRO_ON;
hal.dmp_on = 0;
hal.report = 0;
hal.next_pedo_ms = 0;
hal.next_temp_ms = 0;
#ifdef MPU_DMP_ENABLE
/* To initialize the DMP:
/* To initialize the DMP:
* 1. Call dmp_load_motion_driver_firmware(). This pushes the DMP image in
* inv_mpu_dmp_motion_driver.h into the MPU memory.
* 2. Push the gyro and accel orientation matrix to the DMP.
@@ -3396,11 +3396,11 @@ int mpu_config(void)
* DMP_FEATURE_SEND_CAL_GYRO: Add calibrated gyro data to the FIFO. Cannot
* be used in combination with DMP_FEATURE_SEND_RAW_GYRO.
*/
consoleLog(" writing DMP... ");
if (dmp_load_motion_driver_firmware()) {
consoleLog(" FAIL (DMP) --\r\n");
return -1;
}
consoleLog(" writing DMP... ");
if (dmp_load_motion_driver_firmware()) {
consoleLog(" FAIL (DMP) --\r\n");
return -1;
}
// dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_pdata.orientation));
dmp_register_tap_cb(mpu_tap_func);
dmp_register_android_orient_cb(mpu_android_orient_func);
@@ -3425,8 +3425,8 @@ int mpu_config(void)
hal.dmp_on = 1;
#endif
consoleLog(" OK --\r\n");
return 0;
consoleLog(" OK --\r\n");
return 0;
}
@@ -3453,93 +3453,93 @@ void mpu_get_data(void)
* Let's make them timer-based.
*/
if (timestamp > hal.next_temp_ms) {
hal.next_temp_ms = timestamp + TEMP_READ_MS;
new_temp = 1;
hal.next_temp_ms = timestamp + TEMP_READ_MS;
new_temp = 1;
}
if (hal.new_gyro && hal.dmp_on) {
short gyro[3], accel[3], sensors;
static long quat[4], temperature;
unsigned char more;
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (!more)
hal.new_gyro = 0;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
if (sensors & INV_WXYZ_QUAT) {
mpu.quat.w = quat[0];
mpu.quat.x = quat[1];
mpu.quat.y = quat[2];
mpu.quat.z = quat[3];
mpu_calc_euler_angles(); // Calculate Euler angles
new_data = 1;
}
short gyro[3], accel[3], sensors;
static long quat[4], temperature;
unsigned char more;
/* This function gets new data from the FIFO when the DMP is in
* use. The FIFO can contain any combination of gyro, accel,
* quaternion, and gesture data. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then
* the FIFO isn't being filled with accel data.
* The driver parses the gesture data to determine if a gesture
* event has occurred; on an event, the application will be notified
* via a callback (assuming that a callback function was properly
* registered). The more parameter is non-zero if there are
* leftover packets in the FIFO.
*/
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (!more)
hal.new_gyro = 0;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
if (sensors & INV_WXYZ_QUAT) {
mpu.quat.w = quat[0];
mpu.quat.x = quat[1];
mpu.quat.y = quat[2];
mpu.quat.z = quat[3];
mpu_calc_euler_angles(); // Calculate Euler angles
new_data = 1;
}
} else if (hal.new_gyro) {
short gyro[3], accel[3];
long temperature;
unsigned char sensors, more;
/* This function gets new data from the FIFO. The FIFO can contain
* gyro, accel, both, or neither. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
* being filled with accel data. The more parameter is non-zero if
* there are leftover packets in the FIFO. The HAL can use this
* information to increase the frequency at which this function is
* called.
*/
hal.new_gyro = 0;
mpu_read_fifo(gyro, accel, &sensor_timestamp, &sensors, &more);
if (more)
hal.new_gyro = 1;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
short gyro[3], accel[3];
long temperature;
unsigned char sensors, more;
/* This function gets new data from the FIFO. The FIFO can contain
* gyro, accel, both, or neither. The sensors parameter tells the
* caller which data fields were actually populated with new data.
* For example, if sensors == INV_XYZ_GYRO, then the FIFO isn't
* being filled with accel data. The more parameter is non-zero if
* there are leftover packets in the FIFO. The HAL can use this
* information to increase the frequency at which this function is
* called.
*/
hal.new_gyro = 0;
mpu_read_fifo(gyro, accel, &sensor_timestamp, &sensors, &more);
if (more)
hal.new_gyro = 1;
if (sensors & INV_XYZ_GYRO) {
mpu.gyro.x = gyro[0];
mpu.gyro.y = gyro[1];
mpu.gyro.z = gyro[2];
new_data = 1;
if (new_temp) {
new_temp = 0;
mpu_get_temperature(&temperature, &sensor_timestamp);
mpu.temp = (int16_t)((temperature*100) >> 16); // Convert temperature[q16] to temperature*100[degC]
}
}
if (sensors & INV_XYZ_ACCEL) {
mpu.accel.x = accel[0];
mpu.accel.y = accel[1];
mpu.accel.z = accel[2];
new_data = 1;
}
}
if (new_data) {
// do something if needed
// do something if needed
}
}
@@ -3609,7 +3609,7 @@ void mpu_calc_euler_angles(void) {
// Calculate Euler angles: source <https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles>
roll = atan2(2*(w*x + y*z), 1 - 2*(x*x + y*y)); // roll (x-axis rotation)
pitch = asin(2*(w*y - z*x)); // pitch (y-axis rotation)
pitch = asin(2*(w*y - z*x)); // pitch (y-axis rotation)
yaw = atan2(2*(w*z + x*y), 1 - 2*(y*y + z*z)); // yaw (z-axis rotation)
// Convert [rad] to [deg*100]
@@ -3678,30 +3678,30 @@ void mpu_handle_input(char c)
switch (c) {
/* This command prints the Help text. */
case 'h':
consoleLog("=================== HELP COMMANDS ===================\n");
consoleLog("h: Print Help commands\n");
consoleLog("8: Set Accelerometer sensor on/off\n");
consoleLog("9: Set Gyroscope sensor on/off\n");
consoleLog("r: Print Registers value\n");
consoleLog("a: Print Accelerometer data\n");
consoleLog("g: Print Gyroscope data\n");
consoleLog("q: Print Quaternion data\n");
consoleLog("e: Print Euler angles in degree * 100\n");
consoleLog("t: Print Temperature in degC * 100\n");
consoleLog("p: Print Pedometer data\n");
consoleLog("0: Reset Pedometer\n");
consoleLog("1: Set DMP/MPU frequency 10 Hz\n");
consoleLog("2: Set DMP/MPU frequency 20 Hz\n");
consoleLog("3: Set DMP/MPU frequency 40 Hz\n");
consoleLog("4: Set DMP/MPU frequency 50 Hz\n");
consoleLog("5: Set DMP/MPU frequency 100 Hz\n");
consoleLog(",: Set DMP interrupt to gesture event only\n");
consoleLog(".: Set DMP interrupt to continuous\n");
consoleLog("f: Set DMP on/off\n");
consoleLog("v: Set Quaternion on/off\n");
consoleLog("w: Test out low-power accel mode\n");
consoleLog("s: Run self-test (device must be facing up or down)\n");
consoleLog("=====================================================\n");
consoleLog("=================== HELP COMMANDS ===================\n");
consoleLog("h: Print Help commands\n");
consoleLog("8: Set Accelerometer sensor on/off\n");
consoleLog("9: Set Gyroscope sensor on/off\n");
consoleLog("r: Print Registers value\n");
consoleLog("a: Print Accelerometer data\n");
consoleLog("g: Print Gyroscope data\n");
consoleLog("q: Print Quaternion data\n");
consoleLog("e: Print Euler angles in degree * 100\n");
consoleLog("t: Print Temperature in degC * 100\n");
consoleLog("p: Print Pedometer data\n");
consoleLog("0: Reset Pedometer\n");
consoleLog("1: Set DMP/MPU frequency 10 Hz\n");
consoleLog("2: Set DMP/MPU frequency 20 Hz\n");
consoleLog("3: Set DMP/MPU frequency 40 Hz\n");
consoleLog("4: Set DMP/MPU frequency 50 Hz\n");
consoleLog("5: Set DMP/MPU frequency 100 Hz\n");
consoleLog(",: Set DMP interrupt to gesture event only\n");
consoleLog(".: Set DMP interrupt to continuous\n");
consoleLog("f: Set DMP on/off\n");
consoleLog("v: Set Quaternion on/off\n");
consoleLog("w: Test out low-power accel mode\n");
consoleLog("s: Run self-test (device must be facing up or down)\n");
consoleLog("=====================================================\n");
break;
/* These commands turn off individual sensors. */
@@ -3714,14 +3714,14 @@ void mpu_handle_input(char c)
mpu_setup_gyro();
break;
/* This command prints out the value of each gyro register for debugging.
/* This command prints out the value of each gyro register for debugging.
* If logging is disabled, this function has no effect.
*/
case 'r':
mpu_reg_dump();
break;
/* These commands print individual sensor data. */
/* These commands print individual sensor data. */
case 'a':
hal.report ^= PRINT_ACCEL;
break;
@@ -3850,9 +3850,9 @@ void mpu_handle_input(char c)
/* Test out low-power accel mode. */
case 'w':
if (hal.dmp_on) {
consoleLog("Warning: For low-power mode, DMP needs to be disabled.\n");
consoleLog("Warning: For low-power mode, DMP needs to be disabled.\n");
break; /* LP accel is not compatible with the DMP. */
}
}
mpu_lp_accel_mode(20);
/* When LP accel mode is enabled, the driver automatically configures
* the hardware for latched interrupts. However, the MCU sometimes
@@ -3869,8 +3869,8 @@ void mpu_handle_input(char c)
break;
/* The hardware self test is completely localized in the gyro driver.
* Logging is assumed to be enabled; otherwise, a couple LEDs could
* probably be used here to display the test results.
* Logging is assumed to be enabled; otherwise, a couple LEDs could
* probably be used here to display the test results.
*/
case 's':
mpu_start_self_test();

38
Src/setup.c
View File

@@ -29,17 +29,17 @@
extern volatile ErrStatus status;
// Private variables
static rcu_periph_enum USART_CLK[USARTn] = {USART_AUX_CLK,
USART_MAIN_CLK
};
static rcu_periph_enum USART_CLK[USARTn] = { USART_AUX_CLK,
USART_MAIN_CLK
};
static uint32_t USART_TX_PIN[USARTn] = {USART_AUX_TX_PIN,
USART_MAIN_TX_PIN
};
static uint32_t USART_TX_PIN[USARTn] = { USART_AUX_TX_PIN,
USART_MAIN_TX_PIN
};
static uint32_t USART_RX_PIN[USARTn] = {USART_AUX_RX_PIN,
USART_MAIN_RX_PIN
};
static uint32_t USART_RX_PIN[USARTn] = { USART_AUX_RX_PIN,
USART_MAIN_RX_PIN
};
void gpio_config(void) {
@@ -82,11 +82,11 @@ void gpio_config(void) {
/* =========================== Configure I2C GPIOs =========================== */
/* enable I2C clock */
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(MPU_RCU_I2C);
rcu_periph_clock_enable(MPU_RCU_I2C);
/* connect PB6 to I2C_SCL and PB7 to I2C_SDA */
gpio_af_set(MPU_SCL_GPIO_Port, GPIO_AF_1, MPU_SCL_Pin);
gpio_af_set(MPU_SDA_GPIO_Port, GPIO_AF_1, MPU_SDA_Pin);
gpio_af_set(MPU_SCL_GPIO_Port, GPIO_AF_1, MPU_SCL_Pin);
gpio_af_set(MPU_SDA_GPIO_Port, GPIO_AF_1, MPU_SDA_Pin);
/* configure GPIO port */
gpio_mode_set(MPU_SCL_GPIO_Port, GPIO_MODE_AF, GPIO_PUPD_PULLUP, MPU_SCL_Pin);
@@ -195,7 +195,7 @@ void usart_Tx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t Size) {
// --------------------------- TX Channel ---------------------------
/* enable DMA clock */
rcu_periph_clock_enable(RCU_DMA);
rcu_periph_clock_enable(RCU_DMA);
/* deinitialize DMA channel2 */
dma_deinit(DMA_CH3);
@@ -203,11 +203,11 @@ void usart_Tx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t Size) {
dma_init_struct.memory_addr = (uint32_t)pData;
dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT;
dma_init_struct.number = Size;
dma_init_struct.number = Size;
dma_init_struct.periph_addr = USART1_TDATA_ADDRESS;
dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT;
dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH; // Priorities: *_LOW, *_MEDIUM, *_HIGH, *_ULTRA_HIGH,
dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH; // Priorities: *_LOW, *_MEDIUM, *_HIGH, *_ULTRA_HIGH,
dma_init(DMA_CH3, dma_init_struct);
/* configure DMA mode */
@@ -237,11 +237,11 @@ void usart_Rx_DMA_config(uint32_t selUSART, uint8_t *pData, uint32_t Size) {
dma_init_struct.memory_addr = (uint32_t)pData;
dma_init_struct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
dma_init_struct.memory_width = DMA_MEMORY_WIDTH_8BIT;
dma_init_struct.number = Size;
dma_init_struct.number = Size;
dma_init_struct.periph_addr = USART1_RDATA_ADDRESS;
dma_init_struct.periph_inc = DMA_PERIPH_INCREASE_DISABLE;
dma_init_struct.periph_width = DMA_PERIPHERAL_WIDTH_8BIT;
dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH; // Priorities: *_LOW, *_MEDIUM, *_HIGH, *_ULTRA_HIGH,
dma_init_struct.priority = DMA_PRIORITY_ULTRA_HIGH; // Priorities: *_LOW, *_MEDIUM, *_HIGH, *_ULTRA_HIGH,
dma_init(DMA_CH4, dma_init_struct);
/* configure DMA mode */
@@ -271,8 +271,8 @@ void i2c_config(void) {
#ifdef AUX45_USE_I2C
/* I2C clock configure */
//i2c_clock_config(AUX_I2C, AUX_I2C_SPEED, I2C_DTCY_2); // I2C duty cycle in fast mode
i2c_clock_config(AUX_I2C, AUX_I2C_SPEED, I2C_DTCY_16_9); // I2C duty cycle in fast mode plus
//i2c_clock_config(AUX_I2C, AUX_I2C_SPEED, I2C_DTCY_2); // I2C duty cycle in fast mode
i2c_clock_config(AUX_I2C, AUX_I2C_SPEED, I2C_DTCY_16_9); // I2C duty cycle in fast mode plus
/* I2C address configure */
i2c_mode_addr_config(AUX_I2C, I2C_I2CMODE_ENABLE, I2C_ADDFORMAT_7BITS, AUX_I2C_OWN_ADDRESS7);
/* enable I2C */

8
Src/systick.c
View File

@@ -88,21 +88,21 @@ void delay_decrement(void)
/*!
\brief tick count increment in ms
\param[in] none
\param[out] none
\param[out] none
\retval none
*/
void tick_count_increment()
{
tick_count_ms++;
tick_count_ms++;
}
/*!
\brief get tick count in ms
\param[in] *count: pointer to count
\param[out] none
\param[out] none
\retval none
*/
void get_tick_count_ms(unsigned long *count)
{
*count = tick_count_ms;
*count = tick_count_ms;
}

151
Src/util.c
View File

@@ -27,26 +27,26 @@
// MAIN I2C variables
volatile int8_t i2c_status;
volatile i2c_cmd i2c_ReadWriteCmd;
volatile uint8_t i2c_regAddress;
volatile uint8_t i2c_slaveAddress;
volatile uint8_t* i2c_txbuffer;
volatile uint8_t* i2c_rxbuffer;
volatile uint8_t i2c_nDABytes;
volatile int8_t i2c_nRABytes;
volatile uint8_t buffer[14];
volatile int8_t i2c_status;
volatile i2c_cmd i2c_ReadWriteCmd;
volatile uint8_t i2c_regAddress;
volatile uint8_t i2c_slaveAddress;
volatile uint8_t* i2c_txbuffer;
volatile uint8_t* i2c_rxbuffer;
volatile uint8_t i2c_nDABytes;
volatile int8_t i2c_nRABytes;
volatile uint8_t buffer[14];
#ifdef AUX45_USE_I2C
// AUX I2C variables
volatile int8_t i2c_aux_status;
volatile i2c_cmd i2c_aux_ReadWriteCmd;
volatile uint8_t i2c_aux_regAddress;
volatile uint8_t i2c_aux_slaveAddress;
volatile uint8_t* i2c_aux_txbuffer;
volatile uint8_t* i2c_aux_rxbuffer;
volatile uint8_t i2c_aux_nDABytes;
volatile int8_t i2c_aux_nRABytes;
volatile int8_t i2c_aux_status;
volatile i2c_cmd i2c_aux_ReadWriteCmd;
volatile uint8_t i2c_aux_regAddress;
volatile uint8_t i2c_aux_slaveAddress;
volatile uint8_t* i2c_aux_txbuffer;
volatile uint8_t* i2c_aux_rxbuffer;
volatile uint8_t i2c_aux_nDABytes;
volatile int8_t i2c_aux_nRABytes;
#endif
@@ -55,49 +55,68 @@ volatile int8_t i2c_aux_nRABytes;
void consoleLog(char *message)
{
#ifdef SERIAL_DEBUG
log_i("%s", message);
log_i("%s", message);
#endif
}
/* retarget the C library printf function to the USART */
int fputc(int ch, FILE *f)
#ifdef SERIAL_DEBUG
#ifdef __GNUC__
#define PUTCHAR_PROTOTYPE int __io_putchar(int ch)
#else
#define PUTCHAR_PROTOTYPE int fputc(int ch, FILE *f)
#endif
PUTCHAR_PROTOTYPE {
usart_data_transmit(USART_MAIN, (uint8_t)ch);
while(RESET == usart_flag_get(USART_MAIN, USART_FLAG_TBE));
return ch;
}
#ifdef __GNUC__
int _write(int file, char *data, int len) {
int i;
for (i = 0; i < len; i++) { __io_putchar( *data++ );}
return len;
}
#endif
#endif
void toggle_led(uint32_t gpio_periph, uint32_t pin)
{
usart_data_transmit(USART_MAIN, (uint8_t)ch);
while(RESET == usart_flag_get(USART_MAIN, USART_FLAG_TBE));
return ch;
GPIO_OCTL(gpio_periph) ^= pin;
}
void introDemoLED(uint32_t tDelay)
void intro_demo_led(uint32_t tDelay)
{
int i;
int i;
for (i = 0; i < 6; i++) {
gpio_bit_set(LED1_GPIO_Port, LED1_Pin);
gpio_bit_reset(LED3_GPIO_Port, LED3_Pin);
delay_1ms(tDelay);
gpio_bit_set(LED2_GPIO_Port, LED2_Pin);
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin);
delay_1ms(tDelay);
gpio_bit_set(LED3_GPIO_Port, LED3_Pin);
gpio_bit_reset(LED2_GPIO_Port, LED2_Pin);
delay_1ms(tDelay);
}
for (i = 0; i < 6; i++) {
gpio_bit_set(LED1_GPIO_Port, LED1_Pin);
gpio_bit_reset(LED3_GPIO_Port, LED3_Pin);
delay_1ms(tDelay);
gpio_bit_set(LED2_GPIO_Port, LED2_Pin);
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin);
delay_1ms(tDelay);
gpio_bit_set(LED3_GPIO_Port, LED3_Pin);
gpio_bit_reset(LED2_GPIO_Port, LED2_Pin);
delay_1ms(tDelay);
}
for (i = 0; i < 2; i++) {
gpio_bit_set(LED1_GPIO_Port, LED1_Pin);
gpio_bit_set(LED2_GPIO_Port, LED2_Pin);
gpio_bit_set(LED3_GPIO_Port, LED3_Pin);
gpio_bit_set(LED4_GPIO_Port, LED4_Pin);
gpio_bit_set(LED5_GPIO_Port, LED5_Pin);
delay_1ms(tDelay);
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin);
gpio_bit_reset(LED2_GPIO_Port, LED2_Pin);
gpio_bit_reset(LED3_GPIO_Port, LED3_Pin);
gpio_bit_reset(LED4_GPIO_Port, LED4_Pin);
gpio_bit_reset(LED5_GPIO_Port, LED5_Pin);
}
for (i = 0; i < 2; i++) {
gpio_bit_set(LED1_GPIO_Port, LED1_Pin);
gpio_bit_set(LED2_GPIO_Port, LED2_Pin);
gpio_bit_set(LED3_GPIO_Port, LED3_Pin);
gpio_bit_set(LED4_GPIO_Port, LED4_Pin);
gpio_bit_set(LED5_GPIO_Port, LED5_Pin);
delay_1ms(tDelay);
gpio_bit_reset(LED1_GPIO_Port, LED1_Pin);
gpio_bit_reset(LED2_GPIO_Port, LED2_Pin);
gpio_bit_reset(LED3_GPIO_Port, LED3_Pin);
gpio_bit_reset(LED4_GPIO_Port, LED4_Pin);
gpio_bit_reset(LED5_GPIO_Port, LED5_Pin);
}
}
@@ -113,12 +132,12 @@ int8_t i2c_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_
i2c_ReadWriteCmd = WRITE;
// assign inputs
i2c_status = -1;
i2c_status = -1;
i2c_slaveAddress = slaveAddr << 1; // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
i2c_regAddress = regAddr;
i2c_txbuffer = data;
i2c_nDABytes = length;
i2c_nRABytes = 1;
i2c_txbuffer = data;
i2c_nDABytes = length;
i2c_nRABytes = 1;
// enable the I2C0 interrupt
i2c_interrupt_enable(MPU_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
@@ -170,12 +189,12 @@ int8_t i2c_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uint8_t
i2c_ReadWriteCmd = READ;
// assign inputs
i2c_status = -1;
i2c_status = -1;
i2c_slaveAddress = slaveAddr << 1; // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
i2c_regAddress = regAddr;
i2c_rxbuffer = data;
i2c_nDABytes = length;
i2c_nRABytes = 1;
i2c_rxbuffer = data;
i2c_nDABytes = length;
i2c_nRABytes = 1;
// enable the I2C0 interrupt
i2c_interrupt_enable(MPU_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
@@ -231,12 +250,12 @@ int8_t i2c_aux_writeBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, ui
i2c_aux_ReadWriteCmd = WRITE;
// assign inputs
i2c_aux_status = -1;
i2c_aux_status = -1;
i2c_aux_slaveAddress = slaveAddr << 1; // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
i2c_aux_regAddress = regAddr;
i2c_aux_txbuffer = data;
i2c_aux_nDABytes = length;
i2c_aux_nRABytes = 1;
i2c_aux_txbuffer = data;
i2c_aux_nDABytes = length;
i2c_aux_nRABytes = 1;
// enable the I2C0 interrupt
i2c_interrupt_enable(AUX_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);
@@ -264,12 +283,12 @@ int8_t i2c_aux_readBytes(uint8_t slaveAddr, uint8_t regAddr, uint8_t length, uin
i2c_aux_ReadWriteCmd = READ;
// assign inputs
i2c_aux_status = -1;
i2c_aux_status = -1;
i2c_aux_slaveAddress = slaveAddr << 1; // Address is shifted one position to the left. LSB is reserved for the Read/Write bit.
i2c_aux_regAddress = regAddr;
i2c_aux_rxbuffer = data;
i2c_aux_nDABytes = length;
i2c_aux_nRABytes = 1;
i2c_aux_rxbuffer = data;
i2c_aux_nDABytes = length;
i2c_aux_nRABytes = 1;
// enable the I2C0 interrupt
i2c_interrupt_enable(AUX_I2C, I2C_INT_ERR | I2C_INT_BUF | I2C_INT_EV);