frc971/input/usb_receiver.cc - RealtimeRoboticsGroup/test - Gitiles

 #include <string.h>
 #include <errno.h>
 #include <inttypes.h>

 #include "frc971/input/usb_receiver.h"

 #include "aos/common/logging/logging.h"
 #include "aos/common/control_loop/ControlLoop.h"

 namespace frc971 {

 USBReceiver::USBReceiver(uint8_t expected_robot_id)
     : expected_robot_id_(expected_robot_id) {
   Reset();
 }

 void USBReceiver::RunIteration() {
   if (ReceiveData()) {
     Reset();
   } else {
     static const int kCountsPerSecond = 100000;
     const ::aos::time::Time timestamp =
         ::aos::time::Time(data()->timestamp / kCountsPerSecond,
                           (data()->timestamp * ::aos::time::Time::kNSecInSec /
                            kCountsPerSecond) %
                           ::aos::time::Time::kNSecInSec);

     if (data()->robot_id != expected_robot_id_) {
       LOG(ERROR, "gyro board sent data for robot id %hhd instead of %hhd!"
           " dip switches are %hhx\n",
           data()->robot_id, expected_robot_id_, data()->dip_switches);
       return;
     }
     if (data()->checksum != GYRO_BOARD_DATA_CHECKSUM) {
       LOG(ERROR,
           "gyro board sent checksum %" PRIu16 " instead of %" PRIu16 "!\n",
           data()->checksum, GYRO_BOARD_DATA_CHECKSUM);
       return;
     }

     if (phase_locker_.IsCurrentPacketGood(transfer_received_time_, frame_number_)) {
       LOG(DEBUG, "processing dips %hhx frame %" PRId32 " at %f\n",
           data()->dip_switches, data()->frame_number, timestamp.ToSeconds());

       ProcessData(timestamp);
     }
   }
 }

 void USBReceiver::PhaseLocker::Reset() {
   LOG(INFO, "resetting\n");
   last_good_packet_time_ = ::aos::time::Time(0, 0);
   last_good_sequence_ = 0;
   good_phase_ = guess_phase_ = kUnknownPhase;
   guess_phase_good_ = guess_phase_bad_ = 0;
   good_phase_early_ = good_phase_late_ = 0;
 }

 bool USBReceiver::PhaseLocker::IsCurrentPacketGood(
     const ::aos::time::Time &received_time,
     uint32_t sequence) {
   if (last_good_packet_time_ != ::aos::time::Time(0, 0) &&
       received_time - last_good_packet_time_ > kResetTime) {
     LOG(WARNING, "no good packet received in too long\n");
     Reset();
     return false;
   }
   if (last_good_sequence_ != 0 && sequence - last_good_sequence_ > 100) {
     LOG(WARNING, "skipped too many packets\n");
     Reset();
     return false;
   }

   using ::aos::control_loops::kLoopFrequency;
   // How often we (should) receive packets.
   static const ::aos::time::Time kPacketFrequency =
       kLoopFrequency / kPacketsPerLoopCycle;
   static const ::aos::time::Time kPacketClose =
       kPacketFrequency * 65 / 100;
   static const ::aos::time::Time kSwitchOffset =
       kPacketFrequency * 6 / 10;

   // When we want to receive a packet for the next cycle of control loops.
   ::aos::time::Time next_desired =
       ::aos::control_loops::NextLoopTime(received_time) + kDesiredOffset;
   // If we came up with something more than 1 packet in the past.
   if (next_desired - received_time < -kPacketFrequency) {
     next_desired += kLoopFrequency;
   }
   // How far off of when we want the next packet this one is.
   const ::aos::time::Time offset = next_desired - received_time;

   const int received_phase = sequence % kPacketsPerLoopCycle;

   assert(!(good_phase_early_ != 0 && good_phase_late_ != 0));

   if (good_phase_ == kUnknownPhase &&
       guess_phase_good_ > kMinGoodGuessCycles) {
     good_phase_ = guess_phase_;
     if (guess_phase_offset_ < kPacketFrequency / -2) {
       ++good_phase_;
     } else if (guess_phase_offset_ > kPacketFrequency / 2) {
       --good_phase_;
     }
     LOG(INFO, "locked on to phase %d\n", good_phase_);
   } else if (guess_phase_bad_ > kMaxBadGuessCycles) {
     LOG(INFO, "guessed wrong phase too many times\n");
     Reset();
   }
   if (good_phase_early_ > kSwitchCycles) {
     good_phase_early_ = 0;
     LOG(INFO, "switching from phase %d to %d-1\n",
         good_phase_, good_phase_);
     --good_phase_;
   } else if (good_phase_late_ > kSwitchCycles) {
     good_phase_late_ = 0;
     LOG(INFO, "switching from phase %d to %d+1\n",
         good_phase_, good_phase_);
     ++good_phase_;
   }
   if (good_phase_ == kUnknownPhase) {
     LOG(DEBUG, "guessing which packet is good\n");

     // If it's close to the right time.
     if (offset.abs() < kPacketClose) {
       if (guess_phase_ == kUnknownPhase) {
         if (offset.abs() < kPacketFrequency * 55 / 100) {
           guess_phase_ = received_phase;
           guess_phase_offset_ = offset;
         }
       } else if (received_phase == guess_phase_) {
         LOG(DEBUG, "guessed right phase %d\n", received_phase);
         ++guess_phase_good_;
         guess_phase_bad_ = 0;
         guess_phase_offset_ = (guess_phase_offset_ * 9 + offset) / 10;
       }
     } else if (guess_phase_ != kUnknownPhase &&
                received_phase == guess_phase_) {
       LOG(DEBUG, "guessed wrong phase %d\n", received_phase);
       ++guess_phase_bad_;
       guess_phase_good_ = ::std::max(0, guess_phase_good_ -
                                      (kMinGoodGuessCycles / 10));
     }
     return false;
   } else {  // we know what phase we're looking for
     // Deal with it if the above logic for tweaking the phase that we're
     // using wrapped it around.
     if (good_phase_ == -1) {
       good_phase_ = kPacketsPerLoopCycle;
     } else if (good_phase_ == kPacketsPerLoopCycle) {
       LOG(DEBUG, "dewrapping\n");
       good_phase_ = 0;
     }
     assert(good_phase_ >= 0);
     assert(good_phase_ < kPacketsPerLoopCycle);

     if (received_phase == good_phase_) {
       if (offset < -kSwitchOffset) {
         ++good_phase_early_;
         good_phase_late_ = 0;
       } else if (offset > kSwitchOffset) {
         ++good_phase_late_;
         good_phase_early_ = 0;
       } else {
         good_phase_early_ = good_phase_late_ = 0;
       }
       last_good_packet_time_ = received_time;
       last_good_sequence_ = sequence;

       return true;
     } else {
       return false;
     }
   }
 }

 void USBReceiver::StaticTransferCallback(libusb::Transfer *transfer,
                                          void *self) {
   static_cast<USBReceiver *>(self)->TransferCallback(transfer);
 }

 void USBReceiver::TransferCallback(libusb::Transfer *transfer) {
   transfer_received_time_ = ::aos::time::Time::Now();
   if (transfer->status() == LIBUSB_TRANSFER_COMPLETED) {
     LOG(DEBUG, "transfer %p completed\n", transfer);
     completed_transfer_ = transfer;
   } else if (transfer->status() == LIBUSB_TRANSFER_TIMED_OUT) {
     LOG(WARNING, "transfer %p timed out\n", transfer);
     completed_transfer_ = kTransferFailed;
   } else if (transfer->status() == LIBUSB_TRANSFER_CANCELLED) {
     LOG(DEBUG, "transfer %p cancelled\n", transfer);
   } else {
     LOG(FATAL, "transfer %p has status %d\n", transfer, transfer->status());
   }
   transfer->Submit();
 }

 bool USBReceiver::ReceiveData() {
   // Loop and then return once we get a good one.
   while (true) {
     completed_transfer_ = NULL;
     while (completed_transfer_ == NULL) {
       libusb_.HandleEvents();
     }
     if (completed_transfer_ == kTransferFailed) {
       LOG(WARNING, "transfer failed\n");
       return true;
     }

     if (completed_transfer_->read_bytes() <
         static_cast<ssize_t>(sizeof(GyroBoardData))) {
       LOG(ERROR, "read %d bytes instead of at least %zd\n",
           completed_transfer_->read_bytes(), sizeof(GyroBoardData));
       continue;
     }

     memcpy(data(), completed_transfer_->data(),
            sizeof(GyroBoardData));

     if (data()->unknown_frame) {
       LOG(WARNING, "unknown frame number\n");
       return true;
     }
     uint32_t frame_number_before = frame_number_;
     frame_number_ = data()->frame_number;
     if (frame_number_ < 0) {
       LOG(WARNING, "negative frame number %" PRId32 "\n", frame_number_);
       return true;
     }
     if (frame_number_before == 0) {
       LOG(INFO, "frame number starting at %" PRId32 "\n", frame_number_);
     } else if (frame_number_ - frame_number_before != 1) {
       LOG(WARNING, "frame number went from %" PRId32" to %" PRId32 "\n",
           frame_number_before, frame_number_);
     }
     if (frame_number_ < last_frame_number_) {
       LOG(WARNING, "frame number went down\n");
       return true;
     }
     last_frame_number_ = frame_number_;

     return false;
   }
 }

 void USBReceiver::Reset() {
   typedef ::std::unique_ptr<libusb::IsochronousTransfer> TransferType;
   for (TransferType &c : transfers_) {
     c.reset();
   }
   dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
       libusb_.FindDeviceWithVIDPID(kVid, kPid));
   if (!dev_handle_) {
     LOG(ERROR, "couldn't find device. exiting\n");
     exit(1);
   }
   for (TransferType &c : transfers_) {
     c.reset(new libusb::IsochronousTransfer(kDataLength, 1,
                                             StaticTransferCallback, this));
     c->FillIsochronous(dev_handle_.get(), kEndpoint, kReadTimeout);
     c->Submit();
   }

   last_frame_number_ = frame_number_ = 0;
   phase_locker_.Reset();
 }

 constexpr ::aos::time::Time USBReceiver::kReadTimeout;
 constexpr ::aos::time::Time USBReceiver::kDesiredOffset;
 constexpr ::aos::time::Time USBReceiver::kResetTime;

 }  // namespace frc971
	#include <string.h>
	#include <errno.h>
	#include <inttypes.h>

	#include "frc971/input/usb_receiver.h"

	#include "aos/common/logging/logging.h"
	#include "aos/common/control_loop/ControlLoop.h"

	namespace frc971 {

	USBReceiver::USBReceiver(uint8_t expected_robot_id)
	: expected_robot_id_(expected_robot_id) {
	Reset();
	}

	void USBReceiver::RunIteration() {
	if (ReceiveData()) {
	Reset();
	} else {
	static const int kCountsPerSecond = 100000;
	const ::aos::time::Time timestamp =
	::aos::time::Time(data()->timestamp / kCountsPerSecond,
	(data()->timestamp * ::aos::time::Time::kNSecInSec /
	kCountsPerSecond) %
	::aos::time::Time::kNSecInSec);

	if (data()->robot_id != expected_robot_id_) {
	LOG(ERROR, "gyro board sent data for robot id %hhd instead of %hhd!"
	" dip switches are %hhx\n",
	data()->robot_id, expected_robot_id_, data()->dip_switches);
	return;
	}
	if (data()->checksum != GYRO_BOARD_DATA_CHECKSUM) {
	LOG(ERROR,
	"gyro board sent checksum %" PRIu16 " instead of %" PRIu16 "!\n",
	data()->checksum, GYRO_BOARD_DATA_CHECKSUM);
	return;
	}

	if (phase_locker_.IsCurrentPacketGood(transfer_received_time_, frame_number_)) {
	LOG(DEBUG, "processing dips %hhx frame %" PRId32 " at %f\n",
	data()->dip_switches, data()->frame_number, timestamp.ToSeconds());

	ProcessData(timestamp);
	}
	}
	}

	void USBReceiver::PhaseLocker::Reset() {
	LOG(INFO, "resetting\n");
	last_good_packet_time_ = ::aos::time::Time(0, 0);
	last_good_sequence_ = 0;
	good_phase_ = guess_phase_ = kUnknownPhase;
	guess_phase_good_ = guess_phase_bad_ = 0;
	good_phase_early_ = good_phase_late_ = 0;
	}

	bool USBReceiver::PhaseLocker::IsCurrentPacketGood(
	const ::aos::time::Time &received_time,
	uint32_t sequence) {
	if (last_good_packet_time_ != ::aos::time::Time(0, 0) &&
	received_time - last_good_packet_time_ > kResetTime) {
	LOG(WARNING, "no good packet received in too long\n");
	Reset();
	return false;
	}
	if (last_good_sequence_ != 0 && sequence - last_good_sequence_ > 100) {
	LOG(WARNING, "skipped too many packets\n");
	Reset();
	return false;
	}

	using ::aos::control_loops::kLoopFrequency;
	// How often we (should) receive packets.
	static const ::aos::time::Time kPacketFrequency =
	kLoopFrequency / kPacketsPerLoopCycle;
	static const ::aos::time::Time kPacketClose =
	kPacketFrequency * 65 / 100;
	static const ::aos::time::Time kSwitchOffset =
	kPacketFrequency * 6 / 10;

	// When we want to receive a packet for the next cycle of control loops.
	::aos::time::Time next_desired =
	::aos::control_loops::NextLoopTime(received_time) + kDesiredOffset;
	// If we came up with something more than 1 packet in the past.
	if (next_desired - received_time < -kPacketFrequency) {
	next_desired += kLoopFrequency;
	}
	// How far off of when we want the next packet this one is.
	const ::aos::time::Time offset = next_desired - received_time;

	const int received_phase = sequence % kPacketsPerLoopCycle;

	assert(!(good_phase_early_ != 0 && good_phase_late_ != 0));

	if (good_phase_ == kUnknownPhase &&
	guess_phase_good_ > kMinGoodGuessCycles) {
	good_phase_ = guess_phase_;
	if (guess_phase_offset_ < kPacketFrequency / -2) {
	++good_phase_;
	} else if (guess_phase_offset_ > kPacketFrequency / 2) {
	--good_phase_;
	}
	LOG(INFO, "locked on to phase %d\n", good_phase_);
	} else if (guess_phase_bad_ > kMaxBadGuessCycles) {
	LOG(INFO, "guessed wrong phase too many times\n");
	Reset();
	}
	if (good_phase_early_ > kSwitchCycles) {
	good_phase_early_ = 0;
	LOG(INFO, "switching from phase %d to %d-1\n",
	good_phase_, good_phase_);
	--good_phase_;
	} else if (good_phase_late_ > kSwitchCycles) {
	good_phase_late_ = 0;
	LOG(INFO, "switching from phase %d to %d+1\n",
	good_phase_, good_phase_);
	++good_phase_;
	}
	if (good_phase_ == kUnknownPhase) {
	LOG(DEBUG, "guessing which packet is good\n");

	// If it's close to the right time.
	if (offset.abs() < kPacketClose) {
	if (guess_phase_ == kUnknownPhase) {
	if (offset.abs() < kPacketFrequency * 55 / 100) {
	guess_phase_ = received_phase;
	guess_phase_offset_ = offset;
	}
	} else if (received_phase == guess_phase_) {
	LOG(DEBUG, "guessed right phase %d\n", received_phase);
	++guess_phase_good_;
	guess_phase_bad_ = 0;
	guess_phase_offset_ = (guess_phase_offset_ * 9 + offset) / 10;
	}
	} else if (guess_phase_ != kUnknownPhase &&
	received_phase == guess_phase_) {
	LOG(DEBUG, "guessed wrong phase %d\n", received_phase);
	++guess_phase_bad_;
	guess_phase_good_ = ::std::max(0, guess_phase_good_ -
	(kMinGoodGuessCycles / 10));
	}
	return false;
	} else { // we know what phase we're looking for
	// Deal with it if the above logic for tweaking the phase that we're
	// using wrapped it around.
	if (good_phase_ == -1) {
	good_phase_ = kPacketsPerLoopCycle;
	} else if (good_phase_ == kPacketsPerLoopCycle) {
	LOG(DEBUG, "dewrapping\n");
	good_phase_ = 0;
	}
	assert(good_phase_ >= 0);
	assert(good_phase_ < kPacketsPerLoopCycle);

	if (received_phase == good_phase_) {
	if (offset < -kSwitchOffset) {
	++good_phase_early_;
	good_phase_late_ = 0;
	} else if (offset > kSwitchOffset) {
	++good_phase_late_;
	good_phase_early_ = 0;
	} else {
	good_phase_early_ = good_phase_late_ = 0;
	}
	last_good_packet_time_ = received_time;
	last_good_sequence_ = sequence;

	return true;
	} else {
	return false;
	}
	}
	}

	void USBReceiver::StaticTransferCallback(libusb::Transfer *transfer,
	void *self) {
	static_cast<USBReceiver *>(self)->TransferCallback(transfer);
	}

	void USBReceiver::TransferCallback(libusb::Transfer *transfer) {
	transfer_received_time_ = ::aos::time::Time::Now();
	if (transfer->status() == LIBUSB_TRANSFER_COMPLETED) {
	LOG(DEBUG, "transfer %p completed\n", transfer);
	completed_transfer_ = transfer;
	} else if (transfer->status() == LIBUSB_TRANSFER_TIMED_OUT) {
	LOG(WARNING, "transfer %p timed out\n", transfer);
	completed_transfer_ = kTransferFailed;
	} else if (transfer->status() == LIBUSB_TRANSFER_CANCELLED) {
	LOG(DEBUG, "transfer %p cancelled\n", transfer);
	} else {
	LOG(FATAL, "transfer %p has status %d\n", transfer, transfer->status());
	}
	transfer->Submit();
	}

	bool USBReceiver::ReceiveData() {
	// Loop and then return once we get a good one.
	while (true) {
	completed_transfer_ = NULL;
	while (completed_transfer_ == NULL) {
	libusb_.HandleEvents();
	}
	if (completed_transfer_ == kTransferFailed) {
	LOG(WARNING, "transfer failed\n");
	return true;
	}

	if (completed_transfer_->read_bytes() <
	static_cast<ssize_t>(sizeof(GyroBoardData))) {
	LOG(ERROR, "read %d bytes instead of at least %zd\n",
	completed_transfer_->read_bytes(), sizeof(GyroBoardData));
	continue;
	}

	memcpy(data(), completed_transfer_->data(),
	sizeof(GyroBoardData));

	if (data()->unknown_frame) {
	LOG(WARNING, "unknown frame number\n");
	return true;
	}
	uint32_t frame_number_before = frame_number_;
	frame_number_ = data()->frame_number;
	if (frame_number_ < 0) {
	LOG(WARNING, "negative frame number %" PRId32 "\n", frame_number_);
	return true;
	}
	if (frame_number_before == 0) {
	LOG(INFO, "frame number starting at %" PRId32 "\n", frame_number_);
	} else if (frame_number_ - frame_number_before != 1) {
	LOG(WARNING, "frame number went from %" PRId32" to %" PRId32 "\n",
	frame_number_before, frame_number_);
	}
	if (frame_number_ < last_frame_number_) {
	LOG(WARNING, "frame number went down\n");
	return true;
	}
	last_frame_number_ = frame_number_;

	return false;
	}
	}

	void USBReceiver::Reset() {
	typedef ::std::unique_ptr<libusb::IsochronousTransfer> TransferType;
	for (TransferType &c : transfers_) {
	c.reset();
	}
	dev_handle_ = ::std::unique_ptr<LibUSBDeviceHandle>(
	libusb_.FindDeviceWithVIDPID(kVid, kPid));
	if (!dev_handle_) {
	LOG(ERROR, "couldn't find device. exiting\n");
	exit(1);
	}
	for (TransferType &c : transfers_) {
	c.reset(new libusb::IsochronousTransfer(kDataLength, 1,
	StaticTransferCallback, this));
	c->FillIsochronous(dev_handle_.get(), kEndpoint, kReadTimeout);
	c->Submit();
	}

	last_frame_number_ = frame_number_ = 0;
	phase_locker_.Reset();
	}

	constexpr ::aos::time::Time USBReceiver::kReadTimeout;
	constexpr ::aos::time::Time USBReceiver::kDesiredOffset;
	constexpr ::aos::time::Time USBReceiver::kResetTime;

	} // namespace frc971