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 ));
}
