static inline bool_t acquire_baro_calibration(void) { // start baro calibration acquisition uint8_t cmd=0x17; // send cmd 23 navdata_write(&cmd, 1); uint8_t calibBuffer[22]; if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0) { perror("acquire_baro_calibration: read failed"); return FALSE; } baro_calibration.ac1 = calibBuffer[0] << 8 | calibBuffer[1]; baro_calibration.ac2 = calibBuffer[2] << 8 | calibBuffer[3]; baro_calibration.ac3 = calibBuffer[4] << 8 | calibBuffer[5]; baro_calibration.ac4 = calibBuffer[6] << 8 | calibBuffer[7]; baro_calibration.ac5 = calibBuffer[8] << 8 | calibBuffer[9]; baro_calibration.ac6 = calibBuffer[10] << 8 | calibBuffer[11]; baro_calibration.b1 = calibBuffer[12] << 8 | calibBuffer[13]; baro_calibration.b2 = calibBuffer[14] << 8 | calibBuffer[15]; baro_calibration.mb = calibBuffer[16] << 8 | calibBuffer[17]; baro_calibration.mc = calibBuffer[18] << 8 | calibBuffer[19]; baro_calibration.md = calibBuffer[20] << 8 | calibBuffer[21]; printf("Calibration AC1: %d\n", baro_calibration.ac1); printf("Calibration AC2: %d\n", baro_calibration.ac2); printf("Calibration AC3: %d\n", baro_calibration.ac3); printf("Calibration AC4: %d\n", baro_calibration.ac4); printf("Calibration AC5: %d\n", baro_calibration.ac5); printf("Calibration AC6: %d\n", baro_calibration.ac6); printf("Calibration B1: %d\n", baro_calibration.b1); printf("Calibration B2: %d\n", baro_calibration.b2); printf("Calibration MB: %d\n", baro_calibration.mb); printf("Calibration MC: %d\n", baro_calibration.mc); printf("Calibration MD: %d\n", baro_calibration.md); baro_calibrated = TRUE; return TRUE; }
bool_t navdata_init() { if (nav_fd <= 0) { nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK); if (nav_fd == -1) { perror("navdata_init: Unable to open /dev/ttyO1 - "); return FALSE; } } fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking //set port options struct termios options; //Get the current options for the port tcgetattr(nav_fd, &options); //Set the baud rates to 460800 cfsetispeed(&options, B460800); cfsetospeed(&options, B460800); options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode options.c_iflag = 0; //clear input options options.c_lflag = 0; //clear local options options.c_oflag &= ~OPOST; //clear output options (raw output) //Set the new options for the port tcsetattr(nav_fd, TCSANOW, &options); // stop acquisition uint8_t cmd=0x02; navdata_write(&cmd, 1); // read some potential dirt (retry alot of times) char tmp[100]; for(int i = 0; i < 100; i++) { uint16_t dirt = read(nav_fd, tmp, sizeof tmp); (void) dirt; cmd=0x02; navdata_write(&cmd, 1); usleep(200); } baro_calibrated = FALSE; if(!acquire_baro_calibration()) return FALSE; // start acquisition cmd = 0x01; navdata_write(&cmd, 1); navdata_imu_available = FALSE; navdata_baro_available = FALSE; previousUltrasoundHeight = 0; nav_port.checksum_errors = 0; nav_port.bytesRead = 0; nav_port.totalBytesRead = 0; nav_port.packetsRead = 0; nav_port.isInitialized = TRUE; return TRUE; }
static void navdata_open_server (void) { int32_t one = 1; navdata_write ((int8_t*)&one, sizeof( one )); }