Convert gyro_reading over to event loops.
LPD8806 wasn't going to fit in well, but it's never been used on a
robot. So take the easy way out and remove it.
Change-Id: Ib4d29b40d63a8663878ff1b074057be8b2df9259
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index 2ef83ce..c6e72a4 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -119,23 +119,6 @@
)
cc_library(
- name = "lpd8806",
- srcs = [
- "LPD8806.cc",
- ],
- hdrs = [
- "LPD8806.h",
- ],
- restricted_to = ["//tools:roborio"],
- deps = [
- "//aos/mutex",
- "//aos/time",
- "//frc971/queues:gyro",
- "//third_party:wpilib",
- ],
-)
-
-cc_library(
name = "loop_output_handler",
srcs = [
"loop_output_handler.cc",
diff --git a/frc971/wpilib/LPD8806.cc b/frc971/wpilib/LPD8806.cc
deleted file mode 100644
index 6db54d5..0000000
--- a/frc971/wpilib/LPD8806.cc
+++ /dev/null
@@ -1,87 +0,0 @@
-#include "frc971/wpilib/LPD8806.h"
-
-#include "frc971/queues/gyro.q.h"
-
-#include "frc971/wpilib/ahal/SPI.h"
-#undef ERROR
-
-namespace frc971 {
-namespace wpilib {
-
-LPD8806::LPD8806(int chips)
- : chips_(chips),
- data_(new LED[chips * 2]),
- spi_(new frc::SPI(frc::SPI::kOnboardCS1)) {
- memset(data_.get(), 0, sizeof(LED[chips_ * 2]));
-
- // 2 MHz is the maximum frequency the datasheet recommends.
- spi_->SetClockRate(2e6);
- spi_->SetChipSelectActiveHigh();
-
- // Clock is inverted due to the level translator chip.
- spi_->SetClockActiveLow();
- spi_->SetSampleDataOnRising();
- spi_->SetMSBFirst();
-}
-
-void LPD8806::SetColor(int led, uint32_t hex_color) {
- CHECK_LT(led, chips_ * 2);
- ::aos::MutexLocker locker(&data_mutex_);
- data_[led].red = TranslateColor(hex_color, RED);
- data_[led].green = TranslateColor(hex_color, GREEN);
- data_[led].blue = TranslateColor(hex_color, BLUE);
-}
-
-uint8_t LPD8806::TranslateColor(uint32_t hex_color, Type type) {
- switch (type) {
- case RED:
- return ((hex_color >> 16) & 0xFF) | 0x01;
- case GREEN:
- return ((hex_color >> 8) & 0xFF) | 0x01;
- case BLUE:
- return (hex_color & 0xFF) | 0x01;
- }
-
- LOG(FATAL, "Not sure what color type %d is\n", static_cast<int>(type));
-}
-
-void LPD8806::operator()() {
- // The buffer we're going to send.
- // With 64 leds, it takes about 1ms to send them all, which fits within the
- // 5ms cycle used by the gyro_reader.
- LED buffer[64];
-
- while (run_) {
- if (next_led_to_send_ < chips_ * 2) {
- ::aos::MutexLocker locker(&data_mutex_);
- memcpy(buffer, &data_[next_led_to_send_], sizeof(buffer));
- next_led_to_send_ += 64;
- } else {
- CHECK_EQ(chips_ * 2, next_led_to_send_);
- next_led_to_send_ = 0;
- memset(buffer, 0, sizeof(buffer));
- }
-
- // Wait until right after the gyro gets a reading.
- ::frc971::sensors::gyro_reading.FetchNextBlocking();
-
- int spi_send_result = (spi_->Write(
- static_cast<uint8_t *>(static_cast<void *>(buffer)), sizeof(buffer)));
-
- switch (spi_send_result) {
- case -1:
- LOG(INFO, "SPI::Write failed\n");
- // Who knows what state it's in now, so start fresh next cycle.
- next_led_to_send_ = chips_ * 2;
- break;
- case sizeof(buffer):
- break;
- default:
- LOG(FATAL, "SPI::Write returned something weird: %d\n",
- static_cast<int>(spi_send_result));
- }
- }
-}
-
-} // namespace wpilib
-} // namespace frc971
diff --git a/frc971/wpilib/LPD8806.h b/frc971/wpilib/LPD8806.h
deleted file mode 100644
index 697f180..0000000
--- a/frc971/wpilib/LPD8806.h
+++ /dev/null
@@ -1,70 +0,0 @@
-#ifndef FRC971_WPILIB_LPD8806_H_
-#define FRC971_WPILIB_LPD8806_H_
-
-#include <atomic>
-#include <memory>
-
-#include "aos/mutex/mutex.h"
-
-#include "frc971/wpilib/ahal/SPI.h"
-#undef ERROR
-
-namespace frc971 {
-namespace wpilib {
-
-// Handles sending out colors to a string of LPD8806 LED drivers with a chip to
-// gate the clock+data lines attached to SPI CS1.
-//
-// This is currently implemented by sending data for an entire 64 LED strip
-// right after receiving a gyro message to avoid interfering with the SPI
-// transactions for the gyro.
-//
-// This is designed to be passed into ::std::thread's constructor so it will run
-// as a separate thread.
-class LPD8806 {
- public:
- // chips is the number of actual chips in the string. There will be twice this
- // many LEDs.
- LPD8806(int chips);
-
- // For ::std::thread to call.
- //
- // Loops until Quit() is called writing values out.
- void operator()();
-
- void Quit() { run_ = false; }
-
- int chips() const { return chips_; }
-
- // Sets the color to be sent out next refresh cycle for a given LED.
- // red, green, and blue are 0-1 (inclusive).
- void SetColor(int led, uint32_t hex_color);
-
- private:
- struct LED {
- uint8_t green;
- uint8_t red;
- uint8_t blue;
- };
-
- enum Type { GREEN, RED, BLUE };
-
- static_assert(sizeof(LED[4]) == 12, "LED needs to pack into arrays nicely");
-
- uint8_t TranslateColor(uint32_t hex_color, Type type);
-
- const int chips_;
- int next_led_to_send_ = 0;
-
- ::std::unique_ptr<LED[]> data_;
- ::aos::Mutex data_mutex_;
-
- ::std::unique_ptr<frc::SPI> spi_;
-
- ::std::atomic<bool> run_{true};
-};
-
-} // namespace wpilib
-} // namespace frc971
-
-#endif // FRC971_WPILIB_LPD8806_H_
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index 925588e..894e187 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -29,7 +29,10 @@
joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
".aos.joystick_state")),
uid_sender_(event_loop_->MakeSender<::frc971::sensors::Uid>(
- ".frc971.sensors.gyro_part_id")) {
+ ".frc971.sensors.gyro_part_id")),
+ gyro_reading_sender_(
+ event_loop_->MakeSender<::frc971::sensors::GyroReading>(
+ ".frc971.sensors.gyro_reading")) {
PCHECK(
system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
@@ -119,7 +122,7 @@
const double angle_rate = gyro_.ExtractAngle(result);
const double new_angle = angle_rate / static_cast<double>(kReadingRate);
- auto message = ::frc971::sensors::gyro_reading.MakeMessage();
+ auto message = gyro_reading_sender_.MakeMessage();
if (zeroed) {
angle += (new_angle + zero_offset) * number_readings;
message->angle = angle;
diff --git a/frc971/wpilib/gyro_sender.h b/frc971/wpilib/gyro_sender.h
index 61b7704..338ad32 100644
--- a/frc971/wpilib/gyro_sender.h
+++ b/frc971/wpilib/gyro_sender.h
@@ -32,6 +32,7 @@
::aos::EventLoop *event_loop_;
::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
::aos::Sender<::frc971::sensors::Uid> uid_sender_;
+ ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
// Readings per second.
static const int kReadingRate = 200;