/** 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 BMX160 9 DOF accelerometer/gyro/compass. Bosch has not published driver code for the BMX160, so drivers used here are for the similar BMI160. This requires the definition of BMI160_CHIP_ID in the file bmi160_defs.h to be changed from 0xD1 to 0xD8; this is the only change needed as the code will otherwise function with either chip. 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 v1.70 MPLAB : MPLAB X v5.50 */ /* (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 samples to average // Macro definitions //#define _EEDATA(N) __attribute__((space(eedata), aligned(N))) /** 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 "BMI160_driver-master/bmi160.h" // Function prototypes int8_t bmi160_read(uint8_t dev_id, uint8_t address, uint8_t *pData, uint16_t nCount); int8_t bmi160_write(uint8_t dev_id, uint8_t address, uint8_t *pData, uint16_t nCount); void delay_millisecs(uint16_t time); // Global variables //double __eeprom gPitchOffset = 0; // sensor pitch offset //double __eeprom gRollOffset = 0; // sensor roll offset double gPitchOffset = 0; // sensor pitch offset double gRollOffset = 0; // sensor roll offset /* Main application */ int main(void) { int8_t rslt = BMI160_OK; // result of BMI160 operations uint8_t i = 0; // iterator for averaging uint8_t cmdChar; // received command character struct bmi160_dev sensor; // sensor data structure struct bmi160_sensor_data accel; // sensor acceleration data structure //struct bmi160_sensor_data gyro; // sensor gyro data structure struct Avg { // structure to hold averaging data int32_t x; int32_t y; int32_t z; } avg; const float degprad = 57.2957795; // degrees per radian double pitch; // sensor pitch, nose up is positive double roll; // 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 // initialize the PIC24 // UART2 is initialized to 19200, 8, N, 1 // I2C1 runs at 100 kHz SYSTEM_Initialize(); printf("AOS PIC24 v1.0.0\npitch, roll\n"); // Set up the BMI160 data structure and initialize the sensor, which resets // the device and overwrites all registers with default values. sensor.id = BMI160_I2C_ADDR; sensor.intf = BMI160_I2C_INTF; sensor.read = (bmi160_read_fptr_t) &bmi160_read; sensor.write = (bmi160_write_fptr_t) &bmi160_write; sensor.delay_ms = (bmi160_delay_fptr_t) &delay_millisecs; rslt = bmi160_init(&sensor); if(rslt != BMI160_OK) { printf("Init fail %d\n", rslt); } // end if // Select the output data rate, range, bandwidth, and power of // accelerometer sensor. sensor.accel_cfg.odr = BMI160_ACCEL_ODR_1600HZ; sensor.accel_cfg.range = BMI160_ACCEL_RANGE_2G; sensor.accel_cfg.bw = BMI160_ACCEL_BW_NORMAL_AVG4; sensor.accel_cfg.power = BMI160_ACCEL_NORMAL_MODE; // Select the output data rate, range, bandwidth, and power of // gyroscope sensor in case we use it in the future. sensor.gyro_cfg.odr = BMI160_GYRO_ODR_3200HZ; sensor.gyro_cfg.range = BMI160_GYRO_RANGE_2000_DPS; sensor.gyro_cfg.bw = BMI160_GYRO_BW_NORMAL_MODE; sensor.gyro_cfg.power = BMI160_GYRO_SUSPEND_MODE; // Set the sensor configuration rslt = bmi160_set_sens_conf(&sensor); if(rslt != BMI160_OK) { printf("Config fail %d\n", rslt); } // end if while(1) { // main loop __builtin_clrwdt(); // clear watchdog timer avg.x = avg.y = avg.z = 0; // clear accumulators for(i = 0; i < NUMAVG; i++) { rslt = bmi160_get_sensor_data(BMI160_ACCEL_SEL, &accel, NULL, &sensor); if(rslt == BMI160_OK) { avg.x += accel.x; avg.y += accel.y; avg.z += accel.z; __delay_ms(50); } else { break; // there was a sensor error } // end if } // end for if(rslt == BMI160_OK) { avg.x /= NUMAVG; avg.y /= NUMAVG; avg.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; // apply offsets roll -= gRollOffset; // 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) fabs(roundf(pitch * 10)); // integer pitch * 10 pitchFracPart = pitchIntPart % 10; // fractional pitch * 10 rollIntPart = (int16_t) fabs(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); //printf("p = %0.3f, r = %0.3f\n\n", pitch, roll); } 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' : { // set current pitch and roll to zero gPitchOffset = pitch + gPitchOffset; // remove offsets from current values gRollOffset = roll + gRollOffset; // then set as new offsets break; } // end case case 'Z' : { // set offset values to zero gPitchOffset = 0.0; gRollOffset = 0.0; break; } // end case case 'r' : { // set 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()) #define RETRY_MAX 50 // define the retry count #define DEVICE_TIMEOUT 50 // define slave timeout int8_t bmi160_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *pData, uint16_t nCount) { I2C1_MESSAGE_STATUS status = I2C1_MESSAGE_PENDING; uint8_t writeBuffer[2]; uint16_t retryTimeOut, slaveTimeOut; writeBuffer[0] = reg_addr; // 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 one-byte register address to device I2C1_MasterWrite(writeBuffer, 1, (uint16_t) dev_id, &status); // 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(BMI160_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 // 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_id, &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 (BMI160_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(BMI160_E_COM_FAIL); } // end if return(BMI160_OK); } // end bmi160_read()) int8_t bmi160_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *pData, uint16_t nCount) { I2C1_MESSAGE_STATUS status = I2C1_MESSAGE_PENDING; uint8_t writeBuffer[2]; uint16_t retryTimeOut, slaveTimeOut; uint16_t counter; uint8_t *pD; 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_id, &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(BMI160_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(BMI160_E_COM_FAIL); break; } // end if pD++; reg_addr++; } // end for return(BMI160_OK); } // end bmi160_write()) // wrap the __delay_ms macro in a function so its pointer can be passed to the // bmi160 routines void delay_millisecs(uint16_t time) { __delay_ms(time); } // end delay_millisecs() /** End of File */