//获取传感器数据 void get_data(void) { iicsend=1; R_IICA0_Master_Send(adr, (uint8_t *)&flow_tx, 1, 40); while(iicsend){NOP();} #ifndef OPTIMIZE delay_ms(2); iicreceive=1; R_IICA0_Master_Receive(adr, (uint8_t *)&px4flow_data, 22, 40); while(iicreceive){NOP();} #endif }
//高度数据解析和滤波(单位:厘米) void hight_filter(void) { float hightbuf; #ifdef OPTIMIZE iicreceive=1; R_IICA0_Master_Receive(adr, (uint8_t *)&px4flow_data, 22, 40); while(iicreceive){NOP();} #endif hightbuf=((float)px4flow_data.ground_distance)/10; hight=(hightbuf-init_hight)*0.8;//(hight_before*0.2)+((hightbuf-init_hight)*0.8);/*减去初始高度*/ hight_before=hight; }
MD_STATUS read_light(lightData * dat){ MD_STATUS ret = MD_OK; int len = 0; int i; while (iica0_busy || IICBSY0){ ; } //Wait until the xfer is complete dat->light = 0; iica0_busy = 1; ret = R_IICA0_Master_Receive(LIGHT_ADDR, i2cbuf, 2, 0); while (iica0_busy || IICBSY0){ ; } //Wait until the xfer is complete if (ret != MD_OK){ md_err(ret, "light read"); return ret; } else { dat->time = millis; dat->light = (uint16_t)i2cbuf[0] | ((uint16_t)i2cbuf[1] << 8); } return ret; }
MD_STATUS read_accel(accelData * dat){ MD_STATUS ret; int i; dat->xraw = 0; dat->yraw = 0; dat->zraw = 0; dat->x = 0.0; dat->y = 0.0; dat->z = 0.0; while (iica0_busy || IICBSY0){ ; } //Make sure bus is ready for xfer i2cbuf[0] = ACCEL_REG_DATA; iica0_busy = 1; ret = R_IICA0_Master_Send(ACCEL_ADDR, i2cbuf, 1, 0); if (ret != MD_OK){ md_err(ret, "accel set reg"); return ret; } while (iica0_busy || IICBSY0){ ; } //Wait until the xfer is complete i2cbuf[0] = 0; iica0_busy = 1; ret = R_IICA0_Master_Receive(ACCEL_ADDR, i2cbuf, 6, 0); while (iica0_busy || IICBSY0){ ; } //Wait until the xfer is complete if (ret != MD_OK){ md_err(ret, "accel read"); return ret; } else { dat->time = millis; dat->xraw = (signed int)((uint16_t)i2cbuf[0] | ((uint16_t)i2cbuf[1] << 8)); dat->yraw = (signed int)((uint16_t)i2cbuf[2] | ((uint16_t)i2cbuf[3] << 8)); dat->zraw = (signed int)((uint16_t)i2cbuf[4] | ((uint16_t)i2cbuf[5] << 8)); dat->x = (float)dat->xraw/ACCEL_SCALE; dat->y = (float)dat->yraw/ACCEL_SCALE; dat->z = (float)dat->zraw/ACCEL_SCALE; } return ret; }
MD_STATUS read_temp(tempData * dat){ MD_STATUS ret = MD_OK; dat->raw = 0; dat->tempC = 0.0; dat->tempF = 0.0; while (iica0_busy || IICBSY0){ ; } //Make sure bus is ready for xfer i2cbuf[0] = TEMP_REG_DATA; iica0_busy = 1; ret = R_IICA0_Master_Send(TEMP_ADDR, i2cbuf, 1, 0); if (ret != MD_OK){ md_err(ret, "temp set reg"); return ret; } while (iica0_busy || IICBSY0){ ; } //Wait until the xfer is complete i2cbuf[0] = 0; iica0_busy = 1; ret = R_IICA0_Master_Receive(TEMP_ADDR, i2cbuf, 2, 0); while (iica0_busy || IICBSY0){ ; } //Wait until the xfer is complete if (ret != MD_OK){ md_err(ret, "temp read"); return ret; } else { dat->time = millis; dat->raw = (uint16_t)i2cbuf[1] | ((uint16_t)i2cbuf[0] << 8); if (dat->raw & 0x8000){ //Negative temperature dat->tempC = ((float)(dat->raw-65536))/128.0; } else { //Positive temperature dat->tempC = ((float)dat->raw)/128.0; } dat->tempF = ((9.0/5.0)*dat->tempC)+32; } return ret; }