redid the sensor reading code for the bbb+cape
diff --git a/bbb_cape/src/bbb/bbb.gyp b/bbb_cape/src/bbb/bbb.gyp
index c954482..1722bce 100644
--- a/bbb_cape/src/bbb/bbb.gyp
+++ b/bbb_cape/src/bbb/bbb.gyp
@@ -3,6 +3,11 @@
'include_dirs': [
'..',
],
+ 'direct_dependent_settings': {
+ 'include_dirs': [
+ '..',
+ ],
+ },
},
'targets': [
{
@@ -70,6 +75,17 @@
],
},
{
+ 'target_name': 'sensor_generation',
+ 'type': 'static_library',
+ 'sources': [
+ 'sensor_generation.q',
+ ],
+ 'variables': {
+ 'header_path': 'bbb',
+ },
+ 'includes': ['../../../aos/build/queues.gypi'],
+ },
+ {
'target_name': 'packet_finder',
'type': 'static_library',
'sources': [
@@ -126,5 +142,25 @@
'<(AOS)/build/aos.gyp:logging',
],
},
+ {
+ 'target_name': 'sensor_reader',
+ 'type': 'static_library',
+ 'sources': [
+ 'sensor_reader.cc',
+ ],
+ 'dependencies': [
+ 'uart_reader',
+ 'packet_finder',
+ 'data_struct',
+ '<(AOS)/common/common.gyp:time',
+ 'sensor_generation',
+ ],
+ 'export_dependent_settings': [
+ 'uart_reader',
+ 'packet_finder',
+ 'data_struct',
+ '<(AOS)/common/common.gyp:time',
+ ],
+ },
],
}
diff --git a/bbb_cape/src/bbb/byte_reader.h b/bbb_cape/src/bbb/byte_reader.h
index 7818407..16e364c 100644
--- a/bbb_cape/src/bbb/byte_reader.h
+++ b/bbb_cape/src/bbb/byte_reader.h
@@ -17,7 +17,7 @@
// Returns the number of bytes read, -1 if there is an error in errno, or -2
// if reading takes longer than timeout.
virtual ssize_t ReadBytes(AlignedChar *dest, size_t max_bytes,
- const ::aos::time::Time &timeout) = 0;
+ const ::aos::time::Time &timeout_time) = 0;
};
} // namespace bbb
diff --git a/bbb_cape/src/bbb/packet_finder.cc b/bbb_cape/src/bbb/packet_finder.cc
index 447d3a8..713ea69 100644
--- a/bbb_cape/src/bbb/packet_finder.cc
+++ b/bbb_cape/src/bbb/packet_finder.cc
@@ -32,9 +32,8 @@
int zeros_found = 0;
while (true) {
size_t already_read = ::std::max(0, packet_bytes_);
- ssize_t new_bytes =
- reader_->ReadBytes(buf_ + already_read, packet_size_ - already_read,
- timeout_time - ::Time::Now());
+ ssize_t new_bytes = reader_->ReadBytes(
+ buf_ + already_read, packet_size_ - already_read, timeout_time);
if (new_bytes < 0) {
if (new_bytes == -1) {
LOG(ERROR, "ReadBytes(%p, %zd) failed with %d: %s\n",
@@ -50,8 +49,8 @@
if (!irq_priority_increased_) {
// TODO(brians): Do this cleanly.
- int chrt_result = system(
- "bash -c 'chrt -r -p 55 $(top -n1 | fgrep irq/89 | cut -d\" \" -f2)'");
+ int chrt_result = system("bash -c 'chrt -r -p 55"
+ " $(top -n1 | fgrep irq/89 | cut -d\" \" -f2)'");
if (chrt_result == -1) {
LOG(FATAL, "system(chrt -r -p 55 the_irq) failed\n");
} else if (!WIFEXITED(chrt_result) || WEXITSTATUS(chrt_result) != 0) {
diff --git a/bbb_cape/src/bbb/packet_finder.h b/bbb_cape/src/bbb/packet_finder.h
index b86c0c9..9b74583 100644
--- a/bbb_cape/src/bbb/packet_finder.h
+++ b/bbb_cape/src/bbb/packet_finder.h
@@ -6,6 +6,7 @@
#include "aos/common/logging/logging.h"
#include "aos/common/time.h"
+#include "aos/common/macros.h"
#include "bbb/byte_reader.h"
@@ -61,6 +62,8 @@
// Whether we've increased the priority of the IRQ yet.
bool irq_priority_increased_ = false;
+
+ DISALLOW_COPY_AND_ASSIGN(PacketFinder);
};
} // namespace bbb
diff --git a/bbb_cape/src/bbb/sensor_generation.q b/bbb_cape/src/bbb/sensor_generation.q
new file mode 100644
index 0000000..a121999
--- /dev/null
+++ b/bbb_cape/src/bbb/sensor_generation.q
@@ -0,0 +1,14 @@
+package bbb;
+
+// Each different set of values here represents a different set of sensor
+// values (ie different encoder zeros etc).
+message SensorGeneration {
+ // The PID of the process reading sensor values.
+ int32_t reader_pid;
+ // A count of how many times a sensor reading process sees the cape reset.
+ uint32_t cape_resets;
+};
+
+// A new message is placed on here by the process that reads sensor values each
+// time it starts up or detects the cape resetting.
+queue SensorGeneration sensor_generation;
diff --git a/bbb_cape/src/bbb/sensor_reader.cc b/bbb_cape/src/bbb/sensor_reader.cc
new file mode 100644
index 0000000..2c36ae3
--- /dev/null
+++ b/bbb_cape/src/bbb/sensor_reader.cc
@@ -0,0 +1,61 @@
+#include "bbb/sensor_reader.h"
+
+#include <sys/types.h>
+#include <unistd.h>
+#include <inttypes.h>
+
+#include "bbb/sensor_generation.q.h"
+
+namespace bbb {
+
+SensorReader::SensorReader(const ::std::string &/*cape_code*/)
+ : reader_(750000), packet_finder_(&reader_, DATA_STRUCT_SEND_SIZE - 4) {
+ static_assert(sizeof(SensorGeneration::reader_pid) >= sizeof(pid_t),
+ "pid_t is really big");
+ ResetHappened();
+}
+
+const DataStruct *SensorReader::ReadPacket() {
+ static constexpr ::aos::time::Time kTimeout =
+ ::aos::time::Time::InSeconds(0.5);
+
+ while (true) {
+ ::aos::time::Time next_timeout = last_received_time_ + kTimeout;
+ if (next_timeout <= ::aos::time::Time::Now()) {
+ LOG(WARNING, "too long since good packet received\n");
+ Reset(false);
+ }
+ if (packet_finder_.ReadPacket(next_timeout)) {
+ last_received_time_ = ::aos::time::Time::Now();
+ const DataStruct *data = packet_finder_.get_packet<DataStruct>();
+ // TODO(brians): Check the flash checksum and reflash it if necessary.
+ if (data->timestamp < last_cape_timestamp_) {
+ LOG(WARNING, "cape timestamp decreased: %" PRIu64 " to %" PRIu64 "\n",
+ last_cape_timestamp_, data->timestamp);
+ ResetHappened();
+ }
+ last_cape_timestamp_ = data->timestamp;
+ return data;
+ }
+ LOG(WARNING, "receiving packet failed\n");
+ }
+}
+
+::aos::time::Time SensorReader::GetCapeTimestamp() {
+ return ::aos::time::Time::InUS(last_cape_timestamp_ * 10);
+}
+
+void SensorReader::Reset(bool reflash) {
+ LOG(INFO, "Reset(%s)\n", reflash ? "true" : "false");
+ // TODO(brians): Actually reset it and maybe reflash it.
+ ResetHappened();
+}
+
+void SensorReader::ResetHappened() {
+ sensor_generation.MakeWithBuilder().reader_pid(getpid())
+ .cape_resets(cape_resets_++).Send();
+ last_received_time_ = ::aos::time::Time::Now();
+ last_cape_timestamp_ = 0;
+}
+
+} // namespace bbb
diff --git a/bbb_cape/src/bbb/sensor_reader.h b/bbb_cape/src/bbb/sensor_reader.h
new file mode 100644
index 0000000..16b2496
--- /dev/null
+++ b/bbb_cape/src/bbb/sensor_reader.h
@@ -0,0 +1,50 @@
+#ifndef BBB_CAPE_SRC_BBB_SENSOR_READER_H_
+#define BBB_CAPE_SRC_BBB_SENSOR_READER_H_
+
+#include <string>
+
+#include "aos/common/time.h"
+
+#include "bbb/uart_reader.h"
+#include "bbb/packet_finder.h"
+#include "bbb/data_struct.h"
+
+namespace bbb {
+
+// A class that handles reading sensor values from the BBB cape.
+// Automatically handles the gyro data and exposes the other data for other code
+// to deal with.
+class SensorReader {
+ public:
+ // The relative priority that tasks doing this should get run at (ie what to
+ // pass to ::aos::Init(int)).
+ static const int kRelativePriority = 5;
+
+ // cape_code is the name of the code that should be deployed to the cape if
+ // it's not already there.
+ SensorReader(const ::std::string &cape_code);
+
+ // Reads in 1 data packet, handles the gyro data in it, and returns a pointer
+ // to it.
+ const DataStruct *ReadPacket();
+
+ // Returns the timestamp the cape sent with the most recently read packet.
+ ::aos::time::Time GetCapeTimestamp();
+
+ private:
+ // Resets the cape.
+ void Reset(bool reflash);
+ // Called after a reset happens.
+ void ResetHappened();
+
+ UartReader reader_;
+ PacketFinder packet_finder_;
+
+ int cape_resets_ = 0;
+ ::aos::time::Time last_received_time_ = ::aos::time::Time::InSeconds(0);
+ uint64_t last_cape_timestamp_;
+};
+
+} // namespace bbb
+
+#endif // BBB_CAPE_SRC_BBB_SENSOR_READER_H_
diff --git a/bbb_cape/src/bbb/uart_reader.cc b/bbb_cape/src/bbb/uart_reader.cc
index f4ebaca..cebe127 100644
--- a/bbb_cape/src/bbb/uart_reader.cc
+++ b/bbb_cape/src/bbb/uart_reader.cc
@@ -54,8 +54,10 @@
}
ssize_t UartReader::ReadBytes(AlignedChar *dest, size_t max_bytes,
- const ::aos::time::Time &timeout) {
+ const ::aos::time::Time &timeout_time) {
do {
+ ::aos::time::Time timeout = timeout_time - ::aos::time::Time::Now();
+ if (timeout < ::aos::time::Time(0, 0)) return -2;
struct timeval timeout_timeval = timeout.ToTimeval();
switch (select(fd_ + 1, &fd_set_, NULL, NULL, &timeout_timeval)) {
case 0:
diff --git a/bbb_cape/src/bbb/uart_reader.h b/bbb_cape/src/bbb/uart_reader.h
index 2d038ea..c441167 100644
--- a/bbb_cape/src/bbb/uart_reader.h
+++ b/bbb_cape/src/bbb/uart_reader.h
@@ -6,6 +6,7 @@
#include <sys/select.h>
#include "aos/common/time.h"
+#include "aos/common/macros.h"
#include "bbb/byte_reader.h"
@@ -17,12 +18,14 @@
virtual ~UartReader();
virtual ssize_t ReadBytes(AlignedChar *dest, size_t max_bytes,
- const ::aos::time::Time &timeout) override;
+ const ::aos::time::Time &timeout_time) override;
private:
const int fd_;
// Gets initialized to only contain fd_.
fd_set fd_set_;
+
+ DISALLOW_COPY_AND_ASSIGN(UartReader);
};
} // namespace bbb
diff --git a/bbb_cape/src/cape/data_struct.h b/bbb_cape/src/cape/data_struct.h
index 31a909f..9e5ee6c 100644
--- a/bbb_cape/src/cape/data_struct.h
+++ b/bbb_cape/src/cape/data_struct.h
@@ -9,7 +9,8 @@
#pragma pack(push, 1)
// Be careful with declaration order in here. ARM doesn't like unaligned
-// accesses!
+// accesses and this structure is packed, so messing the order up will cause the
+// compiler to generate very inefficient code to access fields.
struct DATA_STRUCT_NAME {
int64_t gyro_angle;
@@ -56,6 +57,48 @@
int32_t posedge_value, negedge_value;
uint8_t posedge_count, negedge_count;
} test;
+
+ // This is for the comp and practice robots.
+ struct {
+ int32_t left_drive;
+ int32_t right_drive;
+ int32_t shooter_angle;
+ int32_t shooter;
+ int32_t indexer;
+ int32_t wrist;
+
+ int32_t capture_top_rise;
+ int32_t capture_top_fall;
+ int32_t capture_bottom_fall_delay;
+ int32_t capture_wrist_rise;
+ int32_t capture_shooter_angle_rise;
+
+ uint16_t battery_voltage;
+ uint16_t left_drive_hall;
+ uint16_t right_drive_hall;
+
+ int8_t top_rise_count;
+
+ int8_t top_fall_count;
+
+ int8_t bottom_rise_count;
+
+ int8_t bottom_fall_delay_count;
+ int8_t bottom_fall_count;
+
+ int8_t wrist_rise_count;
+
+ int8_t shooter_angle_rise_count;
+
+ struct {
+ uint8_t wrist_hall_effect : 1;
+ uint8_t angle_adjust_bottom_hall_effect : 1;
+ uint8_t top_disc : 1;
+ uint8_t bottom_disc : 1;
+ uint8_t loader_top : 1;
+ uint8_t loader_bottom : 1;
+ };
+ } main;
};
} __attribute__((aligned(8)));
#pragma pack(pop)