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
00059 switch (pSensor->twi_inf_mode) {
00060 case SENSOR_TWI_REG_BYTE_DATA_BYTE:
00061 status = TWID_Write(pTwid, pSensor->twi_slave_addr, 0, 0, ®8[1], 1, 0);
00062 status |= TWID_Read(pTwid, pSensor->twi_slave_addr, 0, 0, pData, 1, 0);
00063 break;
00064
00065 case SENSOR_TWI_REG_2BYTE_DATA_BYTE:
00066 status = TWID_Write(pTwid, pSensor->twi_slave_addr, 0, 0, reg8, 2, 0);
00067 status |= TWID_Read(pTwid, pSensor->twi_slave_addr, 0, 0, pData, 1, 0);
00068 break;
00069
00070 case SENSOR_TWI_REG_BYTE_DATA_2BYTE:
00071 status = TWID_Write(pTwid, pSensor->twi_slave_addr, 0, 0, ®8[1], 1, 0);
00072 status |= TWID_Read(pTwid, pSensor->twi_slave_addr, 0, 0, pData, 2, 0);
00073 break;
00074
00075 default:
00076 return SENSOR_TWI_ERROR;
00077 }
00078
00079 if (status) return SENSOR_TWI_ERROR;
00080
00081 return SENSOR_OK;
00082 }
00083
00084
00085
00086
00087
00088
00089
00090
00091 static sendorStatus_t sensor_twi_write_reg(Twid *pTwid,
00092 uint16_t reg,
00093 uint8_t *pData)
00094 {
00095 uint8_t status;
00096
00097 switch (pSensor->twi_inf_mode) {
00098 case SENSOR_TWI_REG_BYTE_DATA_BYTE:
00099 status = TWID_Write(pTwid, pSensor->twi_slave_addr, reg, 1, pData, 1, 0);
00100 break;
00101
00102 case SENSOR_TWI_REG_2BYTE_DATA_BYTE:
00103 status = TWID_Write(pTwid, pSensor->twi_slave_addr, reg, 2, pData, 1, 0);
00104 break;
00105
00106 case SENSOR_TWI_REG_BYTE_DATA_2BYTE:
00107 status = TWID_Write(pTwid, pSensor->twi_slave_addr, reg, 1, pData, 2, 0);
00108 break;
00109
00110 default:
00111 return SENSOR_TWI_ERROR;
00112 }
00113
00114 if (status) return SENSOR_TWI_ERROR;
00115
00116 return SENSOR_OK;
00117 }
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128 static sendorStatus_t sensor_check_pid(Twid *pTwid,
00129 uint16_t reg_h,
00130 uint16_t reg_l,
00131 uint16_t pid,
00132 uint16_t ver_mask)
00133 {
00134 uint8_t status;
00135 uint16_t pid_high = 0, pid_low = 0;
00136 status = sensor_twi_read_reg(pTwid, reg_h, (uint8_t *)&pid_high);
00137 status |= sensor_twi_read_reg(pTwid, reg_l, (uint8_t *)&pid_low);
00138
00139 if (status) return SENSOR_TWI_ERROR;
00140
00141 if ((pid & ver_mask) == (((pid_high << 8) | (pid_low & 0xFF)) & ver_mask))
00142 return SENSOR_OK;
00143 else {
00144 printf("BBB - PID: %04x%02x ", pid_high, (uint8_t)pid_low);
00145 return SENSOR_ID_ERROR;
00146 }
00147 }
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161 sendorStatus_t sensor_twi_write_regs(Twid *pTwid, const sensorReg_t *pReglist)
00162 {
00163 uint8_t status;
00164 const sensorReg_t *pNext = pReglist;
00165 volatile uint32_t delay;
00166
00167 while (!((pNext->reg == SENSOR_REG_TERM) && (pNext->val == SENSOR_VAL_TERM))) {
00168 status = sensor_twi_write_reg(pTwid, pNext->reg, (uint8_t *)(&pNext->val));
00169
00170 for (delay = 0; delay <= 10000; delay++);
00171
00172 if (status) return SENSOR_TWI_ERROR;
00173
00174 pNext++;
00175 }
00176
00177 return SENSOR_OK;
00178 }
00179
00180
00181
00182
00183
00184
00185
00186
00187 sendorStatus_t sensor_twi_read_regs(Twid *pTwid, const sensorReg_t *pReglist)
00188 {
00189 uint8_t status;
00190 const sensorReg_t *pNext = pReglist;
00191 volatile uint32_t delay;
00192 uint16_t val;
00193
00194 while (!((pNext->reg == SENSOR_REG_TERM) && (pNext->val == SENSOR_VAL_TERM))) {
00195 status = sensor_twi_read_reg(pTwid, pNext->reg, (uint8_t *)&val);
00196
00197
00198 for (delay = 0; delay <= 10000; delay++);
00199
00200 if (status) return SENSOR_TWI_ERROR;
00201
00202 pNext++;
00203 }
00204
00205 return SENSOR_OK;
00206 }
00207
00208
00209
00210
00211
00212
00213
00214 sendorStatus_t sensor_setup(Twid *pTwid,
00215 const sensorProfile_t *sensor_profile,
00216 sensorOutputResolution_t resolution)
00217 {
00218 uint8_t i;
00219 sendorStatus_t status = SENSOR_OK;
00220
00221 for (i = 0; i < SENDOR_SUPPORTED_OUTPUTS; i++) {
00222 if (sensor_profile->outputConf[i]->supported) {
00223 if (sensor_profile->outputConf[i]->output_resolution == resolution)
00224 break;
00225 }
00226 }
00227
00228 if (i >= SENDOR_SUPPORTED_OUTPUTS)
00229 return SENSOR_RESOLUTION_NOT_SUPPORTED;
00230
00231 pSensor = sensor_profile;
00232 status = sensor_check_pid(pTwid,
00233 pSensor->pid_high_reg,
00234 pSensor->pid_low_reg,
00235 (pSensor->pid_high) << 8 | pSensor->pid_low,
00236 pSensor->version_mask);
00237
00238 if (status) return SENSOR_ID_ERROR;
00239
00240 return sensor_twi_write_regs(pTwid, pSensor->outputConf[i]->output_setting);
00241
00242 }
00243
00244
00245
00246
00247
00248
00249
00250
00251 sendorStatus_t sensor_get_output(sensorOutputFormat_t *format,
00252 uint32_t *width,
00253 uint32_t *height,
00254 sensorOutputResolution_t resolution)
00255 {
00256 uint8_t i;
00257
00258 for (i = 0; i < SENDOR_SUPPORTED_OUTPUTS; i++) {
00259 if (pSensor->outputConf[i]->supported) {
00260 if (pSensor->outputConf[i]->output_resolution == resolution) {
00261 *format = pSensor->outputConf[i]->output_format;
00262 *width = pSensor->outputConf[i]->output_width;
00263 *height = pSensor->outputConf[i]->output_height;
00264 return SENSOR_OK;
00265 }
00266 }
00267 }
00268
00269 return SENSOR_RESOLUTION_NOT_SUPPORTED;
00270 }