/** Generated main.c file from MPLAB Code Configurator @Company MBARI @File Name main.c @Summary This is the edited main.c file for the Attitude/Orientation Sensor. The template was generated by MPLAB Code Configurator. @Description This code runs on the MBARI AOS PIC24 board to sense transducer pitch and roll during deployments of the Simrad WBT Mini sonar system. The sensor is a Bosch BMA456 3-axis accelerometer. The PIC24 watchdog timer is enabled in hardware and times out in 8.456 seconds. Compile with small code and data models, optimization "s". Generation Information : Product Revision : PIC24 / dsPIC33 / PIC32MM MCUs - 1.171.0 Device : PIC24FV32KA302 The generated drivers are tested against the following: Compiler : XC16 v2.10 MPLAB : MPLAB X v6.20 */ /* (c) 2020 Microchip Technology Inc. and its subsidiaries. You may use this software and any derivatives exclusively with Microchip products. THIS SOFTWARE IS SUPPLIED BY MICROCHIP "AS IS". NO WARRANTIES, WHETHER EXPRESS, IMPLIED OR STATUTORY, APPLY TO THIS SOFTWARE, INCLUDING ANY IMPLIED WARRANTIES OF NON-INFRINGEMENT, MERCHANTABILITY, AND FITNESS FOR A PARTICULAR PURPOSE, OR ITS INTERACTION WITH MICROCHIP PRODUCTS, COMBINATION WITH ANY OTHER PRODUCTS, OR USE IN ANY APPLICATION. IN NO EVENT WILL MICROCHIP BE LIABLE FOR ANY INDIRECT, SPECIAL, PUNITIVE, INCIDENTAL OR CONSEQUENTIAL LOSS, DAMAGE, COST OR EXPENSE OF ANY KIND WHATSOEVER RELATED TO THE SOFTWARE, HOWEVER CAUSED, EVEN IF MICROCHIP HAS BEEN ADVISED OF THE POSSIBILITY OR THE DAMAGES ARE FORESEEABLE. TO THE FULLEST EXTENT ALLOWED BY LAW, MICROCHIP'S TOTAL LIABILITY ON ALL CLAIMS IN ANY WAY RELATED TO THIS SOFTWARE WILL NOT EXCEED THE AMOUNT OF FEES, IF ANY, THAT YOU HAVE PAID DIRECTLY TO MICROCHIP FOR THIS SOFTWARE. MICROCHIP PROVIDES THIS SOFTWARE CONDITIONALLY UPON YOUR ACCEPTANCE OF THESE TERMS. */ // Constant definitions #define FCY 6000000 // must define clock freq for __delay_ms routine #define NUMAVG 16 // number of accelerometer samples to average, must be <= 16 #define BMA4_READ_WRITE_LEN 8 // length of BMA456 I2C reads and writes #define RETRY_MAX 3 // define the I2C retry count #define DEVICE_TIMEOUT 3 // define I2C slave timeout #define POFFADDRL 0 // EEPROM address of pitch offset lower word #define POFFADDRU 1 // EEPROM address of pitch offset upper word #define ROFFADDRL 2 // EEPROM address of roll offset lower word #define ROFFADDRU 3 // EEPROM address of roll offset upper word /** Section: Included Files */ #include "mcc_generated_files/system.h" #include "mcc_generated_files/i2c1.h" #include "mcc_generated_files/tmr1.h" #include "mcc_generated_files/uart2.h" #include #include #include #include "BMA456-Sensor-API-master/bma456_an.h" #include "EEPROM.h" // Function prototypes int8_t bma456_read(uint8_t reg_addr, uint8_t *pData, uint32_t nCount, void *intf_ptr); int8_t bma456_write(uint8_t reg_addr, uint8_t *pData, uint32_t nCount , void *intf_ptr); void delay_microsecs(uint32_t time, void *intf_ptr); void bma4_error_codes_print_result(const char api_name[], int8_t rslt); void save_offsets(void); void read_offsets(void); // Type definitions typedef union offset_u { // stores offsets as floats but make them accessible as uint16 for eeprom storage float f; uint16_t u[2]; } offset_t; // Global variables offset_t gPitchOffset; // sensor pitch offset offset_t gRollOffset; // sensor roll offset static uint8_t gDevAddr = BMA4_I2C_ADDR_PRIMARY; // variable that holds the I2C device address /* Main application */ int main(void) { int8_t rslt = BMA4_OK; // result of BMA456 operations uint8_t cmdChar; // received command character uint8_t sampCnt; // accel sample counter struct bma4_dev sensor = {0}; // sensor data structure struct bma4_accel accel_data = {0}; // sensor acceleration data structure struct bma4_accel_config accel_conf = {0}; // sensor acceleration configuration data structure const float degprad = 57.2957795; // degrees per radian float pitch = 0; // sensor pitch, nose up is positive float roll = 0; // sensor roll, clockwise is positive int16_t pitchIntPart; // integer part of the pitch int16_t pitchFracPart; // fractional part of the pitch int16_t rollIntPart; // integer part of the roll int16_t rollFracPart; // fractional part of the roll uint32_t met = 0; // mission elapsed time in seconds int32_t accum_x; // accumulator for x accel average int32_t accum_y; // accumulator for y accel average int32_t accum_z; // accumulator for z accel average float avg_x; // average of x accel float avg_y; // average of y accel float avg_z; // average of z accel // initialize the PIC24 // UART2 is initialized to 19200, 8, N, 1 // I2C1 runs at 100 kHz SYSTEM_Initialize(); __delay_ms(1000); read_offsets(); // get the pitch and roll offsets from eeprom printf("AOS PIC24 v2.0.0\nmet, pitch, roll\n"); // Set up the BMA456 data structure and initialize the sensor, which resets // the device and overwrites all registers with default values. sensor.chip_id = 0x00; // this will get set correctly by the init process sensor.intf = BMA4_I2C_INTF; sensor.bus_read = (bma4_read_fptr_t) bma456_read; sensor.bus_write = (bma4_write_fptr_t) bma456_write; sensor.delay_us = (bma4_delay_us_fptr_t) delay_microsecs; sensor.variant = BMA45X_VARIANT; sensor.intf_ptr = &gDevAddr; sensor.read_write_len = BMA4_READ_WRITE_LEN; sensor.perf_mode_status = BMA4_DISABLE; rslt = bma456_an_init(&sensor); bma4_error_codes_print_result("bma456_an_init status", rslt); // The BMA456 data sheet says to load a configuration file to enable the // features of the sensor, but this isn't necessary for just sensing acceleration // Select the output data rate, range, bandwidth, and filter of // accelerometer sensor. accel_conf.odr = BMA4_OUTPUT_DATA_RATE_50HZ; accel_conf.range = BMA4_ACCEL_RANGE_2G; accel_conf.bandwidth = BMA4_ACCEL_CIC_AVG8; accel_conf.perf_mode = BMA4_CIC_AVG_MODE; // Set the accelerometer configuration rslt = bma4_set_accel_config(&accel_conf, &sensor); bma4_error_codes_print_result("bma4_set_accel_config status", rslt); // Enable accelerometer after set of configurations rslt = bma4_set_accel_enable(1, &sensor); bma4_error_codes_print_result("bma4_set_accel_enable status", rslt); while(1) { // main loop __builtin_clrwdt(); // clear watchdog timer // Normally you should read the interrupt status from the BMA456 to make // sure that another sample is ready, but this has proven to be unreliable. // Use delays to make sure that you don't sample too fast. // Read the accel x, y, z data multiple times accum_x = accum_y = accum_z = 0; // clear accumulators for(sampCnt = 0; sampCnt < NUMAVG; sampCnt++) { rslt = bma4_read_accel_xyz(&accel_data, &sensor); accum_x += accel_data.x; accum_y += accel_data.y; accum_z += accel_data.z; __delay_ms(30); // must not sample faster than BMA456 output data rate } // end for bma4_error_codes_print_result("bma4_read_accel_xyz status", rslt); if(rslt == BMA4_OK) { avg_x = accum_x / NUMAVG; avg_y = accum_y / NUMAVG; avg_z = accum_z / NUMAVG; //printf("x = %d, y = %d, z = %d\n", (int16_t) avg_x, (int16_t) avg_y, (int16_t) avg_z); // calculate angles from sensor counts pitch = degprad * atanf(-avg_y / sqrtf((avg_z * avg_z) + (avg_x * avg_x))); roll = degprad * atanf(-avg_z / sqrtf((avg_y * avg_y) + (avg_x * avg_x))); pitch -= gPitchOffset.f; // apply offsets roll -= gRollOffset.f; //printf("p = %0.3lf, r = %0.3lf\n", pitch, roll); // convert the pitch and roll floats to integer numbers so they // can be printed without using the floating-point print routines, // which consume too much memory pitchIntPart = (int16_t) fabsf(roundf(pitch * 10)); // integer pitch * 10 pitchFracPart = pitchIntPart % 10; // fractional pitch * 10 rollIntPart = (int16_t) fabsf(roundf(roll * 10)); // integer roll * 10 rollFracPart = rollIntPart % 10; // fractional roll * 10 printf("%02d:%02d:%02d, ", (uint16_t) met / 3600, (uint16_t) ((met % 3600) / 60), (uint16_t) (met % 60)); printf("%s%d.%d, %s%d.%d\n", (pitch > -0.05) ? "" : "-", pitchIntPart / 10, pitchFracPart, (roll > -0.05) ? "" : "-", rollIntPart / 10, rollFracPart); } else { printf("Read fail %d\n", rslt); printf("System reset\n"); while(1); // hang here until watchdog resets system } // end if met++; // increment mission clock if(UART2_IsRxReady()) { // if a command char has been received cmdChar = UART2_Read(); // get the command switch(cmdChar) { case 'z' : { // zero the current pitch and roll gPitchOffset.f = pitch + gPitchOffset.f; // remove offsets from current values gRollOffset.f = roll + gRollOffset.f; // then set as new offsets break; } // end case case 'Z' : { // zero the offset values (i.e. apply no offsets) gPitchOffset.f = 0.0; gRollOffset.f = 0.0; break; } // end case case 's' : { // save the offset values to EEPROM save_offsets(); break; } // end case case 'r' : { // reset the mission elapsed time to zero met = 0; break; } // end case } // end switch while(UART2_IsRxReady()) { // clear any other chars from receive buffer cmdChar = UART2_Read(); } // end while } // end if while(!TMR1_GetElapsedThenClear()); // wait here until 1 sec has elapsed } // end while return 1; } // end main()) // Read data from the BMA456 int8_t bma456_read(uint8_t reg_addr, uint8_t *pData, uint32_t nCount, void *intf_ptr) { I2C1_MESSAGE_STATUS status = I2C1_MESSAGE_PENDING; uint8_t writeBuffer[2]; uint16_t retryTimeOut, slaveTimeOut; uint8_t dev_addr = *(uint8_t*)intf_ptr; writeBuffer[0] = reg_addr; // It's possible that the slave device will be slow. // As a work around on these slaves, the application can // try resending the transaction. retryTimeOut = 0; slaveTimeOut = 0; while(status != I2C1_MESSAGE_FAIL) { // write one-byte register address to device I2C1_MasterWrite(writeBuffer, 1, (uint16_t) dev_addr, &status); // wait for the message to be sent or status has changed. while(status == I2C1_MESSAGE_PENDING) { //printf("Message pending\n"); // add some delay here __delay_ms(1); // timeout checking // check for max retry and skip this byte if(slaveTimeOut >= DEVICE_TIMEOUT) { printf("Device timeout\n"); return(BMA4_E_COM_FAIL); } else slaveTimeOut++; } // end while if(status == I2C1_MESSAGE_COMPLETE) break; // if status is I2C1_MESSAGE_ADDRESS_NO_ACK, // or I2C1_DATA_NO_ACK, // The device may be busy and needs more time for the last // write so we can retry writing the data; this is why we // use a while loop here. // check for max retry and skip this byte if(retryTimeOut >= RETRY_MAX) { printf("Retry timeout\n"); break; } else retryTimeOut++; } // end while // this portion will read the bytes starting at the address sent above if(status == I2C1_MESSAGE_COMPLETE) { retryTimeOut = 0; slaveTimeOut = 0; while(status != I2C1_MESSAGE_FAIL) { // read bytes from the device I2C1_MasterRead(pData, (uint8_t) nCount, (uint16_t) dev_addr, &status); //printf("read 0x%02x from reg 0x%02x\n", pData[0], reg_addr); // wait for the message to be sent or status has changed. while(status == I2C1_MESSAGE_PENDING) { // add some delay here __delay_ms(1); // timeout checking // check for max retry and skip this byte if (slaveTimeOut == DEVICE_TIMEOUT) return(BMA4_E_COM_FAIL); else slaveTimeOut++; } // end while if(status == I2C1_MESSAGE_COMPLETE) // if success then we're done break; // if status is I2C1_MESSAGE_ADDRESS_NO_ACK, // or I2C1_DATA_NO_ACK, // The device may be busy and needs more time for the last // write so we can retry writing the data; this is why we // use a while loop here. // check for max retry and skip this byte if(retryTimeOut == RETRY_MAX) break; else retryTimeOut++; } // end while } // end if // exit if the last transaction failed if(status == I2C1_MESSAGE_FAIL) { return(BMA4_E_COM_FAIL); } // end if return(BMA4_OK); } // end bma456_read()) // Write data to the BMA456 int8_t bma456_write(uint8_t reg_addr, uint8_t *pData, uint32_t nCount , void *intf_ptr) { I2C1_MESSAGE_STATUS status = I2C1_MESSAGE_PENDING; uint8_t writeBuffer[2]; uint16_t retryTimeOut, slaveTimeOut; uint16_t counter; uint8_t *pD; uint8_t dev_addr = *(uint8_t*)intf_ptr; pD = pData; for(counter = 0; counter < nCount; counter++) { // Load the buffer with the address of the register and the byte to be // written. writeBuffer[0] = reg_addr; writeBuffer[1] = *pD; // It's possible that the slave device will be slow. // As a work around on these slaves, the application can // retry sending the transaction. retryTimeOut = 0; slaveTimeOut = 0; while(status != I2C1_MESSAGE_FAIL) { // write two bytes (register address then data) to device I2C1_MasterWrite(writeBuffer, 2, dev_addr, &status); //printf("write 0x%02x to reg 0x%02x\n", writeBuffer[1], writeBuffer[0]); // wait for the message to be sent or status has changed. while(status == I2C1_MESSAGE_PENDING) { // add some delay here __delay_ms(1); // timeout checking // check for max retry and skip this byte if (slaveTimeOut == DEVICE_TIMEOUT) return(BMA4_E_COM_FAIL); else slaveTimeOut++; } // end while if(status == I2C1_MESSAGE_COMPLETE) break; // if status is I2C1_MESSAGE_ADDRESS_NO_ACK, // or I2C1_DATA_NO_ACK, // The device may be busy and needs more time for the last // write so we can retry writing the data; this is why we // use a while loop here. // check for max retry and skip this byte if(retryTimeOut == RETRY_MAX) break; else retryTimeOut++; } // end while // exit if the last transaction failed if(status == I2C1_MESSAGE_FAIL) { return(BMA4_E_COM_FAIL); break; } // end if pD++; reg_addr++; } // end for return(BMA4_OK); } // end bma456_write()) // Wrap the __delay_us macro in a function so its pointer can be passed to the // BMA456 routines void delay_microsecs(uint32_t time, void *intf_ptr) { if(time <= 65535) { __delay_us((uint16_t) time); } else { __delay_ms(time/1000); __delay_us(time%1000); } // end if } // end delay_microsecs() // Print a BMA456 error code void bma4_error_codes_print_result(const char api_name[], int8_t rslt) { if(rslt != BMA4_OK) { printf("%s\t", api_name); if(rslt == BMA4_E_NULL_PTR) { printf("Error [%d] : Null pointer\r\n", rslt); } else if (rslt == BMA4_E_COM_FAIL) { printf("Error [%d] : Communication failure\r\n", rslt); } else if (rslt == BMA4_E_DEV_NOT_FOUND) { printf("Error [%d] : Device not found\r\n", rslt); } else if (rslt == BMA4_E_INVALID_SENSOR) { printf("Error [%d] : Invalid sensor\r\n", rslt); } else if (rslt == BMA4_E_CONFIG_STREAM_ERROR) { printf("Error [%d] : Invalid configuration stream\r\n", rslt); } else if (rslt == BMA4_E_SELF_TEST_FAIL) { printf("Error [%d] : Self test failed\r\n", rslt); } else if (rslt == BMA4_E_INVALID_STATUS) { printf("Error [%d] : Invalid status\r\n", rslt); } else if (rslt == BMA4_E_OUT_OF_RANGE) { printf("Error [%d] : Out of Range\r\n", rslt); } else if (rslt == BMA4_E_INT_LINE_INVALID) { printf("Error [%d] : Interrupt line invalid\r\n", rslt); } else if (rslt == BMA4_E_RD_WR_LENGTH_INVALID) { printf("Error [%d] : Read/write length invalid\r\n", rslt); } else if (rslt == BMA4_E_AVG_MODE_INVALID_CONF) { printf("Error [%d] : Invalid bandwidth and ODR combination in Accel Averaging mode\r\n", rslt); } else { /* For more error codes refer "*_defs.h" */ printf("Error [%d] : Unknown error code\r\n", rslt); } // end if } // end if } // end bma4_error_codes_print_result() // Save pitch and roll offsets to eeprom void save_offsets(void) { EepSetup(); EepWrite(POFFADDRL, gPitchOffset.u[0]); // write 4 words to EEPROM EepSetup(); EepWrite(POFFADDRU, gPitchOffset.u[1]); EepSetup(); EepWrite(ROFFADDRL, gRollOffset.u[0]); EepSetup(); EepWrite(ROFFADDRU, gRollOffset.u[1]); } // end save_offsets() // Read pitch and roll offsets from eeprom void read_offsets(void) { gPitchOffset.u[0] = (uint16_t) EepRead(POFFADDRL); // read pitch lower word gPitchOffset.u[1] = (uint16_t) EepRead(POFFADDRU); // read pitch upper word gRollOffset.u[0] = (uint16_t) EepRead(ROFFADDRL); // read roll lower word gRollOffset.u[1] = (uint16_t) EepRead(ROFFADDRU); // read roll upper word } // end read_offsets()) /** End of File */