Exemple #1
0
static void imuTask(IMU *imu)
{
        char stream_type;
        uint16_t gyro_fsr_dps, accel_fsr, update_rate_hz;
        uint16_t q1_offset, q2_offset, q3_offset, q4_offset;
        float yaw_offset_degrees;
        uint16_t flags;
    bool stream_response_received = false;
    double last_valid_packet_time = 0.0;
    int partial_binary_packet_count = 0;
    int stream_response_receive_count = 0;
    int stream_response_timeout_count = 0;
    int timeout_count = 0;
    int discarded_bytes_count = 0;
    int port_reset_count = 0;
    double last_stream_command_sent_timestamp = 0.0;
    int updates_in_last_second = 0;
    double last_second_start_time = 0;

        SerialPort *pport = imu->GetSerialPort();
    try {
                pport->SetReadBufferSize(512);
                pport->SetTimeout(1.0);
                pport->EnableTermination('\n');
                pport->Flush();
                pport->Reset();
    } catch(std::exception ex) {
    }

    int cmd_packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer,
                                                        imu->current_stream_type, imu->update_rate_hz );

    try {
        pport->Reset();
        pport->Write( protocol_buffer, cmd_packet_length );
        pport->Flush();
#ifdef DEBUG_IMU_RX
        port_reset_count++;
        SmartDashboard::PutNumber("nav6_PortResets", (double)port_reset_count);
#endif
        last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
    } catch (std::exception ex) {
    }

    while (!stop) {
        try {

            // Wait, with delays to conserve CPU resources, until
            // bytes have arrived.

            while ( !stop && ( pport->GetBytesReceived() < 1 ) ) {
                delayMillis(1000/update_rate_hz);
            }

            int packets_received = 0;
                uint32_t bytes_read = pport->Read( protocol_buffer, 256 );
            if (bytes_read > 0) {
                byte_count += bytes_read;
                uint32_t i = 0;
                // Scan the buffer looking for valid packets
                while (i < bytes_read) {

                    // Attempt to decode a packet

                    int bytes_remaining = bytes_read - i;

                        if ( protocol_buffer[i] != PACKET_START_CHAR ) {
                                /* Skip over received bytes until a packet start is detected. */
                                i++;
#ifdef DEBUG_IMU_RX
                                discarded_bytes_count++;
                                SmartDashboard::PutNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
#endif
                                continue;
                        } else {
                                if ( ( bytes_remaining > 2 ) &&
                                         ( protocol_buffer[i+1] == '#' ) ) {
                                        /* Binary packet received; next byte is packet length-2 */
                                        uint8_t total_expected_binary_data_bytes = protocol_buffer[i+2];
                                        total_expected_binary_data_bytes += 2;
                                        while ( bytes_remaining < total_expected_binary_data_bytes ) {
                                                /* This binary packet contains an embedded     */
                                                /* end-of-line character.  Continue to receive */
                                                /* more data until entire packet is received.  */
                                                uint32_t additional_received_data_length = pport->Read(additional_received_data, 256);
                                                if ( additional_received_data_length > 0 ) {
                                                                        byte_count += additional_received_data_length;
                                                                        memcpy(protocol_buffer + bytes_read, additional_received_data, additional_received_data_length);
                                                                        bytes_read += additional_received_data_length;
                                                                        bytes_remaining += additional_received_data_length;
                                                } else {
                                                        /* Timeout waiting for remainder of binary packet */
                                                        i++;
                                                        bytes_remaining--;
#ifdef DEBUG_IMU_RX
                                                partial_binary_packet_count++;
                                                SmartDashboard::PutNumber("nav6 Partial Binary Packets", (double)partial_binary_packet_count);
#endif
                                                continue;
                                                }
                                        }
                                }
                        }

                    int packet_length = imu->DecodePacketHandler(&protocol_buffer[i],bytes_remaining);
                    if (packet_length > 0) {
                        packets_received++;
                        update_count++;
                        last_valid_packet_time = Timer::GetFPGATimestamp();
                        updates_in_last_second++;
                        if ((last_valid_packet_time - last_second_start_time ) > 1.0 ) {
#ifdef DEBUG_IMU_RX
                                SmartDashboard::PutNumber("nav6 UpdatesPerSec", (double)updates_in_last_second);
                                updates_in_last_second = 0;
#endif
                                last_second_start_time = last_valid_packet_time;
                        }
                        i += packet_length;
                    }
                    else
                    {
                                        packet_length = IMUProtocol::decodeStreamResponse( &protocol_buffer[i], bytes_remaining, stream_type,
                                                          gyro_fsr_dps, accel_fsr, update_rate_hz,
                                                          yaw_offset_degrees,
                                                          q1_offset, q2_offset, q3_offset, q4_offset,
                                                          flags );
                                        if ( packet_length > 0 )
                                        {
                                                packets_received++;
                                                imu->SetStreamResponse( stream_type,
                                                                  gyro_fsr_dps, accel_fsr, update_rate_hz,
                                                                  yaw_offset_degrees,
                                                                  q1_offset, q2_offset, q3_offset, q4_offset,
                                                                  flags );
                            stream_response_received = true;
                            i += packet_length;
#ifdef DEBUG_IMU_RX
                            stream_response_receive_count++;
                                SmartDashboard::PutNumber("nav6 Stream Responses", (double)stream_response_receive_count);
#endif
                        }
                        else {
                            // current index is not the start of a valid packet; increment
                            i++;
#ifdef DEBUG_IMU_RX
                                discarded_bytes_count++;
                                        SmartDashboard::PutNumber("nav6 Discarded Bytes", (double)discarded_bytes_count);
#endif
                        }
                    }
                }

                if ( ( packets_received == 0 ) && ( bytes_read == 256 ) ) {
                    // Workaround for issue found in SerialPort implementation:
                    // No packets received and 256 bytes received; this
                    // condition occurs in the SerialPort.  In this case,
                    // reset the serial port.
                    pport->Reset();
#ifdef DEBUG_IMU_RX
                    port_reset_count++;
                        SmartDashboard::PutNumber("nav6_PortResets", (double)port_reset_count);
#endif
                }

                // If a stream configuration response has not been received within three seconds
                // of operation, (re)send a stream configuration request

                if ( !stream_response_received && ((Timer::GetFPGATimestamp() - last_stream_command_sent_timestamp ) > 3.0 ) ) {
                    int cmd_packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer, imu->current_stream_type, imu->update_rate_hz );
                    try {
                        last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
                        pport->Write( protocol_buffer, cmd_packet_length );
                        pport->Flush();
                    } catch (std::exception ex2) {
#ifdef DEBUG_IMU_RX
                        stream_response_timeout_count++;
                        SmartDashboard::PutNumber("nav6 Stream Response Timeouts", (double)stream_response_timeout_count);
#endif
                    }
                }
                else {
                    // If no bytes remain in the buffer, and not awaiting a response, sleep a bit
                    if ( stream_response_received && ( pport->GetBytesReceived() == 0 ) ) {
                        delayMillis(1000/update_rate_hz);
                    }
                }

                /* If receiving data, but no valid packets have been received in the last second */
                /* the navX MXP may have been reset, but no exception has been detected.         */
                /* In this case , trigger transmission of a new stream_command, to ensure the    */
                /* streaming packet type is configured correctly.                                */

                if ( ( Timer::GetFPGATimestamp() - last_valid_packet_time ) > 1.0 ) {
                        stream_response_received = false;
                }
            }
        } catch (std::exception ex) {
            // This exception typically indicates a Timeout
            stream_response_received = false;
#ifdef DEBUG_IMU_RX
                timeout_count++;
                SmartDashboard::PutNumber("nav6 Serial Port Timeouts", (double)timeout_count);
            SmartDashboard::PutString("LastNavException", ex.what());
#endif
        }
    }
}
Exemple #2
0
static void imuTask(IMU *imu) 
{
    bool stream_response_received = false;
    double last_stream_command_sent_timestamp = 0.0;
	stop = false;
	SerialPort *pport = imu->GetSerialPort();
	pport->SetReadBufferSize(512);
	pport->SetTimeout(1.0);
	pport->EnableTermination('\n');
	pport->Flush();
	pport->Reset();

	char stream_type;
	uint16_t gyro_fsr_dps, accel_fsr, update_rate_hz;
	uint16_t q1_offset, q2_offset, q3_offset, q4_offset;
	float yaw_offset_degrees;
	uint16_t flags;
	
    // Give the nav6 circuit a few seconds to initialize, then send the stream configuration command.
    int cmd_packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer, imu->current_stream_type, imu->update_rate_hz ); 
    pport->Reset();
    pport->Write( protocol_buffer, cmd_packet_length );
    pport->Flush();
    last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
		
	while (!stop)
	{ 
		uint32_t bytes_read = pport->Read( protocol_buffer, 256 );
		if ( bytes_read > 0 )
		{
			int packets_received = 0;
			byte_count += bytes_read;
			uint32_t i = 0;
			// Scan the buffer looking for valid packets
			while ( i < bytes_read )
			{
				int bytes_remaining = bytes_read - i;
				int packet_length = imu->DecodePacketHandler( &protocol_buffer[i], bytes_remaining ); 
				if ( packet_length > 0 )
				{
					packets_received++;
					update_count++;
					i += packet_length;
				}
				else 
				{
					packet_length = IMUProtocol::decodeStreamResponse( &protocol_buffer[i], bytes_remaining, stream_type,
							  gyro_fsr_dps, accel_fsr, update_rate_hz,
							  yaw_offset_degrees, 
							  q1_offset, q2_offset, q3_offset, q4_offset,
							  flags );
					if ( packet_length > 0 ) 
					{
						packets_received++;
						imu->SetStreamResponse( stream_type, 
								  gyro_fsr_dps, accel_fsr, update_rate_hz,
								  yaw_offset_degrees, 
								  q1_offset, q2_offset, q3_offset, q4_offset,
								  flags );
						stream_response_received = true;
						i += packet_length;
					}
					else // current index is not the start of a valid packet we're interested in; increment
					{
						i++;
					}
				}
			}
			if ( ( packets_received == 0 ) && ( bytes_read == 256 ) ) {
				// No packets received and 256 bytes received; this
				// condition occurs in the SerialPort.  In this case,
				// reset the serial port.
				pport->Reset();
			}
			
			if ( !stream_response_received && ((Timer::GetFPGATimestamp() - last_stream_command_sent_timestamp ) > 3.0 ) ) {
				cmd_packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer, imu->current_stream_type, imu->update_rate_hz ); 
				last_stream_command_sent_timestamp = Timer::GetFPGATimestamp();
				pport->Write( protocol_buffer, cmd_packet_length );
				pport->Flush();
			}	            				
		}
		else {
			// This exception typically indicates a Timeout
			stream_response_received = false;
		}
	}
}