void IMU::Restart() {
stop = true;
pserial_port->Reset();
m_task->Stop();
pserial_port->Reset();
InitializeYawHistory();
update_count = 0;
byte_count = 0;
m_task->Restart();
}
/**
* Initialize the IMU.
*/
void IMU::InitIMU()
{
// The IMU serial port configuration is 8 data bits, no parity, one stop bit.
// No flow control is used.
// Conveniently, these are the defaults used by the WPILib's SerialPort class.
//
// In addition, the WPILib's SerialPort class also defaults to:
//
// Timeout period of 5 seconds
// Termination ('\n' character)
// Transmit immediately
InitializeYawHistory();
yaw_offset = 0;
// set the nav6 into "YPR" update mode
int packet_length = IMUProtocol::encodeStreamCommand( protocol_buffer, current_stream_type, update_rate_hz );
pserial_port->Write( protocol_buffer, packet_length );
}