Пример #1
0
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  */

}
Пример #2
0
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;
}
Пример #3
0
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;
}