em7186.cpp

00001 /**
00002 * @file         em7186.c
00003 *
00004 * @brief        Sample interface for the em7186.
00005 *
00006 * @authors      David Vincent, Joe Miller
00007 * @date         05/12/2016
00008 * @copyright    (C) 2015, 2016 PNI Corp
00009 *
00010 * @copyright    Disclosure to third parties or reproduction in any form
00011 *               whatsoever, without prior written consent, is strictly forbidden
00012 *
00013 */
00014 #include "em7186.h"
00015 
00016 
00017 float   em7186_sensor_scale[128];
00018 u32     timestampNonWake;
00019 u32     timestampWake;
00020 u8      printData = 1; // a switch to enable/disable display data (vs logData)
00021 u8      logData = 0;   // a switch to enable/disable log data to SD Card 
00022 u8      sensorEnabled[64];
00023 u8      haveSensorInfo = 0;
00024 u16     magMaxRate = 0;
00025 u16     accelMaxRate = 0;
00026 u16     gyroMaxRate = 0;
00027 u32     timestamp;
00028 u16     timestampPtr[2];
00029 
00030 SensorDescriptor sensorInformation[128];
00031 SensorConfiguration sensorConfiguration[127];
00032 
00033 
00034 ParamInfo sensorInfoParamList[128] =
00035 {
00036     { 0, 16 },
00037     { 1, 16 },
00038     { 2, 16 },
00039     { 3, 16 },
00040     { 4, 16 },
00041     { 5, 16 },
00042     { 6, 16 },
00043     { 7, 16 },
00044     { 8, 16 },
00045     { 9, 16 },
00046     { 10, 16 },
00047     { 11, 16 },
00048     { 12, 16 },
00049     { 13, 16 },
00050     { 14, 16 },
00051     { 15, 16 },
00052     { 16, 16 },
00053     { 17, 16 },
00054     { 18, 16 },
00055     { 19, 16 },
00056     { 20, 16 },
00057     { 21, 16 },
00058     { 22, 16 },
00059     { 23, 16 },
00060     { 24, 16 },
00061     { 25, 16 },
00062     { 26, 16 },
00063     { 27, 16 },
00064     { 28, 16 },
00065     { 29, 16 },
00066     { 30, 16 },
00067     { 31, 16 },
00068     { 32, 16 },
00069     { 33, 16 },
00070     { 34, 16 },
00071     { 35, 16 },
00072     { 36, 16 },
00073     { 37, 16 },
00074     { 38, 16 },
00075     { 39, 16 },
00076     { 40, 16 },
00077     { 41, 16 },
00078     { 42, 16 },
00079     { 43, 16 },
00080     { 44, 16 },
00081     { 45, 16 },
00082     { 46, 16 },
00083     { 47, 16 },
00084     { 48, 16 },
00085     { 49, 16 },
00086     { 50, 16 },
00087     { 51, 16 },
00088     { 52, 16 },
00089     { 53, 16 },
00090     { 54, 16 },
00091     { 55, 16 },
00092     { 56, 16 },
00093     { 57, 16 },
00094     { 58, 16 },
00095     { 59, 16 },
00096     { 60, 16 },
00097     { 61, 16 },
00098     { 62, 16 },
00099     { 63, 16 },
00100     { 64, 16 },
00101     { 65, 16 },
00102     { 66, 16 },
00103     { 67, 16 },
00104     { 68, 16 },
00105     { 69, 16 },
00106     { 70, 16 },
00107     { 71, 16 },
00108     { 72, 16 },
00109     { 73, 16 },
00110     { 74, 16 },
00111     { 75, 16 },
00112     { 76, 16 },
00113     { 77, 16 },
00114     { 78, 16 },
00115     { 79, 16 },
00116     { 80, 16 },
00117     { 81, 16 },
00118     { 82, 16 },
00119     { 83, 16 },
00120     { 84, 16 },
00121     { 85, 16 },
00122     { 86, 16 },
00123     { 87, 16 },
00124     { 88, 16 },
00125     { 89, 16 },
00126     { 90, 16 },
00127     { 91, 16 },
00128     { 92, 16 },
00129     { 93, 16 },
00130     { 94, 16 },
00131     { 95, 16 },
00132     { 96, 16 },
00133     { 97, 16 },
00134     { 98, 16 },
00135     { 99, 16 },
00136     { 100, 16 },
00137     { 101, 16 },
00138     { 102, 16 },
00139     { 103, 16 },
00140     { 104, 16 },
00141     { 105, 16 },
00142     { 106, 16 },
00143     { 107, 16 },
00144     { 108, 16 },
00145     { 109, 16 },
00146     { 110, 16 },
00147     { 111, 16 },
00148     { 112, 16 },
00149     { 113, 16 },
00150     { 114, 16 },
00151     { 115, 16 },
00152     { 116, 16 },
00153     { 117, 16 },
00154     { 118, 16 },
00155     { 119, 16 },
00156     { 120, 16 },
00157     { 121, 16 },
00158     { 122, 16 },
00159     { 123, 16 },
00160     { 124, 16 },
00161     { 125, 16 },
00162     { 126, 16 },
00163     { 127, 16 },
00164 };
00165 
00166 
00167 u32 em7186_i2c_init()
00168 {
00169     u8 buffer[1];
00170     em7186_i2c_read(PRODUCT_ID_REG, buffer, 1);
00171     
00172     switch(buffer[0])
00173     {
00174         case PRODUCT_ID_7180:
00175             printf("SENtral found\n\r");
00176             break;   
00177         case PRODUCT_ID_7184:
00178             printf("SENtral-A found\n\r");        
00179             break;   
00180         case PRODUCT_ID_7186:
00181             printf("SENtral-A2 found\n\r");        
00182             break;   
00183         default:
00184             printf("SENtral NOT FOUND, ID returned = %u\n\r",buffer[0]);
00185             return 0;
00186   
00187      }  
00188     return 1;
00189 }
00190 
00191 void firmwareTransfer(char srcDestCode)
00192 {
00193     switch (srcDestCode) {
00194 //        case 'r':
00195 //            if (displayText) printf("begin Upload of firmware to  SENtral RAM\n\r");
00196 //            SENtral_InterruptPin.disable_irq();
00197 //            if (!) {
00198 //                if (displayText) printf("Error uploading SENtral firmware to RAM\n\r");
00199 //            }
00200 //            SENtral_InterruptPin.enable_irq();
00201 //            break;
00202 //            
00203 //        case 'e':
00204 //            if (displayText) printf("begin Upload of firmware to PNI Module EEPROM\n\r");
00205 //            SENtral_InterruptPin.disable_irq();
00206 //            if (!) {
00207 //                if (displayText) printf("Error uploading SENtral firmware to EEPROM\n\r");
00208 //            }
00209 //            SENtral_InterruptPin.enable_irq();
00210 //            break;
00211 //            
00212 //        case 's':
00213 //            if (displayText) printf("begin Upload of firmware to SDCard\n\r");
00214 //            SENtral_InterruptPin.disable_irq();
00215 //            if (!) {
00216 //                if (displayText) printf("Error uploading SENtral firmware to SDCard\n\r");
00217 //            }
00218 //            SENtral_InterruptPin.enable_irq();
00219 //            break;
00220             
00221         case 'R':
00222             if (displayText) printf("Transfering firmware from SDCard to SENtral RAM\n\r");
00223             SENtral_InterruptPin.disable_irq();
00224             if (!em7186_firmware_Transfer2RAM(fw)) {
00225                 if (displayText) printf("Error transfering SENtral firmware to RAM\n\r");
00226                 break;
00227             }
00228             // Enable CPU
00229             em7186_i2c_write_value(CHIP_CONTROL_REG, CHIP_CONTROL_CPU_RUN);        
00230             em7186_set_scale_factors();
00231             SENtral_InterruptPin.enable_irq();
00232             break;
00233             
00234         case 'E':
00235             if (displayText) printf("Transfering firmware from SDCard to PNI Module EEPROM\n\r");
00236             SENtral_InterruptPin.disable_irq();
00237             if (!em7186_firmware_Transfer2EE(fw)) {
00238                 if (displayText) printf("Error transfering SENtral firmware to EEPROM\n\r");
00239             }
00240             break;
00241 
00242         default:
00243             serialCommandMode = 0;
00244 
00245     }
00246 
00247 
00248 }
00249 
00250 
00251 
00252 
00253 u32 em7186_i2c_write_value(u8 registerAddress, u8 value)
00254 {
00255     u32 status = em7186_i2c_write(registerAddress, &value, 1);
00256     return status;
00257 }
00258 
00259 
00260 //=========================================================================
00261 // Helper functions
00262 //=========================================================================
00263 u8  get_3_axis_sensor_data(SensorData3Axis *data, float scale, u8* buffer)
00264 {
00265     SensorData3AxisRaw rawData;
00266     memcpy(&rawData, &buffer[1], sizeof(rawData));
00267     data->x = (float)rawData.x * scale;
00268     data->y = (float)rawData.y * scale;
00269     data->z = (float)rawData.z * scale;
00270     data->extra = rawData.status;
00271 
00272     return 1;
00273 }
00274 u8  get_3_axis_uncal_sensor_data(SensorData6Axis *data, float scale, u8* buffer)
00275 {
00276     SensorData3AxisUncalRaw rawData;
00277     memcpy(&rawData, &buffer[1], sizeof(rawData));
00278     data->x = (float)rawData.x * scale;
00279     data->y = (float)rawData.y * scale;
00280     data->z = (float)rawData.z * scale;
00281     data->x_bias = (float)rawData.x_bias * scale;
00282     data->y_bias = (float)rawData.y_bias * scale;
00283     data->z_bias = (float)rawData.z_bias * scale;
00284     data->extra = rawData.status;
00285 
00286     return 1;
00287 }
00288 u8  get_rotation_vector(SensorData4Axis *rv, float quaternionScale, u8* buffer)
00289 {
00290     RotationVectorRaw rawData;
00291     memcpy(&rawData, &buffer[1], sizeof(rawData));
00292     rv->x = (float)rawData.x * quaternionScale;
00293     rv->y = (float)rawData.y * quaternionScale;
00294     rv->z = (float)rawData.z * quaternionScale;
00295     rv->w = (float)rawData.w * quaternionScale;
00296     rv->extra = (float)rawData.accuracy * ((float)M_PI / powf(2.0f, 14.0f));
00297 
00298     return 1;
00299 }
00300 
00301 //=========================================================================
00302 // Core functions
00303 //=========================================================================
00304 u32 em7186_firmware_Transfer2RAM(const u8 *firmwareName)
00305 {
00306    
00307     // reset Sentral
00308     em7186_i2c_write_value(RESET_REQ_REG, 1);
00309     em7186_i2c_write_value(CHIP_CONTROL_REG, CHIP_CONTROL_HOST_UPLOAD);
00310 
00311     ///////////////////////////////////////////////////////////////////////////
00312     // Load firmware from file
00313     ///////////////////////////////////////////////////////////////////////////
00314     // Open firmware file
00315     FILE *fw_h = fopen(firmwareName, "rb");
00316     if (!fw_h)
00317     {
00318         if (displayText) printf("ERROR: Unable to open file\n\r");
00319         return 0;
00320     }
00321     
00322     if (displayText) printf("Firmware file found\n\r");
00323 
00324     // Read the firmware header
00325     struct fwHeader
00326     {
00327         u8 imageSignatureLsb;
00328         u8 imageSignatureMsb;
00329         u16 flags;
00330         u32 crc;
00331         u32 reserved;
00332         u16 imageLength;
00333         u16 reserved2;
00334     } fwHeader;
00335     int hsize = fread(&fwHeader, 1, FIRMWARE_HEADER_SIZE, fw_h);
00336     if ( hsize != FIRMWARE_HEADER_SIZE)
00337     {
00338         if (displayText) printf("ERROR: File smaller than expected header size %d\n\r", hsize);
00339         fclose(fw_h);
00340         return 0;
00341     }
00342 
00343     // Validate firmware
00344     if (fwHeader.imageSignatureLsb != IMAGE_SIGNATURE_LSB || fwHeader.imageSignatureMsb != IMAGE_SIGNATURE_MSG)
00345     {
00346         if (displayText) printf("ERROR: Firmware version doesn't match\n\r");
00347         fclose(fw_h);
00348         return 0;
00349     }
00350 
00351     // TODO: ensure that firmware version matches rom version
00352 
00353     // Read the firmware image
00354     u8 *fw = (u8 *)malloc(fwHeader.imageLength);
00355     u32 firmwareSize = fread(fw, 1, fwHeader.imageLength, fw_h);
00356     fclose(fw_h);
00357     if (firmwareSize != fwHeader.imageLength || firmwareSize % 4)
00358     {
00359         if (displayText) printf("ERROR: Firmware size must break on 4 byte boundary\n\r");
00360         free(fw);
00361         return 0;
00362     }
00363 
00364     ///////////////////////////////////////////////////////////////////////////
00365     // Upload firmware to RAM
00366     ///////////////////////////////////////////////////////////////////////////
00367     // TODO: set upload address if needed. (zero by default)
00368 
00369     if (displayText) printf("Uploading Firmware to SENtral RAM...\n\r");
00370 
00371     s32 bytesWritten = 0,
00372         bytesRemaining = firmwareSize;
00373     u8 bytesToWrite, i,
00374         buf[MAX_I2C_WRITE],
00375         maxI2cWrite = MAX_I2C_WRITE;
00376 
00377     while (bytesRemaining > 0)
00378     {
00379         if (bytesRemaining < MAX_I2C_WRITE)      
00380             bytesToWrite = bytesRemaining;
00381         else
00382             bytesToWrite = maxI2cWrite;
00383 
00384         // Reverse byte order per word
00385         for (i = 0; i < bytesToWrite; i += 4)
00386         {
00387             buf[i + 0] = fw[bytesWritten + i + 3];
00388             buf[i + 1] = fw[bytesWritten + i + 2];
00389             buf[i + 2] = fw[bytesWritten + i + 1];
00390             buf[i + 3] = fw[bytesWritten + i + 0];
00391         }
00392 
00393         if (!em7186_i2c_write(SR_UPLOAD_DATA_REG, buf, bytesToWrite))
00394         {
00395             if (displayText) printf("ERROR: Problem writing to sentral\n\r");
00396             free(fw);
00397             return 0;
00398         }
00399 
00400         bytesRemaining -= bytesToWrite;
00401         bytesWritten += bytesToWrite;
00402     }
00403 
00404     free(fw);
00405 
00406     if (displayText) printf("Firmware Uploaded......");
00407 
00408     // Read and verify CRC
00409     u32 hostCRC = 0;
00410     em7186_i2c_read(HOST_CRC_REG, (u8*)&hostCRC, 4);
00411     u32 fwHeaderCRC = fwHeader.crc;//((u32*)fwHeader)[1];
00412     if (hostCRC != fwHeaderCRC)
00413     {
00414         if (displayText) printf("ERROR: CRC doesn't match\n\r");
00415         return 0;
00416     }
00417     
00418     if (displayText) printf(" CRC Match!!\n\r");
00419     return 1;
00420 }
00421 
00422 
00423 
00424 
00425 u32 em7186_firmware_Transfer2EE(const u8 *firmwareName)
00426 {
00427     u8  buffer[16];
00428     u8  trys = 1;
00429     u16 count;
00430     // Open firmware file
00431     FILE *fw_h = fopen(firmwareName, "rb");
00432     if (!fw_h) {
00433         if (displayText) printf("ERROR: Unable to open file\n\r");
00434         return 0;
00435     }
00436 
00437     if (displayText) printf("Firmware file found\n\r");
00438 
00439     // Read the firmware header
00440     struct fwHeader {
00441         u8 imageSignatureLsb;
00442         u8 imageSignatureMsb;
00443         u16 flags;
00444         u32 crc;
00445         u32 reserved;
00446         u16 imageLength;
00447         u16 reserved2;
00448     } fwHeader;
00449 
00450     int hsize = fread(&fwHeader, 1, FIRMWARE_HEADER_SIZE, fw_h);
00451     if ( hsize != FIRMWARE_HEADER_SIZE) {
00452         if (displayText) printf("ERROR: File smaller than expected header size %d\n\r", hsize);
00453         fclose(fw_h);
00454         return 0;
00455     }
00456 
00457     // Validate firmware
00458     if (fwHeader.imageSignatureLsb != IMAGE_SIGNATURE_LSB || fwHeader.imageSignatureMsb != IMAGE_SIGNATURE_MSG) {
00459         if (displayText) printf("ERROR: Firmware version doesn't match\n\r");
00460         fclose(fw_h);
00461         return 0;
00462     }
00463 
00464     // Read the firmware image From the SD Card
00465     u8 *fw = (u8 *)malloc(fwHeader.imageLength);
00466     u32 firmwareSize = fread(fw, 1, fwHeader.imageLength, fw_h);
00467     fclose(fw_h);
00468     if (firmwareSize != fwHeader.imageLength || firmwareSize % 4) {
00469         if (displayText) printf("ERROR: Firmware size must break on 4 byte boundary\n\r");
00470         free(fw);
00471         return 0;
00472     }
00473     if (displayText) printf("Firmware size = %u\n\r",firmwareSize);
00474 
00475 
00476    u8 headerBytes[FIRMWARE_HEADER_SIZE];
00477    memcpy(headerBytes,(u8*)&fwHeader,FIRMWARE_HEADER_SIZE); // create a bytewise copy of fwHeader
00478 
00479 
00480     ///////////////////////////////////////////////////////////////////////////
00481     // Upload firmware to EEPROM
00482     ///////////////////////////////////////////////////////////////////////////
00483 
00484     do {
00485         // request SENtral passthrough mode
00486         em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 1);
00487         em7186_i2c_read(PASS_THROUGH_RDY_REG, buffer, 1);
00488     } while((buffer[0] != 1) && (++trys < 20));
00489 
00490 
00491     if (trys <20) {
00492 
00493         // Reverse byte order per word
00494 
00495 
00496         if (displayText) printf("SENtral confirmed passthrough mode after %u try(s)\n\r",trys);
00497 
00498 
00499         // write header portion to EEPROM from SDCard
00500         //key: EE_Write(u8 I2C_Addr, u16 EE_MemAddr, u8*buffer, u16 length)
00501         u32 status = EE_Write(0xA0, 0, (u8*)&fwHeader, FIRMWARE_HEADER_SIZE);
00502         if (displayText) printf("Header Sent to EEPROM......\n\r",firmwareSize);
00503         wait_ms(5);
00504         EE_Read(0xA0, 0, buffer, FIRMWARE_HEADER_SIZE);
00505         status = 0;
00506 
00507         // Readback EEPROM to verify header write
00508         for (count = 0; count<FIRMWARE_HEADER_SIZE; count++) {
00509             if (headerBytes[count] != buffer[count]) {
00510                 status = 1;  // 1 = fail
00511                 if (displayText) printf("Failed Header readback from EEPROM at %u, value=%u\n\r",count,headerBytes[count]);
00512                 break;
00513             }
00514         }
00515 
00516         if (!status) {
00517            u32 bytesWritten = 0;
00518             u32 bytesRemaining = firmwareSize;
00519             u8 bytesToWrite,
00520                maxI2cWrite = 16; // kind of small but ensures page alignment
00521 
00522             while (bytesRemaining > 0) {
00523                 if (bytesRemaining < maxI2cWrite)
00524                     bytesToWrite = bytesRemaining;
00525                 else
00526                     bytesToWrite = maxI2cWrite;
00527 
00528                 if (!EE_Write(0xA0, (bytesWritten+FIRMWARE_HEADER_SIZE), &fw[bytesWritten], bytesToWrite)) {
00529                     if (displayText) printf("\n\rCould not write to EEPROM\n\r");
00530                     free(fw);
00531                     wait_ms(5);
00532                     em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 0);
00533                     return 0;
00534                 }
00535                 bytesWritten += bytesToWrite;
00536                 bytesRemaining -= bytesToWrite;
00537                 if (displayText) printf("\r%u",bytesWritten);
00538                 wait_ms(5);
00539             }
00540 
00541             if (displayText) printf("\n\rFirmware Transfered from SDCard to EEPROM.  Now Verifying.....\n\r");
00542 
00543             u32 bytesRead = 0;
00544             bytesRemaining = firmwareSize;
00545             u8 bytesToRead,
00546                maxI2cRead = 16;
00547             status = 0; 
00548             
00549             while ((bytesRemaining > 0) && (status == 0)) {
00550                 if (bytesRemaining < maxI2cRead)
00551                     bytesToRead = bytesRemaining;
00552                 else
00553                     bytesToRead = maxI2cRead;
00554                     
00555                 if (!EE_Read(0xA0, (bytesRead+FIRMWARE_HEADER_SIZE), buffer, bytesToRead)) {
00556                     if (displayText) printf("\n\rCould not Read EEPROM\n\r");
00557                     free(fw);
00558                     wait_ms(5);
00559                     em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 0);
00560                     return 0;
00561                 }
00562 
00563                 for (count = 0; count<bytesToRead; count++) {
00564                     if (fw[bytesRead+count] != buffer[count]) {
00565                         status = 1;  // 1 = fail
00566                         if (displayText) printf("Failed firmware readback from EEPROM at %u, value=%u\n\r",bytesRead+FIRMWARE_HEADER_SIZE+count,buffer[count]);
00567                         wait_ms(5);
00568                         em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 0);
00569                         return 0;
00570                     }
00571                 }
00572                 bytesRead += bytesToRead;
00573                 bytesRemaining -= bytesToRead;
00574                 if (displayText) printf("\r%u",bytesRead);
00575                 wait_ms(1);
00576             }
00577             if(status == 0)
00578             {
00579                 if (displayText) printf("\n\rVerify EEPROM **** SUCCESSFUL *****\n\r");
00580             }
00581         } else {
00582             if (displayText) printf("\r\nCould not write to EEPROM (Header)\n\r");
00583             wait_ms(5);
00584             em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 0);
00585             return 0;
00586         }
00587 
00588         free(fw);
00589 
00590     } else {
00591         if (displayText) printf("Could not confirm SENtral Passthrough mode\n\r");
00592         return 0;
00593     }
00594 
00595     em7186_i2c_write_value(PASS_THROUGH_CFG_REG, 0); // turn off passthrough mode
00596 
00597     return 1;
00598 }
00599 
00600 
00601 
00602 
00603 
00604 u32 em7186_param_read(u8 *values, u8 page, ParamInfo *paramList, u8 numParams)
00605 {
00606     
00607     u8 i, paramAck, pageSelectValue;
00608     u16 valIndex = 0;
00609     for (i = 0; i < numParams; i++)
00610     {
00611         pageSelectValue = page | (paramList[i].size << 4);
00612         em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, pageSelectValue);
00613         em7186_i2c_write_value(PARAM_REQUEST_REG, paramList[i].paramNo);
00614         do
00615         {
00616             em7186_i2c_read(PARAM_ACK_REG, &paramAck, 1);
00617             if (paramAck == 0x80)
00618             {
00619                 em7186_i2c_write_value(PARAM_REQUEST_REG, 0);
00620                 em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, 0);
00621                 
00622 
00623                 return 0;
00624             }
00625         } while (paramAck != paramList[i].paramNo);
00626         em7186_i2c_read(PARAM_SAVE_REG, &values[valIndex], paramList[i].size);
00627 //      printf("%u ", values[valIndex]);
00628         valIndex += paramList[i].size;
00629     }
00630     em7186_i2c_write_value(PARAM_REQUEST_REG, 0);
00631     em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, 0);
00632     
00633 
00634     return 1;
00635 }
00636 u32 em7186_param_write(u8 *values, u8 page, ParamInfo *paramList, u8 numParams)
00637 {
00638     
00639 
00640     u8 i, paramAck, paramNum, pageSelectValue;
00641     u16 valIndex = 0;
00642     for (i = 0; i < numParams; i++)
00643     {
00644         pageSelectValue = page | (paramList[i].size << 4);
00645         em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, pageSelectValue);
00646 
00647         em7186_i2c_write(PARAM_LOAD_REG, &values[valIndex], (u16)paramList[i].size);
00648 
00649         paramNum = paramList[i].paramNo | 0x80;
00650         em7186_i2c_write_value(PARAM_REQUEST_REG, paramNum);
00651         do
00652         {
00653             em7186_i2c_read(PARAM_ACK_REG, &paramAck, 1);
00654             if (paramAck == 0x80)
00655             {
00656                 em7186_i2c_write_value(PARAM_REQUEST_REG, 0);
00657                 em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, 0);
00658                 
00659 
00660                 return 0;
00661             }
00662         } while (paramAck != paramNum);
00663 
00664         valIndex += paramList[i].size;
00665     }
00666 
00667     em7186_i2c_write_value(PARAM_REQUEST_REG, 0);
00668     em7186_i2c_write_value(PARAM_PAGE_SELECT_REG, 0);
00669     
00670 
00671     return 1;
00672 }
00673 u32 em7186_read_fifo(u8 *buffer)
00674 {
00675     // Check number of bytes available
00676     u16 bytesAvailable, bytesRead = 0;
00677 
00678     em7186_i2c_read(BYTES_REMANING_REG, (u8*)&bytesAvailable, 2);
00679 
00680 
00681     //printf("FIFO bytesAvailable:%u\n\r",bytesAvailable);
00682 
00683     
00684 
00685 #define CONTINUOUS_FIFO_READ
00686 #ifdef CONTINUOUS_FIFO_READ
00687     bytesRead = em7186_i2c_read(0, buffer, bytesAvailable);
00688 #else
00689     u16 i, bytesToRead;
00690     while (bytesAvailable > 0)
00691     {
00692         // Break on 50 byte fifo register block
00693         bytesToRead = bytesRead % 50 + I2C_MAX_READ > 50 ? 50 - bytesRead % 50 : I2C_MAX_READ;
00694         // Make sure we don't read more than is available in the fifo
00695         bytesToRead = min(bytesAvailable, bytesToRead);
00696         if (!em7186_i2c_read(bytesRead % 50, &buffer[bytesRead], bytesToRead))
00697         {
00698             
00699             return 0;
00700         }
00701         bytesAvailable -= bytesToRead;
00702         bytesRead += bytesToRead;
00703     }
00704 #endif
00705     
00706     //printf("FIFO bytesRead:%u\n\r",bytesRead);
00707 
00708     return bytesRead;
00709 }
00710 u32 em7186_parse_next_fifo_block(u8* buffer, u32 size)
00711 {
00712     u8 sensorId = buffer[0];
00713     
00714 //    if (sensorId < SENSOR_TYPE_ACCELEROMETER_WAKE ||
00715 //        sensorId == SENSOR_TYPE_DEBUG ||
00716 //        sensorId == SENSOR_TYPE_TIMESTAMP ||
00717 //        sensorId == SENSOR_TYPE_TIMESTAMP_OVERFLOW ||
00718 //        sensorId == SENSOR_TYPE_META ||
00719 //        sensorId == SENSOR_TYPE_RAW_GYRO ||
00720 //        sensorId == SENSOR_TYPE_RAW_MAG ||
00721 //        sensorId == SENSOR_TYPE_RAW_ACCEL
00722 //        )
00723 //    {
00724 //     }
00725 //    else
00726 //    {
00727 //        timestamp = timestampWake;
00728 //        timestampPtr = (u16*)&timestampWake;
00729 //    }
00730 
00731     switch(sensorId)
00732     {
00733         case 0:
00734         {
00735             //printf("Padding: %d\n\r", size);
00736             return size;
00737         }
00738         case SENSOR_TYPE_ACCELEROMETER:
00739         case SENSOR_TYPE_ACCELEROMETER_WAKE:
00740         case SENSOR_TYPE_MAGNETIC_FIELD:
00741         case SENSOR_TYPE_MAGNETIC_FIELD_WAKE:
00742         case SENSOR_TYPE_GYROSCOPE:
00743         case SENSOR_TYPE_GYROSCOPE_WAKE:
00744         case SENSOR_TYPE_GRAVITY:
00745         case SENSOR_TYPE_GRAVITY_WAKE:
00746         case SENSOR_TYPE_LINEAR_ACCELERATION:
00747         case SENSOR_TYPE_LINEAR_ACCELERATION_WAKE:
00748         case SENSOR_TYPE_ORIENTATION:
00749         case SENSOR_TYPE_ORIENTATION_WAKE:
00750         {
00751             SensorData3Axis sensorData;
00752             get_3_axis_sensor_data(&sensorData, em7186_sensor_scale[sensorId], buffer);
00753             if (printData) printf("%u %s: %f, %f, %f, %f\n\r", timestamp,em7186_sensor_name[sensorId], sensorData.x, sensorData.y, sensorData.z, sensorData.extra);
00754             if (logData) fprintf(flog,"%u,%u,%f,%f,%f,%f\n", timestamp,sensorId, sensorData.x, sensorData.y, sensorData.z, sensorData.extra);
00755             
00756             return 8;
00757         }
00758         case SENSOR_TYPE_LIGHT:
00759         case SENSOR_TYPE_LIGHT_WAKE:
00760         case SENSOR_TYPE_PROXIMITY:
00761         case SENSOR_TYPE_PROXIMITY_WAKE:
00762         case SENSOR_TYPE_RELATIVE_HUMIDITY:
00763         case SENSOR_TYPE_RELATIVE_HUMIDITY_WAKE:
00764         case SENSOR_TYPE_ACTIVITY:
00765         case SENSOR_TYPE_ACTIVITY_WAKE:
00766         {
00767             float sensorData = (float)((buffer[2] << 8) + buffer[1]) * em7186_sensor_scale[sensorId];
00768             if (printData) printf("%u %s: %fLux\n\r", timestamp,em7186_sensor_name[sensorId], sensorData);
00769             if (logData) fprintf(flog,"%u,%u,%u,%f\n",timestamp, sensorId, sensorData);
00770             return 3;
00771         }
00772         case SENSOR_TYPE_PRESSURE:
00773         case SENSOR_TYPE_PRESSURE_WAKE:
00774         {
00775             float pressure = (float)((buffer[3] << 16) + (buffer[2] << 8) + buffer[1]) * em7186_sensor_scale[sensorId];
00776             if (printData) printf("%u %s: %fPa\n\r", timestamp, em7186_sensor_name[sensorId], pressure);
00777             if (logData) fprintf(flog,"%u,%u,%f\n",timestamp,sensorId,pressure);
00778             return 4;
00779         }
00780         case SENSOR_TYPE_TEMPERATURE:
00781         case SENSOR_TYPE_TEMPERATURE_WAKE:
00782         case SENSOR_TYPE_AMBIENT_TEMPERATURE:
00783         case SENSOR_TYPE_AMBIENT_TEMPERATURE_WAKE:
00784         {
00785             s16 *sensorData = (s16*)&buffer[1];
00786             float temp = (float)(sensorData[0]) * em7186_sensor_scale[sensorId];
00787             if (printData) printf("%u %s: %fC\n\r", timestamp, em7186_sensor_name[sensorId], temp);
00788             if (logData) fprintf(flog,"%u,%u,%f\n",timestamp,sensorId, temp);
00789             return 3;
00790         }
00791         case SENSOR_TYPE_ROTATION_VECTOR:
00792         case SENSOR_TYPE_ROTATION_VECTOR_WAKE:
00793         case SENSOR_TYPE_GAME_ROTATION_VECTOR:
00794         case SENSOR_TYPE_GAME_ROTATION_VECTOR_WAKE:
00795         case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR:
00796         case SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR_WAKE:
00797         case SENSOR_TYPE_PDR:
00798         case SENSOR_TYPE_PDR_WAKE:
00799         case 60:
00800         {
00801             SensorData4Axis rotationVector;
00802             get_rotation_vector(&rotationVector, em7186_sensor_scale[sensorId], buffer);
00803             if (printData) printf("%u %s: %f, %f, %f, %f, %f\n\r", timestamp, em7186_sensor_name[sensorId], rotationVector.x, rotationVector.y, rotationVector.z, rotationVector.w, rotationVector.extra);
00804             if (logData) fprintf(flog,"%u,%u,%f,%f,%f,%f,%f\n", timestamp,sensorId, rotationVector.x, rotationVector.y, rotationVector.z, rotationVector.w, rotationVector.extra);
00805             return 11;
00806         }
00807         case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED:
00808         case SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED_WAKE:
00809         case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED:
00810         case SENSOR_TYPE_GYROSCOPE_UNCALIBRATED_WAKE:
00811         {
00812             SensorData6Axis sensorDataUncal;
00813             get_3_axis_uncal_sensor_data(&sensorDataUncal, em7186_sensor_scale[sensorId], buffer);
00814             if (printData) printf("%u %s: %f, %f, %f, %f, %f, %f, %f\n\r", timestamp, em7186_sensor_name[sensorId], sensorDataUncal.x, sensorDataUncal.y, sensorDataUncal.z, sensorDataUncal.x_bias, sensorDataUncal.y_bias, sensorDataUncal.z_bias, sensorDataUncal.extra);
00815             if (logData) fprintf(flog,"%u,%u,%f,%f,%f,%f,%f,%f,%f\n", timestamp,sensorId, sensorDataUncal.x, sensorDataUncal.y, sensorDataUncal.z, sensorDataUncal.x_bias, sensorDataUncal.y_bias, sensorDataUncal.z_bias, sensorDataUncal.extra);
00816             return 14;
00817         }
00818         case SENSOR_TYPE_STEP_COUNTER:
00819         case SENSOR_TYPE_STEP_COUNTER_WAKE:
00820         {
00821             u16 sensorData = (buffer[2] << 8) + buffer[1];
00822             if (printData) printf("%u %s: %u\n\r", timestamp, em7186_sensor_name[sensorId], sensorData);
00823             if (logData) fprintf(flog,"%u,%u,%u\n", timestamp,sensorId, sensorData);
00824            return 3;
00825         }
00826         case SENSOR_TYPE_HEART_RATE:       
00827         case SENSOR_TYPE_HEART_RATE_WAKE:
00828         {
00829             u8 sensorData = buffer[1];
00830     
00831     // Heart Rate
00832             if (printData) printf("%u %s: %u\n\r", timestamp, em7186_sensor_name[sensorId], sensorData);
00833             if (logData) fprintf(flog,"%u,%u,%u\n",timestamp,sensorId, sensorData);
00834             return 3;
00835         }
00836         case SENSOR_TYPE_CAR_MAG_DATA:
00837         case SENSOR_TYPE_CAR_MAG_DATA_WAKE:
00838         {
00839             // u8 SensorId, float[3] RawMagData, u8 Status, u8 expansion data 
00840             float RawMagData[3];
00841             memcpy(RawMagData, &buffer[1], sizeof(RawMagData));
00842     
00843             if (printData) printf("%u Car Detect Mag Data = %3.3f,%3.3f,%3.3f\n\r", timestamp, RawMagData[0], RawMagData[1], RawMagData[2]);
00844             if (logData) fprintf(flog,"%u,%u,%f,%f,%f\n", timestamp,sensorId, RawMagData[0], RawMagData[1], RawMagData[2]);
00845 
00846             return 15;
00847         }
00848     
00849         case SENSOR_TYPE_SIGNIFICANT_MOTION:
00850         case SENSOR_TYPE_SIGNIFICANT_MOTION_WAKE:
00851         case SENSOR_TYPE_STEP_DETECTOR:
00852         case SENSOR_TYPE_STEP_DETECTOR_WAKE:
00853         case SENSOR_TYPE_TILT_DETECTOR:
00854         case SENSOR_TYPE_TILT_DETECTOR_WAKE:
00855         case SENSOR_TYPE_PICK_UP_GESTURE:
00856         case SENSOR_TYPE_PICK_UP_GESTURE_WAKE:
00857         case SENSOR_TYPE_WAKE_GESTURE:
00858         case SENSOR_TYPE_WAKE_GESTURE_WAKE:
00859         {
00860            if (printData) printf("%u %s\n\r", timestamp, em7186_sensor_name[sensorId]);
00861            if (logData) fprintf(flog,"%u,%u\n", timestamp,sensorId);
00862              return 1;
00863         }
00864         case SENSOR_TYPE_TIMESTAMP:
00865         case SENSOR_TYPE_TIMESTAMP_WAKE:
00866         {
00867             u16* uPacket = (u16*)&buffer[1];
00868             timestampPtr[0] = uPacket[0];
00869             timestamp = *(u32*)timestampPtr;
00870             return 3;
00871         }
00872         case SENSOR_TYPE_TIMESTAMP_OVERFLOW:
00873         case SENSOR_TYPE_TIMESTAMP_OVERFLOW_WAKE:
00874         {
00875             u16* uPacket = (u16*)&buffer[1];
00876             timestampPtr[1] = uPacket[0];
00877             timestampPtr[0] = 0;
00878             timestamp = *(u32*)timestampPtr;
00879             return 3;
00880         }
00881         case SENSOR_TYPE_META:
00882         case SENSOR_TYPE_META_WAKE: 
00883         {
00884             if (reportMetaData) 
00885             {
00886                 if (printData) 
00887                 {
00888                     if (sensorId == SENSOR_TYPE_META_WAKE)
00889                     {
00890                         printf("%u WAKEUP %s:, %s/%u, %u\n\r", timestamp, em7186_meta_event_name[buffer[1]], em7186_sensor_name[buffer[2]],buffer[2], buffer[3]);
00891                     }
00892                     else 
00893                     {
00894                         printf("%u %s: %s/%u, %u\n\r", timestamp, em7186_meta_event_name[buffer[1]], em7186_sensor_name[buffer[2]], buffer[2], buffer[3]);
00895                         // special hook for sample script
00896                         if (buffer[2] == 14)
00897                         {
00898                             if (buffer[3] == 0 ) {
00899                                 green_LED = 1;
00900                                 printf("PASS **** The RM3100 Magnetometer ASIC and sensors are operating properly\n\r");
00901                             } else {
00902                                 green_LED = 1;
00903                                 wait(0.1);
00904                                 green_LED = 0;
00905                                 printf("FAIL **** The RM3100 Magnetometer ASIC and sensors fail code = %u\n\r",buffer[3]);
00906                             }
00907                         }                
00908                     }
00909                 }
00910                 if (logData) fprintf(flog,"%u,%u,%u,%u,%u\n",timestamp,sensorId, buffer[1],buffer[2],buffer[3]);
00911             }
00912             return 4;
00913         }
00914         case SENSOR_TYPE_RAW_ACCEL:
00915         case SENSOR_TYPE_RAW_GYRO:
00916         {
00917             SensorData3Axis sensorData;
00918             float* fPacket = (float*)&buffer[7];
00919             sensorData.x = 256 * (s8)buffer[2] + buffer[1]; //s16 will convert to float here
00920             sensorData.y = 256 * (s8)buffer[4] + buffer[3];
00921             sensorData.z = 256 * (s8)buffer[6] + buffer[5];
00922             sensorData.extra = fPacket[0];
00923             if (printData) printf("%u %s: %3.0f, %3.0f, %3.0f, %3.1f\n\r", timestamp, em7186_sensor_name[sensorId], sensorData.x, sensorData.y, sensorData.z, sensorData.extra);
00924             if (logData) fprintf(flog,"%u,%u,%f,%f,%f,%f\n", timestamp,sensorId, sensorData.x, sensorData.y, sensorData.z, sensorData.extra);
00925             return 11;
00926         }
00927         case SENSOR_TYPE_RAW_MAG:  //jm modified to s32 for RM3100
00928         {
00929             SensorData3Axis sensorData;
00930             //s32* sPacket = (s32*)&buffer[1];
00931             float* fPacket = (float*)&buffer[1];
00932             //sensorData.x = sPacket[0];    //s32 will convert to float here
00933             //sensorData.y = sPacket[1];
00934             //sensorData.z = sPacket[2];
00935             sensorData.x = (float)(buffer[1] + 256 * (buffer[2] + 256 * (buffer[3] + 256 * (buffer[4]))));  //s32 will convert to float here
00936             sensorData.y = buffer[5] + 256 * (buffer[6] + 256 * (buffer[7] + 256 * (buffer[8])));
00937             sensorData.z = buffer[9] + 256 * (buffer[10] + 256 * (buffer[11] + 256 * (buffer[12])));
00938             sensorData.extra = fPacket[3];
00939             if (printData) printf("%u %s: %f, %f, %f\n\r", timestamp, em7186_sensor_name[sensorId], sensorData.x, sensorData.y, sensorData.z);
00940             if (logData) fprintf(flog,"%u,%u,%f,%f,%f\n", timestamp,sensorId, sensorData.x, sensorData.y, sensorData.z);
00941             return 17;
00942         }
00943         case SENSOR_TYPE_DEBUG:
00944         {
00945             u8 packetSize = buffer[1] & 0x3F;
00946             u8 i;
00947             for (i = 0; i < packetSize; i++)
00948             {
00949                 if (printData) printf("%c", (u8)buffer[2 + i]);
00950             }
00951             return 14;
00952         }
00953         default:
00954         {
00955             // Parsing error or unkown sensor type. Clear out the rest of the 
00956             // buffer and start clean on the next read.
00957             if (printData) printf("%u Other: 0x%x: %d bytes skipped\n\r", timestamp, buffer[0], size);
00958             break;
00959         }
00960     
00961     } // end switch(sensorId) 
00962     
00963     return 0;
00964 }
00965 u32 em7186_parse_fifo(u8* buffer, u32 size)
00966 {
00967     u32 index = 0;
00968     u32 bytesUsed;
00969     u32 bytesRemaining = size;
00970 
00971     //if (displayText) printf("FIFO #Bytes to parse:%u\n\r",bytesRemaining);
00972 
00973 
00974     if (size == 0) return size;
00975 
00976     do {
00977         bytesUsed = em7186_parse_next_fifo_block(&buffer[index], bytesRemaining);
00978         index += bytesUsed;
00979         bytesRemaining -= bytesUsed;
00980     } while (bytesUsed > 0 && bytesRemaining > 0);
00981 
00982     return size - bytesRemaining;
00983 }
00984 u32 em7186_set_sensor_rate(u8 sensorId, u16 rate)
00985 {
00986 #ifdef BHI160
00987     u8 paramPage = PARAM_PAGE_SENSOR_INFO;
00988     ParamInfo param[] = { sensorId + 64, 2 };
00989 #else
00990     u8 paramPage = PARAM_PAGE_SENSOR_CONF;
00991     ParamInfo param[] = { sensorId, 2 };
00992 #endif
00993     return em7186_param_write((u8*)&rate, paramPage, param, 1);
00994 }
00995 
00996 
00997 u32 em7186_set_sensor_delay(u8 sensorId, u16 delay)
00998 {
00999 #ifdef BHI160
01000     u8 paramPage = PARAM_PAGE_SENSOR_INFO;
01001     ParamInfo param[] = { sensorId + 64, 2 };
01002 #else
01003     u8 paramPage = PARAM_PAGE_SENSOR_CONF;
01004     ParamInfo param[] = { sensorId, 2 };
01005 #endif
01006     u16 config[2];
01007 
01008     em7186_param_read((u8*)config, paramPage, param, 1);
01009     config[1] = delay;
01010     return em7186_param_write((u8*)config, paramPage, param, 1);
01011 }
01012 u32 em7186_set_meta_event(u8 eventId, u8 enable, u8 enableInt)
01013 {
01014     unsigned long metaEventBits;
01015     ParamInfo param[] = { 1, 8 };
01016 
01017     em7186_param_read((u8*)&metaEventBits, PARAM_PAGE_SYSTEM, param, 1);
01018     unsigned long bitMask = !(0x03 << ((eventId - 1) * 2));
01019     unsigned long setting = ((enable ? 0x02 : 0x00) | (enableInt ? 0x01 : 0x00)) << ((eventId - 1) * 2);
01020     metaEventBits = (metaEventBits & bitMask) | setting;
01021     em7186_param_write((u8*)&metaEventBits, PARAM_PAGE_SYSTEM, param, 1);
01022 
01023     return 1;
01024 }
01025 u8  em7186_set_scale_factors()
01026 {
01027     // Fixed range
01028     em7186_sensor_scale[SENSOR_TYPE_ORIENTATION_WAKE] =
01029         em7186_sensor_scale[SENSOR_TYPE_ORIENTATION] =
01030         360.0f / powf(2.0f, 15.0f); // Fixed
01031 
01032     em7186_sensor_scale[SENSOR_TYPE_ROTATION_VECTOR] =
01033         em7186_sensor_scale[SENSOR_TYPE_ROTATION_VECTOR_WAKE] =
01034         em7186_sensor_scale[SENSOR_TYPE_GAME_ROTATION_VECTOR] =
01035         em7186_sensor_scale[SENSOR_TYPE_GAME_ROTATION_VECTOR_WAKE] =
01036         em7186_sensor_scale[SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR] =
01037         em7186_sensor_scale[SENSOR_TYPE_GEOMAGNETIC_ROTATION_VECTOR_WAKE] =
01038         em7186_sensor_scale[SENSOR_TYPE_PDR] =
01039         em7186_sensor_scale[SENSOR_TYPE_PDR_WAKE] =
01040         em7186_sensor_scale[60] =
01041         1.0f / powf(2.0f, 14.0f);  // Fixed
01042     
01043 
01044 
01045     // Can change during system operation (if desired)
01046     float accelDynamicRange = 4.0f; // g
01047     float magDynamicRange = 1000.0f; // uT 
01048     float gyroDynamicRange = 2000.0f; // d/s
01049 
01050     // Change depending on dynamic range
01051     em7186_sensor_scale[SENSOR_TYPE_ACCELEROMETER] =
01052         em7186_sensor_scale[SENSOR_TYPE_ACCELEROMETER_WAKE] =
01053         em7186_sensor_scale[SENSOR_TYPE_GRAVITY] =
01054         em7186_sensor_scale[SENSOR_TYPE_GRAVITY_WAKE] =
01055         em7186_sensor_scale[SENSOR_TYPE_LINEAR_ACCELERATION] =
01056         em7186_sensor_scale[SENSOR_TYPE_LINEAR_ACCELERATION_WAKE] =
01057         9.81f * accelDynamicRange / powf(2.0f, 15.0f);
01058 
01059     em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD] =
01060         em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD_WAKE] =
01061         em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED] =
01062         em7186_sensor_scale[SENSOR_TYPE_MAGNETIC_FIELD_UNCALIBRATED_WAKE] =
01063         magDynamicRange / powf(2.0f, 15.0f);
01064 
01065     em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE] =
01066         em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE_WAKE] =
01067         em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE_UNCALIBRATED] =
01068         em7186_sensor_scale[SENSOR_TYPE_GYROSCOPE_UNCALIBRATED_WAKE] =
01069         (3.1415927f / 180.0f) * gyroDynamicRange / powf(2.0f, 15.0f);
01070 
01071     // Could change depending on dynamic range of physical sensor
01072     em7186_sensor_scale[SENSOR_TYPE_PRESSURE] =
01073         em7186_sensor_scale[SENSOR_TYPE_PRESSURE_WAKE] =
01074         1.0f / 128.0f;              // Subject to change
01075 
01076     em7186_sensor_scale[SENSOR_TYPE_LIGHT] =
01077         em7186_sensor_scale[SENSOR_TYPE_LIGHT_WAKE] =
01078         10000.0f / powf(2.0f, 16.0f);    // Subject to change
01079 
01080     em7186_sensor_scale[SENSOR_TYPE_PROXIMITY] =
01081         em7186_sensor_scale[SENSOR_TYPE_PROXIMITY_WAKE] =
01082         100.0f / powf(2.0f, 16.0f); // Subject to change
01083 
01084     em7186_sensor_scale[SENSOR_TYPE_RELATIVE_HUMIDITY] =
01085         em7186_sensor_scale[SENSOR_TYPE_RELATIVE_HUMIDITY_WAKE] =
01086         100.0f / powf(2.0f, 16.0f);  // Subject to change
01087 
01088     em7186_sensor_scale[SENSOR_TYPE_TEMPERATURE] =
01089         em7186_sensor_scale[SENSOR_TYPE_TEMPERATURE_WAKE] =
01090         em7186_sensor_scale[SENSOR_TYPE_AMBIENT_TEMPERATURE] =
01091         em7186_sensor_scale[SENSOR_TYPE_AMBIENT_TEMPERATURE_WAKE] =
01092         150.0f / powf(2.0f, 15.0f); // Subject to change
01093 
01094     em7186_sensor_scale[SENSOR_TYPE_ACTIVITY] =
01095         em7186_sensor_scale[SENSOR_TYPE_ACTIVITY_WAKE] =
01096         1.0f;
01097 
01098     return 1;
01099 }
01100 
01101 /*
01102 // Warm start parameter transfer. The parameter list and flags are presently going
01103 //   through a major revision therefore, these functions are now on hold
01104 
01105 u32 em7186_warm_start_load(const char *filename)
01106 {
01107     FILE *fin;
01108     fin = fopen(filename, "rb");
01109     if (!fin) return 0;
01110     WarmStartParams warmStartParams;
01111     fread(&warmStartParams, 1, sizeof(warmStartParams), fin);
01112     fclose(fin);
01113 
01114     em7186_param_write((u8*)&warmStartParams, PARAM_PAGE_WARM_START, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
01115 
01116     return 1;
01117 }
01118 u32 em7186_warm_start_save(const char *filename)
01119 {
01120     WarmStartParams warmStartParams;
01121     em7186_param_read((u8*)&warmStartParams, PARAM_PAGE_WARM_START, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
01122 
01123     FILE *fout;
01124     fout = fopen(filename, "wb");
01125     if (!fout) return 0;
01126     fwrite(&warmStartParams, 1, sizeof(warmStartParams), fout);
01127     fclose(fout);
01128     if (displayText) printf("Warm start parmeters Saved to warmstart.dat\n\r");
01129     return 1;
01130 }*/
01131 
01132 
01133 
01134 //=========================================================================
01135 // Additional Control Functions
01136 //=========================================================================
01137 u32 em7186_ap_suspend(u8 suspend)
01138 {
01139     u32 status = 0;
01140 
01141     if (suspend) status = em7186_i2c_write_value(HOST_INTERFACE_CTRL_REG, HOST_INTERFACE_CTRL_AP_SUSPEND);
01142     else status = em7186_i2c_write_value(HOST_INTERFACE_CTRL_REG, 0);
01143 
01144     return status;
01145 }
01146 
01147 u32 em7186_flush_sensor(u8 sensorId)
01148 {
01149     return em7186_i2c_write_value(FIFO_FLUSH_REG, sensorId);
01150 }
01151 
01152 void disableSensors()
01153 {
01154    u8 i;
01155     for ( i = 0; i<64 ;i++)
01156     {
01157         if (sensorEnabled[i] == TRUE)
01158         {
01159             em7186_set_sensor_rate(sensorEnabled[i+1], 0);
01160             sensorEnabled[i] = FALSE;
01161         }
01162     }        
01163 }
01164 
01165 u32 em7186_set_fifo_watermarks(u16 minRemainingWakeFifo, u16 minRemainingNonWakeFifo)
01166 {
01167     // if min remaining values are non-zero the watermark will be set that many bytes from the end of the fifo.
01168     // if a zero value is used the watermark is disabled.
01169     u16 config[4];
01170     ParamInfo param[1] = { { PARAM_FIFO_CONTROL, 8 } };
01171     
01172     em7186_param_read((u8*)config, PARAM_PAGE_SYSTEM, param, 1);
01173     config[0] = minRemainingWakeFifo ? config[1] - minRemainingWakeFifo : 0;
01174     config[2] = minRemainingNonWakeFifo ? config[3] - minRemainingNonWakeFifo : 0;
01175     return em7186_param_write((u8*)config, PARAM_PAGE_SYSTEM, param, 1);
01176 }
01177 
01178 u32 em7186_enable_raw_sensors(u8 enableMag, u8 enableAccel, u8 enableGyro)
01179 {
01180     u8 value = 0x00;
01181     if (enableMag) value |= 0x04;
01182     if (enableAccel) value |= 0x01;
01183     if (enableGyro) value |= 0x02;
01184 
01185     ParamInfo param[1] = { { 127, 1 } };
01186     
01187     em7186_param_write(&value, PARAM_PAGE_WARM_START, param, 1);
01188 
01189     return 1;
01190 }
01191 
01192 void resetCpu()
01193 {
01194     disableSensors();
01195     em7186_i2c_write_value(0xB6, 63);
01196 }
01197 
01198 u32 paramListSize(ParamInfo *paramList, u8 numParams)
01199 {
01200     u8 i;
01201     u32 size = 0;
01202     for (i = 0; i < numParams; i++)
01203     {
01204         size += paramList[i].size;
01205     }
01206     return size;
01207 }
01208 
01209 void displaySavedParams(u8 *values, ParamInfo *paramList, u8 numParams)
01210 {
01211     u8 i;
01212     u16 valueIndex = 0;
01213     for (i = 0; i < numParams; i++)
01214     {
01215         float* floatVals = (float*)&values[valueIndex];
01216         u32* hexVals = (u32*)&values[valueIndex];
01217 
01218         if (paramList[i].size >= 4)
01219         {
01220             printf("%d 1: %f, %08x\n\r", paramList[i].paramNo, floatVals[0], hexVals[0]);
01221         }
01222         if (paramList[i].size >= 8)
01223         {
01224             printf("%d 2: %f, %08x\n\r", paramList[i].paramNo, floatVals[1], hexVals[1]);
01225         }
01226         if (paramList[i].size == 1)
01227         {
01228             u8* hexVals = (u8*)&values[valueIndex];
01229             printf("%d 1: %d\n\r", paramList[i].paramNo, hexVals[0]);
01230         }
01231         valueIndex += paramList[i].size;
01232     }
01233 }
01234 
01235 void displayParams(u8 paramPage, ParamInfo *paramList, u8 numParams)
01236 {
01237     u32 size = paramListSize(paramList, numParams);
01238     u8 *values = (u8 *)malloc(size);
01239     em7186_param_read(values, paramPage, paramList, numParams);
01240     displaySavedParams(values, paramList, numParams);
01241     free(values);
01242 }
01243 
01244 /*
01245 // Warm start parameter transfer. The parameter list and flags are presently going
01246 //   through a major revision therefore, these functions are now on hold
01247 
01248 void warmStart()
01249 {
01250     // Save warm start params
01251     em7186_warm_start_save(warmStartFile);
01252 
01253     if (displayText) printf("\n\r\n\r-------------------- CPU Reset -------------------------------------\n\r");
01254     // Reset the cpu
01255     resetCpu();
01256 
01257     if (displayText) printf("\n\r\n\r---------------- Parameters after reset ----------------------------\n\r");
01258     // Read the warmstart params after reset
01259     displayParams(2, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
01260 
01261     if (displayText) printf("\n\r\n\r---------------- Parameters after upload----------------------------\n\r");
01262     // Load warmstart parameters
01263     em7186_warm_start_load(warmStartFile);
01264 
01265     // Read the warmstart params after warmstart
01266     displayParams(2, warmStartList, sizeof(warmStartList) / sizeof(warmStartList[0]));
01267 }*/
01268 
01269 char* strBits(void const * const ptr, u8 numBytes, char* str)
01270 {
01271     u8 *bytes = (u8*)ptr;
01272     u8 i, j;
01273     for (i = 0; i < numBytes; i++)
01274     {
01275         for (j = 0; j < 8; j++)
01276         {
01277             str[i * 8 + (7 - j)] = bytes[(numBytes - 1) - i] & (1 << j) ? '1' : '0';
01278         }
01279     }
01280     str[numBytes * 8] = '\0';
01281     return str;
01282 }
01283 
01284 void displayStatusRegisters()
01285 {
01286     u8 buf[4];
01287     char str[17];
01288     printf("\n------------ Displaying Status Registers -----------\n\r");
01289     em7186_i2c_read(HOST_STATUS_REG, buf, 3);
01290     printf("Host Status:       % 5u, %s\n\r", buf[0], strBits(&buf[0], sizeof(buf[0]), str));
01291     printf("Interrupt Status:  % 5u, %s\n\r", buf[1], strBits(&buf[1], sizeof(buf[0]), str));
01292     printf("Chip Status:       % 5u, %s\n\r", buf[2], strBits(&buf[2], sizeof(buf[0]), str));
01293     em7186_i2c_read(ERR_REG, buf, 4);
01294     printf("Error Register:    % 5u, %s\n\r", buf[0], strBits(&buf[0], sizeof(buf[0]), str));
01295     printf("Interrupt State:   % 5u, %s\n\r", buf[1], strBits(&buf[1], sizeof(buf[0]), str));
01296     printf("Debug Value:       % 5u, %s\n\r", buf[2], strBits(&buf[2], sizeof(buf[0]), str));
01297     printf("Debug State:       % 5u, %s\n\r", buf[3], strBits(&buf[3], sizeof(buf[0]), str));
01298     em7186_i2c_read(BYTES_REMANING_REG, buf, 2);
01299     u16* v = (u16*)&buf;
01300     printf("Bytes Remaining:   % 5u, %s\n\n\r", v[0], strBits(&v[0], sizeof(v[0]), str));
01301 }
01302 
01303 void displaySensorStatusBits(u8 id, const SensorStatus *status)
01304 {
01305     printf("|% 4u |      % 4u | % 4u |   % 4u |      % 4u | % 4u |  % 4u |\n\r", 
01306         id, status->dataAvailable, status->i2cNack, status->deviceIdError, 
01307         status->transientError, status->dataLost, status->powerMode);
01308 }
01309 
01310 void displaySensorStatus()
01311 {
01312     SensorStatus sensorStatus[32];
01313     ParamInfo param[] = { PARAM_SENSOR_STATUS_BANK_0, 32 };
01314     em7186_param_read((u8*)sensorStatus, PARAM_PAGE_SYSTEM, param, 1);
01315     printf("+------------------------------------------------------------+\n\r");
01316     printf("|                       SENSOR STATUS                        |\n\r");
01317     printf("+-----+-----------+------+--------+-----------+------+-------+\n\r");
01318     printf("| ID  | Data      | I2C  | DEVICE | Transient | Data | Power |\n\r");
01319     printf("|     | Available | NACK | ID ERR | Error     | Lost | Mode  |\n\r");
01320     printf("+-----+-----------+------+--------+-----------+------+-------+\n\r");
01321     u8 i;
01322     for (i = 0; i < 32; i++)
01323     {
01324         displaySensorStatusBits(i + 1, &sensorStatus[i]);
01325     }
01326     param[0].paramNo = PARAM_SENSOR_STATUS_BANK_0 + 4;
01327     em7186_param_read((u8*)sensorStatus, PARAM_PAGE_SYSTEM, param, 1);
01328     for (i = 0; i < 16; i++)
01329     {
01330         displaySensorStatusBits(i + 65, &sensorStatus[i]);
01331     }
01332     printf("+-----+-----------+------+--------+-----------+------+-------+\n\r");
01333 
01334 }
01335 
01336 void getPhysicalSensorStatus()
01337 {
01338     ParamInfo param[] = { PARAM_PHYSICAL_SENSOR_STATUS, 15 };
01339     em7186_param_read((u8*)&physicalSensorStatus, PARAM_PAGE_SYSTEM, param, 1);
01340 }
01341 
01342 void displayPhysicalSensorStatus()
01343 {
01344     getPhysicalSensorStatus();
01345     printf("+----------------------------------------------------------------------------------------+\n\r");
01346     printf("|                                  PHYSICAL SENSOR STATUS                                |\n\r");
01347     printf("+--------+--------+---------+-----+-----------+------+--------+-----------+------+-------+\n\r");
01348     printf("| Sensor | Sample | Dynamic | ID  | Data      | I2C  | DEVICE | Transient | Data | Power |\n\r");
01349     printf("|        | Rate   | Range   |     | Available | NACK | ID ERR | Error     | Lost | Mode  |\n\r");
01350     printf("+--------+--------+---------+-----+-----------+------+--------+-----------+------+-------+\n\r");
01351 
01352     printf("| Accel  |   % 4u |    % 4u ", physicalSensorStatus.accel.sampleRate, physicalSensorStatus.accel.dynamicRange);
01353     displaySensorStatusBits(1, &physicalSensorStatus.accel.status);
01354 
01355     printf("| Gyro   |   % 4u |    % 4u ", physicalSensorStatus.gyro.sampleRate, physicalSensorStatus.gyro.dynamicRange);
01356     displaySensorStatusBits(2, &physicalSensorStatus.gyro.status);
01357 
01358     printf("| Mag    |   % 4u |    % 4u ", physicalSensorStatus.mag.sampleRate, physicalSensorStatus.mag.dynamicRange);
01359     displaySensorStatusBits(3, &physicalSensorStatus.mag.status);
01360     printf("+--------+--------+---------+-----+-----------+------+--------+-----------+------+-------+\n\r");
01361 }
01362 void displayPhysicalSensorInformation()
01363 {
01364     typedef struct
01365     {
01366         u8 sensorType;
01367         u8 driverId;
01368         u8 driverVersion;
01369         u8 current;
01370         u16 currentDynamicRange;
01371         u8 flags;
01372         u8 reserved;
01373         u16 currentRate;
01374         u8 numAxes;
01375         u8 orientationMatrix[5];
01376     } PhysicalSensorInformation;
01377  
01378     PhysicalSensorInformation s;
01379 
01380     u64 physicalSensorPresent = 0;
01381     ParamInfo param = { 32, 4 };
01382     em7186_param_read((u8*)&physicalSensorPresent, PARAM_PAGE_SYSTEM, &param, 1);
01383     
01384     u32 i;
01385     param.size = 16;
01386 
01387     printf("\n+----------------------------------+--------+---------+-------+---------+------------+------+\n\r");
01388     printf("| Sensor                           | Driver | Driver  | Power | Current | Current    | Num  |\n\r");
01389     printf("|                                  | ID     | Version |       | Range   | Rate       | Axes |\n\r");
01390     printf("+----------------------------------+--------+---------+-------+---------+------------+------+\n\r");
01391     for (i = 0; i < 64; i++)
01392     {
01393         if (physicalSensorPresent & (1LL << i))
01394         {
01395             param.paramNo = i+32;
01396             em7186_param_read((u8*)&s, PARAM_PAGE_SYSTEM, &param, 1);
01397             printf("| %-32s |   % 4u |    % 4u |  % 4u |    % 4u |       % 4u | % 4u |\n\r",
01398                 em7186_sensor_name[s.sensorType], s.driverId, s.driverVersion, s.current, s.currentDynamicRange, s.currentRate, s.numAxes);
01399         }
01400     }
01401     printf("+----------------------------------+--------+---------+-------+---------+------------+------+\n\r");
01402 }
01403 
01404 void getSensorInformation()
01405 {
01406     em7186_param_read((u8*)sensorInformation, PARAM_PAGE_SENSOR_INFO, sensorInfoParamList, sizeof(sensorInfoParamList) / sizeof(sensorInfoParamList[0]));
01407     haveSensorInfo = 1;
01408 
01409     magMaxRate = sensorInformation[SENSOR_TYPE_MAGNETIC_FIELD].maxRate;
01410     accelMaxRate = sensorInformation[SENSOR_TYPE_ACCELEROMETER].maxRate;
01411     gyroMaxRate = sensorInformation[SENSOR_TYPE_GYROSCOPE].maxRate;
01412 }
01413 
01414 void getSensorConfiguration()
01415 {
01416     u8 i;
01417     ParamInfo param[1] = { 0, 8 };
01418     for (i = 1; i<sizeof(sensorInformation) / sizeof(sensorInformation[0]); i++)
01419     { 
01420         if (sensorInformation[i].sensorId > 0)
01421         {
01422             param[0].paramNo = sensorInformation[i].sensorId;
01423             em7186_param_read((u8*)&sensorConfiguration[i], PARAM_PAGE_SENSOR_CONF, param, 1);
01424         }
01425     }
01426 }
01427 
01428 void displaySensorConfiguration()
01429 {
01430     if (!haveSensorInfo) { getSensorInformation(); };
01431     getSensorConfiguration();
01432     printf("+-------------------------------------------------------------------------+\n\r");
01433     printf("|                          Sensor Configuration                           |\n\r");
01434     printf("+----------------------------------+-------+-------+-------------+--------+\n\r");
01435     printf("| Sensor                           | Rate  | Delay | Sensitivity | Range  |\n\r");
01436     printf("+----------------------------------+-------+-------+-------------+--------+\n\r");
01437     u8 i;
01438     for (i = 0; i < sizeof(sensorInformation) / sizeof(sensorInformation[0]); i++)
01439     {
01440         if (sensorInformation[i].sensorId > 0)
01441         {
01442             printf("| %-32s | % 5u | % 5u | % 11u | % 6u |\n\r",
01443                 em7186_sensor_name[sensorInformation[i].sensorId],
01444                 sensorConfiguration[i].sampleRate,
01445                 sensorConfiguration[i].maxReportLatency, 
01446                 sensorConfiguration[i].changeSensitivity, 
01447                 sensorConfiguration[i].dynamicRange);
01448         }
01449     }
01450     printf("+----------------------------------+-------+-------+-------------+--------+\n\r");
01451 }
01452 
01453 void displaySensorInformation()
01454 {
01455     if (!haveSensorInfo) { getSensorInformation(); };
01456     printf("+------------------------------------------------------------------------------------------+\n\r");
01457     printf("|                                  Sensor Information                                      |\n\r");
01458     printf("+----------------------------------------+---------+-------+-------+-----+----------+------+\n\r");
01459     printf("| ID  |Sensor                            | Driver  | Power | Range | Res | Rate     | Size |\n\r");
01460     printf("+-----|----------------------------------+---------+-------+-------+-----+----------+------+\n\r");
01461     u8 i;
01462     for (i = 0; i < sizeof(sensorInfoParamList) / sizeof(sensorInfoParamList[0]); i++)
01463     {
01464         if (sensorInformation[i].sensorId > 0)
01465         {
01466             printf("|%3u  | %-32s | % 3u.%-3u |  % 4u | % 5u | % 3u | %-3u-% 4u |  % 3u |\n\r",
01467                 i,
01468                 em7186_sensor_name[sensorInformation[i].sensorId], sensorInformation[i].driverId, 
01469                 sensorInformation[i].driverVersion, sensorInformation[i].power,
01470                 sensorInformation[i].maxRange, sensorInformation[i].resolution, 
01471                 sensorInformation[i].minRate, sensorInformation[i].maxRate,
01472                 sensorInformation[i].eventSize);
01473         }
01474     }
01475     printf("+-----|----------------------------------+---------+-------+-------+-----+----------+------+\n\r");
01476 }
01477 
01478 
01479 void read_BuildVersion(void) {
01480 
01481     ParamInfo param; // elements:   {u8 paramNo; u8 size;}
01482 
01483     printf("\n\r\n\rSENtral Firmware build version information from PramIO Page 9\n\r");
01484 
01485     struct pni_version {
01486         u8 major;
01487         u8 minor;
01488         u8 patch;
01489         u8 other;
01490         u32 build;
01491     };
01492 
01493     struct pni_version pni_ver;
01494 
01495 
01496     param.paramNo = 1;
01497     param.size = 8;
01498     em7186_param_read((u8 *)&pni_ver, 9, &param, 1);
01499 
01500     printf("  major = %u\n\r", pni_ver.major);
01501     printf("  minor = %u\n\r", pni_ver.minor);
01502     printf("  patch = %u\n\r", pni_ver.patch);
01503     printf("  other = %u\n\r", pni_ver.other);
01504     printf("  build = %u\n\r", pni_ver.build);
01505 
01506 
01507 }