Add working simple receiver test code
It makes all the motors spin back and forth, which is hilarious.
Change-Id: I150f07fbd91f71d725efa13d36f94dd2102b86d1
diff --git a/motors/BUILD b/motors/BUILD
index b11df45..3b6433d 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -1,159 +1,183 @@
-load('//motors:macros.bzl', 'hex_from_elf')
+load("//motors:macros.bzl", "hex_from_elf")
load("//tools:environments.bzl", "mcu_cpus")
cc_binary(
- name = 'medium_salsa.elf',
- srcs = [
- 'medium_salsa.cc',
- ],
- deps = [
- ':util',
- ':motor',
- ':motor_controls',
- '//motors/core',
- '//motors/peripheral:can',
- '//motors/peripheral:adc',
- '//motors/usb:legacy',
- ],
- restricted_to = mcu_cpus,
+ name = "medium_salsa.elf",
+ srcs = [
+ "medium_salsa.cc",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":motor",
+ ":motor_controls",
+ ":util",
+ "//motors/core",
+ "//motors/peripheral:adc",
+ "//motors/peripheral:can",
+ "//motors/usb:legacy",
+ ],
)
cc_library(
- name = 'motor',
- visibility = ['//visibility:public'],
- hdrs = [
- 'motor.h',
- ],
- srcs = [
- 'motor.cc',
- ],
- deps = [
- ':algorithms',
- ':util',
- '//motors/core',
- '//motors/peripheral:configuration',
-
- '//motors/peripheral:adc',
- '//motors/peripheral:can',
- '//motors/usb:cdc',
- ],
- restricted_to = mcu_cpus,
+ name = "motor",
+ srcs = [
+ "motor.cc",
+ ],
+ hdrs = [
+ "motor.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ ":algorithms",
+ ":util",
+ "//motors/core",
+ "//motors/peripheral:adc",
+ "//motors/peripheral:can",
+ "//motors/peripheral:configuration",
+ "//motors/usb:cdc",
+ ],
)
hex_from_elf(
- name = 'medium_salsa',
- restricted_to = mcu_cpus,
+ name = "medium_salsa",
+ restricted_to = mcu_cpus,
)
cc_library(
- name = 'util',
- visibility = ['//visibility:public'],
- hdrs = [
- 'util.h',
- ],
- deps = [
- '//motors/core',
- ],
- restricted_to = mcu_cpus,
+ name = "util",
+ hdrs = [
+ "util.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//motors/core",
+ ],
)
genrule(
- name = 'doc',
- srcs = [
- 'NOTES.md',
- ],
- outs = [
- 'NOTES.html',
- ],
- cmd = ' '.join([
- 'pandoc',
- '-f', 'markdown_github-hard_line_breaks',
- '-t', 'html5',
- '-o', '$@', '$<',
- ]),
+ name = "doc",
+ srcs = [
+ "NOTES.md",
+ ],
+ outs = [
+ "NOTES.html",
+ ],
+ cmd = " ".join([
+ "pandoc",
+ "-f",
+ "markdown_github-hard_line_breaks",
+ "-t",
+ "html5",
+ "-o",
+ "$@",
+ "$<",
+ ]),
)
cc_library(
- name = 'algorithms',
- hdrs = [
- 'algorithms.h',
- ],
- srcs = [
- 'algorithms.cc',
- ],
- compatible_with = mcu_cpus,
+ name = "algorithms",
+ srcs = [
+ "algorithms.cc",
+ ],
+ hdrs = [
+ "algorithms.h",
+ ],
+ compatible_with = mcu_cpus,
)
cc_test(
- name = 'algorithms_test',
- srcs = [
- 'algorithms_test.cc',
- ],
- deps = [
- ':algorithms',
- '//aos/testing:googletest',
- ],
+ name = "algorithms_test",
+ srcs = [
+ "algorithms_test.cc",
+ ],
+ deps = [
+ ":algorithms",
+ "//aos/testing:googletest",
+ ],
)
cc_library(
- name = 'math',
- visibility = ['//visibility:public'],
- hdrs = [
- 'math.h',
- ],
- srcs = [
- 'math.cc',
- ],
- compatible_with = mcu_cpus,
+ name = "math",
+ srcs = [
+ "math.cc",
+ ],
+ hdrs = [
+ "math.h",
+ ],
+ compatible_with = mcu_cpus,
+ visibility = ["//visibility:public"],
)
cc_test(
- name = 'math_test',
- srcs = [
- 'math_test.cc',
- ],
- deps = [
- ':math',
- '//aos/testing:googletest',
- '//third_party/googletest:googlemock',
- ],
+ name = "math_test",
+ srcs = [
+ "math_test.cc",
+ ],
+ deps = [
+ ":math",
+ "//aos/testing:googletest",
+ "//third_party/googletest:googlemock",
+ ],
)
cc_library(
- name = 'motor_controls',
- hdrs = [
- 'motor_controls.h',
- ],
- srcs = [
- 'motor_controls.cc',
- ],
- deps = [
- ':math',
- ':motor',
- '//motors/peripheral:configuration',
- '//third_party/eigen',
- ],
- restricted_to = mcu_cpus,
+ name = "motor_controls",
+ srcs = [
+ "motor_controls.cc",
+ ],
+ hdrs = [
+ "motor_controls.h",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":math",
+ ":motor",
+ "//motors/peripheral:configuration",
+ "//third_party/eigen",
+ ],
)
cc_binary(
- name = 'button_board.elf',
- srcs = [
- 'button_board.cc',
- ],
- deps = [
- ':util',
- '//motors/core',
- '//motors/peripheral:can',
- '//motors/peripheral:adc',
- '//motors/usb',
- '//motors/usb:cdc',
- '//motors/usb:hid',
- ],
- restricted_to = mcu_cpus,
+ name = "button_board.elf",
+ srcs = [
+ "button_board.cc",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":util",
+ "//motors/core",
+ "//motors/peripheral:adc",
+ "//motors/peripheral:can",
+ "//motors/usb",
+ "//motors/usb:cdc",
+ "//motors/usb:hid",
+ ],
)
hex_from_elf(
- name = 'button_board',
- restricted_to = mcu_cpus,
+ name = "button_board",
+ restricted_to = mcu_cpus,
+)
+
+cc_binary(
+ name = "simple_receiver.elf",
+ srcs = [
+ "simple_receiver.cc",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":util",
+ "//motors/core",
+ "//motors/peripheral:adc",
+ "//motors/peripheral:can",
+ "//motors/usb",
+ "//motors/usb:cdc",
+ ],
+)
+
+hex_from_elf(
+ name = "simple_receiver",
+ restricted_to = mcu_cpus,
)
diff --git a/motors/medium_salsa.cc b/motors/medium_salsa.cc
index f2ee181..0b9d89d 100644
--- a/motors/medium_salsa.cc
+++ b/motors/medium_salsa.cc
@@ -243,7 +243,7 @@
while (true) {
unsigned char command_data[8];
int command_length;
- can_receive_command(command_data, &command_length, 0);
+ can_receive(command_data, &command_length, 0);
if (command_length == 4) {
uint32_t result = command_data[0] << 24 | command_data[1] << 16 |
command_data[2] << 8 | command_data[3];
diff --git a/motors/peripheral/can.c b/motors/peripheral/can.c
index cf03259..b46a8b4 100644
--- a/motors/peripheral/can.c
+++ b/motors/peripheral/can.c
@@ -27,9 +27,15 @@
// TODO(Brian): Do something about CAN errors and warnings (enable interrupts?).
-void can_init(uint32_t id0, uint32_t id1) {
- printf("can_init\n");
+static uint32_t prio_id_for_id(uint32_t can_id) {
+ if (can_id & CAN_EFF_FLAG) {
+ return can_id & ~CAN_EFF_FLAG;
+ } else {
+ return can_id << 18;
+ }
+}
+void can_init(uint32_t id0, uint32_t id1) {
SIM_SCGC6 |= SIM_SCGC6_FLEXCAN0;
// Put it into freeze mode and wait for it to actually do that.
@@ -66,14 +72,14 @@
CAN0_RXIMRS[0] = (1 << 31) /* Want to filter out RTRs. */ |
(0 << 30) /* Want to only get standard frames. */ |
(0x1FFC0000) /* Filter on the id. */;
- CAN0_MESSAGES[0].prio_id = id0 << 18;
+ CAN0_MESSAGES[0].prio_id = prio_id_for_id(id0);
CAN0_MESSAGES[0].control_timestamp =
CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_RX_EMPTY);
CAN0_RXIMRS[1] = (1 << 31) /* Want to filter out RTRs. */ |
(0 << 30) /* Want to only get standard frames. */ |
(0x1FFC0000) /* Filter on the id. */;
- CAN0_MESSAGES[1].prio_id = id1 << 18;
+ CAN0_MESSAGES[1].prio_id = prio_id_for_id(id1);
CAN0_MESSAGES[1].control_timestamp =
CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_RX_EMPTY);
@@ -111,8 +117,8 @@
CAN0_MCR &= ~CAN_MCR_HALT;
}
-static void can_vesc_process_rx(volatile CanMessageBuffer *buffer,
- unsigned char *data_out, int *length_out) {
+static void can_process_rx(volatile CanMessageBuffer *buffer,
+ unsigned char *data_out, int *length_out) {
// Wait until the buffer is marked as not being busy. The reference manual
// says to do this, although it's unclear how we could get an interrupt
// asserted while it's still busy. Maybe if the interrupt was slow and now
@@ -170,7 +176,7 @@
// doing stuff...
CAN0_IFLAG1 = 1 << mailbox;
- message_buffer->prio_id = (can_id << 18);
+ message_buffer->prio_id = prio_id_for_id(can_id);
// Copy only the bytes from data that we're supposed to onto the stack, and
// then move it into the message buffer 32 bits at a time (because it might
// get unhappy about writing individual bytes). Plus, we have to byte-swap
@@ -185,14 +191,16 @@
message_buffer->data[0] = __builtin_bswap32(data_words[0]);
message_buffer->data[1] = __builtin_bswap32(data_words[1]);
}
- // TODO(Brian): Set IDE and SRR for extended frames.
- message_buffer->control_timestamp =
- CAN_MB_CONTROL_INSERT_DLC(length) |
- CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_DATA);
+ uint32_t control_timestamp = CAN_MB_CONTROL_INSERT_DLC(length) |
+ CAN_MB_CONTROL_INSERT_CODE(CAN_MB_CODE_TX_DATA);
+ if (can_id & CAN_EFF_FLAG) {
+ control_timestamp |= CAN_MB_CONTROL_IDE | CAN_MB_CONTROL_SRR;
+ }
+ message_buffer->control_timestamp = control_timestamp;
return 0;
}
-void can_receive_command(unsigned char *data, int *length, int mailbox) {
+void can_receive(unsigned char *data, int *length, int mailbox) {
if (0) {
static int i = 0;
if (i++ == 10000) {
@@ -205,5 +213,5 @@
*length = -1;
return;
}
- can_vesc_process_rx(&CAN0_MESSAGES[mailbox], data, length);
+ can_process_rx(&CAN0_MESSAGES[mailbox], data, length);
}
diff --git a/motors/peripheral/can.h b/motors/peripheral/can.h
index 54bdce8..98083c9 100644
--- a/motors/peripheral/can.h
+++ b/motors/peripheral/can.h
@@ -10,6 +10,8 @@
extern "C" {
#endif
+#define CAN_EFF_FLAG UINT32_C(0x80000000) /* EFF/SFF is set in the MSB */
+
void can_init(uint32_t id0, uint32_t id1);
// Mailbox is 2 or 3 for the two send mailboxes.
@@ -18,7 +20,7 @@
// Sets *length to -1 if there isn't a new piece of data to receive.
// Mailbox is 0 or 1 for the two receive mailboxes.
-void can_receive_command(unsigned char *data, int *length, int mailbox);
+void can_receive(unsigned char *data, int *length, int mailbox);
#ifdef __cplusplus
}
diff --git a/motors/pistol_grip/controller.cc b/motors/pistol_grip/controller.cc
index a77977b..9f38eb8 100644
--- a/motors/pistol_grip/controller.cc
+++ b/motors/pistol_grip/controller.cc
@@ -250,7 +250,7 @@
{
uint8_t data[8];
int length;
- can_receive_command(data, &length, 0);
+ can_receive(data, &length, 0);
if (length > 0) {
last_command_time = micros();
trigger_goal_position =
@@ -291,7 +291,7 @@
{
uint8_t data[8];
int length;
- can_receive_command(data, &length, 1);
+ can_receive(data, &length, 1);
if (length == 8) {
last_command_time = micros();
wheel_goal_position =
diff --git a/motors/pistol_grip/drivers_station.cc b/motors/pistol_grip/drivers_station.cc
index 33c6caa..d78cfd4 100644
--- a/motors/pistol_grip/drivers_station.cc
+++ b/motors/pistol_grip/drivers_station.cc
@@ -115,7 +115,7 @@
uint8_t can_data[8];
int length;
- can_receive_command(can_data, &length, 0);
+ can_receive(can_data, &length, 0);
if (length == 8) {
last_command_time = micros();
trigger_position =
@@ -133,7 +133,7 @@
update_report = true;
}
- can_receive_command(can_data, &length, 1);
+ can_receive(can_data, &length, 1);
if (length == 8) {
last_command_time = micros();
wheel_position =
diff --git a/motors/simple_receiver.cc b/motors/simple_receiver.cc
new file mode 100644
index 0000000..ca9910e
--- /dev/null
+++ b/motors/simple_receiver.cc
@@ -0,0 +1,133 @@
+// This file has the main for the Teensy on the simple receiver board.
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <atomic>
+#include <cmath>
+
+#include "motors/core/time.h"
+#include "motors/core/kinetis.h"
+#include "motors/peripheral/adc.h"
+#include "motors/peripheral/can.h"
+#include "motors/usb/usb.h"
+#include "motors/usb/cdc.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace motors {
+namespace {
+
+::std::atomic<teensy::AcmTty *> global_stdout{nullptr};
+
+} // namespace
+
+extern "C" {
+
+void *__stack_chk_guard = (void *)0x67111971;
+
+int _write(int /*file*/, char *ptr, int len) {
+ teensy::AcmTty *const tty = global_stdout.load(::std::memory_order_acquire);
+ if (tty != nullptr) {
+ return tty->Write(ptr, len);
+ }
+ return 0;
+}
+
+void __stack_chk_fail(void);
+
+void DoTest() {
+ uint32_t time = micros();
+ while (true) {
+ for (int i = 0; i < 6; ++i) {
+ const uint32_t end = time_add(time, 500000);
+ while (true) {
+ const bool done = time_after(micros(), end);
+ double current;
+ if (done) {
+ current = -6;
+ } else {
+ current = 6;
+ }
+ const int32_t current_int = current * 1000;
+ uint32_t id = CAN_EFF_FLAG;
+ id |= i;
+ id |= (0x01 /* SET_CURRENT */) << 8;
+ uint8_t data[4] = {
+ static_cast<uint8_t>((current_int >> 24) & 0xFF),
+ static_cast<uint8_t>((current_int >> 16) & 0xFF),
+ static_cast<uint8_t>((current_int >> 8) & 0xFF),
+ static_cast<uint8_t>((current_int >> 0) & 0xFF),
+ };
+ can_send(id, data, sizeof(data), 2);
+ if (done) {
+ break;
+ }
+ delay(5);
+ }
+ time = end;
+ }
+ }
+}
+
+} // extern "C"
+
+extern "C" int main(void) {
+ // for background about this startup delay, please see these conversations
+ // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
+ // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
+ delay(400);
+
+ // Set all interrupts to the second-lowest priority to start with.
+ for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
+
+ // Now set priorities for all the ones we care about. They only have meaning
+ // relative to each other, which means centralizing them here makes it a lot
+ // more manageable.
+ NVIC_SET_SANE_PRIORITY(IRQ_USBOTG, 0x7);
+
+ // Builtin LED.
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
+
+ // Set up the CAN pins.
+ PORTA_PCR12 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ delay(100);
+
+ teensy::UsbDevice usb_device(0, 0x16c0, 0x0492);
+ usb_device.SetManufacturer("Seems Reasonable LLC");
+ usb_device.SetProduct("Simple Receiver Board");
+
+ teensy::AcmTty tty0(&usb_device);
+ global_stdout.store(&tty0, ::std::memory_order_release);
+ usb_device.Initialize();
+
+ can_init(0, 1);
+ salsa::AdcInitJoystick();
+
+ // Leave the LEDs on for a bit longer.
+ delay(300);
+ printf("Done starting up\n");
+
+ // Done starting up, now turn the LED off.
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 0;
+
+ DoTest();
+
+ return 0;
+}
+
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
+} // namespace motors
+} // namespace frc971