00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033 #include "board.h"
00034
00035
00036
00037
00038 static const sensorProfile_t* pSensor;
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050 static sendorStatus_t sensor_twi_read_reg(Twid * pTwid,
00051 uint16_t reg,
00052 uint8_t * pData)
00053 {
00054 uint8_t status;
00055 uint8_t reg8[2];
00056 reg8[0] = reg >> 8;
00057 reg8[1] = reg & 0xff;
00058 switch (pSensor->twi_inf_mode){
00059 case SENSOR_TWI_REG_BYTE_DATA_BYTE:
00060 status = TWID_Write(pTwid, pSensor->twi_slave_addr, 0, 0, ®8[1], 1, 0);
00061 status |= TWID_Read(pTwid, pSensor->twi_slave_addr, 0, 0, pData, 1, 0);
00062 break;
00063
00064 case SENSOR_TWI_REG_2BYTE_DATA_BYTE:
00065 status = TWID_Write(pTwid, pSensor->twi_slave_addr, 0, 0, reg8, 2, 0);
00066 status |= TWID_Read(pTwid, pSensor->twi_slave_addr, 0, 0, pData, 1, 0);
00067 break;
00068 case SENSOR_TWI_REG_BYTE_DATA_2BYTE:
00069 status = TWID_Write(pTwid, pSensor->twi_slave_addr, 0, 0, ®8[1], 1, 0);
00070 status |= TWID_Read(pTwid, pSensor->twi_slave_addr, 0, 0, pData, 2, 0);
00071 break;
00072 default:
00073 return SENSOR_TWI_ERROR;
00074 }
00075 if (status) return SENSOR_TWI_ERROR;
00076 return SENSOR_OK;
00077 }
00078
00079
00080
00081
00082
00083
00084
00085
00086 static sendorStatus_t sensor_twi_write_reg(Twid * pTwid,
00087 uint16_t reg,
00088 uint8_t *pData)
00089 {
00090 uint8_t status;
00091 switch (pSensor->twi_inf_mode){
00092 case SENSOR_TWI_REG_BYTE_DATA_BYTE:
00093 status = TWID_Write(pTwid, pSensor->twi_slave_addr, reg, 1, pData, 1, 0);
00094 break;
00095
00096 case SENSOR_TWI_REG_2BYTE_DATA_BYTE:
00097 status = TWID_Write(pTwid, pSensor->twi_slave_addr, reg, 2, pData, 1, 0);
00098 break;
00099
00100 case SENSOR_TWI_REG_BYTE_DATA_2BYTE:
00101 status = TWID_Write(pTwid, pSensor->twi_slave_addr, reg, 1, pData, 2, 0);
00102 break;
00103
00104 default:
00105 return SENSOR_TWI_ERROR;
00106 }
00107 if (status) return SENSOR_TWI_ERROR;
00108 return SENSOR_OK;
00109 }
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120 static sendorStatus_t sensor_check_pid(Twid * pTwid,
00121 uint16_t reg_h,
00122 uint16_t reg_l,
00123 uint16_t pid,
00124 uint16_t ver_mask)
00125 {
00126 uint8_t status;
00127 uint16_t pid_high=0, pid_low=0;
00128 status = sensor_twi_read_reg(pTwid, reg_h, (uint8_t*)&pid_high);
00129 status |= sensor_twi_read_reg(pTwid, reg_l,(uint8_t*)&pid_low);
00130 if (status) return SENSOR_TWI_ERROR;
00131 if ((pid & ver_mask) == (((pid_high << 8 )| (pid_low & 0xFF)) & ver_mask)) {
00132 return SENSOR_OK;
00133 }
00134 else {
00135 printf("BBB - PID: %04x%02x ", pid_high, (uint8_t)pid_low);
00136 return SENSOR_ID_ERROR;
00137 }
00138 }
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152 sendorStatus_t sensor_twi_write_regs(Twid * pTwid, const sensorReg_t * pReglist)
00153 {
00154 uint8_t status;
00155 const sensorReg_t *pNext = pReglist;
00156 volatile uint32_t delay;
00157
00158 while (!((pNext->reg == SENSOR_REG_TERM) && (pNext->val == SENSOR_VAL_TERM))) {
00159 status = sensor_twi_write_reg( pTwid, pNext->reg, (uint8_t *)(&pNext->val));
00160 for (delay = 0; delay <= 10000; delay++) ;
00161 if (status) return SENSOR_TWI_ERROR;
00162 pNext++;
00163 }
00164 return SENSOR_OK;
00165 }
00166
00167
00168
00169
00170
00171
00172
00173
00174 sendorStatus_t sensor_twi_read_regs(Twid * pTwid, const sensorReg_t * pReglist)
00175 {
00176 uint8_t status;
00177 const sensorReg_t *pNext = pReglist;
00178 volatile uint32_t delay;
00179 uint16_t val;
00180 while (!((pNext->reg == SENSOR_REG_TERM) && (pNext->val == SENSOR_VAL_TERM))) {
00181 status = sensor_twi_read_reg( pTwid, pNext->reg, (uint8_t *)&val);
00182
00183 for (delay = 0; delay <= 10000; delay++) ;
00184 if (status) return SENSOR_TWI_ERROR;
00185 pNext++;
00186 }
00187 return SENSOR_OK;
00188 }
00189
00190
00191
00192
00193
00194
00195
00196 sendorStatus_t sensor_setup(Twid * pTwid,
00197 const sensorProfile_t *sensor_profile,
00198 sensorOutputResolution_t resolution)
00199 {
00200 uint8_t i;
00201 sendorStatus_t status = SENSOR_OK;
00202 for (i = 0; i< SENDOR_SUPPORTED_OUTPUTS; i++) {
00203 if (sensor_profile->outputConf[i]->supported){
00204 if (sensor_profile->outputConf[i]->output_resolution == resolution) {
00205 break;
00206 }
00207 }
00208 }
00209 if (i >= SENDOR_SUPPORTED_OUTPUTS)
00210 return SENSOR_RESOLUTION_NOT_SUPPORTED;
00211 pSensor = sensor_profile;
00212 status = sensor_check_pid(pTwid,
00213 pSensor->pid_high_reg,
00214 pSensor->pid_low_reg,
00215 (pSensor->pid_high) << 8 | pSensor->pid_low,
00216 pSensor->version_mask);
00217 if (status) return SENSOR_ID_ERROR;
00218 return sensor_twi_write_regs(pTwid, pSensor->outputConf[i]->output_setting);
00219
00220 }
00221
00222
00223
00224
00225
00226
00227
00228
00229 sendorStatus_t sensor_get_output(sensorOutputFormat_t *format,
00230 uint32_t *width,
00231 uint32_t* height,
00232 sensorOutputResolution_t resolution)
00233 {
00234 uint8_t i;
00235 for (i = 0; i< SENDOR_SUPPORTED_OUTPUTS; i++) {
00236 if (pSensor->outputConf[i]->supported){
00237 if (pSensor->outputConf[i]->output_resolution == resolution) {
00238 *format = pSensor->outputConf[i]->output_format;
00239 *width = pSensor->outputConf[i]->output_width;
00240 *height = pSensor->outputConf[i]->output_height;
00241 return SENSOR_OK;
00242 }
00243 }
00244 }
00245 return SENSOR_RESOLUTION_NOT_SUPPORTED;
00246 }