From c5cbb525a9135a513244f2702ac88fc60fe40b63 Mon Sep 17 00:00:00 2001 From: Casey Duckering Date: Sat, 25 Oct 2014 15:35:14 -0700 Subject: [PATCH 1/6] New radio protocol YAML file copied from Daemon --- .../common_defs/radio_protocol_ng.yaml | 344 ++++++++++++++++++ 1 file changed, 344 insertions(+) create mode 100644 angel-player/src/chrome/content/common_defs/radio_protocol_ng.yaml diff --git a/angel-player/src/chrome/content/common_defs/radio_protocol_ng.yaml b/angel-player/src/chrome/content/common_defs/radio_protocol_ng.yaml new file mode 100644 index 00000000..92caed22 --- /dev/null +++ b/angel-player/src/chrome/content/common_defs/radio_protocol_ng.yaml @@ -0,0 +1,344 @@ +- name: uint8_t + repr: unsigned + kind: alien + size: 1 +- name: uint16_t + repr: unsigned + kind: alien + size: 2 +- name: uint32_t + repr: unsigned + kind: alien + size: 4 +- name: uint64_t + repr: unsigned + kind: alien + size: 8 + +# Radio needs to transmit the following: + +# Robot Host +# --------------------------------------------------- +# Telemetry → +# Control signals ← +# Code ← +# Bulk state → +# Debug <-> +# Sensor probing <-> + + + +# NDL3 ports: +# NDL3_UBJSON_PORT +# Reliable, structured UBJSON data +# Usage: +# Debug +# NDL3_STRING_PORT +# Reliable, unstructured data +# Usage: +# stdout +# stdin? +# NDL3_CODE_PORT +# Reliable, structured binary data +# Usage: +# Code +# Bulk state? +# NDL3_CONFIG_PORT +# Reliable, structured binary data +# Usage: +# start/stop VM +# patch? +# device probing +# NDL3_FAST_PORT +# Unreliable, structured UBJSON data +# Usage: +# Telemetry +# Controls + + + + +# UBJSON (reliable) port: +# const TYPE_MEMOIZE_CLEAR 0x00 +# const TYPE_MEMOIZE_STRING 0x01 +- name: TYPE_MEMOIZE_CLEAR + kind: const + value: 0x00 +- name: TYPE_MEMOIZE_STRING + kind: const + value: 0x01 + +# [ +# 123, // type +# { // Only for TYPE_MEMOIZE_STRING +# 123: “str” // Memoize the string “str” with id 123 +# … +# } +# ] + +# String port: +# Raw, unencapsulated strings for stdin/stdout + +# Code port: +# #define ID_SEND_CODE_BLOB 0x00 +- name: ID_SEND_CODE_BLOB + kind: const + value: 0x00 +# #define ID_GET_TRACE_BLOB 0x01 +- name: ID_GET_TRACE_BLOB + kind: const + value: 0x01 +- name: code_port + kind: struct + packed: true + slots: + - name: id + type: uint8_t + - name: len + type: uint32_t + - name: data + type: uint8_t[] +# struct code_port (packed) { +# uint8_t id +# uint32_t len +# uint8_t[] data +# } + +# Config port: +# #define ID_START_VM +- name: ID_START_VM + kind: const + value: 0x00 +# #define ID_STOP_VM +- name: ID_STOP_VM + kind: const + value: 0x01 +# #define ID_LOAD_MAIN_THREAD 0x02 +- name: ID_LOAD_MAIN_THREAD + kind: const + value: 0x02 +# #define ID_CONTROL_UNFREEZE 0x10 +- name: ID_CONTROL_UNFREEZE + kind: const + value: 0x10 +# #define ID_CONTROL_STOP 0x11 +- name: ID_CONTROL_STOP + kind: const + value: 0x11 +# #define ID_CONTROL_UNPOWERED 0x12 +- name: ID_CONTROL_UNPOWERED + kind: const + value: 0x12 +# #define ID_CONTROL_SET_AUTON 0x13 +- name: ID_CONTROL_SET_AUTON + kind: const + value: 0x13 +# #define ID_CONTROL_SET_TELEOP 0x14 +- name: ID_CONTROL_SET_TELEOP + kind: const + value: 0x14 +# #define ID_DEVICE_GET_LIST 0x20 +- name: ID_DEVICE_GET_LIST + kind: const + value: 0x20 +# #define ID_DEVICE_READ_DESCRIPTOR 0x21 +- name: ID_DEVICE_READ_DESCRIPTOR + kind: const + value: 0x21 +# #define ID_DEVICE_ENABLE_BLINK 0x22 +- name: ID_DEVICE_ENABLE_BLINK + kind: const + value: 0x22 + +- name: device_list_t + kind: struct + packed: true + slots: + - name: count + type: uint32_t + - name: dids + type: uint16_t[] + +- name: device_read_descriptor_req_t + kind: struct + packed: true + slots: + - name: did + type: uint64_t + - name: start + type: uint16_t + - name: len + type: uint16_t + +- name: LENGTH_AT_LEAST_ONE + kind: const + value: 0x01 + +- name: device_read_descriptor_resp_t + kind: struct + packed: true + slots: + - name: did + type: uint64_t + - name: data + type: uint8_t[LENGTH_AT_LEAST_ONE] + +- name: device_blink_t + kind: struct + packed: true + slots: + - name: did + type: uint64_t + - name: blink_on + type: uint8_t + +- name: config_port_data + kind: union + slots: + - name: device_list + type: device_list_t + - name: device_read_descriptor_req + type: device_read_descriptor_req_t + - name: device_read_descriptor_resp + type: device_read_descriptor_resp_t + - name: device_blink + type: device_blink_t + - name: nothing + type: uint8_t + + +- name: config_port + kind: struct + packed: true + slots: + - name: id + type: uint8_t + - name: data + type: config_port_data +# struct config_port (packed) { +# uint8_t id +# union { +# struct device_list { +# uint32_t count +# uint64_t[] dids +# } +# struct device_read_descriptor_req { +# uint64_t did +# uint16_t start +# uint16_t len +# } +# struct device_read_descriptor_resp { +# uint8_t[] data +# } +# struct device_blink { +# uint8_t blink_on +# uint64_t did +# } +# } +# } + +# Fast port: +# const TYPE_JOY_DATA 0x00 +# const TYPE_TELEM_DATA 0x01 +- name: TYPE_JOY_DATA + kind: const + value: 0x00 +- name: TYPE_TELEM_DATA + kind: const + value: 0x01 + +# [ +# nnn, // type +# { +# 123: [1,0,1,…] // Telemetry or joystick, can be repeated +# } // 123 = memoized string +# // array = samples (for telemetry) +# // channel (for joystick) +# ] + + + + + + + + + + + + + + + + + +# More detailed brain dumps: + +# Notes on UBJSON +# UBJSON is only available to Lua. Any state that needs to be available to C cannot be sent via UBJSON (easily). Any data from UBJSON can be condidered “untrusted” (not resistant against tampering from within studencode) +# String memoization +# The purpose of string memoization is to reduce the number of bytes that need to be sent repeatedly. For example, suppose there was a string “joy0-analog” that needed to be sent in every joystick packet. This takes up 11 bytes in every telemetry packet that conveys no additional information after the first time. String memoization solves this by having the host send a unique number and the string to the controller once at the beginning. The string will then be replaced by this number, which will usually take only 1 byte. +# The host can memoize strings sent to the controller. The controller can also send back a TYPE_MEMOIZE_STRING back to the host for e.g. telemetry. IDs may overlap between the two. +# TYPE_MEMOIZE_CLEAR clears both directions +# String port +# This port is directly mapped to stdio. No encapsulation +# printf() from robot appears here +# data written to this port should be made available via scanf, etc. +# Code/trace +# Code is always host-->robot +# Trace is always robot-->host +# Code is not run immediately. Running the code is controlled by the config port. +# How to trigger trace is TBD +# Config +# The majority of host “controlling” the robot happens here. This includes field control logic. +# VM state control +# The host can completely stop execution of any studentcode by sending a stop command. This tears down the VM and frees all of the associated state. +# Start will cause a VM to be created. Standard libraries will be loaded but no studentcode will be. +# Load main thread is used to load the studentcode. The studentcode must have been sent earlier using the code port. +# Replacing the studentcode should work. This needs to be worked out on the runtime dide. Loading studentcode when there is already a studentcode should tear down the student-controlled parts of the VM without rebooting the entire VM. +# Patching will be controlled here. Exact behavior TBD. +# Field control logic +# Field control logic is here (rather than e.g. a UBJSON port) so that it is impossible for students to cheat it. +# There are three stop/run states (unfreeze, stop, unpowered) because of corner-cases with e.g. servos. There are extensive notes on this elsewhere. +# Autonomous and teleop are needed by the C code to block access to the joystick (PiER never did this). It will also be exposed to Lua +# Smartdevice logic +# Smartdevices will be enumerated by the firmware at TBD time. The host can request a list of devices using the get list command. This will return a list of smart device UUIDs. +# The firmware will automatically handle bw allocation and most of the details of communicating with devices. The UI needs to handle human-readable descriptors and binding devices to meaningful names. +# The UI can get descriptions of the devices by reading the descriptor information. See smartdevice spec. +# In order to help identify devices that are already mounted on the robot, the UI can command the LED on the smartdevice to blink. +# Joystick +# Joystick packets contain a number of channels that each contain a number of values. Each individual value can be referred to as - +# For the Xbox 360 joystick, there would be 2 channels: button, analog with 11 and 8 values +# Multiple joysticks can be sent in the same packet +# Telemetry +# Telemetry contains channels that can contain 1 or more samples +# Behavior very similar to joysticks +# Telemetry is batched up and sent at intervals + + + + + + + + + + + + + +# Sample exchanges (PC <--> robot); +# config port: 0x01 → (stop VM, reset (almost) all state) +# config port: 0x20 → (get device list) +# config port: ← 0x20 0x01 0x00 0x00 0x00 0xAA 0xBB 0xCC 0xDD 0xEE 0xFF 0x00 0x01 +# config port: 0x21 [0xAA … 0x01] 0x00 0x00 0x02 0x00 → (read descriptor length) +# config port: ← 0x21 0x55 0x00 +# config port: 0x21 [0xAA … 0x01] 0x00 0x00 0x55 0x00 → (read entire descriptor) +# config port: ← 0x21 … +# config port: 0x00 → (start VM) +# code port: 0x00 0x23 0x01 0xXX 0xXX … → (buffer code) +# config port: 0x02 → (load code into main thread) +# ubjson reliable: [1, {0: “joy0-analog”, 1: “joy0-button”}] → (memoize string) +# config port: 0x10 → (unfreeze) +# ubjson fast: [0, 0: [1,1,0.5, …], 1: [true, false, …]] → (joystick) +# ... From 51c7daa03311e087917ade4799440d19bc687791 Mon Sep 17 00:00:00 2001 From: Casey Duckering Date: Sat, 25 Oct 2014 15:46:52 -0700 Subject: [PATCH 2/6] Clean up unused printf handling radio code --- controller/src/radio.c | 40 ---------------------------------------- 1 file changed, 40 deletions(-) diff --git a/controller/src/radio.c b/controller/src/radio.c index 48d20e08..816c01a1 100644 --- a/controller/src/radio.c +++ b/controller/src/radio.c @@ -54,46 +54,6 @@ void ndFree(void * to_free, void * userdata); -// Redirect printf, etc. to the radio - -// con_write -// devoptab_t -// consoleInit -/* -ssize_t nocash_write(struct _reent *r, int fd, const char *ptr, size_t len) { - nocashWrite(ptr, len); - return len; -} - -ssize_t con_write(struct _reent *r, int fd, const char *ptr, size_t len) { -} - -static const devoptab_t dotab_stdout = { - "con", - 0, - NULL, - NULL, - con_write, - NULL, - NULL, - NULL -}; - - -void consoleInit() { - static uint8_t firstConsoleInit = true; - - if (firstConsoleInit) { - devoptab_list[STD_OUT] = &dotab_stdout; - devoptab_list[STD_ERR] = &dotab_stdout; - setvbuf(stdout, NULL , _IONBF, 0); - setvbuf(stderr, NULL , _IONBF, 0); - firstConsoleInit = false; - } -} -*/ - - static portTASK_FUNCTION_PROTO(radioNewTask, pvParameters); BaseType_t radioInit() { From ba8b5b667ecc5bf71b4b1a427fd134106d84e697 Mon Sep 17 00:00:00 2001 From: Casey Duckering Date: Sat, 25 Oct 2014 15:37:06 -0700 Subject: [PATCH 3/6] Functions to support trasmitting on the rest of the radio ports --- controller/inc/radio.h | 8 ++++++++ controller/src/radio.c | 37 ++++++++++++++++++++++++++++++++++++- 2 files changed, 44 insertions(+), 1 deletion(-) diff --git a/controller/inc/radio.h b/controller/inc/radio.h index f256b451..df4f6be7 100644 --- a/controller/inc/radio.h +++ b/controller/inc/radio.h @@ -20,10 +20,18 @@ #include "inc/FreeRTOS.h" +#include "radio_protocol_ng.h" // NOLINT(build/include) + BaseType_t radioInit(); + +// Do not modify or release the data after calling these. +// The radio takes ownership of releasing the memeory. void radioPushUbjson(const char *ubjson, size_t len); void radioPushString(const char *str, size_t len); +void radioPushBulk(const code_port *data, size_t len); +void radioPushConfig(const config_port *data, size_t len); +void radioPushFast(const char *ubjson, size_t len); #endif // INC_RADIO_H_ diff --git a/controller/src/radio.c b/controller/src/radio.c index 816c01a1..00ce4135 100644 --- a/controller/src/radio.c +++ b/controller/src/radio.c @@ -16,6 +16,7 @@ // under the License #include +#include #include @@ -81,6 +82,30 @@ void radioPushString(const char *str, size_t len) { }; xQueueSend(radioQueue, &msg, 0); } +void radioPushBulk(const code_port *data, size_t len) { + RadioMessage msg = { + .port = NDL3_CODE_PORT, + .str = (const char*)data, + .len = len + }; + xQueueSend(radioQueue, &msg, 0); +} +void radioPushConfig(const config_port *data, size_t len) { + RadioMessage msg = { + .port = NDL3_CONFIG_PORT, + .str = (const char*)data, + .len = len + }; + xQueueSend(radioQueue, &msg, 0); +} +void radioPushFast(const char *ubjson, size_t len) { + RadioMessage msg = { + .port = NDL3_FAST_PORT, + .str = ubjson, + .len = len + }; + xQueueSend(radioQueue, &msg, 0); +} // TODO(cduck): Move this to uart_serial_driver.c and make it work @@ -146,6 +171,7 @@ static portTASK_FUNCTION_PROTO(radioNewTask, pvParameters) { NDL3_open(target, NDL3_CODE_PORT); NDL3_open(target, NDL3_CONFIG_PORT); NDL3_open(target, NDL3_FAST_PORT); + NDL3_setopt(target, NDL3_FAST_PORT, NDL3_PORT_UNRELIABLE); char * recvMsg = NULL; const uint8_t prefixLen = 1; @@ -206,7 +232,16 @@ static portTASK_FUNCTION_PROTO(radioNewTask, pvParameters) { // Send code to runtime if (recvMsg && recvSize >= 1) { // Trust this to free recvMsg - // TODO(cduck): Do something + // TODO(cduck): Send to radio config thread instead + switch (recvMsg[0]) { + case ID_CONTROL_UNFREEZE: setGameMode(4); break; + case ID_CONTROL_STOP: setGameMode(3); break; + case ID_CONTROL_UNPOWERED: setGameMode(1); break; + case ID_CONTROL_SET_AUTON: setGameMode(2); break; + case ID_CONTROL_SET_TELEOP: setGameMode(4); break; + default: break; + } + printf("Got config data\n"); vPortFree(recvMsg); } else { From 9ecc3c1484888238b42cf04e169ff8f8f2dc92ba Mon Sep 17 00:00:00 2001 From: Casey Duckering Date: Sat, 25 Oct 2014 17:44:17 -0700 Subject: [PATCH 4/6] Change setGameMode to a public function --- controller/inc/runtime.h | 1 + 1 file changed, 1 insertion(+) diff --git a/controller/inc/runtime.h b/controller/inc/runtime.h index 5bf85e6b..96dbfaa0 100644 --- a/controller/inc/runtime.h +++ b/controller/inc/runtime.h @@ -44,6 +44,7 @@ extern volatile int gameMode; BaseType_t runtimeInit(); +void setGameMode(RuntimeMode mode); void runtimeSendRadioMsg(RuntimeMessageType type, void* info, size_t infoLen); // Takes responsibility for freeing void runtimeRecieveUbjson(char *ubjson, size_t len); From b610c6922411cc9c150e9a8f7e45d7f157ac70e0 Mon Sep 17 00:00:00 2001 From: Vincent Donato Date: Sat, 25 Oct 2014 18:48:48 -0700 Subject: [PATCH 5/6] Initial code for radio config thread and device enumeration --- controller/inc/radio_config.h | 28 +++++++++ controller/src/radio_config.c | 109 ++++++++++++++++++++++++++++++++++ 2 files changed, 137 insertions(+) create mode 100644 controller/inc/radio_config.h create mode 100644 controller/src/radio_config.c diff --git a/controller/inc/radio_config.h b/controller/inc/radio_config.h new file mode 100644 index 00000000..c29c579f --- /dev/null +++ b/controller/inc/radio_config.h @@ -0,0 +1,28 @@ +// Licensed to Pioneers in Engineering under one +// or more contributor license agreements. See the NOTICE file +// distributed with this work for additional information +// regarding copyright ownership. Pioneers in Engineering licenses +// this file to you under the Apache License, Version 2.0 (the +// "License"); you may not use this file except in compliance +// with the License. You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, +// software distributed under the License is distributed on an +// "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY +// KIND, either express or implied. See the License for the +// specific language governing permissions and limitations +// under the License + +#ifndef INC_RADIO_CONFIG_H_ +#define INC_RADIO_CONFIG_H_ + +#include "inc/FreeRTOS.h" +#include "inc/radio.h" +#include "inc/smartsensor/ssutil.h" +#include "radio_protocol_ng.h" // NOLINT(build/include) + +BaseType_t radioConfigInit(); +void receiveConfigPort(config_port *port, size_t len); +#endif // INC_RADIO_CONFIG_H_ diff --git a/controller/src/radio_config.c b/controller/src/radio_config.c new file mode 100644 index 00000000..e41b6b7b --- /dev/null +++ b/controller/src/radio_config.c @@ -0,0 +1,109 @@ +// Licensed to Pioneers in Engineering under one +// or more contributor license agreements. See the NOTICE file +// distributed with this work for additional information +// regarding copyright ownership. Pioneers in Engineering licenses +// this file to you under the Apache License, Version 2.0 (the +// "License"); you may not use this file except in compliance +// with the License. You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, +// software distributed under the License is distributed on an +// "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY +// KIND, either express or implied. See the License for the +// specific language governing permissions and limitations +// under the License + +#include +#include +#include + +#include "inc/radio_config.h" +#include "inc/task.h" +#include "inc/queue.h" +#include "inc/smartsensor/ssutil.h" + +typedef struct { + config_port *port; + size_t len; +} ConfigMessage; + +QueueHandle_t configMessageQueue = NULL; +// void sensorUpdateCallback(uint16_t index, SSState *sensor); + +static portTASK_FUNCTION_PROTO(radioConfigTask, pvParameters); + +BaseType_t radioConfigInit() { + configMessageQueue = xQueueCreate(100, sizeof(ConfigMessage)); + /* + // Get updates from smart sensor protocol + registerSensorUpdateCallback(&sensorUpdateCallback); + */ + return xTaskCreate(radioConfigTask, "Radio Config", 2048, NULL, + tskIDLE_PRIORITY, NULL); +} + +void receiveConfigPort(config_port *port, size_t len) { + ConfigMessage msg = { + .port = port, + .len = len, + }; + xQueueSend(configMessageQueue, &msg, 0); +} + +config_port *getDeviceList() { + config_port *port = pvPortMalloc(sizeof(uint8_t) + sizeof(uint32_t) + + SMART_ID_LEN*numSensors); + port->id = ID_DEVICE_GET_LIST; + port->data.device_list.count = numSensors; + for (int i = 0; i < numSensors; i++) { + uint64_t temp = 0; + for (int j = SMART_ID_LEN-1; j > 0; j--) { + temp = ((temp << 8) | sensorArr[i]->id[j]); + } + port->data.device_list.dids[i] = temp; + } + return port; +} + +static portTASK_FUNCTION_PROTO(radioConfigTask, pvParameters) { + (void) pvParameters; + while (1) { + ConfigMessage msg; + while (xQueueReceive(configMessageQueue, &msg, portMAX_DELAY) == pdTRUE) { + switch (msg.port->id) { + case ID_START_VM: + break; + case ID_STOP_VM: + break; + case ID_LOAD_MAIN_THREAD: + break; + case ID_CONTROL_UNFREEZE: + break; + case ID_CONTROL_STOP: + break; + case ID_CONTROL_UNPOWERED: + break; + case ID_CONTROL_SET_AUTON: + break; + case ID_CONTROL_SET_TELEOP: + break; + case ID_DEVICE_GET_LIST: { + config_port *deviceList = getDeviceList(); + size_t size = sizeof(uint8_t) + sizeof(uint32_t) + + SMART_ID_LEN*numSensors; + radioPushConfig(deviceList, size); + } + break; + case ID_DEVICE_READ_DESCRIPTOR: + break; + case ID_DEVICE_ENABLE_BLINK: + break; + default: + break; + } + vPortFree(msg.port); + } + } +} From 0fe6e114da5c55b5d2217ab7bc5f04e47849181a Mon Sep 17 00:00:00 2001 From: Casey Duckering Date: Sat, 25 Oct 2014 22:08:02 -0700 Subject: [PATCH 6/6] Controller game state control also over config port --- controller/src/radio_config.c | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/controller/src/radio_config.c b/controller/src/radio_config.c index e41b6b7b..5cde87a4 100644 --- a/controller/src/radio_config.c +++ b/controller/src/radio_config.c @@ -23,6 +23,7 @@ #include "inc/task.h" #include "inc/queue.h" #include "inc/smartsensor/ssutil.h" +#include "inc/runtime.h" typedef struct { config_port *port; @@ -72,7 +73,7 @@ static portTASK_FUNCTION_PROTO(radioConfigTask, pvParameters) { while (1) { ConfigMessage msg; while (xQueueReceive(configMessageQueue, &msg, portMAX_DELAY) == pdTRUE) { - switch (msg.port->id) { + switch (msg.port->id) { // TODO(vdonato): Implement the rest case ID_START_VM: break; case ID_STOP_VM: @@ -80,14 +81,19 @@ static portTASK_FUNCTION_PROTO(radioConfigTask, pvParameters) { case ID_LOAD_MAIN_THREAD: break; case ID_CONTROL_UNFREEZE: + // TODO(vdonato): Figure out what to do break; case ID_CONTROL_STOP: + setGameMode(RuntimeModePaused); break; case ID_CONTROL_UNPOWERED: + setGameMode(RuntimeModeDisabled); break; case ID_CONTROL_SET_AUTON: + setGameMode(RuntimeModeAutonomous); break; case ID_CONTROL_SET_TELEOP: + setGameMode(RuntimeModeTeleop); break; case ID_DEVICE_GET_LIST: { config_port *deviceList = getDeviceList();