void nps_fdm_run_step(bool launch __attribute__((unused)), double *commands, int commands_nb)
{
// read state
if (get_msg(&crrcsim, crrcsim.data_buffer) <= 0) {
return; // nothing on the socket
}
// send commands
send_servo_cmd(&crrcsim, commands);
//printf("new data %c %c\n", crrcsim.data_buffer[2], crrcsim.data_buffer[33]);
switch (crrcsim.data_buffer[2]) {
case 'S': /* IMU packet without GPS */
decode_imupacket(&fdm, crrcsim.data_buffer);
break;
case 'N': /* IMU packet with GPS */
decode_imupacket(&fdm, crrcsim.data_buffer);
/******************************************
*check GPS data packet
******************************************/
//if(data_buffer[31]==(byte)0x55 && data_buffer[32]==(byte)0x55 && data_buffer[33]=='G')
if (crrcsim.data_buffer[33] == 'G') {
decode_gpspacket(&fdm, &crrcsim.data_buffer[31]);
}
break;
case 'I': /* IMU packet with GPS and AHRS */
decode_imupacket(&fdm, crrcsim.data_buffer);
/******************************************
*check GPS data packet
******************************************/
if (crrcsim.data_buffer[33] == 'G') {
decode_gpspacket(&fdm, &crrcsim.data_buffer[31]);
}
if (crrcsim.data_buffer[66] == 'A') {
decode_ahrspacket(&fdm, &crrcsim.data_buffer[66]);
}
break;
default :
printf("invalid data packet...!\n");
} /* end case */
}
bool mnav_read_nonblock()
{
const double time_out = 0.0025; // 1/4 of 100hz interval
static int nbytes = 0;
static uint8_t input_buffer[FULL_PACKET_SIZE]={0,};
// set mode as nonblocking
sPort2.set_nonblocking();
if ( nbytes < 3 ) {
// Find start of packet: the header (2 bytes) starts with 0x55 0x55
// bail if we find nothing in the input buffer
int headerOK = 0;
while ( (headerOK != 2) && (get_Time() - start_time < time_out) ) {
if ( sPort2.read_port((char *)input_buffer,1) == 1 ) {
if ( input_buffer[0] == 0x55 ) {
headerOK++;
} else {
headerOK = 0;
}
} else {
// exit immediately (not waiting for timeout) if
// nothing in the input buffer.
return false;
}
}
if ( headerOK != 2 ) {
// printf("Timeout (1) looking for header.\n");
return false;
}
headerOK = 0;
while ( (headerOK != 1) && (get_Time() - start_time < time_out) ) {
if ( sPort2.read_port((char *)&input_buffer[2],1) == 1 ) {
headerOK = 1;
}
}
if ( headerOK != 1 ) {
// printf("Timeout (2) looking for header.\n");
return false;
}
nbytes = 3;
} else {
// printf("Restarting packet read\n");
}
// Read packet contents
switch (input_buffer[2]) {
case 'S': // IMU packet without GPS (< 100hz)
case 's': // IMU packet without GPS (100hz)
// printf("no gps data being sent ... :-(\n");
while ( nbytes < SENSOR_PACKET_LENGTH
&& (get_Time() - start_time < time_out) )
{
int result = sPort2.read_port((char *)input_buffer+nbytes,
SENSOR_PACKET_LENGTH-nbytes);
if ( result > 0 ) {
nbytes += result;
}
}
if ( nbytes < SENSOR_PACKET_LENGTH ) {
// printf("Timeout reading IMU packet\n");
return false;
}
// check checksum
if ( checksum(input_buffer,SENSOR_PACKET_LENGTH) ) {
decode_imupacket(&imu_data, input_buffer);
imu_data_valid = true;
} else {
if ( display_on ) {
printf("[imu]:checksum error...!\n");
}
imu_data.status = ChecksumError;
};
break;
case 'N': // IMU packet with GPS (< 100hz)
case 'n': // IMU packet with GPS (100hz)
while ( nbytes < FULL_PACKET_SIZE
&& (get_Time() - start_time < time_out) )
{
int result = sPort2.read_port((char *)input_buffer+nbytes,
FULL_PACKET_SIZE-nbytes);
if ( result > 0 ) {
nbytes += result;
}
}
if ( nbytes < FULL_PACKET_SIZE ) {
if ( display_on ) {
printf("Timeout reading IMU+GPS packet, but will resume next frame...\n");
}
return false;
}
// printf("G P S D A T A A V A I L A B L E\n");
// check checksum
if ( checksum(input_buffer,FULL_PACKET_SIZE) ) {
decode_imupacket(&imu_data, input_buffer);
imu_data_valid = true;
// check GPS data packet
if(input_buffer[33]=='G') {
decode_gpspacket(&gps_data, input_buffer);
gps_data_valid = true;
} else {
printf("[gps]:data error...!\n");
gps_data.status = NotValid;
} // end if(checksum(input_buffer...
} else {
if ( display_on ) {
printf("[imu]:checksum error(gps)...!\n");
}
gps_data.status = ChecksumError;
imu_data.status = ChecksumError;
}
break;
default :
if ( display_on ) {
printf("[imu] invalid data packet ... !\n");
}
} // end case
nbytes = 0;
return true;
}
bool mnav_read()
{
imu_data_valid = false;
gps_data_valid = false;
int headerOK = 0;
int nbytes = 0;
uint8_t input_buffer[FULL_PACKET_SIZE]={0,};
int trouble_count = 1;
// Find start of packet: the header (2 bytes) starts with 0x5555
while ( headerOK != 2 ) {
while ( 1 != sPort2.read_port((char *)input_buffer,1) );
if ( input_buffer[0] == 0x55 ) {
headerOK++;
trouble_count = 1;
} else {
headerOK = 0;
trouble_count++;
}
if ( trouble_count % 10000 == 0 ) {
printf("Having trouble finding a valid packet header.\n");
}
}
headerOK = 0; while ( 1 != sPort2.read_port((char *)&input_buffer[2],1) );
nbytes = 3;
// Read packet contents
switch (input_buffer[2]) {
case 'S': // IMU packet without GPS (< 100hz)
case 's': // IMU packet without GPS (100hz)
// printf("no gps data being sent ... :-(\n");
while ( nbytes < SENSOR_PACKET_LENGTH ) {
nbytes += sPort2.read_port((char *)input_buffer+nbytes,
SENSOR_PACKET_LENGTH-nbytes);
}
// check checksum
if ( checksum(input_buffer,SENSOR_PACKET_LENGTH) ) {
decode_imupacket(&imu_data, input_buffer);
imu_data_valid = true;
} else {
if ( display_on ) {
printf("[imu]:checksum error...!\n");
}
imu_data.status = ChecksumError;
};
break;
case 'N': // IMU packet with GPS (< 100hz)
case 'n': // IMU packet with GPS (100hz)
while ( nbytes < FULL_PACKET_SIZE ) {
nbytes += sPort2.read_port((char *)input_buffer+nbytes,
FULL_PACKET_SIZE-nbytes);
}
// printf("G P S D A T A A V A I L A B L E\n");
// check checksum
if ( checksum(input_buffer,FULL_PACKET_SIZE) ) {
decode_imupacket(&imu_data, input_buffer);
imu_data_valid = true;
// check GPS data packet
if(input_buffer[33]=='G') {
decode_gpspacket(&gps_data, input_buffer);
gps_data_valid = true;
} else {
printf("[gps]:data error...!\n");
gps_data.status = NotValid;
} // end if(checksum(input_buffer...
} else {
if ( display_on ) {
printf("[imu]:checksum error(gps)...!\n");
}
gps_data.status = ChecksumError;
imu_data.status = ChecksumError;
}
break;
default :
if ( display_on ) {
printf("[imu] invalid data packet ... !\n");
}
} // end case
return imu_data_valid;
}