From db344fee1b0086e9d22622926364ee74cf155249 Mon Sep 17 00:00:00 2001 From: Kyle Zentner Date: Sun, 23 Nov 2014 23:44:13 -0800 Subject: [PATCH 1/4] Receive PiEMOS data using new radio task. --- controller/inc/radio.h | 5 +++++ controller/src/main.c | 4 ---- controller/src/radio.c | 39 ++++++++++++++++++++++++++++++++++++--- 3 files changed, 41 insertions(+), 7 deletions(-) diff --git a/controller/inc/radio.h b/controller/inc/radio.h index f256b451..04b0c15a 100644 --- a/controller/inc/radio.h +++ b/controller/inc/radio.h @@ -25,5 +25,10 @@ BaseType_t radioInit(); void radioPushUbjson(const char *ubjson, size_t len); void radioPushString(const char *str, size_t len); +// TODO(kzentner): This was moved here from main.c +// TODO(rqou): This really doesn't go here. +extern int8_t PiEMOSAnalogVals[7]; +extern uint8_t PiEMOSDigitalVals[8]; + #endif // INC_RADIO_H_ diff --git a/controller/src/main.c b/controller/src/main.c index 8de228bc..ec1cd416 100644 --- a/controller/src/main.c +++ b/controller/src/main.c @@ -52,10 +52,6 @@ uint8_t *code_buffer; uint32_t code_buffer_len; -// TODO(rqou): This really doesn't go here. -int8_t PiEMOSAnalogVals[7]; -uint8_t PiEMOSDigitalVals[8]; - // TODO(rqou): Wat r abstraction? // TODO(rqou): My macro-fu is not up to par extern ngl_error *ngl_set_motor(ngl_float motor, ngl_float val); diff --git a/controller/src/radio.c b/controller/src/radio.c index 816c01a1..830a9c36 100644 --- a/controller/src/radio.c +++ b/controller/src/radio.c @@ -41,6 +41,11 @@ typedef struct { QueueHandle_t radioQueue = NULL; +// TODO(kzentner): This was moved here from main.c +// TODO(rqou): This really doesn't go here. +int8_t PiEMOSAnalogVals[7]; +uint8_t PiEMOSDigitalVals[8]; + // Hard coded volatile uint64_t host_addr = 0; // Bytes reversed @@ -159,6 +164,14 @@ static portTASK_FUNCTION_PROTO(radioNewTask, pvParameters) { portTickType time = xTaskGetTickCount(); portTickType lastTime = time; + // Initialize PiEMOS values. + for (int i = 0; i < 7; i++) { + PiEMOSAnalogVals[i] = 0.0; + } + for (int i = 0; i < 8; i++) { + PiEMOSDigitalVals[i] = 0; + } + while (1) { recvMsg = NULL; recvSize = 0; @@ -239,9 +252,29 @@ static portTASK_FUNCTION_PROTO(radioNewTask, pvParameters) { recXbeeHeader = (xbee_rx64_header*)&(recXbeePacket->payload); if (recXbeeHeader->xbee_api_type == XBEE_API_TYPE_RX64) { host_addr = recXbeeHeader->xbee_src_addr; - if (uartRecvSize >= prefixLen && recXbeeHeader->data[0] == NDL3_IDENT) { - NDL3_L2_push(target, (uint8_t*)recXbeeHeader->data+1, - recXbeePacket->length-sizeof(xbee_rx64_header)-prefixLen); + uint8_t ident_byte = recXbeeHeader->data[0]; + switch (ident_byte) { + case NDL3_IDENT: + { + NDL3_L2_push(target, (uint8_t*)recXbeeHeader->data+1, + recXbeePacket->length-sizeof(xbee_rx64_header)-prefixLen); + } + break; + case PIER_INCOMINGDATA_IDENT: + { + pier_incomingdata *incomingData = + (pier_incomingdata *)(recXbeeHeader->data); + // TODO(kzentner): This was copied from main.c + // TODO(rqou): This code is terribly hardcoded. + for (int i = 0; i < 7; i++) { + PiEMOSAnalogVals[i] = + (float)((int)incomingData->analog[i] - 127) / 127.0f * 100.0f; + } + for (int i = 0; i < 8; i++) { + PiEMOSDigitalVals[i] = !!(incomingData->digital & (1 << i)); + } + } + break; } } vPortFree(recXbeePacket); From b9d46f810de9efd78faeecfb174954a2eb610469 Mon Sep 17 00:00:00 2001 From: Kyle Zentner Date: Mon, 24 Nov 2014 00:38:19 -0800 Subject: [PATCH 2/4] Add PiEMOS to runtime interface. --- controller/inc/runtime_interface.h | 2 ++ controller/src/runtime_interface.c | 43 ++++++++++++++++++++++++++++++ 2 files changed, 45 insertions(+) diff --git a/controller/inc/runtime_interface.h b/controller/inc/runtime_interface.h index a40574ea..d92d6e22 100644 --- a/controller/inc/runtime_interface.h +++ b/controller/inc/runtime_interface.h @@ -46,6 +46,8 @@ int lua_set_led_val(lua_State *L); int lua_get_analog_val(lua_State *L); int lua_set_analog_val(lua_State *L); int lua_set_grizzly_val(lua_State *L); +int lua_get_piemos_analog_val(lua_State *L); +int lua_get_piemos_digital_val(lua_State *L); diff --git a/controller/src/runtime_interface.c b/controller/src/runtime_interface.c index 1df45eab..ccfeaa11 100644 --- a/controller/src/runtime_interface.c +++ b/controller/src/runtime_interface.c @@ -65,6 +65,8 @@ void runtime_register(TenshiRuntimeState s) { LUA_REG(get_analog_val), LUA_REG(set_analog_val), LUA_REG(set_grizzly_val), + LUA_REG(get_piemos_analog_val), + LUA_REG(get_piemos_digital_val), {NULL, NULL} }; @@ -82,6 +84,9 @@ void lua_register_all(lua_State *L) { LUA_REGISTER(get_analog_val); LUA_REGISTER(set_analog_val); LUA_REGISTER(set_grizzly_val); + + LUA_REGISTER(get_piemos_analog_val); + LUA_REGISTER(get_piemos_digital_val); } // Runtime required @@ -282,6 +287,44 @@ int lua_set_grizzly_val(lua_State *L) { return 0; } +// get_piemos_analog_val(idx): +// Get the analog value at PiEMOS index . +// is 1 indexed. 1 is the first channel. 7 is the last channel. +int lua_get_piemos_analog_val(lua_State *L) { + int was_num = 1; + int idx = lua_tointegerx(L, 1, &was_num); + // Check that the index is in range and was a number. + if (idx < 1 || idx > 7 || !was_num) { + return luaL_error(L, "Invalid index for PiEMOS analog value: %s.\n", + lua_tolstring(L, 1, NULL)); + } + // Pop the index argument off the stack. + lua_pop(L, 1); + + // Compensate for 1 indexing. + float val = PiEMOSAnalogVals[idx - 1] / 100.0f; + lua_pushnumber(L, val); + return 1; +} + +// get_piemos_digital_val(idx): +// Get the digital value at PiEMOS index . +// is 1 indexed. 1 is the first channel. 8 is the last channel. +int lua_get_piemos_digital_val(lua_State *L) { + int was_num = 1; + int idx = lua_tointegerx(L, 1, &was_num); + // Check that the index is in range and was a number. + if (idx < 1 || idx > 8 || !was_num) { + return luaL_error(L, "Invalid index for PiEMOS digital value: %s.\n", + lua_tolstring(L, 1, NULL)); + } + // Pop the index argument off the stack. + lua_pop(L, 1); + + // Compensate for 1 indexing. + lua_pushboolean(L, PiEMOSDigitalVals[idx - 1]); + return 1; +} // Not hooked up to lua From 94896c74b587c424400c9551a3b31f65788e848b Mon Sep 17 00:00:00 2001 From: Kyle Zentner Date: Mon, 24 Nov 2014 00:46:38 -0800 Subject: [PATCH 3/4] Implement PiEMOS protocol in runtime. --- vm/lua/tenshi-runtime/src/lua/piemos.lua | 78 +++++++++++++++++++ .../tenshi-runtime/src/lua/sensor_actor.lua | 1 + vm/lua/tenshi-runtime/src/runtime_entry.c | 8 ++ 3 files changed, 87 insertions(+) create mode 100644 vm/lua/tenshi-runtime/src/lua/piemos.lua diff --git a/vm/lua/tenshi-runtime/src/lua/piemos.lua b/vm/lua/tenshi-runtime/src/lua/piemos.lua new file mode 100644 index 00000000..cfd135e6 --- /dev/null +++ b/vm/lua/tenshi-runtime/src/lua/piemos.lua @@ -0,0 +1,78 @@ +-- 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 + +local piemos = {} + +local channels = { + digital = {}, + analog = {} +} + +-- Initialize all the channels. +for i = 1, 7 do + channels.analog[i] = { value = 0.0 } +end + +for i = 1, 8 do + channels.digital[i] = { value = false } +end + + +function piemos.get_axis(idx) + return channels.analog[idx].value +end + +function piemos.get_button(idx) + return channels.digital[idx].value +end + +function piemos.get_channel(name) + -- Name should be of the form PiEMOSAnalogVals-1 throught + -- PiEMOSAnalogVals-7 or PiEMOSDigitalVals-1 through PiEMOSDigitalVals-8. + local dashidx = name:find("-") + local array = name:sub(1, dashidx - 1) + local idx = tonumber(name:sub(dashidx + 1)) + + if array == 'PiEMOSAnalogVals' then + return channels.analog[idx] + elseif array == 'PiEMOSDigitalVals' then + return channels.digital[idx] + end +end + +function piemos.__process_radio() + + for i = 1, 7 do + local s = channels.analog[i] + s.value = __runtimeinternal.get_piemos_analog_val(i) + -- If there is a __downstream mailbox, send data to it + if s.__downstream then + s.__downstream:send({val}) + end + end + + for i = 1, 8 do + local s = channels.digital[i] + s.value = __runtimeinternal.get_piemos_digital_val(i) + -- If there is a __downstream mailbox, send data to it + if s.__downstream then + s.__downstream:send({val}) + end + end +end + +return piemos diff --git a/vm/lua/tenshi-runtime/src/lua/sensor_actor.lua b/vm/lua/tenshi-runtime/src/lua/sensor_actor.lua index ae5e2a75..82a27c69 100644 --- a/vm/lua/tenshi-runtime/src/lua/sensor_actor.lua +++ b/vm/lua/tenshi-runtime/src/lua/sensor_actor.lua @@ -41,4 +41,5 @@ for s,_ in pairs(changed_sensors) do changed_sensors[s] = nil end +piemos.__process_radio() pieles.__process_radio() diff --git a/vm/lua/tenshi-runtime/src/runtime_entry.c b/vm/lua/tenshi-runtime/src/runtime_entry.c index a9e20af0..fc2b8151 100644 --- a/vm/lua/tenshi-runtime/src/runtime_entry.c +++ b/vm/lua/tenshi-runtime/src/runtime_entry.c @@ -33,6 +33,7 @@ #include "../game.lc.h" // NOLINT(build/include) #include "../get_device.lc.h" // NOLINT(build/include) #include "../pieles.lc.h" // NOLINT(build/include) +#include "../piemos.lc.h" // NOLINT(build/include) #include "../sensor_actor.lc.h" // NOLINT(build/include) #include "../trap_global.lc.h" // NOLINT(build/include) #include "../triggers.lc.h" // NOLINT(build/include) @@ -133,6 +134,12 @@ static int tenshi_open_pieles(lua_State *L) { return 1; } +static int tenshi_open_piemos(lua_State *L) { + luaL_loadbuffer(L, piemos_lc, sizeof(piemos_lc), "piemos.lua"); + lua_pcall(L, 0, LUA_MULTRET, 0); + return 1; +} + static int tenshi_open_game(lua_State *L) { luaL_loadbuffer(L, game_lc, sizeof(game_lc), "game.lua"); lua_pcall(L, 0, LUA_MULTRET, 0); @@ -191,6 +198,7 @@ static const luaL_Reg tenshi_loadedlibs_phase2[] = { {"__actuators", tenshi_open_actuators}, {"triggers", tenshi_open_triggers}, {"pieles", tenshi_open_pieles}, + {"piemos", tenshi_open_piemos}, {"game", tenshi_open_game}, {"ubjson", tenshi_open_ubjson}, {NULL, NULL} From d7ab6652daed16b440bdb256cb3cf4271ce1a082 Mon Sep 17 00:00:00 2001 From: Kyle Zentner Date: Mon, 24 Nov 2014 15:47:33 -0800 Subject: [PATCH 4/4] Instantly update status LED from runtime. --- controller/src/led_driver.c | 67 ++++++++++++++++++++++--------------- 1 file changed, 40 insertions(+), 27 deletions(-) diff --git a/controller/src/led_driver.c b/controller/src/led_driver.c index f08c6330..9ca99222 100755 --- a/controller/src/led_driver.c +++ b/controller/src/led_driver.c @@ -245,33 +245,8 @@ void led_driver_panic(void) { (1 << GPIO_PIN(PINDEF_YELLOW_LED)); } -static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) { - (void) pvParameters; - - uint8_t current_led_pattern = 0; - uint8_t prev_led_pattern = 0; - uint8_t pattern_step_index = 0; - uint8_t current_step_ticks = 0; - - while (1) { - current_led_pattern = led_driver_current_mode; - - // New pattern --> reinit - if (current_led_pattern != prev_led_pattern) { - prev_led_pattern = current_led_pattern; - pattern_step_index = 0; - current_step_ticks = 0; - } - - // Set/clear the appropriate LEDs - uint8_t led_states = driver_patterns[current_led_pattern] - .entries[pattern_step_index].led_states; - led_states &= ~led_driver_fixed_mask; - led_states |= led_driver_fixed_pattern & led_driver_fixed_mask; - if (button_driver_get_button_state(0)) - led_states = led_driver_fixed_pattern; - if (button_driver_get_button_state(1)) led_states = ~led_states; - +static void update_hardware(uint8_t change_mask, uint8_t led_states) { + if (change_mask & LED_YELLOW) { if (led_states & LED_YELLOW) { GPIO_BANK(PINDEF_YELLOW_LED)->BSRRL = (1 << GPIO_PIN(PINDEF_YELLOW_LED)); @@ -279,6 +254,8 @@ static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) { GPIO_BANK(PINDEF_YELLOW_LED)->BSRRH = (1 << GPIO_PIN(PINDEF_YELLOW_LED)); } + } + if (change_mask & LED_BLUE) { if (led_states & LED_BLUE) { GPIO_BANK(PINDEF_BLUE_LED)->BSRRL = (1 << GPIO_PIN(PINDEF_BLUE_LED)); @@ -286,6 +263,8 @@ static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) { GPIO_BANK(PINDEF_BLUE_LED)->BSRRH = (1 << GPIO_PIN(PINDEF_BLUE_LED)); } + } + if (change_mask & LED_GREEN) { if (led_states & LED_GREEN) { GPIO_BANK(PINDEF_GREEN_LED)->BSRRL = (1 << GPIO_PIN(PINDEF_GREEN_LED)); @@ -293,6 +272,8 @@ static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) { GPIO_BANK(PINDEF_GREEN_LED)->BSRRH = (1 << GPIO_PIN(PINDEF_GREEN_LED)); } + } + if (change_mask & LED_RED) { if (led_states & LED_RED) { GPIO_BANK(PINDEF_RED_LED)->BSRRL = (1 << GPIO_PIN(PINDEF_RED_LED)); @@ -300,6 +281,37 @@ static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) { GPIO_BANK(PINDEF_RED_LED)->BSRRH = (1 << GPIO_PIN(PINDEF_RED_LED)); } + } +} + +static portTASK_FUNCTION_PROTO(led_driver_task, pvParameters) { + (void) pvParameters; + + uint8_t current_led_pattern = 0; + uint8_t prev_led_pattern = 0; + uint8_t pattern_step_index = 0; + uint8_t current_step_ticks = 0; + + while (1) { + current_led_pattern = led_driver_current_mode; + + // New pattern --> reinit + if (current_led_pattern != prev_led_pattern) { + prev_led_pattern = current_led_pattern; + pattern_step_index = 0; + current_step_ticks = 0; + } + + // Set/clear the appropriate LEDs + uint8_t led_states = driver_patterns[current_led_pattern] + .entries[pattern_step_index].led_states; + led_states &= ~led_driver_fixed_mask; + led_states |= led_driver_fixed_pattern & led_driver_fixed_mask; + if (button_driver_get_button_state(0)) + led_states = led_driver_fixed_pattern; + if (button_driver_get_button_state(1)) led_states = ~led_states; + + update_hardware(0xff, led_states); // Increment tick/advance step if (++current_step_ticks == @@ -335,6 +347,7 @@ uint8_t led_driver_get_mode(void) { void led_driver_set_fixed(uint8_t pattern, uint8_t mask) { led_driver_fixed_pattern = pattern; led_driver_fixed_mask = mask; + update_hardware(led_driver_fixed_mask, led_driver_fixed_pattern); } uint8_t led_driver_get_fixed_pattern() { return led_driver_fixed_pattern;