From e8c066bbae7dec323b2e9769e9eb29fd6bede8f2 Mon Sep 17 00:00:00 2001 From: Jerome Guzzi Date: Thu, 9 Jul 2020 16:44:59 +0200 Subject: [PATCH] Initial commit --- .gitignore | 2 + README.md | 0 client/streamer/CMakeLists.txt | 9 + client/streamer/REAME.md | 3 + client/streamer/launch/ai_deck.launch | 11 + client/streamer/package.xml | 32 + client/streamer/script/client.py | 187 +++++ client/streamer/script/opencv_node.py | 33 + client/streamer/script/ros_node.py | 31 + crazyflie/README.md | 6 + crazyflie/aideck.c | 213 ++++++ crazyflie/aideck_protocol.c | 142 ++++ gap/streamer/Makefile | 8 + gap/streamer/REAME.md | 8 + gap/streamer/camera.c | 669 ++++++++++++++++++ gap/streamer/camera.h | 40 ++ gap/streamer/common.h | 39 ++ gap/streamer/config.h | 49 ++ gap/streamer/config.ini | 10 + gap/streamer/io.c | 223 ++++++ gap/streamer/io.h | 17 + gap/streamer/led.c | 29 + gap/streamer/led.h | 8 + gap/streamer/main.c | 103 +++ gap/streamer/nina.c | 81 +++ gap/streamer/nina.h | 10 + gap/streamer/uart.c | 75 ++ gap/streamer/uart.h | 11 + gap/streamer/uart_protocol.c | 66 ++ gap/streamer/uart_protocol.h | 28 + nina/streamer/Makefile | 3 + nina/streamer/REAME.md | 8 + nina/streamer/load.sh | 1 + nina/streamer/main/Kconfig.projbuild | 26 + nina/streamer/main/component.mk | 8 + nina/streamer/main/spi.c | 138 ++++ nina/streamer/main/spi.h | 32 + nina/streamer/main/streamer.c | 438 ++++++++++++ nina/streamer/main/wifi.c | 199 ++++++ nina/streamer/main/wifi.h | 53 ++ nina/streamer/notes.md | 61 ++ nina/streamer/sdkconfig | 751 +++++++++++++++++++++ protocol/README.md | 77 +++ protocol/base.py | 67 ++ protocol/example_config.py | 22 + protocol/generate.py | 61 ++ protocol/stream_config.py | 27 + protocol/templates/crazyflie.aideck_p.h.j2 | 50 ++ protocol/templates/gap._protocol.c.j2 | 46 ++ protocol/templates/gap.protocol.h.j2 | 16 + protocol/templates/macros.j2 | 53 ++ 51 files changed, 4280 insertions(+) create mode 100644 .gitignore create mode 100644 README.md create mode 100644 client/streamer/CMakeLists.txt create mode 100644 client/streamer/REAME.md create mode 100644 client/streamer/launch/ai_deck.launch create mode 100644 client/streamer/package.xml create mode 100644 client/streamer/script/client.py create mode 100644 client/streamer/script/opencv_node.py create mode 100644 client/streamer/script/ros_node.py create mode 100644 crazyflie/README.md create mode 100644 crazyflie/aideck.c create mode 100644 crazyflie/aideck_protocol.c create mode 100644 gap/streamer/Makefile create mode 100644 gap/streamer/REAME.md create mode 100644 gap/streamer/camera.c create mode 100644 gap/streamer/camera.h create mode 100644 gap/streamer/common.h create mode 100644 gap/streamer/config.h create mode 100644 gap/streamer/config.ini create mode 100644 gap/streamer/io.c create mode 100644 gap/streamer/io.h create mode 100644 gap/streamer/led.c create mode 100644 gap/streamer/led.h create mode 100644 gap/streamer/main.c create mode 100644 gap/streamer/nina.c create mode 100644 gap/streamer/nina.h create mode 100644 gap/streamer/uart.c create mode 100644 gap/streamer/uart.h create mode 100644 gap/streamer/uart_protocol.c create mode 100644 gap/streamer/uart_protocol.h create mode 100644 nina/streamer/Makefile create mode 100644 nina/streamer/REAME.md create mode 100644 nina/streamer/load.sh create mode 100644 nina/streamer/main/Kconfig.projbuild create mode 100644 nina/streamer/main/component.mk create mode 100644 nina/streamer/main/spi.c create mode 100644 nina/streamer/main/spi.h create mode 100644 nina/streamer/main/streamer.c create mode 100644 nina/streamer/main/wifi.c create mode 100644 nina/streamer/main/wifi.h create mode 100644 nina/streamer/notes.md create mode 100644 nina/streamer/sdkconfig create mode 100644 protocol/README.md create mode 100644 protocol/base.py create mode 100644 protocol/example_config.py create mode 100644 protocol/generate.py create mode 100644 protocol/stream_config.py create mode 100644 protocol/templates/crazyflie.aideck_p.h.j2 create mode 100644 protocol/templates/gap._protocol.c.j2 create mode 100644 protocol/templates/gap.protocol.h.j2 create mode 100644 protocol/templates/macros.j2 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ce8c7f3 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +__pycache__ +build diff --git a/README.md b/README.md new file mode 100644 index 0000000..e69de29 diff --git a/client/streamer/CMakeLists.txt b/client/streamer/CMakeLists.txt new file mode 100644 index 0000000..81227f2 --- /dev/null +++ b/client/streamer/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 2.8.3) +project(ai_sail_stream_receiver) + +find_package(catkin REQUIRED COMPONENTS + rospy + sensor_msgs +) + +catkin_python_setup() diff --git a/client/streamer/REAME.md b/client/streamer/REAME.md new file mode 100644 index 0000000..5f4e413 --- /dev/null +++ b/client/streamer/REAME.md @@ -0,0 +1,3 @@ +# AI-Deck Streamer [ROS] client + +... diff --git a/client/streamer/launch/ai_deck.launch b/client/streamer/launch/ai_deck.launch new file mode 100644 index 0000000..55cde2d --- /dev/null +++ b/client/streamer/launch/ai_deck.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/client/streamer/package.xml b/client/streamer/package.xml new file mode 100644 index 0000000..00d5693 --- /dev/null +++ b/client/streamer/package.xml @@ -0,0 +1,32 @@ + + + ai_sail_stream_receiver + 0.0.1 + A minimal driver that republish to ROS image streamed via sockets or image pipes by the AI-deck + + + Jerome Guzzi + + + + + MIT + + + + + + + Jerome Guzzi + + catkin + sensor_msgs + rospy + sensor_msgs + rospy + + + + + + diff --git a/client/streamer/script/client.py b/client/streamer/script/client.py new file mode 100644 index 0000000..4c2a26c --- /dev/null +++ b/client/streamer/script/client.py @@ -0,0 +1,187 @@ +# This is an extension of the example provided by bitcraze: +# - added raw format +# - added color +# - added pipe as transport + +import fcntl +import os +import socket +import struct +import threading +from datetime import datetime as dt +from typing import Iterator, List, Tuple, Any +import signal + +import cv2 +import numpy as np + +SOCK_REC_SIZE = 256 +F_SETPIPE_SZ = 1031 + + +class RepeatTimer(threading.Timer): + def run(self) -> None: + while not self.finished.wait(self.interval): # type: ignore + self.function(*self.args, **self.kwargs) # type: ignore + + +class Client: + color: int + encoding: str = "" + shape: Tuple[int, int] = (0, 0) + fps: float = 0.0 + frame: np.ndarray + ts: List[dt] + receive: Iterator[bytes] + ok: bool + + @property + def size(self) -> int: + return self.shape[0] * self.shape[1] + + def run(self) -> Iterator[np.ndarray]: + has_received_header = False + has_received_shape = False + self.encoding = "" + bs = b'' + self.color = 0 + for rx in self.receive: + bs += rx + if not has_received_header: + start_raw = bs.find(b"?IMG") + start_jpeg = bs.find(b"\xff\xd8") + if start_raw >= 0 and (start_jpeg < 0 or start_jpeg > start_raw): + self.encoding = "raw" + bs = bs[start_raw + 4:] + buffer = b'' + has_received_header = True + has_received_shape = False + if start_jpeg >= 0 and (start_raw < 0 or start_raw > start_jpeg): + self.encoding = "jpeg" + has_received_header = True + bs = bs[start_jpeg:] + buffer = b'' + if not has_received_header: + bs = b'' + continue + if self.encoding == 'raw': + if has_received_header and not has_received_shape: + buffer += bs + if len(buffer) >= 6: + w, h, self.color = struct.unpack('HHH', buffer[:6]) + self.shape = (h, w) + buffer = buffer[6:] + has_received_shape = True + bs = b'' + elif has_received_shape: + end = bs.find(b"!IMG") + if end >= 0: + buffer += bs[0: end] + bs = bs[end:] + if(len(buffer) == self.size): + frame = np.frombuffer(buffer, dtype=np.uint8) + frame = frame.reshape(self.shape) + if self.color == 1: + frame = cv2.cvtColor(frame, cv2.COLOR_BAYER_BG2BGR) + elif self.color == 2: + frame = cv2.cvtColor(frame, cv2.COLOR_BAYER_GB2BGR) + elif self.color == 3: + frame = cv2.cvtColor(frame, cv2.COLOR_BAYER_GR2BGR) + elif self.color == 4: + frame = cv2.cvtColor(frame, cv2.COLOR_BAYER_RG2BGR) + self.ts.append(dt.now()) + yield frame + else: + print(f"Expected {self.size} pixels ... got {len(buffer)}") + print(buffer[:8]) + has_received_header = False + has_received_shape = False + else: + buffer += bs + bs = b'' + else: + if has_received_header: + end = bs.find(b"\xff\xd9") + if end >= 0: + buffer += bs[0: end] + # print('buffer len', len(buffer)) + data = np.frombuffer(buffer, dtype=np.uint8) + frame = cv2.imdecode(data, cv2.IMREAD_GRAYSCALE) + if frame is not None: + self.shape = frame.shape + self.ts.append(dt.now()) + yield frame + bs = bs[end:] + has_received_header = False + else: + buffer += bs + bs = b'' + + def update_fps(self) -> None: + if(len(self.ts) == 0): + self.fps = 0.0 + elif len(self.ts) == 1: + self.fps = 1.0 + else: + self.fps = len(self.ts) / (self.ts[-1] - self.ts[0]).total_seconds() + self.ts = self.ts[-2:] + if self.fps or self.shape: + print(f'\r{self.shape[1]} x {self.shape[0]} {self.encoding} ({self.color}) @ {self.fps:.1f} fps', end='') + + def read_socket(self, host: str, port: int = 5000) -> Iterator[bytes]: + while self.ok: + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as c: + print(f"Connecting to socket on {host}:{port}...") + # c.connect((host, port)) + c.settimeout(5) + try: + c.connect((host, port)) + except (socket.timeout, OSError, ConnectionRefusedError): + continue + print("Socket connected, ready to get images") + first = True + while self.ok: + try: + yield c.recv(SOCK_REC_SIZE) + if first: + print("Getting images") + first = False + except socket.timeout: + break + print("Close socket") + + def read_pipe(self, path: str) -> Iterator[bytes]: + print(f"Connecting to pipe at {path} ...") + if not os.path.exists(path): + os.mkfifo(path) + with open(path, 'rb') as c: + fcntl.fcntl(c, F_SETPIPE_SZ, 1000000) + print(f"Connected to pipe at {path}, ready to get images") + first = True + while 1: + # os.read(fd, n) + # Read at most n bytes from file descriptor fd. + yield c.read(SOCK_REC_SIZE) + if first: + print("Getting images") + first = False + + def exit_gracefully(self, signum: Any, frame: Any) -> None: + self.ok = False + + def __init__(self, pipe: str = '', host: str = '', port: int = 5000, + keep_track_of_fps: bool = True) -> None: + self.fps = 0.0 + self.ts = [] + self.ok = True + signal.signal(signal.SIGINT, self.exit_gracefully) + signal.signal(signal.SIGTERM, self.exit_gracefully) + if keep_track_of_fps: + RepeatTimer(1.0, self.update_fps).start() + if pipe: + self.receive = self.read_pipe(pipe) + elif host: + self.receive = self.read_socket(host, port) + else: + self.receive = iter([]) + print("No source") diff --git a/client/streamer/script/opencv_node.py b/client/streamer/script/opencv_node.py new file mode 100644 index 0000000..d98618b --- /dev/null +++ b/client/streamer/script/opencv_node.py @@ -0,0 +1,33 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# This is an extension of the example provided by bitcraze: +# - added raw format +# - added color +# - added pipe as transport + +import argparse +import cv2 +from client import Client +import numpy as np + + +def display(frame: np.ndarray) -> None: + cv2.imshow('camera stream', frame) + cv2.waitKey(10) + + +def main() -> None: + # Args for setting IP/port of AI-deck. Default settings are for when + # AI-deck is in AP mode. + parser = argparse.ArgumentParser(description='Connect to AI-deck streamer') + parser.add_argument("-host", default="192.168.4.1", metavar="host", help="AI-deck host") + parser.add_argument("-port", type=int, default='5000', metavar="port", help="AI-deck port") + parser.add_argument("-pipe", type=str, default='', metavar="pipe", help="Pipe path") + args = parser.parse_args() + client = Client(host=args.host, port=args.port, pipe=args.pipe) + for frame in client.run(): + display(frame) + + +if __name__ == '__main__': + main() diff --git a/client/streamer/script/ros_node.py b/client/streamer/script/ros_node.py new file mode 100644 index 0000000..cc0ff8f --- /dev/null +++ b/client/streamer/script/ros_node.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# This is an extension of the example provided by bitcraze: +# - added raw format +# - added color +# - added pipe as transport + +import rospy +from cv_bridge import CvBridge +from sensor_msgs.msg import Image +from client import Client + + +def main() -> None: + rospy.init_node('himax_driver', anonymous=True) + image_pub = rospy.Publisher("image_raw", Image, queue_size=1) + bridge = CvBridge() + pipe = rospy.get_param('~pipe', '') + host = rospy.get_param('~host', '') + port = rospy.get_param('~port', 5000) + client = Client(host=host, port=port, pipe=pipe, keep_track_of_fps=False) + for seq, frame in enumerate(client.run()): + msg = bridge.cv2_to_imgmsg(frame) + msg.header.stamp = rospy.Time.now() + msg.header.seq = seq + image_pub.publish(msg) + rospy.sleep(0) + + +if __name__ == '__main__': + main() diff --git a/crazyflie/README.md b/crazyflie/README.md new file mode 100644 index 0000000..ca8f674 --- /dev/null +++ b/crazyflie/README.md @@ -0,0 +1,6 @@ +# Crazyflie AI-Deck driver + +- list of improvements +(mostly just uart protocol) + +explain logging diff --git a/crazyflie/aideck.c b/crazyflie/aideck.c new file mode 100644 index 0000000..6999f86 --- /dev/null +++ b/crazyflie/aideck.c @@ -0,0 +1,213 @@ +/* +This file is an extension of the original example provided by Bitcraze + */ +#define DEBUG_MODULE "AIDECK" + +#include +#include +#include + +#include "stm32fxxx.h" +#include "config.h" +#include "console.h" +#include "uart1.h" +#include "debug.h" +#include "deck.h" +#include "FreeRTOS.h" +#include "task.h" +#include "queue.h" +#include +#include +#include +#include "log.h" +#include "param.h" +#include "system.h" +#include "uart1.h" +#include "uart2.h" +#include "timers.h" +#include "worker.h" + +// Include the autogenerate code implementing the protocol +#include "aideck_protocol.c" + +static xTimerHandle timer; +static void configTimer(xTimerHandle timer) +{ + workerSchedule(update_config, NULL); +} + +static bool isInit = false; + +// Uncomment when NINA printout read is desired from console +// #define DEBUG_NINA_PRINT + +#ifdef DEBUG_NINA_PRINT +static void NinaTask(void *param) +{ + systemWaitStart(); + vTaskDelay(M2T(1000)); + DEBUG_PRINT("Starting reading out NINA debugging messages:\n"); + vTaskDelay(M2T(2000)); + + // Pull the reset button to get a clean read out of the data + pinMode(DECK_GPIO_IO4, OUTPUT); + digitalWrite(DECK_GPIO_IO4, LOW); + vTaskDelay(10); + digitalWrite(DECK_GPIO_IO4, HIGH); + pinMode(DECK_GPIO_IO4, INPUT_PULLUP); + + // Read out the byte the NINA sends and immediately send it to the console. + uint8_t byte; + while (1) + { + if (uart2GetDataWithTimout(&byte) == true) + { + consolePutchar(byte); + } + if(in_byte != old_byte) + { + old_byte = in_byte; + uart2SendData(1, &old_byte); + } + } +} +#endif + +// Read n bytes from UART, returning the read size before ev. timing out. +static int read_uart_bytes(int size, uint8_t *buffer) +{ + uint8_t *byte = buffer; + for (int i = 0; i < size; i++) { + if(uart1GetDataWithTimout(byte)) + { + byte++; + } + else + { + return i; + } + } + return size; +} + +// Read UART 1 while looking for structured messages. +// When none are found, print everything to console. +static uint8_t header_buffer[HEADER_LENGTH]; +static void read_uart_message() +{ + uint8_t *byte = header_buffer; + int n = 0; + input_t *input; + input_t *begin = (input_t *) inputs; + input_t *end = begin + INPUT_NUMBER; + for (input = begin; input < end; input++) input->valid = 1; + while(n < HEADER_LENGTH) + { + if(uart1GetDataWithTimout(byte)) + { + int valid = 0; + for (input = begin; input < end; input++) { + if(!(input->valid)) continue; + if(*byte != (input->header)[n]){ + input->valid = 0; + } + else{ + valid = 1; + } + } + n++; + if(valid) + { + // Keep reading + byte++; + continue; + } + } + // forward to console and return; + for (size_t i = 0; i < n; i++) { + consolePutchar(header_buffer[i]); + } + return; + } + // Found message + for (input = begin; input < end; input++) + { + if(input->valid) break; + } + uint8_t buffer[input->size]; + int size = read_uart_bytes(input->size, buffer); + if( size == input->size ) + { + // Call the corresponding callback + input->callback(buffer); + } + else{ + DEBUG_PRINT("Failed to receive message %4s: (%d vs %d bytes received)\n", + input->header, size, input->size); + } +} + +static void Gap8Task(void *param) +{ + systemWaitStart(); + vTaskDelay(M2T(1000)); + + // Pull the reset button to get a clean read out of the data + pinMode(DECK_GPIO_IO4, OUTPUT); + digitalWrite(DECK_GPIO_IO4, LOW); + vTaskDelay(10); + digitalWrite(DECK_GPIO_IO4, HIGH); + pinMode(DECK_GPIO_IO4, INPUT_PULLUP); + DEBUG_PRINT("Starting UART listener\n"); + while (1) + { + read_uart_message(); + } +} + +static void aideckInit(DeckInfo *info) +{ + if (isInit) + return; + + // Intialize the UART for the GAP8 + uart1Init(115200); + // Initialize task for the GAP8 + xTaskCreate(Gap8Task, AI_DECK_GAP_TASK_NAME, AI_DECK_TASK_STACKSIZE, NULL, + AI_DECK_TASK_PRI, NULL); + +#ifdef DEBUG_NINA_PRINT + // Initialize the UART for the NINA + uart2Init(115200); + // Initialize task for the NINA + xTaskCreate(NinaTask, AI_DECK_NINA_TASK_NAME, AI_DECK_TASK_STACKSIZE, NULL, + AI_DECK_TASK_PRI, NULL); +#endif + timer = xTimerCreate( "configTimer", M2T(1000), pdTRUE, NULL, configTimer ); + xTimerStart(timer, 1000); + + isInit = true; +} + +static bool aideckTest() +{ + return true; +} + +static const DeckDriver aideck_deck = { + .vid = 0xBC, + .pid = 0x12, + .name = "bcAI", + + .usedPeriph = 0, + .usedGpio = 0, // FIXME: Edit the used GPIOs + + .init = aideckInit, + .test = aideckTest, +}; + +PARAM_GROUP_START(deck) +PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, bcAIDeck, &isInit) +PARAM_GROUP_STOP(deck) + +DECK_DRIVER(aideck_deck); diff --git a/crazyflie/aideck_protocol.c b/crazyflie/aideck_protocol.c new file mode 100644 index 0000000..8dbe7fb --- /dev/null +++ b/crazyflie/aideck_protocol.c @@ -0,0 +1,142 @@ +/* + This file was autogenerate on 07/09/20 from the following Python config: + + from base import Config, c_uint8, c_uint16 # type: ignore + + header_length = 4 + + + class Camera(Config): + marginTop: c_uint16 + marginRight: c_uint16 + marginBottom: c_uint16 + marginLeft: c_uint16 + format: c_uint8 # noqa + step: c_uint8 + target_value: c_uint8 + + + class Stream(Config): + on: c_uint8 + format: c_uint8 # noqa + transport: c_uint8 + + + configs = [ + Camera(name='camera', group='aideck_HIMAX', header="!CAM"), + Stream(name='stream', group='aideck_stream', header="!STR"), + ] + + inputs = [] + +*/ + +#define HEADER_LENGTH 4 +#define INPUT_NUMBER 2 + +typedef struct { + const char *header; + uint8_t size; + void (*callback)(void *); + bool valid; +} input_t; +// --- config camera + +typedef struct { + uint16_t marginTop; + uint16_t marginRight; + uint16_t marginBottom; + uint16_t marginLeft; + uint8_t format; + uint8_t step; + uint8_t target_value; +} __attribute__((packed)) camera_t; + + +void log_camera(camera_t *value) +{ + DEBUG_PRINT("marginTop=%u, marginRight=%u, marginBottom=%u, marginLeft=%u, format=%u, step=%u, target_value=%u\n", value->marginTop, value->marginRight, value->marginBottom, value->marginLeft, value->format, value->step, value->target_value); +} + +static struct { + camera_t value, dvalue; + const char *header; +} __camera__config = { .header= "!CAM" }; + +static void __camera_cb(void *buffer) +{ + camera_t *value = (camera_t *)buffer; + __camera__config.value = __camera__config.dvalue = *value; + DEBUG_PRINT("GAP has updated camera config\n"); + log_camera(&(__camera__config.value)); +} + +// --- config stream + +typedef struct { + uint8_t on; + uint8_t format; + uint8_t transport; +} __attribute__((packed)) stream_t; + + +void log_stream(stream_t *value) +{ + DEBUG_PRINT("on=%u, format=%u, transport=%u\n", value->on, value->format, value->transport); +} + +static struct { + stream_t value, dvalue; + const char *header; +} __stream__config = { .header= "!STR" }; + +static void __stream_cb(void *buffer) +{ + stream_t *value = (stream_t *)buffer; + __stream__config.value = __stream__config.dvalue = *value; + DEBUG_PRINT("GAP has updated stream config\n"); + log_stream(&(__stream__config.value)); +} +static input_t config.inputs[INPUT_NUMBER] = { + { .header = "!CAM", .callback = __camera_cb, .size = sizeof(camera_t) }, + { .header = "!STR", .callback = __stream_cb, .size = sizeof(stream_t) } +}; +void update_config(void *data) +{ + if(memcmp(&(__camera__config.value), &(__camera__config.dvalue), sizeof(camera_t))) + { + DEBUG_PRINT("Will request GAP to update camera config\n"); + log_camera(&(__camera__config.value)); + uart1SendData(HEADER_LENGTH, (uint8_t *) __camera__config.header); + uart1SendData(sizeof(camera_t), (uint8_t *)&(__camera__config.value)); + __camera__config.dvalue = __camera__config.value; + return; + } + + if(memcmp(&(__stream__config.value), &(__stream__config.dvalue), sizeof(stream_t))) + { + DEBUG_PRINT("Will request GAP to update stream config\n"); + log_stream(&(__stream__config.value)); + uart1SendData(HEADER_LENGTH, (uint8_t *) __stream__config.header); + uart1SendData(sizeof(stream_t), (uint8_t *)&(__stream__config.value)); + __stream__config.dvalue = __stream__config.value; + return; + } + +} + +PARAM_GROUP_START(aideck_HIMAX) +PARAM_ADD(PARAM_UINT16, marginTop, &(__camera__config.value.marginTop)) +PARAM_ADD(PARAM_UINT16, marginRight, &(__camera__config.value.marginRight)) +PARAM_ADD(PARAM_UINT16, marginBottom, &(__camera__config.value.marginBottom)) +PARAM_ADD(PARAM_UINT16, marginLeft, &(__camera__config.value.marginLeft)) +PARAM_ADD(PARAM_UINT8, format, &(__camera__config.value.format)) +PARAM_ADD(PARAM_UINT8, step, &(__camera__config.value.step)) +PARAM_ADD(PARAM_UINT8, target_value, &(__camera__config.value.target_value)) +PARAM_GROUP_STOP(aideck_HIMAX) + +PARAM_GROUP_START(aideck_stream) +PARAM_ADD(PARAM_UINT8, on, &(__stream__config.value.on)) +PARAM_ADD(PARAM_UINT8, format, &(__stream__config.value.format)) +PARAM_ADD(PARAM_UINT8, transport, &(__stream__config.value.transport)) +PARAM_GROUP_STOP(aideck_stream) diff --git a/gap/streamer/Makefile b/gap/streamer/Makefile new file mode 100644 index 0000000..635c3bd --- /dev/null +++ b/gap/streamer/Makefile @@ -0,0 +1,8 @@ +APP = test +APP_SRCS += main.c uart.c camera.c led.c nina.c io.c +APP_CFLAGS += -O3 -g +APP_LDFLAGS += -lgaptools -lgaplib + +RUNNER_CONFIG = $(CURDIR)/config.ini + +include $(RULES_DIR)/pmsis_rules.mk diff --git a/gap/streamer/REAME.md b/gap/streamer/REAME.md new file mode 100644 index 0000000..ae4c5a9 --- /dev/null +++ b/gap/streamer/REAME.md @@ -0,0 +1,8 @@ +# GAP Image streamer + +- change image config dynamically + [-> list] + +- change streaming config dynamically + +- log diff --git a/gap/streamer/camera.c b/gap/streamer/camera.c new file mode 100644 index 0000000..6205d4d --- /dev/null +++ b/gap/streamer/camera.c @@ -0,0 +1,669 @@ +#include "bsp/camera.h" +#include "bsp/camera/himax.h" +#include "common.h" +#include "camera.h" +#include "uart.h" +#include "io.h" +#include "led.h" +#include "nina.h" +#include "tools/frame_streamer.h" + +// Apparently binning cannot be changed once the camera is running. +// [QQVGA] window is fine instead + +#define HIMAX_MODEL_ID_H 0x0000 +#define HIMAX_MODEL_ID_L 0x0001 +#define HIMAX_FRAME_COUNT 0x0005 +#define HIMAX_PIXEL_ORDER 0x0006 +// R&W registers +// Sensor mode control +#define HIMAX_MODE_SELECT 0x0100 +#define HIMAX_IMG_ORIENTATION 0x0101 +#define HIMAX_SW_RESET 0x0103 +#define HIMAX_GRP_PARAM_HOLD 0x0104 +// Sensor exposure gain control +#define HIMAX_INTEGRATION_H 0x0202 +#define HIMAX_INTEGRATION_L 0x0203 +#define HIMAX_ANALOG_GAIN 0x0205 +#define HIMAX_DIGITAL_GAIN_H 0x020E +#define HIMAX_DIGITAL_GAIN_L 0x020F +// Frame timing control +#define HIMAX_FRAME_LEN_LINES_H 0x0340 +#define HIMAX_FRAME_LEN_LINES_L 0x0341 +#define HIMAX_LINE_LEN_PCK_H 0x0342 +#define HIMAX_LINE_LEN_PCK_L 0x0343 +// Binning mode control +#define HIMAX_READOUT_X 0x0383 +#define HIMAX_READOUT_Y 0x0387 +#define HIMAX_BINNING_MODE 0x0390 +// Test pattern control +#define HIMAX_TEST_PATTERN_MODE 0x0601 +// Black level control +#define HIMAX_BLC_CFG 0x1000 +#define HIMAX_BLC_TGT 0x1003 +#define HIMAX_BLI_EN 0x1006 +#define HIMAX_BLC2_TGT 0x1007 +// Sensor reserved +#define HIMAX_DPC_CTRL 0x1008 +#define HIMAX_SINGLE_THR_HOT 0x100B +#define HIMAX_SINGLE_THR_COLD 0x100C +// VSYNC,HSYNC and pixel shift register +#define HIMAX_VSYNC_HSYNC_PIXEL_SHIFT_EN 0x1012 +// Automatic exposure gain control +#define HIMAX_AE_CTRL 0x2100 +#define HIMAX_AE_TARGET_MEAN 0x2101 +#define HIMAX_AE_MIN_MEAN 0x2102 +#define HIMAX_CONVERGE_IN_TH 0x2103 +#define HIMAX_CONVERGE_OUT_TH 0x2104 +#define HIMAX_MAX_INTG_H 0x2105 +#define HIMAX_MAX_INTG_L 0x2106 +#define HIMAX_MIN_INTG 0x2107 +#define HIMAX_MAX_AGAIN_FULL 0x2108 +#define HIMAX_MAX_AGAIN_BIN2 0x2109 +#define HIMAX_MIN_AGAIN 0x210A +#define HIMAX_MAX_DGAIN 0x210B +#define HIMAX_MIN_DGAIN 0x210C +#define HIMAX_DAMPING_FACTOR 0x210D +#define HIMAX_FS_CTRL 0x210E +#define HIMAX_FS_60HZ_H 0x210F +#define HIMAX_FS_60HZ_L 0x2110 +#define HIMAX_FS_50HZ_H 0x2111 +#define HIMAX_FS_50HZ_L 0x2112 +#define HIMAX_FS_HYST_TH 0x2113 +// Motion detection control +#define HIMAX_MD_CTRL 0x2150 +#define HIMAX_I2C_CLEAR 0x2153 +#define HIMAX_WMEAN_DIFF_TH_H 0x2155 +#define HIMAX_WMEAN_DIFF_TH_M 0x2156 +#define HIMAX_WMEAN_DIFF_TH_L 0x2157 +#define HIMAX_MD_THH 0x2158 +#define HIMAX_MD_THM1 0x2159 +#define HIMAX_MD_THM2 0x215A +#define HIMAX_MD_THL 0x215B +// Sensor timing control +#define HIMAX_QVGA_WIN_EN 0x3010 +#define HIMAX_SIX_BIT_MODE_EN 0x3011 +#define HIMAX_PMU_AUTOSLEEP_FRAMECNT 0x3020 +#define HIMAX_ADVANCE_VSYNC 0x3022 +#define HIMAX_ADVANCE_HSYNC 0x3023 +#define HIMAX_EARLY_GAIN 0x3035 +// IO and clock control +#define HIMAX_BIT_CONTROL 0x3059 +#define HIMAX_OSC_CLK_DIV 0x3060 +#define HIMAX_ANA_Register_11 0x3061 +#define HIMAX_IO_DRIVE_STR 0x3062 +#define HIMAX_IO_DRIVE_STR2 0x3063 +#define HIMAX_ANA_Register_14 0x3064 +#define HIMAX_OUTPUT_PIN_STATUS_CONTROL 0x3065 +#define HIMAX_ANA_Register_17 0x3067 +#define HIMAX_PCLK_POLARITY 0x3068 + + +#define MAX_IMAGE_SIZE (324 * 324) + +static struct pi_device camera; +static int imgTransferDone = 0; +static pi_task_t task1; +static PI_L2 unsigned char *L2_image; +static int should_crop = 1; +static int camera_image_width; +static int camera_image_height; +static int cropped_image_width; +static int cropped_image_height; +static int width; +static int height; +static unsigned int image_size_bytes, input_size_bytes; +static frame_streamer_t *streamer = NULL; +static volatile int stream_done; +static pi_buffer_t buffer; + +// FRAME_STREAMER_FORMAT_RAW +// FRAME_STREAMER_FORMAT_JPEG + +// Let's limit to RAW streams at first, so we don't have to dealloc the jpegstreamer ... + +static int set_size(camera_config_t); +static void set_format(uint8_t format); +static void set_target_value(uint8_t value); +static camera_config_t camera_config; +static camera_config_t desired_camera_config; +static volatile int _should_set_camera_config = 0; +static stream_config_t stream_config = {.on=0, .format=FRAME_STREAMER_FORMAT_RAW}; +static void update_streamer(); + +#ifdef FIX_GAINS +static int n = 0; +#endif + +//private from https://github.com/GreenWaves-Technologies/gap_sdk/blob/master/tools/gap_tools/frame_streamer/frame_streamer.c + +#include "bsp/transport.h" + +typedef struct +{ + struct pi_transport_header header; + frame_streamer_open_req_t req; +} frame_streamer_open_req_full_t; + + +struct frame_streamer_s { + struct pi_device *transport; + frame_streamer_format_e format; + int channel; + struct pi_transport_header header; + frame_streamer_open_req_full_t req; + void *jpeg; + unsigned int height; + unsigned int width; +}; + + +int init_streamer(struct pi_device *transport, uint8_t format) +{ + struct frame_streamer_conf frame_streamer_conf; + frame_streamer_conf_init(&frame_streamer_conf); + frame_streamer_conf.transport = transport; + frame_streamer_conf.format = format; + frame_streamer_conf.width = width; + frame_streamer_conf.height = height; + frame_streamer_conf.depth = 1; + frame_streamer_conf.name = "camera"; + streamer = frame_streamer_open(&frame_streamer_conf); + if (streamer){ + pi_buffer_init(&buffer, PI_BUFFER_TYPE_L2, L2_image); + update_streamer(); + return 0; + } + LOG("Failed to open streamer\n"); + return -1; +} + + +void set_stream_config(stream_config_t *config) +{ + if(stream_config.on && !config->on) + { + stream_config.on = 0; + // put_nina_to_sleep(); + } + else if(!stream_config.on && config->on) + { + if(!streamer) + { + struct pi_device *transport = NULL; + if(config->transport == TRANSPORT_WIFI) + { + // wake_up_nina(); + transport = open_wifi(); + } + else if(config->transport == TRANSPORT_PIPE) + { + transport = open_pipe("/tmp/image_pipe"); + } + if(transport && init_streamer(transport, config->format) == 0) + { + stream_config.on = 1; + } + } + else{ + stream_config.on = 1; + } + } + // Not allowed to change format of an initialized streamer + if(!streamer) + { + if(config->format < 2) + stream_config.format = config->format; + if(config->transport < 2) + stream_config.transport = config->transport; + } + send_stream_config(&stream_config); + LOG("Has set stream to on = %d, format = %d, transport = %d\n", + stream_config.on, stream_config.format, stream_config.transport); +} + +void set_camera_config(camera_config_t *_config) +{ + desired_camera_config = *_config; + _should_set_camera_config = 1; +} + +int _set_camera_config(camera_config_t *_config) +{ + int error = 0; + if(streamer && stream_config.format == FRAME_STREAMER_FORMAT_JPEG) + { + // Not allowed to change buffer size! + } + else + { + error = set_size(*_config); + if(!error) + { + camera_config.top = _config->top; + camera_config.bottom = _config->bottom; + camera_config.right = _config->right; + camera_config.left = _config->left; + camera_config.step = _config->step; + } + } + if(!error && camera_config.format != _config->format && _config->format >=0 && _config->format < 4) + { + set_format(_config->format); + camera_config.format = _config->format; + } + if(!error && camera_config.target_value != _config->target_value) + { + set_target_value(_config->target_value); + camera_config.target_value = _config->target_value; + } + // camera_config = *_config; + update_streamer(); + send_camera_config(&camera_config); + LOG("Has set camera config to margin = [%d %d %d %d], step = %d, format = %d, target = %d\n", + camera_config.top, camera_config.right, camera_config.bottom, camera_config.left, + camera_config.step, camera_config.format, camera_config.target_value); + if(error) + { + uart_flush_rx(); + } + return error; +} + +static void update_streamer() +{ + pi_buffer_set_format(&buffer, width, height, 1, PI_BUFFER_FORMAT_GRAY); + // LOG("Set buffer %d x %d = %d\n", buffer.width, buffer.height, pi_buffer_size(&buffer)); + // I cannot, as the struct is private! but maybe I don't need it, at least for RAW + streamer->width = width; + streamer->height = height; + if(stream_config.format == FRAME_STREAMER_FORMAT_JPEG) + return; + // Let us use info to tell NINA the width and color format of the frame! + // 0: GS, 1: BG, 2: GB, 3: GR, 4: RG + uint32_t color = 0; // Grazyscale + if((camera_config.format == FULL || camera_config.format == QVGA) && camera_config.step == 1) + { + // BAYER + color = (camera_config.top % 2 << 1) + (camera_config.left % 2) + 1; + } + // bytes 1-2: width, bytes 3-4: color + streamer->header.info = width + (color << 16); +} + +static int32_t pi_camera_reg_get16(struct pi_device *device, uint32_t reg_addr_l, uint32_t reg_addr_h, uint16_t *value) +{ + uint8_t *v = (uint8_t *) value; + pi_camera_reg_get(&camera, reg_addr_l, v); + return pi_camera_reg_get(&camera, reg_addr_h, v+1); +} + +static int32_t pi_camera_reg_set16(struct pi_device *device, uint32_t reg_addr_l, uint32_t reg_addr_h, uint16_t *value) +{ + uint8_t *v = (uint8_t *) value; + pi_camera_reg_set(&camera, reg_addr_l, v); + return pi_camera_reg_get(&camera, reg_addr_h, v+1); +} + +void static fix_gains(); +static void enqueue_capture(); + +void start_camera_loop() +{ + enqueue_capture(NULL); +} + +static void streamer_handler(void *task) +{ + if(task) + { + pi_task_push((pi_task_t *)task); + } + else{ + enqueue_capture(NULL); + } +} + +static void end_of_frame(void *task) { + + pi_camera_control(&camera, PI_CAMERA_CMD_STOP, 0); + if(_should_set_camera_config) + { + // set_format(config.format); + _set_camera_config(&desired_camera_config); + _should_set_camera_config = 0; + } + + // LOG("New frame %03d %03d %03d %03d ...\n", L2_image[0], L2_image[1], L2_image[2], L2_image[3]); + + toggle_led(); + #ifdef FIX_GAINS + fix_gains(); + #endif + + if(should_crop) + { + unsigned char * origin = (unsigned char *) L2_image; + unsigned char * ptr_crop = (unsigned char *) L2_image; + int init_offset = camera_image_width * camera_config.top + camera_config.left; + int outid = 0; + int step = camera_config.step; + for(int i=0; i camera_image_height) + { + LOG_ERROR("Wrong image size top %d, bottom %d, height = %d\n", + c.top, c.bottom, camera_image_height); + return 1; + } + if(c.left < 0 || c.right <0 || (c.left + c.right) > camera_image_width) + { + LOG_ERROR("Wrong image size left %d, right %d, width = %d\n", + c.left, c.right, camera_image_width); + return 1; + } + if(c.step <= 0 || c.step >= camera_image_width || c.step >= camera_image_height) + { + LOG_ERROR("Wrong step size %d, width = %d, height = %d\n", + c.step, camera_image_width, camera_image_height); + return 1; + } + cropped_image_width = camera_image_width - c.left - c.right; + cropped_image_height = camera_image_height - c.bottom - c.top; + should_crop = c.left || c.right || c.bottom || c.top; + width = cropped_image_width / c.step; + height = cropped_image_height / c.step; + // input_size_bytes = CAM_DSMPL_W * CAM_DSMPL_H * sizeof(unsigned char); + image_size_bytes = camera_image_width * camera_image_height * sizeof(unsigned char); + input_size_bytes = width * height * sizeof(unsigned char); + LOG("Will capture %d x %d frames, crop (%d) with margins of %d %d %d %d pixels, " + "sample every %d pixels, to finally get %d x %d frames\n", + camera_image_width, camera_image_height, should_crop, c.top, c.right, c.bottom, c.left, + c.step, width, height); + return 0; +} + +int init_camera(camera_config_t _camera_config, stream_config_t _stream_config) +{ + struct pi_himax_conf cam_conf; + pi_himax_conf_init(&cam_conf); + + cam_conf.format = PI_CAMERA_QVGA; + + pi_open_from_conf(&camera, &cam_conf); + + if (pi_camera_open(&camera)) + { + LOG_ERROR("Failed to open Himax camera\n"); + return -1; + } + + if(_set_camera_config(&_camera_config)) + return -1; + configure_camera_registers(); + + // wait the camera to setup + // if(rt_platform() == ARCHI_PLATFORM_BOARD) + #ifndef GVSOC + pi_time_wait_us(1000000); + #endif + + // allocate the memory of L2 for the image buffer + L2_image = pi_l2_malloc(MAX_IMAGE_SIZE); + if (L2_image == NULL){ + LOG_ERROR("Failed to alloc L2 memeory of size %d\n", image_size_bytes) + return -1; + } + // LOG("L2 Image alloc\t%dB\t@ 0x%08x:\t%s", image_size_bytes, (unsigned int) L2_image, L2_image?"Ok":"Failed"); + + set_stream_config(&_stream_config); + LOG("Initialized Himax camera\n"); + return 0; +} + +void close_camera() +{ + //TODO (Jerome, GAPSDK): should free streamer + pi_camera_close(&camera); + pi_l2_free(L2_image, image_size_bytes); + LOG("Closed camera\n"); +} + +#ifdef FIX_GAINS +static uint8_t a_gain[FIX_GAINS]; +static uint16_t integration[FIX_GAINS]; +static uint16_t d_gain[FIX_GAINS]; + +static uint16_t median16(int n, uint16_t x[]) { + uint16_t temp; + int i, j; + for(i=0; i=FIX_GAINS) + { + #ifdef DEBUG_AE + uint8_t value8; + uint16_t value16; + pi_camera_reg_get16(&camera, HIMAX_INTEGRATION_L, HIMAX_INTEGRATION_H, &value16); + LOG("integration time: %d\n", value16); + pi_camera_reg_get(&camera, HIMAX_ANALOG_GAIN, &value8); + LOG("analog gain: %d\n", value8); + pi_camera_reg_get16(&camera, HIMAX_DIGITAL_GAIN_L, HIMAX_DIGITAL_GAIN_H, &value16); + LOG("digital gain %d\n", value16); + #endif + return; + } + + // Force the control to be on + uint8_t value8; + if(n < 2) + { + value8 = 0x01; + pi_camera_reg_set(&camera, HIMAX_AE_CTRL, &value8); + } + + pi_camera_reg_get16(&camera, HIMAX_INTEGRATION_L, HIMAX_INTEGRATION_H, integration+n); + pi_camera_reg_get(&camera, HIMAX_ANALOG_GAIN, a_gain+n); + pi_camera_reg_get16(&camera, HIMAX_DIGITAL_GAIN_L, HIMAX_DIGITAL_GAIN_H, d_gain+n); + + n++; + + if(n==FIX_GAINS) + { + uint16_t m_integration = median16(FIX_GAINS, integration); + uint16_t m_dgain = median16(FIX_GAINS, d_gain); + uint8_t m_again = median8(FIX_GAINS, a_gain); + + #ifdef DEBUG_AE + LOG("---- AE Cumulated values ----\n"); + LOG("integration time:"); + for (size_t i = 0; i < FIX_GAINS; i++) { + LOG("%d, ", integration[i]); + } + LOG("=> median %d \n", m_integration); + LOG("analog gain: "); + for (size_t i = 0; i < FIX_GAINS; i++) { + LOG("%d, ", a_gain[i]); + } + LOG("=> median %d\n", m_again); + LOG("digital gain: "); + for (size_t i = 0; i < FIX_GAINS; i++) { + LOG("%d, ", d_gain[i]); + } + LOG("=> median %d\n", m_dgain); + LOG("-----------------------------\n"); + #endif + + LOG("Disable AE control after %d steps and fix registers:\n" + "\t\t- INTEGRATION: %d\n" + "\t\t- DIGITAL GAIN: %d\n" + "\t\t- ANALOG GAIN: %d\n", FIX_GAINS, m_integration, m_dgain, m_again); + + uint8_t value8 = 0x00; + pi_camera_reg_set(&camera, HIMAX_AE_CTRL, &value8); + pi_camera_reg_set16(&camera, HIMAX_INTEGRATION_L, HIMAX_INTEGRATION_H, &m_integration); + pi_camera_reg_set16(&camera, HIMAX_DIGITAL_GAIN_L, HIMAX_DIGITAL_GAIN_H, &m_dgain); + pi_camera_reg_set(&camera, HIMAX_ANALOG_GAIN, &m_again); + } +} + +#endif diff --git a/gap/streamer/camera.h b/gap/streamer/camera.h new file mode 100644 index 0000000..08699fd --- /dev/null +++ b/gap/streamer/camera.h @@ -0,0 +1,40 @@ +#ifndef __CAMERA_H__ +#define __CAMERA_H__ + +#include "pmsis.h" + +#define QVGA 0 // 324 x 244 +#define QQVGA 1 // 162 x 122 +#define FULL 2 // 324 x 324 +#define HALF 3 // 162 x 162 + +// #define DEBUG_AE +// needed also to be able to change binning while running +#define FIX_GAINS 10 +#define TRANSPORT_WIFI 0 +#define TRANSPORT_PIPE 1 + +typedef struct StreamConfig +{ + uint8_t on; + uint8_t format; // 0 RAW, 1 JPEG + uint8_t transport; // 0 WIFI, 1 PIPE +} __attribute__((packed)) stream_config_t; + +typedef struct CameraConfig +{ + uint16_t top; + uint16_t right; + uint16_t bottom; + uint16_t left; + uint8_t format; + uint8_t step; + uint8_t target_value; +} __attribute__((packed)) camera_config_t; + +int init_camera(camera_config_t, stream_config_t); +unsigned char* grab_camera_frame(); +void close_camera(); +void start_camera_loop(); + +#endif // __CAMERA_H__ diff --git a/gap/streamer/common.h b/gap/streamer/common.h new file mode 100644 index 0000000..33179a6 --- /dev/null +++ b/gap/streamer/common.h @@ -0,0 +1,39 @@ +#ifndef __COMMON_H__ +#define __COMMON_H__ + +#include "pmsis.h" +#include "uart.h" + +#define VERBOSE + +#define LOG_UART +#define LOG_JTAG + +#define ABS(x) ( (x<0) ? (-x) : x ) +#define MAX(x,y) ( (x>y) ? x : y ) +#define MIN(x,y) ( (x * + * Date: 10.04.2019 * + *----------------------------------------------------------------------------*/ + +#ifndef PULP_FRONTNET_CONFIG +#define PULP_FRONTNET_CONFIG + +#include "camera.h" + +#define TARGET_INTENSITY 112 // = 0x70 + +#ifdef GVSOC +// Targeting GVSOC ... +// The image used to feed the camera is 324 x 244 +#define MARGIN_TOP 74 +#define MARGIN_BOTTOM 74 +#define MARGIN_LEFT 82 +#define MARGIN_RIGHT 82 +#define DSMPL_RATIO 1 // Down-sampling ratio (after cropping) +#define FORMAT QVGA + +#else + +#define MARGIN_TOP 33 +#define MARGIN_BOTTOM 33 +#define MARGIN_LEFT 0 +#define MARGIN_RIGHT 2 +#define DSMPL_RATIO 1 // Down-sampling ratio (after cropping) +#define FORMAT HALF + +#endif diff --git a/gap/streamer/config.ini b/gap/streamer/config.ini new file mode 100644 index 0000000..02d0e86 --- /dev/null +++ b/gap/streamer/config.ini @@ -0,0 +1,10 @@ +[board.devices.nina_w10] +include = devices/nina_w10.json +interface = spim1 +cs = 0 +gpio_ready = gpio18 +config.gpio_ready = gpio18 + + +[config] +runner.peripherals=true \ No newline at end of file diff --git a/gap/streamer/io.c b/gap/streamer/io.c new file mode 100644 index 0000000..50a679c --- /dev/null +++ b/gap/streamer/io.c @@ -0,0 +1,223 @@ +/* + * Copyright 2019 GreenWaves Technologies, SAS + * + * Licensed 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 "io.h" +#include "common.h" +#include "bsp/fs.h" +#include "bsp/transport.h" +#include "bsp/bsp.h" + +#define CHUNK_SIZE 8192 +static uint8_t jpeg_header[306]; +static uint8_t jpeg_footer[2]; +typedef enum +{ + HEADER, /* Next packet is header */ + FOOTER, /* Next packet is footer */ + DATA /* Next packet is data */ +} ImageState_t; +typedef enum +{ + RAW, + JPEG, + UNKNOWN +} image_format_t; +static ImageState_t state = HEADER; +static image_format_t format = UNKNOWN; +#define RAW_HEADER "?IMG" +uint16_t raw_header[5]; +const char raw_footer[] = "!IMG"; +static bool new_frame = 0; +typedef struct pi_transport_header header_t; + +static int send_fifo(void *fifo, const char*buffer, size_t size) +{ + int ret = 0; + int steps = size / CHUNK_SIZE; + for(int i=0; i 95) + { + format = (image_format_t) buffer32[3]; + state = HEADER; + // printf("Got streaming request for %s with format %d == RAW, %d == JPEG\n", + // buffer+32, format == RAW, format == JPEG); + } + break; + case 1024: // HEADER + header = (header_t *) buffer; + // printf("Header %d %d %d\n", header->channel, header->packet_size, header->info); + if(header->info <= 1 && format == JPEG) + { + new_frame = (header->info == 1); + // printf("new_frame: %d\n", new_frame); + } + else if (header->info > 1 && format == RAW){ + // RAW + raw_header[2] = (int16_t) (0xFFFF & header->info); + raw_header[3] = (int16_t) (header->packet_size / raw_header[2]); + raw_header[4] = (int16_t) (header->info >> 16); + // printf("Received first RAW HEADER: shape (%d, %d), color %d\n", + // raw_header[2], raw_header[3], raw_header[4]); + //printf("RAW HEADER 10 %04x %04x %04x...\n", raw_header[0], raw_header[1], raw_header[2]); + send_fifo(fifo, (char *)raw_header, 10); + } + else{ + LOG_ERROR("Unexpected header %d %d %d\n", header->channel, header->packet_size, header->info); + } + break; + default: + // printf("Should read image %d \n", size); + if(format == RAW) + { + send_fifo(fifo, (char *)buffer, size); + send_fifo(fifo, raw_footer, 4); + } + else if(format == JPEG) + { + switch (state) { + case HEADER: + // printf("Setting JPEG header of %i bytes", size); + memcpy(&jpeg_header, (uint8_t*) buffer, sizeof(jpeg_header)); + send_fifo(fifo, (const char*) &jpeg_header, sizeof(jpeg_header) ); + state = FOOTER; + break; + case FOOTER: + // printf("Setting JPEG footer of %i bytes", size); + memcpy(&jpeg_footer, (uint8_t*) buffer, sizeof(jpeg_footer)); + state = DATA; + break; + case DATA: + send_fifo(fifo, (const char*) buffer, size); + if (new_frame) { + send_fifo(fifo, (const char*) &jpeg_footer, sizeof(jpeg_footer) ); + send_fifo(fifo, (const char*) &jpeg_header, sizeof(jpeg_header) ); + } + break; + } + } + break; + } +} + +int __pipe_open(struct pi_device *device) +{ + if(device->data) return 0; + return -1; +} + +int __pipe_connect(struct pi_device *device, int channel, void (*rcv_callback(void *arg, struct pi_transport_header)), void *arg) +{ + return 0; +} + +int __pipe_send_async(struct pi_device *device, void *buffer, size_t size, pi_task_t *task) +{ + // printf("pipe api send async\n"); + void *fifo = device->data; + if(!fifo) return -1; + // at first a sync implementation to check the interface! + handle_image_stream(fifo, (uint8_t *)buffer, (int32_t) size); + // printf("-- pipe api send async\n"); + pi_task_push(task); + return 0; +} + +void __pipe_close(struct pi_device *device) +{ + // printf("pipe api close\n"); +} + +int __pipe_receive_async(struct pi_device *device, void *buffer, size_t size, pi_task_t *task) +{ + // printf("pipe api receive async\n"); + return 0; +} + + +static pi_transport_api_t pipe_api = +{ + .open = &__pipe_open, + .connect = &__pipe_connect, + .send_async = &__pipe_send_async, + .receive_async = &__pipe_receive_async, + .close = &__pipe_close, +}; + +static struct pi_transport_conf pipe_transport_conf = {.api = &pipe_api}; +static struct pi_device fs; +static struct pi_device pipe = {.data=NULL, .config=&pipe_transport_conf}; + +static int init_fs() +{ + struct pi_fs_conf conf; + pi_fs_conf_init(&conf); + conf.type = PI_FS_HOST; + pi_open_from_conf(&fs, &conf); + return pi_fs_mount(&fs); +} + +static void close_fs() +{ + pi_fs_unmount(&fs); +} + +struct pi_device * open_pipe(char *path) +{ + if(pipe.data) return NULL; + init_fs(); + void *fifo = pi_fs_open(&fs, path, PI_FS_FLAGS_WRITE); + if(!fifo) + { + LOG_ERROR("Failed to open pipe\n"); + return NULL; + } + pipe.data = fifo; + pi_transport_open(&pipe); + memcpy(raw_header, RAW_HEADER, 4); + LOG("Opened pipe\n"); + return &pipe; +} + +void close_pipe() +{ + if(pipe.data) return; + pi_fs_close(pipe.data); + close_fs(); + LOG("Closed pipe\n"); +} + + +// void read_file(char *path, unsigned char *line, unsigned int length) +// { +// void *file = pi_fs_open(&fs, path, PI_FS_FLAGS_READ); +// pi_fs_read(file, line, length); +// pi_fs_close(file); +// } diff --git a/gap/streamer/io.h b/gap/streamer/io.h new file mode 100644 index 0000000..e1af684 --- /dev/null +++ b/gap/streamer/io.h @@ -0,0 +1,17 @@ +/* + */ + + +#ifndef __IO_H__ +#define __IO_H__ + + +// int init_fs(); +// void close_fs(); +struct pi_device * open_pipe(char *path); +void close_pipe(); + +// int PushImageToFifo(void *fifo, unsigned int W, unsigned int H, unsigned char *OutBuffer); +// void read_file(char *path, unsigned char *line, unsigned int length); + +#endif //__IO_H__ diff --git a/gap/streamer/led.c b/gap/streamer/led.c new file mode 100644 index 0000000..d6b9a8d --- /dev/null +++ b/gap/streamer/led.c @@ -0,0 +1,29 @@ +#include "pmsis.h" +#include "led.h" +#include "common.h" + +#define LED_GPIO 2 + +static struct pi_device gpio_device; +static uint32_t led_val = 0; + +void init_led() +{ + pi_gpio_pin_configure(&gpio_device, LED_GPIO, PI_GPIO_OUTPUT); + LOG("Initialized LED\n"); +} + +static void update_led(){ + pi_gpio_pin_write(&gpio_device, LED_GPIO, led_val); +} + +void set_led(int value){ + led_val = (uint32_t) value; + pi_gpio_pin_write(&gpio_device, LED_GPIO, led_val); +} + +void toggle_led() +{ + led_val ^= 1; + update_led(); +} diff --git a/gap/streamer/led.h b/gap/streamer/led.h new file mode 100644 index 0000000..d5d78d8 --- /dev/null +++ b/gap/streamer/led.h @@ -0,0 +1,8 @@ +#ifndef __LED_H__ +#define __LED_H__ + +void init_led(); +void set_led(int); +void toggle_led(); + +#endif // __LED_H__ diff --git a/gap/streamer/main.c b/gap/streamer/main.c new file mode 100644 index 0000000..9a16838 --- /dev/null +++ b/gap/streamer/main.c @@ -0,0 +1,103 @@ +#include "pmsis.h" +#include "stdio.h" +#include "led.h" +#include "camera.h" +#include "uart.h" +#include "nina.h" +#include "common.h" +#include "bsp/bsp.h" + +// static struct pi_device wifi; + +// static int open_wifi(struct pi_device *device) +// { +// struct pi_nina_w10_conf nina_conf; +// pi_nina_w10_conf_init(&nina_conf); +// // cannot be left empty :-/ !!! +// nina_conf.ssid = "Hasse"; +// nina_conf.passwd = "AngelicasIphone"; +// nina_conf.ip_addr = "192.168.1.112"; +// +// nina_conf.port = 5555; +// +// // nina_conf.ip_addr = "192.168.201.40"; +// pi_open_from_conf(device, &nina_conf); +// if (pi_transport_open(device)) +// return -1; +// return 0; +// } + +// static pi_task_t timer_task; +// +// static void timer_handle(void *arg) +// { +// // toggle_led(); +// pi_task_push_delayed_us(pi_task_callback(&timer_task, timer_handle, NULL), 500000); +// } + +int main() +{ + bsp_init(); + // with the default PADS config UART RX won't work. + // it would be better to call the specific function ... + // pi_pad_set_function(...) but I don't know the arguments + uint32_t pads_value[] = {0x00055500, 0x0f000000, 0x003fcfff, 0x00000000}; + pi_pad_init(pads_value); + if(init_uart(1)) + { + pmsis_exit(-1); + } + // printf("Will init_nina\n"); + init_nina(0); + // printf("Did init_nina\n"); + // printf("Will init_camera\n"); + camera_config_t camera_config = { + .top = 33, + .right = 2, + .bottom = 33, + .left = 0, + .format = HALF, + .step = 1, + .target_value = 0x60 + }; + stream_config_t stream_config = { + .on = 0, + .format = 0, + .transport = TRANSPORT_WIFI + }; + if(init_camera(camera_config, stream_config)) + { + pmsis_exit(-1); + } + // printf("Did init_camera\n"); + // printf("Will open_wifi\n"); + // if (open_wifi(&wifi)) + // { + // printf("Failed to open wifi\n"); + // return -1; + // } + // printf("Did open_wifi\n"); + // printf("Will init_streamer\n"); + // if(init_streamer(&wifi)) + // { + // printf("Failed to open stream\n"); + // pmsis_exit(-1); + // } + // printf("Did init_streamer\n"); + // pi_time_wait_us(100000); + init_led(); + // pi_task_push_delayed_us(pi_task_callback(&timer_task, timer_handle, NULL), 500000); + // uint8_t byte = 1; + // int i = pi_transport_send(&wifi, &byte, 1); + // printf("P1 %d\n", i); + start_camera_loop(); + LOG("Starting main loop\n"); + while(1) + { + pi_yield(); + } + close_camera(); + close_nina(); + close_uart(); + return 0; +} diff --git a/gap/streamer/nina.c b/gap/streamer/nina.c new file mode 100644 index 0000000..f716842 --- /dev/null +++ b/gap/streamer/nina.c @@ -0,0 +1,81 @@ +#include "pmsis.h" +#include "nina.h" +#include "common.h" +#include "bsp/transport/nina_w10.h" + +#define NINA_GPIO_OUT PI_GPIO_A18_PAD_32_A13 + + +static struct pi_device gpio_device; +static struct pi_device wifi; + +static void wake_up_nina() +{ + // printf("will wake_up_nina\n"); + pi_gpio_pin_write(&gpio_device, NINA_GPIO_OUT, 1); + // printf("did wake_up_nina\n"); +} + +static void put_nina_to_sleep() +{ + // printf("will put_nina_to_sleep\n"); + pi_gpio_pin_write(&gpio_device, NINA_GPIO_OUT, 0); + // printf("did put_nina_to_sleep\n"); +} + +struct pi_device * open_wifi() +{ + wake_up_nina(); + + struct pi_nina_w10_conf nina_conf; + pi_nina_w10_conf_init(&nina_conf); + // cannot be left empty :-/ !!! + nina_conf.ssid = "Hasse"; + nina_conf.passwd = "AngelicasIphone"; + nina_conf.ip_addr = "192.168.1.112"; + nina_conf.port = 5555; + // nina_conf.ip_addr = "192.168.201.40"; + // printf("Will pi_open_from_conf\n"); + pi_open_from_conf(&wifi, &nina_conf); + // printf("Did pi_open_from_conf\n"); + // printf("Will pi_open_from_conf\n"); + if (pi_transport_open(&wifi)) + { + LOG_ERROR("Failed to open wifi\n"); + return NULL; + } + LOG("Opened wifi\n"); + // printf("Did pi_open_from_conf\n"); + return &wifi; +} + + +void init_nina(int active) +{ + LOG("Initialized NINA (GPIO)\n"); + pi_gpio_pin_configure(&gpio_device, NINA_GPIO_OUT, PI_GPIO_OUTPUT); + pi_gpio_pin_write(&gpio_device, NINA_GPIO_OUT, active); +} + +void close_nina() +{ + LOG("Closed wifi\n"); + // Disabled to facilitate developement + // put_nina_to_sleep(); +} + +// printf("Will open_wifi\n"); +// if (open_wifi(&wifi)) +// { +// printf("Failed to open wifi\n"); +// return -1; +// } +// printf("Did open_wifi\n"); +// printf("Will init_streamer\n"); +// if(init_streamer(&wifi)) +// { +// printf("Failed to open stream\n"); +// pmsis_exit(-1); +// } +// printf("Did init_streamer\n"); +// pi_time_wait_us(100000); diff --git a/gap/streamer/nina.h b/gap/streamer/nina.h new file mode 100644 index 0000000..5cb3ca0 --- /dev/null +++ b/gap/streamer/nina.h @@ -0,0 +1,10 @@ +#ifndef __NINA_H__ +#define __NINA_H__ + +// void wake_up_nina(); +// void put_nina_to_sleep(); +void init_nina(int active); +void close_nina(); +struct pi_device * open_wifi(); + +#endif // __NINA_H__ diff --git a/gap/streamer/uart.c b/gap/streamer/uart.c new file mode 100644 index 0000000..4b9be5c --- /dev/null +++ b/gap/streamer/uart.c @@ -0,0 +1,75 @@ +#include "uart.h" +#include "common.h" + +#define CONFIG_HYPERBUS_DATA6_PAD ( PI_PAD_46_B7_SPIM0_SCK ) +// This is due to a HW bug, to be fixed in the future +#define CONFIG_UART_RX_PAD_FUNC ( 0 ) +#define CONFIG_HYPERRAM_DATA6_PAD_FUNC ( 3 ) + +extern int _prf(int (*func)(), void *dest, const char *format, va_list vargs); + +static struct pi_device device; +static int initialized = 0; +static int enabled = 0; + +// Autogenerate private implementation of the uart protocol +#include "_protocol.c" + +int init_uart(int enable_rx) +{ + struct pi_uart_conf conf; + pi_uart_conf_init(&conf); + conf.baudrate_bps = 115200; + conf.enable_tx = 1; + conf.enable_rx = 1; + pi_open_from_conf(&device, &conf); + if (pi_uart_open(&device)) + { + printf("[GAP8 Error]: Failed to open UART\n"); + return -1; + } + initialized = 1; + //start listening + if(enable_rx) + { + LOG("Start UART RX\n"); +#ifdef UART_RX_PROTOCOL + set_rx_enabled(1); + start_rx_protocol(); +#endif + } + LOG("Initialized UART\n"); + return 0; +} + +static int uart_tfp_putc(int c, void *dest) +{ + pi_uart_write(&device, (uint8_t *)&c, 1); + return c; +} + +int uart_printf(const char *fmt, ...) { + if(!initialized) return 0; + va_list va; + va_start(va, fmt); + _prf(uart_tfp_putc, NULL, fmt, va); + va_end(va); + return 0; +} + +void uart_flush_rx() +{ + if(!initialized) return; + pi_uart_ioctl(&device, PI_UART_IOCTL_FLUSH, NULL); + // pi_uart_ioctl(&device, PI_UART_IOCTL_ABORT_RX, NULL); + // pi_uart_ioctl(&device, PI_UART_IOCTL_ENABLE_RX, NULL); +} + +void close_uart() +{ + if(initialized) + { + pi_uart_close(&device); + printf("[GAP8 INFO]: Closed UART\n"); + } +} diff --git a/gap/streamer/uart.h b/gap/streamer/uart.h new file mode 100644 index 0000000..db9d262 --- /dev/null +++ b/gap/streamer/uart.h @@ -0,0 +1,11 @@ +#ifndef __UART_H__ +#define __UART_H__ + +int init_uart(int); +int uart_printf(const char *fmt, ...); +void uart_flush_rx(); +void close_uart(); +void read_byte(); +void set_rx_enabled(int value); + +#endif // __UART_H__ diff --git a/gap/streamer/uart_protocol.c b/gap/streamer/uart_protocol.c new file mode 100644 index 0000000..9b96327 --- /dev/null +++ b/gap/streamer/uart_protocol.c @@ -0,0 +1,66 @@ +#include "protocol.h" + +#define HEADER_LENGTH 4 +#define UART_RX_PROTOCOL +static PI_L2 uint8_t header[HEADER_LENGTH]; +static pi_task_t task; +void send_camera_config(camera_t *config) +{ + if(!enabled) return; +#ifdef CAMERA_HEADER + pi_uart_write(&device, CAMERA_HEADER, HEADER_LENGTH); + pi_uart_write(&device, (uint8_t *)config, sizeof(camera_t)); +#endif +} + +void send_stream_config(stream_t *config) +{ + if(!enabled) return; +#ifdef STREAM_HEADER + pi_uart_write(&device, STREAM_HEADER, HEADER_LENGTH); + pi_uart_write(&device, (uint8_t *)config, sizeof(stream_t)); +#endif +} + + +static void received_header(void *arg) +{ + +#ifdef CAMERA_HEADER + if(memcmp(header, CAMERA_HEADER, HEADER_LENGTH) == 0) + { + PI_L2 uint8_t rx_buffer[sizeof(camera_t)]; + if(pi_uart_read(&device, rx_buffer, sizeof(camera_t))) + { + LOG_ERROR("Failed to read camera config\n"); + } + else{ + set_camera_config((camera_t *)rx_buffer); + } + goto: done + } +#endif + +#ifdef STREAM_HEADER + if(memcmp(header, STREAM_HEADER, HEADER_LENGTH) == 0) + { + PI_L2 uint8_t rx_buffer[sizeof(stream_t)]; + if(pi_uart_read(&device, rx_buffer, sizeof(stream_t))) + { + LOG_ERROR("Failed to read stream config\n"); + } + else{ + set_stream_config((stream_t *)rx_buffer); + } + goto: done + } +#endif + + done: pi_uart_read_async(&device, header, HEADER_LENGTH, &task); +} + +static void start_rx_protocol() +{ + pi_task_callback(&task, (void *) received_header, &task); + pi_uart_read_async(&device, header, HEADER_LENGTH, &task); +} \ No newline at end of file diff --git a/gap/streamer/uart_protocol.h b/gap/streamer/uart_protocol.h new file mode 100644 index 0000000..2ae6a3b --- /dev/null +++ b/gap/streamer/uart_protocol.h @@ -0,0 +1,28 @@ +#include + +typedef struct { + uint16_t marginTop; + uint16_t marginRight; + uint16_t marginBottom; + uint16_t marginLeft; + uint8_t format; + uint8_t step; + uint8_t target_value; +} __attribute__((packed)) camera_t; + +#define CAMERA_HEADER "!CAM" +void send_camera_config(camera_t *config); +// To be implemented if CAMERA_HEADER is defined +void set_camera_config(camera_t *config); + + +typedef struct { + uint8_t on; + uint8_t format; + uint8_t transport; +} __attribute__((packed)) stream_t; + +#define STREAM_HEADER "!STR" +void send_stream_config(stream_t *config); +// To be implemented if STREAM_HEADER is defined +void set_stream_config(stream_t *config); diff --git a/nina/streamer/Makefile b/nina/streamer/Makefile new file mode 100644 index 0000000..aa249b6 --- /dev/null +++ b/nina/streamer/Makefile @@ -0,0 +1,3 @@ +PROJECT_NAME := streamer + +include $(IDF_PATH)/make/project.mk diff --git a/nina/streamer/REAME.md b/nina/streamer/REAME.md new file mode 100644 index 0000000..a245f58 --- /dev/null +++ b/nina/streamer/REAME.md @@ -0,0 +1,8 @@ +# NINA WiFi Streamer + +This is a modification of the code provided as an example by Bitcraze: + - added support for RAW streaming + - added support for deep sleeping until waken up by GAP through a GPIO HIGH + - changed the LED pattern + +The only modified file is `main/streamer.c`. diff --git a/nina/streamer/load.sh b/nina/streamer/load.sh new file mode 100644 index 0000000..dbd8c8e --- /dev/null +++ b/nina/streamer/load.sh @@ -0,0 +1 @@ +openocd -f interface/ftdi/olimex-arm-usb-ocd-h.cfg -f board/esp-wroom-32.cfg -c "adapter_khz 1000" -c "program_esp32 build/partitions_singleapp.bin 0x8000 verify" -c "program_esp32 build/bootloader/bootloader.bin 0x1000 verify" -c "program_esp32 build/streamer.bin 0x10000 verify reset exit" diff --git a/nina/streamer/main/Kconfig.projbuild b/nina/streamer/main/Kconfig.projbuild new file mode 100644 index 0000000..60d2e28 --- /dev/null +++ b/nina/streamer/main/Kconfig.projbuild @@ -0,0 +1,26 @@ +menu "Streamer Configuration" + + config USE_AS_AP + bool "Act as access-point" + default n + help + Act as accesspoint as opposed to connecting to a wifi + + menu "Credentials for connecting to another access-point" + + config EXAMPLE_SSID + string "WiFi SSID" + default "myssid" + depends on !USE_AS_AP + help + SSID (network name) for the example to connect to. + + config EXAMPLE_PASSWORD + string "WiFi Password" + default "mypassword" + depends on !USE_AS_AP + help + WiFi password (WPA or WPA2) for the example to use. + + endmenu +endmenu diff --git a/nina/streamer/main/component.mk b/nina/streamer/main/component.mk new file mode 100644 index 0000000..61f8990 --- /dev/null +++ b/nina/streamer/main/component.mk @@ -0,0 +1,8 @@ +# +# Main component makefile. +# +# This Makefile can be left empty. By default, it will take the sources in the +# src/ directory, compile them and link them into lib(subdirectory_name).a +# in the build directory. This behaviour is entirely configurable, +# please read the ESP-IDF documents if you need to do this. +# diff --git a/nina/streamer/main/spi.c b/nina/streamer/main/spi.c new file mode 100644 index 0000000..eb7aa57 --- /dev/null +++ b/nina/streamer/main/spi.c @@ -0,0 +1,138 @@ +/* + * Copyright (C) 2019 GreenWaves Technologies + * Copyright (C) 2020 Bitcraze AB + * + * Licensed 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 +#include + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/event_groups.h" +#include "freertos/semphr.h" +#include "freertos/queue.h" + +#include "esp_system.h" +#include "esp_wifi.h" +#include "esp_event.h" +#include "esp_log.h" +#include "nvs_flash.h" +#include "tcpip_adapter.h" +#include "esp_event_loop.h" +#include "soc/rtc_cntl_reg.h" +#include "driver/spi_slave.h" +#include "esp_spi_flash.h" +#include "driver/gpio.h" + +#include "spi.h" + +#define GPIO_HANDSHAKE 2 +#define GPIO_NOTIF 32 +#define GPIO_MOSI 19 +#define GPIO_MISO 23 +#define GPIO_SCLK 18 +#define GPIO_CS 5 + +#define BUFFER_LENGTH 2048 + +#define SPI_BUFFER_LEN SPI_MAX_DMA_LEN + +static uint8_t *communication_buffer; + +void my_post_setup_cb(spi_slave_transaction_t *trans) { + WRITE_PERI_REG(GPIO_OUT_W1TS_REG, (1< +#include + +#ifndef __SPI_H__ +#define __SPI_H__ + +#define CMD_PACKET_SIZE 16384 + +/* Initialize the SPI */ +void spi_init(); + +/* Read data from the GAP8 SPI */ +int spi_read_data(uint8_t ** buffer, size_t len); + +#endif \ No newline at end of file diff --git a/nina/streamer/main/streamer.c b/nina/streamer/main/streamer.c new file mode 100644 index 0000000..743b826 --- /dev/null +++ b/nina/streamer/main/streamer.c @@ -0,0 +1,438 @@ +/* + * Copyright (C) 2019 GreenWaves Technologies + * Copyright (C) 2020 Bitcraze AB + * + * Licensed 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. + */ + +/* + * Authors: Esteban Gougeon, GreenWaves Technologies (esteban.gougeon@greenwaves-technologies.com) + * Germain Haugou, GreenWaves Technologies (germain.haugou@greenwaves-technologies.com) + */ + +/* + * Nina firmware for the AI-deck streaming JPEG demo. This demo takes + * JPEG data sent from the GAP8 and forwards it to a TCP socket. The data + * sent on the socket is a continous stream of JPEG images, where the JPEG + * start-of-frame (0xFF 0xD8) and end-of-frame (0xFF 0xD9) is used to + * pick out images from the stream. + * + * The Frame Streamer on the GAP8 will start sending data once it's booted. + * This firmware only uses the JPEG data sent by the Frame Streamer and + * ignores the rest. + * + * The GAP8 communication sequence is described below: + * + * GAP8 sends 4 32-bit unsigned integers (nina_req_t) where: + * * type - Describes the type of package (0x81 is JPEG data) + * * size - The size of data that should be requested + * * info - Used for signaling end-of-frame of the JPEG + * + * When the frame streamer starts it will send the following packages of type 0x81: + * 1) The JPEG header (306 bytes hardcoded in the GAP8 SDK) (info == 1) + * 2) The JPEG footer (2 bytes hardcoded in the GAP8 SDK) (info == 1) + * 3) Continous JPEG data (excluding header and footer) where info == 0 for everything + * except the package with the last data of a frame + * + * Note that if you reflash/restart the Nina you will have to restart the GAP8, since it + * only sends the JPEG header/footer on startup! + * + * This firmware will save the JPEG header and footer and send this on the socket + * bofore and after each frame that is received from the GAP8. + * + * Tested with GAP8 SEK version 3.4. + + **************************************************************************************** + * This is a modification of the above code, which is provided as an example by Bitcraze: + - added support for RAW streaming + - added support for deep sleeping until waken up by GAP through a GPIO HIGH + - changed the LED pattern + + Author: Jérôme Guzzi, jerome@idsia.ch + **************************************************************************************** +*/ + +#include +#include +#include +#include +#include + +#include "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/event_groups.h" +#include "freertos/semphr.h" +#include "freertos/queue.h" + +#include +#include +#include +#include "esp_sleep.h" +#include "esp_log.h" +#include "esp32/ulp.h" +#include "driver/touch_pad.h" +#include "driver/adc.h" +#include "driver/rtc_io.h" +#include "soc/rtc_cntl_reg.h" +#include "soc/sens_reg.h" +#include "soc/rtc.h" +#include "nvs_flash.h" +#include "esp_wifi.h" +#include "wifi.h" +#include "spi.h" + +#define RTC_REGISTER_ADDRESS_BANK_1 ((int *) 0x3FF4804C) +#define RTC_REGISTER_ADDRESS_BANK_2 ((int *) 0x3FF480B0) +#define SLEEP_TIME_USECONDS (1 * 1000000) + +#define SLEEP_AFTER_BOOT +// #define SLEEP_IF_INACTIVE +// #define SLEEP_IF_GPIO_LOW +/* The GAP is connected also on a GPIO (there is another one too)*/ +#define GAP_GPIO_IN 2 + +// If we are of no use, better to sleep and spare energy +void go_to_sleep() +{ + if(!rtc_gpio_is_valid_gpio(GAP_GPIO_IN)) + { + ESP_LOGE("GPIO %d cannot be used to wake up NINA.", GAP_GPIO_IN); + return; + } + esp_sleep_enable_ext0_wakeup(GAP_GPIO_IN, 1); + esp_wifi_stop(); + ESP_LOGI("Going to deep sleep ... will wait for reboot or high on pin %d", GAP_GPIO_IN); + esp_deep_sleep_start(); +} + +/* GAP8 streamer packet type for JPEG related data */ +#define NINA_W10_CMD_SEND_PACKET 0x81 + +// Force AP despite of config ... does not work!!! +// #ifndef CONFIG_USE_AS_AP +// #define CONFIG_USE_AS_AP +// #endif + +/* WiFi SSID/password and AP/station is set from menuconfig */ +#ifdef CONFIG_USE_AS_AP +#define WIFI_MODE AIDECK_WIFI_MODE_SOFTAP +#define CONFIG_EXAMPLE_SSID NULL +#define CONFIG_EXAMPLE_PASSWORD NULL +#else +#define WIFI_MODE AIDECK_WIFI_MODE_STATION +#endif + +/* The LED is connected on a GPIO */ +#define BLINK_GPIO 4 + + +/* The periods of the LED blinking */ +static volatile uint32_t blink_period_ms_1 = 500; +static volatile uint32_t blink_period_ms_2 = 500; + +/* Log tag for printouts */ +static const char *TAG = "streamer"; + +static volatile int connected = 0; +static volatile int source_alive = 0; +static volatile int got_msg = 0; + +static void update_led_pattern() +{ + if(source_alive) + { + if(connected) + { + blink_period_ms_1 = blink_period_ms_2 = 100; + } + else{ + blink_period_ms_1 = blink_period_ms_2 = 500; + } + } + else{ + if(connected) + { + blink_period_ms_1 = 200; + blink_period_ms_2 = 1000; + } + else{ + blink_period_ms_1 = blink_period_ms_2 = 2000; + } + } +} + +/* GAP8 communication structs */ + +typedef struct +{ + uint32_t type; /* Is 0x81 for JPEG related data */ + uint32_t size; /* Size of data to request */ +} __attribute__((packed)) nina_req_t; + +typedef struct +{ + uint32_t channel; + uint32_t size; + uint32_t info; +} __attribute__((packed)) header_t; + +/* Pointer to data transferred via SPI */ +static uint8_t *spi_buffer; + +/* Storage for JPEG header (hardcoded to 306 bytes like in GAP8) */ +static uint8_t jpeg_header[306]; +/* Storage for JPEG footer (hardcoded to 2 bytes like in GAP8) */ +static uint8_t jpeg_footer[2]; + +/* JPEG communication state (see above) */ +typedef enum +{ + HEADER, /* Next packet is header */ + FOOTER, /* Next packet is footer */ + DATA /* Next packet is data */ +} ImageState_t; + +/* JPEG communication state */ +static ImageState_t state = HEADER; + + +// Raw protocol + +#define RAW_HEADER "?IMG" +uint16_t raw_header[5]; +const char raw_footer[] = "!IMG"; +static bool new_frame = 0; + +/* Do we know which image format to receive and forward? */ + +typedef enum +{ + RAW, + JPEG, + UNKNOWN +} image_format_t; + +static image_format_t format = UNKNOWN; +static uint32_t number_of_frames = 0; + +static void handle_gap8_package(uint8_t *buffer, int32_t length) { + got_msg = 1; + nina_req_t *req = (nina_req_t *)buffer; + uint32_t *hs = (uint32_t *)buffer; + // if(format == UNKNOWN) + // { + // printf("handle_gap8_package (%d): %d %d %d ...\n", length, hs[0], hs[1], hs[2]); + // } + int size; + int bs = 0; + switch (req->type) + { + case 1: + if(length > 95) + { + format = (image_format_t) hs[3]; + state = HEADER; + switch (format) { + case RAW: + ESP_LOGI(TAG, "Got streaming request for RAW images"); + break; + case JPEG: + ESP_LOGI(TAG, "Got streaming request for RAW images"); + break; + default: + ESP_LOGI(TAG, "Got streaming request for unknown format %d", format); + format = UNKNOWN; + } + } + break; + case NINA_W10_CMD_SEND_PACKET: + if(req->size == 12) + { + length = spi_read_data(&buffer, req->size * 8); + header_t *header = (header_t *) buffer; + if(header->info <= 1 && format == JPEG) + { + new_frame = (header->info == 1); + if(new_frame) number_of_frames++; + } + else if (header->info > 1 && format == RAW){ + // RAW + raw_header[2] = (int16_t) (0xFFFF & header->info); + raw_header[3] = (int16_t) (header->size / raw_header[2]); + raw_header[4] = (int16_t) (header->info >> 16); + if(number_of_frames == 0) + { + ESP_LOGI(TAG, "First header for RAW images: shape (%d, %d), color %d\n", + raw_header[2], raw_header[3], raw_header[4]); + } + number_of_frames++; + if(wifi_is_socket_connected()) + wifi_send_packet((char *)raw_header, 10); + } + else{ + ESP_LOGE("Unexpected header [%d, %d, %d]", header->channel, header->size, header->info); + } + } + else{ + // Read image of type `req->type` and size `req->size` bytes + // size to read in bits + size = req->size * 8; + if(format == RAW) + { + while(size > 0) + { + bs = 8192; + if(size < bs) bs = size; + length = spi_read_data(&buffer, bs); + // There are still `size - length` bits to read + size -= length; + if(wifi_is_socket_connected()) + wifi_send_packet((const char*) buffer, length / 8); + } + if(wifi_is_socket_connected()) + wifi_send_packet((const char*) raw_footer, 4); + } + else if(format == JPEG) + { + length = spi_read_data(&buffer, size); + switch (state) { + case HEADER: + ESP_LOGI(TAG, "Setting JPEG header of %i bytes", length / 8); + memcpy(&jpeg_header, (uint8_t*) buffer, sizeof(jpeg_header)); + if(wifi_is_socket_connected()) + wifi_send_packet( (const char*) &jpeg_header, sizeof(jpeg_header)); + state = FOOTER; + break; + case FOOTER: + ESP_LOGI(TAG, "Setting JPEG footer of %i bytes", length / 8); + memcpy(&jpeg_footer, (uint8_t*) buffer, sizeof(jpeg_footer)); + state = DATA; + break; + case DATA: + if(wifi_is_socket_connected()) + { + wifi_send_packet( (const char*) buffer, length / 8); + if (new_frame) { + wifi_send_packet( (const char*) &jpeg_footer, sizeof(jpeg_footer) ); + wifi_send_packet( (const char*) &jpeg_header, sizeof(jpeg_header) ); + } + } + break; + } + } + } + break; + } +} + +/* Task for receiving image data from GAP8 and sending to client via WiFi */ +static void img_sending_task(void *pvParameters) { + spi_init(); + memcpy(raw_header, RAW_HEADER, 4); + while (1) + { + int32_t datalength = spi_read_data(&spi_buffer, CMD_PACKET_SIZE); + if (datalength > 0) { + handle_gap8_package(spi_buffer, datalength); + } + } +} + +/* Task for handling WiFi state */ +static void wifi_task(void *pvParameters) { + wifi_bind_socket(); + while (1) { + wifi_wait_for_socket_connected(); + connected = 1; + update_led_pattern(); + wifi_wait_for_disconnect(); + connected = 0; + update_led_pattern(); + ESP_LOGI(TAG, "Client disconnected"); + } +} + +/* Task tp blick the LED */ +static void led_task(void *pvParameters) { + while(1) + { + gpio_set_level(BLINK_GPIO, 0); + vTaskDelay(pdMS_TO_TICKS(blink_period_ms_1)); + gpio_set_level(BLINK_GPIO, 1); + vTaskDelay(pdMS_TO_TICKS(blink_period_ms_2)); + } +} + +void app_main() +{ + int wakeup_cause = esp_sleep_get_wakeup_cause(); + nvs_flash_init(); + ESP_LOGI(TAG, "Wake up cause: %d\n", wakeup_cause); + +#ifdef SLEEP_AFTER_BOOT + if(wakeup_cause != 2) + { + go_to_sleep(); + } +#endif + + xTaskCreate(img_sending_task, "img_sending_task", 4096, NULL, 5, NULL); + +#ifdef SLEEP_IF_INACTIVE + ESP_LOGI("Wait 2s for images ..."); + vTaskDelay(pdMS_TO_TICKS(2000)); + if(got_msg) + { + ESP_LOGI("Yes ... the image source is streaming ... continue the initialization"); + } + else{ + go_to_sleep(); + } +#endif + + rtc_gpio_deinit(GAP_GPIO_IN); + gpio_pad_select_gpio(GAP_GPIO_IN); + gpio_set_direction(GAP_GPIO_IN, GPIO_MODE_INPUT); + + gpio_pad_select_gpio(BLINK_GPIO); + gpio_set_direction(BLINK_GPIO, GPIO_MODE_OUTPUT); + update_led_pattern(); + xTaskCreate(led_task, "led", 4096, NULL, 4, NULL); + +#ifdef CONFIG_USE_AS_AP + ESP_LOGI(TAG, "[WiFi] Will start on AP\n"); +#else + ESP_LOGI(TAG, "[WiFi] Will connect to %s\n", CONFIG_EXAMPLE_SSID); +#endif + wifi_init(WIFI_MODE, CONFIG_EXAMPLE_SSID, CONFIG_EXAMPLE_PASSWORD); + xTaskCreate(wifi_task, "wifi_task", 4096, NULL, 5, NULL); + + while(1) { + got_msg = 0; + vTaskDelay(pdMS_TO_TICKS(2000)); +#ifdef SLEEP_IF_INACTIVE + if(!got_msg) + { + go_to_sleep(); + } +#endif + source_alive = got_msg; + update_led_pattern(); +#ifdef SLEEP_IF_GPIO_LOW + if(gpio_get_level(GAP_GPIO_IN) == 0) + { + go_to_sleep(); + } +#endif + } +} diff --git a/nina/streamer/main/wifi.c b/nina/streamer/main/wifi.c new file mode 100644 index 0000000..52a5901 --- /dev/null +++ b/nina/streamer/main/wifi.c @@ -0,0 +1,199 @@ +/* + * Copyright (C) 2020 Bitcraze AB + * + * Licensed 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 "freertos/FreeRTOS.h" +#include "freertos/task.h" +#include "freertos/event_groups.h" +#include "esp_system.h" +#include "esp_wifi.h" +#include "esp_event_loop.h" +#include "esp_log.h" +#include "nvs_flash.h" + +#include "lwip/err.h" +#include "lwip/sys.h" +#include "lwip/sockets.h" +#include +#include "wifi.h" + +const int WIFI_CONNECTED_BIT = BIT0; +const int WIFI_SOCKET_DISCONNECTED = BIT1; +static EventGroupHandle_t s_wifi_event_group; + +/* Log printout tag */ +static const char *TAG = "wifi"; + +/* Socket for receiving WiFi connections */ +static int sock = -1; +/* Accepted WiFi connection */ +static int conn = -1; + +/* WiFi event handler */ +static esp_err_t event_handler(void *ctx, system_event_t *event) +{ + switch(event->event_id) { + case SYSTEM_EVENT_STA_START: + esp_wifi_connect(); + break; + case SYSTEM_EVENT_STA_GOT_IP: + ESP_LOGI(TAG, "got ip:%s", + ip4addr_ntoa(&event->event_info.got_ip.ip_info.ip)); + xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT); + break; + case SYSTEM_EVENT_STA_DISCONNECTED: + esp_wifi_connect(); + xEventGroupClearBits(s_wifi_event_group, WIFI_CONNECTED_BIT); + ESP_LOGI(TAG,"Disconnected from access point"); + break; + case SYSTEM_EVENT_AP_STACONNECTED: + ESP_LOGI(TAG, "station:"MACSTR" join, AID=%d", + MAC2STR(event->event_info.sta_connected.mac), + event->event_info.sta_connected.aid); + break; + case SYSTEM_EVENT_AP_STADISCONNECTED: + ESP_LOGI(TAG, "station:"MACSTR"leave, AID=%d", + MAC2STR(event->event_info.sta_disconnected.mac), + event->event_info.sta_disconnected.aid); + break; + default: + break; + } + return ESP_OK; +} + +/* Initialize WiFi as AP */ +static void wifi_init_softap() +{ + s_wifi_event_group = xEventGroupCreate(); + + tcpip_adapter_init(); + ESP_ERROR_CHECK(esp_event_loop_init(event_handler, NULL)); + + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + wifi_config_t wifi_config = { + .ap = { + .ssid = WIFI_SSID, + .ssid_len = strlen(WIFI_SSID), + .max_connection = 1, + .authmode = WIFI_AUTH_OPEN + }, + }; + + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP)); + ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM)); + ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &wifi_config)); + ESP_ERROR_CHECK(esp_wifi_start()); + + ESP_LOGI(TAG, "wifi_init_softap finished"); +} + +/* Initialize WiFi as station (connecting to AP) */ +static void wifi_init_sta(const char * ssid, const char * passwd) +{ + s_wifi_event_group = xEventGroupCreate(); + + tcpip_adapter_init(); + ESP_ERROR_CHECK(esp_event_loop_init(event_handler, NULL) ); + + wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT(); + ESP_ERROR_CHECK(esp_wifi_init(&cfg)); + wifi_config_t wifi_config; + memset((void *)&wifi_config, 0, sizeof(wifi_config_t)); + strncpy((char *)wifi_config.sta.ssid, ssid, strlen(ssid)); + strncpy((char *)wifi_config.sta.password, passwd, strlen(passwd)); + + ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA) ); + ESP_ERROR_CHECK(esp_wifi_set_storage(WIFI_STORAGE_RAM)); + ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_STA, &wifi_config) ); + ESP_ERROR_CHECK(esp_wifi_start() ); + + ESP_LOGI(TAG, "wifi_init_sta finished."); +} + +void wifi_bind_socket() { + char addr_str[128]; + int addr_family; + int ip_protocol; + struct sockaddr_in destAddr; + destAddr.sin_addr.s_addr = htonl(INADDR_ANY); + destAddr.sin_family = AF_INET; + destAddr.sin_port = htons(PORT); + addr_family = AF_INET; + ip_protocol = IPPROTO_IP; + inet_ntoa_r(destAddr.sin_addr, addr_str, sizeof(addr_str) - 1); + sock = socket(addr_family, SOCK_STREAM, ip_protocol); + if (sock < 0) { + ESP_LOGE(TAG, "Unable to create socket: errno %d", errno); + } + ESP_LOGI(TAG, "Socket created"); + + int err = bind(sock, (struct sockaddr *)&destAddr, sizeof(destAddr)); + if (err != 0) { + ESP_LOGE(TAG, "Socket unable to bind: errno %d", errno); + } + ESP_LOGI(TAG, "Socket binded"); + + err = listen(sock, 1); + if (err != 0) { + ESP_LOGE(TAG, "Error occured during listen: errno %d", errno); + } + ESP_LOGI(TAG, "Socket listening"); + +} + +void wifi_wait_for_socket_connected() { + ESP_LOGI(TAG, "Waiting for connection"); + struct sockaddr sourceAddr; + uint addrLen = sizeof(sourceAddr); + conn = accept(sock, (struct sockaddr *)&sourceAddr, &addrLen); + if (conn < 0) { + ESP_LOGE(TAG, "Unable to accept connection: errno %d", errno); + } + ESP_LOGI(TAG, "Connection accepted"); +} + +bool wifi_is_socket_connected() { + return conn != -1; +} + +void wifi_wait_for_disconnect() { + xEventGroupWaitBits(s_wifi_event_group, WIFI_SOCKET_DISCONNECTED, pdTRUE, pdFALSE, portMAX_DELAY); +} + +void wifi_send_packet(const char * buffer, size_t size) { + if (conn != -1) { + int err = send(conn, buffer, size, 0); + if (err < 0) { + ESP_LOGE(TAG, "Error occurred during sending: errno %d", errno); + conn = -1; + xEventGroupSetBits(s_wifi_event_group, WIFI_SOCKET_DISCONNECTED); + } + } else { + ESP_LOGE(TAG, "o socket when trying to send data"); + } +} + +void wifi_init(WiFiMode_t mode, const char * ssid, const char * key) { + tcpip_adapter_init(); + + if (AIDECK_WIFI_MODE_SOFTAP == mode) { + wifi_init_softap(); + } else { + wifi_init_sta(ssid, key); + } +} diff --git a/nina/streamer/main/wifi.h b/nina/streamer/main/wifi.h new file mode 100644 index 0000000..4393b03 --- /dev/null +++ b/nina/streamer/main/wifi.h @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2020 Bitcraze AB + * + * Licensed 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 + +#ifndef __WIFI_H__ +#define __WIFI_H__ + +/* The SSID of the AI-deck when in AP mode */ +#define WIFI_SSID "Bitcraze AI-deck example" + +/* The port used for the example */ +#define PORT 5000 + +/* The different modes the WiFi can be initialized as */ +typedef enum { + AIDECK_WIFI_MODE_SOFTAP, /* Act as access-point */ + AIDECK_WIFI_MODE_STATION /* Connect to an access-point */ +} WiFiMode_t; + +/* Initialize the WiFi */ +void wifi_init(WiFiMode_t mode, const char * ssid, const char * key); + +/* Wait (and block) until a connection comes in */ +void wifi_wait_for_socket_connected(); + +/* Bind socket for incomming connections */ +void wifi_bind_socket(); + +/* Check if a client is connected */ +bool wifi_is_socket_connected(); + +/* Wait (and block) for a client to disconnect*/ +void wifi_wait_for_disconnect(); + +/* Send a packet to the client from buffer of size */ +void wifi_send_packet(const char * buffer, size_t size); + +#endif \ No newline at end of file diff --git a/nina/streamer/notes.md b/nina/streamer/notes.md new file mode 100644 index 0000000..86e3efa --- /dev/null +++ b/nina/streamer/notes.md @@ -0,0 +1,61 @@ +MAC Address: A4:CF:12:84:2A:FC +host name espressif + +sleeping +====== + +see https://github.com/u-blox/nina-w10-power-test +and https://github.com/espressif/esp-idf/tree/release/v3.3/examples/system/deep_sleep + +gpio +==== + +https://wiki.bitcraze.io/projects:crazyflie2:expansionboards:aideck + +button = sysboot = gpio_27 = touch1 = #0 + + +see also + +https://github.com/espressif/esp-idf/blob/release/v3.3/components/driver/test/test_gpio.c + + +I cannot use (??) the button as an independent GPIO (to change state) because it triggers rebooting of the NINA. + +Should I try with uart? + + +docs +==== + +https://docs.espressif.com/projects/esp-idf/en/latest/esp32/api-reference/system/log.html + +uart2 Cf + +USART_InitStructure.USART_BaudRate = baudrate; +USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; +USART_InitStructure.USART_WordLength = USART_WordLength_8b; +USART_InitStructure.USART_StopBits = USART_StopBits_1; +USART_InitStructure.USART_Parity = USART_Parity_No ; +USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; + + +#define ESP_OK 0 +#define ESP_FAIL -1 + + + +non so perché ma dopo un po' (qualche ora di test gap non parte più, già successo ieri e basta che si aspetta un po') + + +NINA: + +sono capace a +- mandare e ricevere via UART (mantenendo i LOG attivi), cambiando il led a seconda del messaggio (originato da cfclient) +- attivare / disattivare wifi (e cambiare da AP a managed, almeno a built time) +- abilitare deep sleep (e svegliarlo con sysboot) + +sarebbe bello: +- abilitare light sleep e svegliarlo da uart (sempre via cfclient) +- usare uart per attivare/disattivare wifi e cambiare stream da jpeg a raw. +- stream raw (modificando il codice dal loro firmware) [usando lo streamer del CF a 160 x 96] diff --git a/nina/streamer/sdkconfig b/nina/streamer/sdkconfig new file mode 100644 index 0000000..6611c0b --- /dev/null +++ b/nina/streamer/sdkconfig @@ -0,0 +1,751 @@ +# +# Automatically generated file; DO NOT EDIT. +# Espressif IoT Development Framework Configuration +# +CONFIG_IDF_TARGET="esp32" +CONFIG_IDF_FIRMWARE_CHIP_ID=0x0000 + +# +# SDK tool configuration +# +CONFIG_TOOLPREFIX="xtensa-esp32-elf-" +CONFIG_PYTHON="python" +CONFIG_MAKE_WARN_UNDEFINED_VARIABLES=y + +# +# Application manager +# +CONFIG_APP_COMPILE_TIME_DATE=y +CONFIG_APP_EXCLUDE_PROJECT_VER_VAR= +CONFIG_APP_EXCLUDE_PROJECT_NAME_VAR= +CONFIG_APP_RETRIEVE_LEN_ELF_SHA=16 + +# +# Bootloader config +# +CONFIG_LOG_BOOTLOADER_LEVEL_NONE= +CONFIG_LOG_BOOTLOADER_LEVEL_ERROR= +CONFIG_LOG_BOOTLOADER_LEVEL_WARN= +CONFIG_LOG_BOOTLOADER_LEVEL_INFO=y +CONFIG_LOG_BOOTLOADER_LEVEL_DEBUG= +CONFIG_LOG_BOOTLOADER_LEVEL_VERBOSE= +CONFIG_LOG_BOOTLOADER_LEVEL=3 +CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_8V= +CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_9V=y +CONFIG_BOOTLOADER_FACTORY_RESET= +CONFIG_BOOTLOADER_APP_TEST= +CONFIG_BOOTLOADER_WDT_ENABLE=y +CONFIG_BOOTLOADER_WDT_DISABLE_IN_USER_CODE= +CONFIG_BOOTLOADER_WDT_TIME_MS=9000 +CONFIG_APP_ROLLBACK_ENABLE= + +# +# Security features +# +CONFIG_SECURE_SIGNED_APPS_NO_SECURE_BOOT= +CONFIG_SECURE_BOOT_ENABLED= +CONFIG_FLASH_ENCRYPTION_ENABLED= + +# +# Serial flasher config +# +CONFIG_ESPTOOLPY_PORT="/dev/ttyUSB0" +CONFIG_ESPTOOLPY_BAUD_115200B=y +CONFIG_ESPTOOLPY_BAUD_230400B= +CONFIG_ESPTOOLPY_BAUD_921600B= +CONFIG_ESPTOOLPY_BAUD_2MB= +CONFIG_ESPTOOLPY_BAUD_OTHER= +CONFIG_ESPTOOLPY_BAUD_OTHER_VAL=115200 +CONFIG_ESPTOOLPY_BAUD=115200 +CONFIG_ESPTOOLPY_COMPRESSED=y +CONFIG_FLASHMODE_QIO= +CONFIG_FLASHMODE_QOUT= +CONFIG_FLASHMODE_DIO=y +CONFIG_FLASHMODE_DOUT= +CONFIG_ESPTOOLPY_FLASHMODE="dio" +CONFIG_ESPTOOLPY_FLASHFREQ_80M= +CONFIG_ESPTOOLPY_FLASHFREQ_40M=y +CONFIG_ESPTOOLPY_FLASHFREQ_26M= +CONFIG_ESPTOOLPY_FLASHFREQ_20M= +CONFIG_ESPTOOLPY_FLASHFREQ="40m" +CONFIG_ESPTOOLPY_FLASHSIZE_1MB= +CONFIG_ESPTOOLPY_FLASHSIZE_2MB=y +CONFIG_ESPTOOLPY_FLASHSIZE_4MB= +CONFIG_ESPTOOLPY_FLASHSIZE_8MB= +CONFIG_ESPTOOLPY_FLASHSIZE_16MB= +CONFIG_ESPTOOLPY_FLASHSIZE="2MB" +CONFIG_ESPTOOLPY_FLASHSIZE_DETECT=y +CONFIG_ESPTOOLPY_BEFORE_RESET=y +CONFIG_ESPTOOLPY_BEFORE_NORESET= +CONFIG_ESPTOOLPY_BEFORE="default_reset" +CONFIG_ESPTOOLPY_AFTER_RESET=y +CONFIG_ESPTOOLPY_AFTER_NORESET= +CONFIG_ESPTOOLPY_AFTER="hard_reset" +CONFIG_MONITOR_BAUD_9600B= +CONFIG_MONITOR_BAUD_57600B= +CONFIG_MONITOR_BAUD_115200B=y +CONFIG_MONITOR_BAUD_230400B= +CONFIG_MONITOR_BAUD_921600B= +CONFIG_MONITOR_BAUD_2MB= +CONFIG_MONITOR_BAUD_OTHER= +CONFIG_MONITOR_BAUD_OTHER_VAL=115200 +CONFIG_MONITOR_BAUD=115200 + +# +# Streamer Configuration +# +CONFIG_USE_AS_AP= + +# +# Credentials for connecting to another access-point +# +CONFIG_EXAMPLE_SSID="drone_wifi" +CONFIG_EXAMPLE_PASSWORD="wifi-wifi" + +# +# Partition Table +# +CONFIG_PARTITION_TABLE_SINGLE_APP=y +CONFIG_PARTITION_TABLE_TWO_OTA= +CONFIG_PARTITION_TABLE_CUSTOM= +CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv" +CONFIG_PARTITION_TABLE_FILENAME="partitions_singleapp.csv" +CONFIG_PARTITION_TABLE_OFFSET=0x8000 +CONFIG_PARTITION_TABLE_MD5=y + +# +# Compiler options +# +CONFIG_OPTIMIZATION_LEVEL_DEBUG= +CONFIG_OPTIMIZATION_LEVEL_RELEASE=y +CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED=y +CONFIG_OPTIMIZATION_ASSERTIONS_SILENT= +CONFIG_OPTIMIZATION_ASSERTIONS_DISABLED= +CONFIG_CXX_EXCEPTIONS= +CONFIG_STACK_CHECK_NONE=y +CONFIG_STACK_CHECK_NORM= +CONFIG_STACK_CHECK_STRONG= +CONFIG_STACK_CHECK_ALL= +CONFIG_STACK_CHECK= +CONFIG_WARN_WRITE_STRINGS= +CONFIG_DISABLE_GCC8_WARNINGS= + +# +# Component config +# + +# +# Application Level Tracing +# +CONFIG_ESP32_APPTRACE_DEST_TRAX= +CONFIG_ESP32_APPTRACE_DEST_NONE=y +CONFIG_ESP32_APPTRACE_ENABLE= +CONFIG_ESP32_APPTRACE_LOCK_ENABLE=y +CONFIG_AWS_IOT_SDK= + +# +# Bluetooth +# +CONFIG_BT_ENABLED= +CONFIG_BTDM_CTRL_BR_EDR_SCO_DATA_PATH_EFF=0 +CONFIG_BTDM_CTRL_AUTO_LATENCY_EFF= +CONFIG_BTDM_CONTROLLER_BLE_MAX_CONN_EFF=0 +CONFIG_BTDM_CONTROLLER_BR_EDR_MAX_ACL_CONN_EFF=0 +CONFIG_BTDM_CONTROLLER_BR_EDR_MAX_SYNC_CONN_EFF=0 +CONFIG_BTDM_CONTROLLER_PINNED_TO_CORE=0 +CONFIG_BT_RESERVE_DRAM=0 +CONFIG_BLE_MESH= + +# +# Driver configurations +# + +# +# ADC configuration +# +CONFIG_ADC_FORCE_XPD_FSM= +CONFIG_ADC2_DISABLE_DAC=y + +# +# SPI configuration +# +CONFIG_SPI_MASTER_IN_IRAM= +CONFIG_SPI_MASTER_ISR_IN_IRAM=y +CONFIG_SPI_SLAVE_IN_IRAM= +CONFIG_SPI_SLAVE_ISR_IN_IRAM=y + +# +# eFuse Bit Manager +# +CONFIG_EFUSE_CUSTOM_TABLE= +CONFIG_EFUSE_VIRTUAL= +CONFIG_EFUSE_CODE_SCHEME_COMPAT_NONE= +CONFIG_EFUSE_CODE_SCHEME_COMPAT_3_4=y +CONFIG_EFUSE_CODE_SCHEME_COMPAT_REPEAT= +CONFIG_EFUSE_MAX_BLK_LEN=192 + +# +# ESP32-specific +# +CONFIG_IDF_TARGET_ESP32=y +CONFIG_ESP32_REV_MIN_0=y +CONFIG_ESP32_REV_MIN_1= +CONFIG_ESP32_REV_MIN_2= +CONFIG_ESP32_REV_MIN_3= +CONFIG_ESP32_REV_MIN=0 +CONFIG_ESP32_DPORT_WORKAROUND=y +CONFIG_ESP32_DEFAULT_CPU_FREQ_80= +CONFIG_ESP32_DEFAULT_CPU_FREQ_160=y +CONFIG_ESP32_DEFAULT_CPU_FREQ_240= +CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ=160 +CONFIG_SPIRAM_SUPPORT= +CONFIG_MEMMAP_TRACEMEM= +CONFIG_MEMMAP_TRACEMEM_TWOBANKS= +CONFIG_ESP32_TRAX= +CONFIG_TRACEMEM_RESERVE_DRAM=0x0 +CONFIG_TWO_UNIVERSAL_MAC_ADDRESS= +CONFIG_FOUR_UNIVERSAL_MAC_ADDRESS=y +CONFIG_NUMBER_OF_UNIVERSAL_MAC_ADDRESS=4 +CONFIG_SYSTEM_EVENT_QUEUE_SIZE=32 +CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE=2304 +CONFIG_MAIN_TASK_STACK_SIZE=3584 +CONFIG_IPC_TASK_STACK_SIZE=1024 +CONFIG_TIMER_TASK_STACK_SIZE=3584 +CONFIG_NEWLIB_STDOUT_LINE_ENDING_CRLF=y +CONFIG_NEWLIB_STDOUT_LINE_ENDING_LF= +CONFIG_NEWLIB_STDOUT_LINE_ENDING_CR= +CONFIG_NEWLIB_STDIN_LINE_ENDING_CRLF= +CONFIG_NEWLIB_STDIN_LINE_ENDING_LF= +CONFIG_NEWLIB_STDIN_LINE_ENDING_CR=y +CONFIG_NEWLIB_NANO_FORMAT= +CONFIG_CONSOLE_UART_DEFAULT=y +CONFIG_CONSOLE_UART_CUSTOM= +CONFIG_CONSOLE_UART_NONE= +CONFIG_CONSOLE_UART_NUM=0 +CONFIG_CONSOLE_UART_BAUDRATE=115200 +CONFIG_ULP_COPROC_ENABLED= +CONFIG_ULP_COPROC_RESERVE_MEM=0 +CONFIG_ESP32_PANIC_PRINT_HALT= +CONFIG_ESP32_PANIC_PRINT_REBOOT=y +CONFIG_ESP32_PANIC_SILENT_REBOOT= +CONFIG_ESP32_PANIC_GDBSTUB= +CONFIG_ESP32_DEBUG_OCDAWARE=y +CONFIG_ESP32_DEBUG_STUBS_ENABLE= +CONFIG_INT_WDT=y +CONFIG_INT_WDT_TIMEOUT_MS=300 +CONFIG_INT_WDT_CHECK_CPU1=y +CONFIG_TASK_WDT=y +CONFIG_TASK_WDT_PANIC= +CONFIG_TASK_WDT_TIMEOUT_S=5 +CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU0=y +CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU1=y +CONFIG_BROWNOUT_DET=y +CONFIG_BROWNOUT_DET_LVL_SEL_0=y +CONFIG_BROWNOUT_DET_LVL_SEL_1= +CONFIG_BROWNOUT_DET_LVL_SEL_2= +CONFIG_BROWNOUT_DET_LVL_SEL_3= +CONFIG_BROWNOUT_DET_LVL_SEL_4= +CONFIG_BROWNOUT_DET_LVL_SEL_5= +CONFIG_BROWNOUT_DET_LVL_SEL_6= +CONFIG_BROWNOUT_DET_LVL_SEL_7= +CONFIG_BROWNOUT_DET_LVL=0 +CONFIG_REDUCE_PHY_TX_POWER=y +CONFIG_ESP32_TIME_SYSCALL_USE_RTC_FRC1=y +CONFIG_ESP32_TIME_SYSCALL_USE_RTC= +CONFIG_ESP32_TIME_SYSCALL_USE_FRC1= +CONFIG_ESP32_TIME_SYSCALL_USE_NONE= +CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_RC=y +CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_CRYSTAL= +CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_OSC= +CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_8MD256= +CONFIG_ESP32_RTC_CLK_CAL_CYCLES=1024 +CONFIG_ESP32_DEEP_SLEEP_WAKEUP_DELAY=2000 +CONFIG_ESP32_XTAL_FREQ_40=y +CONFIG_ESP32_XTAL_FREQ_26= +CONFIG_ESP32_XTAL_FREQ_AUTO= +CONFIG_ESP32_XTAL_FREQ=40 +CONFIG_DISABLE_BASIC_ROM_CONSOLE= +CONFIG_NO_BLOBS= +CONFIG_ESP_TIMER_PROFILING= +CONFIG_COMPATIBLE_PRE_V2_1_BOOTLOADERS= +CONFIG_ESP_ERR_TO_NAME_LOOKUP=y +CONFIG_ESP32_DPORT_DIS_INTERRUPT_LVL=5 + +# +# Wi-Fi +# +CONFIG_ESP32_WIFI_STATIC_RX_BUFFER_NUM=10 +CONFIG_ESP32_WIFI_DYNAMIC_RX_BUFFER_NUM=32 +CONFIG_ESP32_WIFI_STATIC_TX_BUFFER= +CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER=y +CONFIG_ESP32_WIFI_TX_BUFFER_TYPE=1 +CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER_NUM=32 +CONFIG_ESP32_WIFI_CSI_ENABLED= +CONFIG_ESP32_WIFI_AMPDU_TX_ENABLED=y +CONFIG_ESP32_WIFI_TX_BA_WIN=6 +CONFIG_ESP32_WIFI_AMPDU_RX_ENABLED=y +CONFIG_ESP32_WIFI_RX_BA_WIN=6 +CONFIG_ESP32_WIFI_NVS_ENABLED=y +CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_0=y +CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_1= +CONFIG_ESP32_WIFI_SOFTAP_BEACON_MAX_LEN=752 +CONFIG_ESP32_WIFI_MGMT_SBUF_NUM=32 +CONFIG_ESP32_WIFI_DEBUG_LOG_ENABLE= +CONFIG_ESP32_WIFI_IRAM_OPT=y +CONFIG_ESP32_WIFI_RX_IRAM_OPT=y + +# +# PHY +# +CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE=y +CONFIG_ESP32_PHY_INIT_DATA_IN_PARTITION= +CONFIG_ESP32_PHY_MAX_WIFI_TX_POWER=20 +CONFIG_ESP32_PHY_MAX_TX_POWER=20 + +# +# Power Management +# +CONFIG_PM_ENABLE= + +# +# ADC-Calibration +# +CONFIG_ADC_CAL_EFUSE_TP_ENABLE=y +CONFIG_ADC_CAL_EFUSE_VREF_ENABLE=y +CONFIG_ADC_CAL_LUT_ENABLE=y + +# +# Event Loop Library +# +CONFIG_EVENT_LOOP_PROFILING= + +# +# ESP HTTP client +# +CONFIG_ESP_HTTP_CLIENT_ENABLE_HTTPS=y +CONFIG_ESP_HTTP_CLIENT_ENABLE_BASIC_AUTH= + +# +# HTTP Server +# +CONFIG_HTTPD_MAX_REQ_HDR_LEN=512 +CONFIG_HTTPD_MAX_URI_LEN=512 +CONFIG_HTTPD_ERR_RESP_NO_DELAY=y +CONFIG_HTTPD_PURGE_BUF_LEN=32 +CONFIG_HTTPD_LOG_PURGE_DATA= + +# +# ESP HTTPS OTA +# +CONFIG_OTA_ALLOW_HTTP= + +# +# Core dump +# +CONFIG_ESP32_ENABLE_COREDUMP_TO_FLASH= +CONFIG_ESP32_ENABLE_COREDUMP_TO_UART= +CONFIG_ESP32_ENABLE_COREDUMP_TO_NONE=y +CONFIG_ESP32_ENABLE_COREDUMP= + +# +# Ethernet +# +CONFIG_DMA_RX_BUF_NUM=10 +CONFIG_DMA_TX_BUF_NUM=10 +CONFIG_EMAC_L2_TO_L3_RX_BUF_MODE=y +CONFIG_EMAC_CHECK_LINK_PERIOD_MS=2000 +CONFIG_EMAC_TASK_PRIORITY=20 +CONFIG_EMAC_TASK_STACK_SIZE=3072 + +# +# FAT Filesystem support +# +CONFIG_FATFS_CODEPAGE_DYNAMIC= +CONFIG_FATFS_CODEPAGE_437=y +CONFIG_FATFS_CODEPAGE_720= +CONFIG_FATFS_CODEPAGE_737= +CONFIG_FATFS_CODEPAGE_771= +CONFIG_FATFS_CODEPAGE_775= +CONFIG_FATFS_CODEPAGE_850= +CONFIG_FATFS_CODEPAGE_852= +CONFIG_FATFS_CODEPAGE_855= +CONFIG_FATFS_CODEPAGE_857= +CONFIG_FATFS_CODEPAGE_860= +CONFIG_FATFS_CODEPAGE_861= +CONFIG_FATFS_CODEPAGE_862= +CONFIG_FATFS_CODEPAGE_863= +CONFIG_FATFS_CODEPAGE_864= +CONFIG_FATFS_CODEPAGE_865= +CONFIG_FATFS_CODEPAGE_866= +CONFIG_FATFS_CODEPAGE_869= +CONFIG_FATFS_CODEPAGE_932= +CONFIG_FATFS_CODEPAGE_936= +CONFIG_FATFS_CODEPAGE_949= +CONFIG_FATFS_CODEPAGE_950= +CONFIG_FATFS_CODEPAGE=437 +CONFIG_FATFS_LFN_NONE=y +CONFIG_FATFS_LFN_HEAP= +CONFIG_FATFS_LFN_STACK= +CONFIG_FATFS_FS_LOCK=0 +CONFIG_FATFS_TIMEOUT_MS=10000 +CONFIG_FATFS_PER_FILE_CACHE=y + +# +# Modbus configuration +# +CONFIG_MB_QUEUE_LENGTH=20 +CONFIG_MB_SERIAL_TASK_STACK_SIZE=2048 +CONFIG_MB_SERIAL_BUF_SIZE=256 +CONFIG_MB_SERIAL_TASK_PRIO=10 +CONFIG_MB_CONTROLLER_SLAVE_ID_SUPPORT= +CONFIG_MB_CONTROLLER_NOTIFY_TIMEOUT=20 +CONFIG_MB_CONTROLLER_NOTIFY_QUEUE_SIZE=20 +CONFIG_MB_CONTROLLER_STACK_SIZE=4096 +CONFIG_MB_EVENT_QUEUE_TIMEOUT=20 +CONFIG_MB_TIMER_PORT_ENABLED=y +CONFIG_MB_TIMER_GROUP=0 +CONFIG_MB_TIMER_INDEX=0 + +# +# FreeRTOS +# +CONFIG_FREERTOS_UNICORE= +CONFIG_FREERTOS_NO_AFFINITY=0x7FFFFFFF +CONFIG_FREERTOS_CORETIMER_0=y +CONFIG_FREERTOS_CORETIMER_1= +CONFIG_FREERTOS_HZ=100 +CONFIG_FREERTOS_ASSERT_ON_UNTESTED_FUNCTION=y +CONFIG_FREERTOS_CHECK_STACKOVERFLOW_NONE= +CONFIG_FREERTOS_CHECK_STACKOVERFLOW_PTRVAL= +CONFIG_FREERTOS_CHECK_STACKOVERFLOW_CANARY=y +CONFIG_FREERTOS_WATCHPOINT_END_OF_STACK= +CONFIG_FREERTOS_INTERRUPT_BACKTRACE=y +CONFIG_FREERTOS_THREAD_LOCAL_STORAGE_POINTERS=1 +CONFIG_FREERTOS_ASSERT_FAIL_ABORT=y +CONFIG_FREERTOS_ASSERT_FAIL_PRINT_CONTINUE= +CONFIG_FREERTOS_ASSERT_DISABLE= +CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=1536 +CONFIG_FREERTOS_ISR_STACKSIZE=1536 +CONFIG_FREERTOS_LEGACY_HOOKS= +CONFIG_FREERTOS_MAX_TASK_NAME_LEN=16 +CONFIG_SUPPORT_STATIC_ALLOCATION= +CONFIG_TIMER_TASK_PRIORITY=1 +CONFIG_TIMER_TASK_STACK_DEPTH=2048 +CONFIG_TIMER_QUEUE_LENGTH=10 +CONFIG_FREERTOS_QUEUE_REGISTRY_SIZE=0 +CONFIG_FREERTOS_USE_TRACE_FACILITY= +CONFIG_FREERTOS_GENERATE_RUN_TIME_STATS= +CONFIG_FREERTOS_DEBUG_INTERNALS= +CONFIG_FREERTOS_CHECK_MUTEX_GIVEN_BY_OWNER=y +CONFIG_FREERTOS_CHECK_PORT_CRITICAL_COMPLIANCE= + +# +# Heap memory debugging +# +CONFIG_HEAP_POISONING_DISABLED=y +CONFIG_HEAP_POISONING_LIGHT= +CONFIG_HEAP_POISONING_COMPREHENSIVE= +CONFIG_HEAP_TRACING= + +# +# libsodium +# + +# +# Log output +# +CONFIG_LOG_DEFAULT_LEVEL_NONE= +CONFIG_LOG_DEFAULT_LEVEL_ERROR= +CONFIG_LOG_DEFAULT_LEVEL_WARN= +CONFIG_LOG_DEFAULT_LEVEL_INFO=y +CONFIG_LOG_DEFAULT_LEVEL_DEBUG= +CONFIG_LOG_DEFAULT_LEVEL_VERBOSE= +CONFIG_LOG_DEFAULT_LEVEL=3 +CONFIG_LOG_COLORS=y + +# +# LWIP +# +CONFIG_L2_TO_L3_COPY= +CONFIG_ETHARP_SUPPORT_VLAN= +CONFIG_LWIP_IRAM_OPTIMIZATION= +CONFIG_LWIP_MAX_SOCKETS=10 +CONFIG_LWIP_RANDOMIZE_INITIAL_LOCAL_PORTS=y +CONFIG_USE_ONLY_LWIP_SELECT= +CONFIG_LWIP_SO_LINGER= +CONFIG_LWIP_SO_REUSE=y +CONFIG_LWIP_SO_REUSE_RXTOALL=y +CONFIG_LWIP_SO_RCVBUF= +CONFIG_LWIP_IP_FRAG=y +CONFIG_LWIP_IP_REASSEMBLY= +CONFIG_LWIP_STATS= +CONFIG_LWIP_ETHARP_TRUST_IP_MAC= +CONFIG_ESP_GRATUITOUS_ARP=y +CONFIG_GARP_TMR_INTERVAL=60 +CONFIG_TCPIP_RECVMBOX_SIZE=32 +CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y +CONFIG_LWIP_DHCP_RESTORE_LAST_IP= + +# +# DHCP server +# +CONFIG_LWIP_DHCPS_LEASE_UNIT=60 +CONFIG_LWIP_DHCPS_MAX_STATION_NUM=8 +CONFIG_LWIP_AUTOIP= +CONFIG_LWIP_IPV6_AUTOCONFIG= +CONFIG_LWIP_NETIF_LOOPBACK=y +CONFIG_LWIP_LOOPBACK_MAX_PBUFS=8 + +# +# TCP +# +CONFIG_LWIP_MAX_ACTIVE_TCP=16 +CONFIG_LWIP_MAX_LISTENING_TCP=16 +CONFIG_TCP_MAXRTX=12 +CONFIG_TCP_SYNMAXRTX=6 +CONFIG_TCP_MSS=1436 +CONFIG_TCP_MSL=60000 +CONFIG_TCP_SND_BUF_DEFAULT=5744 +CONFIG_TCP_WND_DEFAULT=5744 +CONFIG_TCP_RECVMBOX_SIZE=6 +CONFIG_TCP_QUEUE_OOSEQ=y +CONFIG_ESP_TCP_KEEP_CONNECTION_WHEN_IP_CHANGES= +CONFIG_TCP_OVERSIZE_MSS=y +CONFIG_TCP_OVERSIZE_QUARTER_MSS= +CONFIG_TCP_OVERSIZE_DISABLE= + +# +# UDP +# +CONFIG_LWIP_MAX_UDP_PCBS=16 +CONFIG_UDP_RECVMBOX_SIZE=6 +CONFIG_TCPIP_TASK_STACK_SIZE=3072 +CONFIG_TCPIP_TASK_AFFINITY_NO_AFFINITY=y +CONFIG_TCPIP_TASK_AFFINITY_CPU0= +CONFIG_TCPIP_TASK_AFFINITY_CPU1= +CONFIG_TCPIP_TASK_AFFINITY=0x7FFFFFFF +CONFIG_PPP_SUPPORT= + +# +# ICMP +# +CONFIG_LWIP_MULTICAST_PING= +CONFIG_LWIP_BROADCAST_PING= + +# +# LWIP RAW API +# +CONFIG_LWIP_MAX_RAW_PCBS=16 + +# +# SNTP +# +CONFIG_LWIP_DHCP_MAX_NTP_SERVERS=1 +CONFIG_LWIP_SNTP_UPDATE_DELAY=3600000 +CONFIG_LWIP_ESP_LWIP_ASSERT=y + +# +# mbedTLS +# +CONFIG_MBEDTLS_INTERNAL_MEM_ALLOC=y +CONFIG_MBEDTLS_DEFAULT_MEM_ALLOC= +CONFIG_MBEDTLS_CUSTOM_MEM_ALLOC= +CONFIG_MBEDTLS_ASYMMETRIC_CONTENT_LEN=y +CONFIG_MBEDTLS_SSL_IN_CONTENT_LEN=16384 +CONFIG_MBEDTLS_SSL_OUT_CONTENT_LEN=4096 +CONFIG_MBEDTLS_DEBUG= +CONFIG_MBEDTLS_ECP_RESTARTABLE= +CONFIG_MBEDTLS_CMAC_C= +CONFIG_MBEDTLS_HARDWARE_AES=y +CONFIG_MBEDTLS_HARDWARE_MPI=y +CONFIG_MBEDTLS_MPI_USE_INTERRUPT= +CONFIG_MBEDTLS_HARDWARE_SHA=y +CONFIG_MBEDTLS_HAVE_TIME=y +CONFIG_MBEDTLS_HAVE_TIME_DATE= +CONFIG_MBEDTLS_TLS_SERVER_AND_CLIENT=y +CONFIG_MBEDTLS_TLS_SERVER_ONLY= +CONFIG_MBEDTLS_TLS_CLIENT_ONLY= +CONFIG_MBEDTLS_TLS_DISABLED= +CONFIG_MBEDTLS_TLS_SERVER=y +CONFIG_MBEDTLS_TLS_CLIENT=y +CONFIG_MBEDTLS_TLS_ENABLED=y + +# +# TLS Key Exchange Methods +# +CONFIG_MBEDTLS_PSK_MODES= +CONFIG_MBEDTLS_KEY_EXCHANGE_RSA=y +CONFIG_MBEDTLS_KEY_EXCHANGE_DHE_RSA=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ELLIPTIC_CURVE=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_RSA=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_ECDSA=y +CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_RSA=y +CONFIG_MBEDTLS_SSL_RENEGOTIATION=y +CONFIG_MBEDTLS_SSL_PROTO_SSL3= +CONFIG_MBEDTLS_SSL_PROTO_TLS1=y +CONFIG_MBEDTLS_SSL_PROTO_TLS1_1=y +CONFIG_MBEDTLS_SSL_PROTO_TLS1_2=y +CONFIG_MBEDTLS_SSL_PROTO_DTLS= +CONFIG_MBEDTLS_SSL_ALPN=y +CONFIG_MBEDTLS_SSL_SESSION_TICKETS=y + +# +# Symmetric Ciphers +# +CONFIG_MBEDTLS_AES_C=y +CONFIG_MBEDTLS_CAMELLIA_C= +CONFIG_MBEDTLS_DES_C= +CONFIG_MBEDTLS_RC4_DISABLED=y +CONFIG_MBEDTLS_RC4_ENABLED_NO_DEFAULT= +CONFIG_MBEDTLS_RC4_ENABLED= +CONFIG_MBEDTLS_BLOWFISH_C= +CONFIG_MBEDTLS_XTEA_C= +CONFIG_MBEDTLS_CCM_C=y +CONFIG_MBEDTLS_GCM_C=y +CONFIG_MBEDTLS_RIPEMD160_C= + +# +# Certificates +# +CONFIG_MBEDTLS_PEM_PARSE_C=y +CONFIG_MBEDTLS_PEM_WRITE_C=y +CONFIG_MBEDTLS_X509_CRL_PARSE_C=y +CONFIG_MBEDTLS_X509_CSR_PARSE_C=y +CONFIG_MBEDTLS_ECP_C=y +CONFIG_MBEDTLS_ECDH_C=y +CONFIG_MBEDTLS_ECDSA_C=y +CONFIG_MBEDTLS_ECP_DP_SECP192R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP224R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP256R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP384R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP521R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP192K1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP224K1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_SECP256K1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_BP256R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_BP384R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_BP512R1_ENABLED=y +CONFIG_MBEDTLS_ECP_DP_CURVE25519_ENABLED=y +CONFIG_MBEDTLS_ECP_NIST_OPTIM=y + +# +# mDNS +# +CONFIG_MDNS_MAX_SERVICES=10 + +# +# ESP-MQTT Configurations +# +CONFIG_MQTT_PROTOCOL_311=y +CONFIG_MQTT_TRANSPORT_SSL=y +CONFIG_MQTT_TRANSPORT_WEBSOCKET=y +CONFIG_MQTT_TRANSPORT_WEBSOCKET_SECURE=y +CONFIG_MQTT_USE_CUSTOM_CONFIG= +CONFIG_MQTT_TASK_CORE_SELECTION_ENABLED= +CONFIG_MQTT_CUSTOM_OUTBOX= + +# +# NVS +# + +# +# OpenSSL +# +CONFIG_OPENSSL_DEBUG= +CONFIG_OPENSSL_ASSERT_DO_NOTHING= +CONFIG_OPENSSL_ASSERT_EXIT=y + +# +# PThreads +# +CONFIG_ESP32_PTHREAD_TASK_PRIO_DEFAULT=5 +CONFIG_ESP32_PTHREAD_TASK_STACK_SIZE_DEFAULT=3072 +CONFIG_PTHREAD_STACK_MIN=768 +CONFIG_ESP32_DEFAULT_PTHREAD_CORE_NO_AFFINITY=y +CONFIG_ESP32_DEFAULT_PTHREAD_CORE_0= +CONFIG_ESP32_DEFAULT_PTHREAD_CORE_1= +CONFIG_ESP32_PTHREAD_TASK_CORE_DEFAULT=-1 +CONFIG_ESP32_PTHREAD_TASK_NAME_DEFAULT="pthread" + +# +# SPI Flash driver +# +CONFIG_SPI_FLASH_VERIFY_WRITE= +CONFIG_SPI_FLASH_ENABLE_COUNTERS= +CONFIG_SPI_FLASH_ROM_DRIVER_PATCH=y +CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ABORTS=y +CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_FAILS= +CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ALLOWED= +CONFIG_SPI_FLASH_YIELD_DURING_ERASE=y +CONFIG_SPI_FLASH_ERASE_YIELD_DURATION_MS=20 +CONFIG_SPI_FLASH_ERASE_YIELD_TICKS=1 + +# +# SPIFFS Configuration +# +CONFIG_SPIFFS_MAX_PARTITIONS=3 + +# +# SPIFFS Cache Configuration +# +CONFIG_SPIFFS_CACHE=y +CONFIG_SPIFFS_CACHE_WR=y +CONFIG_SPIFFS_CACHE_STATS= +CONFIG_SPIFFS_PAGE_CHECK=y +CONFIG_SPIFFS_GC_MAX_RUNS=10 +CONFIG_SPIFFS_GC_STATS= +CONFIG_SPIFFS_PAGE_SIZE=256 +CONFIG_SPIFFS_OBJ_NAME_LEN=32 +CONFIG_SPIFFS_USE_MAGIC=y +CONFIG_SPIFFS_USE_MAGIC_LENGTH=y +CONFIG_SPIFFS_META_LENGTH=4 +CONFIG_SPIFFS_USE_MTIME=y + +# +# Debug Configuration +# +CONFIG_SPIFFS_DBG= +CONFIG_SPIFFS_API_DBG= +CONFIG_SPIFFS_GC_DBG= +CONFIG_SPIFFS_CACHE_DBG= +CONFIG_SPIFFS_CHECK_DBG= +CONFIG_SPIFFS_TEST_VISUALISATION= + +# +# TCP/IP Adapter +# +CONFIG_IP_LOST_TIMER_INTERVAL=120 +CONFIG_TCPIP_LWIP=y + +# +# Unity unit testing library +# +CONFIG_UNITY_ENABLE_FLOAT=y +CONFIG_UNITY_ENABLE_DOUBLE=y +CONFIG_UNITY_ENABLE_COLOR= +CONFIG_UNITY_ENABLE_IDF_TEST_RUNNER=y +CONFIG_UNITY_ENABLE_FIXTURE= + +# +# Virtual file system +# +CONFIG_SUPPRESS_SELECT_DEBUG_OUTPUT=y +CONFIG_SUPPORT_TERMIOS=y + +# +# Wear Levelling +# +CONFIG_WL_SECTOR_SIZE_512= +CONFIG_WL_SECTOR_SIZE_4096=y +CONFIG_WL_SECTOR_SIZE=4096 + +# +# Wi-Fi Provisioning Manager +# +CONFIG_WIFI_PROV_SCAN_MAX_ENTRIES=16 + +# +# Supplicant +# +CONFIG_WPA_WPS_WARS= diff --git a/protocol/README.md b/protocol/README.md new file mode 100644 index 0000000..970cc7b --- /dev/null +++ b/protocol/README.md @@ -0,0 +1,77 @@ +# UART Protocol generation + +## Protocol + +The Crazyflie and the AI-deck communicate over UART, to which we added a simple application-level protocol to exchange C structures. + +The messages are composed by an `N` byte long header that identify the message type, followed by the buffer (of fixed, known size) containing the structure. + +## Generation + +The script `generate.py` generates the C code that implements the protocol from a specification written in Python. + +```bash +python3 generate.py +``` + +For example, this config file +``` +from base import Config, Input, c_uint8 + +header_length = 4 + +class MyConfig(Config): + an_integer_field: c_uint8 + a_float_field: c_float + +class MyInput(Input): + an_integer_field: c_uint8 + a_float_field: c_float + +configs = [ + MyConfig(name='myconfig', group='myconfig', header="!MYC") +] + +inputs = [ + MyInput(name='my_input', group='myinput', header="\\x90\\x19\\x08\\x31") +] +``` +will generate the code for 1 configuration and 1 input. + +We include the specification for the image streaming gap application. + +The structure are of two types: configuration and inputs. + +### Configuration + +A configuration is group together a set of [Crazyflie firmware] parameters that are synchronized between Crazyflie and GAP. On the Crazyflie side, the driver checks at regular intervals which parameters have changed and send them to the GAP. In turns, the GAP should send the configuration each time it changes (and in particular at initialization and after each requested change). + +For example, the above specification is converted to two parameters: +``` +PARAM_GROUP_START(my_config) +PARAM_ADD(PARAM_UINT8, an_integer_field, &(__my_config__config.value.an_integer_field)) +PARAM_ADD(PARAM_FLOAT, a_float_field, &(__my_config__config.value.a_float_field)) +PARAM_GROUP_STOP(my_config) +``` + + +### Input + +An input is a structured message from the GAP to the Crazyflie. For every input, the user has to implement a callback, which get triggered every time a new message is received. For the above example, the user has to define the callback +``` +static void my_input_callback(my_input_t *); +``` +which is defined in the generate file. + + +## Building + +The generated files should be included in the building tree: +- files in `build/crazyflie/` have to be copied in `/src/deck/driver/src` +- files in `build/gap` in `/src` + +The generate files are not meant to be modified or compiled as standalone file, but included in other files that implement the rest of the UART communication: +- `/src/deck/driver/src/aideck.c` +- `/src/uart.c` + +which are also provided in this repo. diff --git a/protocol/base.py b/protocol/base.py new file mode 100644 index 0000000..f6d94c0 --- /dev/null +++ b/protocol/base.py @@ -0,0 +1,67 @@ +from ctypes import (c_char, c_float, c_int8, c_int16, c_int32, c_uint8, + c_uint16, c_uint32) +from typing import Optional, Type + +_conversions = { + c_float: ('float' , 'FLOAT' , '%.3g'), # noqa + c_uint8: ('uint8_t' , 'UINT8' , '%u' ), # noqa + c_uint16: ('uint16_t', 'UINT16', '%u' ), # noqa + c_uint32: ('uint32_t', 'UINT32', '%u' ), # noqa + c_int8: ('int8_t' , 'INT8' , '%d' ), # noqa + c_int16: ('int16_t' , 'INT16' , '%d' ), # noqa + c_int32: ('int32_t' , 'INT32' , '%d' ), # noqa + c_char: ('uint8_t' , 'UINT16', '%c' ), # noqa +} + + +def to_c(t: Type) -> str: + return _conversions[t][0] + + +def to_cf(t: Type) -> str: + return _conversions[t][1] + + +def to_format(t: Type) -> str: + return _conversions[t][2] + + +class Base: + def __init__(self, name: str, header: str, group: str = ''): + self.name = name + self.group = group + self.header_value = header + self._cb = '__' + self.name + '_cb' + + @property + def typename(self) -> str: + return self.name + "_t" + + @property + def value(self) -> str: + return '__' + self.name + '__config' + + @property + def cb(self) -> str: + return self._cb + + @property + def header(self) -> str: + return '__' + self.name + '_header' + + +class Input(Base): + @property + def set_cb(self) -> str: + return self.name + '_callback' + + def __init__(self, name: str, header: str, group: str = '', + callback: Optional[str] = None): + super(Input, self).__init__(name=name, group=group, + header=header) + if callback is not None: + self._cb = callback + + +class Config(Base): + pass diff --git a/protocol/example_config.py b/protocol/example_config.py new file mode 100644 index 0000000..e0a88c9 --- /dev/null +++ b/protocol/example_config.py @@ -0,0 +1,22 @@ +from base import Config, Input, c_uint8, c_float + +header_length = 4 + + +class MyConfig(Config): + an_integer_field: c_uint8 + a_float_field: c_float + + +class MyInput(Input): + an_integer_field: c_uint8 + a_float_field: c_float + + +configs = [ + MyConfig(name='my_config', group='my_config', header="!MYC") +] + +inputs = [ + MyInput(name='my_input', header="\\x90\\x19\\x08\\x31", callback="my_cb") +] diff --git a/protocol/generate.py b/protocol/generate.py new file mode 100644 index 0000000..084ade8 --- /dev/null +++ b/protocol/generate.py @@ -0,0 +1,61 @@ +import jinja2 +from typing import Type +from base import Base, Config, to_c, to_cf, to_format +import os +from typing import Tuple +from datetime import datetime as dt +import importlib.util +import argparse + + +def _f(item: Tuple[str, Type]) -> str: + name, t = item + return f'{name}={to_format(t)}' + + +def input_struct(t: Base) -> str: + return f'{{ .header = "{t.header_value}", .callback = {t.cb}, .size = sizeof({t.typename}) }}' + + +def is_config(t: Base) -> bool: + return isinstance(t, Config) + + +def main(config_path: str) -> None: + + config_spec = importlib.util.spec_from_file_location("c", config_path) + c = importlib.util.module_from_spec(config_spec) + if not config_spec.loader: + raise RuntimeError(f"Could not load config from {config_path}") + config_spec.loader.exec_module(c) # type: ignore + e = jinja2.Environment(loader=jinja2.FileSystemLoader('./templates')) + e.filters['c'] = to_c + e.filters['cf'] = to_cf + e.filters['f'] = _f + e.filters['input'] = input_struct + e.tests['config'] = is_config + with open(config_path, 'r') as f: + source = f.read() + date = dt.now().strftime("%D") + build_folder = os.path.join(os.path.dirname(__file__), 'build') + os.makedirs(build_folder, exist_ok=True) + cf_build_folder = os.path.join(build_folder, 'crazyflie') + os.makedirs(cf_build_folder, exist_ok=True) + gap_build_folder = os.path.join(build_folder, 'gap') + os.makedirs(gap_build_folder, exist_ok=True) + template = e.get_template('crazyflie.aideck_p.h.j2') + with open(os.path.join(cf_build_folder, 'aideck_protocol.c'), 'w') as f: + f.write(template.render(config=c, source=source, date=date)) + template = e.get_template('gap.protocol.h.j2') + with open(os.path.join(gap_build_folder, 'uart_protocol.h'), 'w') as f: + f.write(template.render(config=c)) + template = e.get_template('gap._protocol.c.j2') + with open(os.path.join(gap_build_folder, 'uart_protocol.c'), 'w') as f: + f.write(template.render(config=c)) + + +if __name__ == '__main__': + parser = argparse.ArgumentParser() + parser.add_argument("path", help="the configuration file path") + args = parser.parse_args() + main(args.path) diff --git a/protocol/stream_config.py b/protocol/stream_config.py new file mode 100644 index 0000000..657626b --- /dev/null +++ b/protocol/stream_config.py @@ -0,0 +1,27 @@ +from base import Config, c_uint8, c_uint16 # type: ignore + +header_length = 4 + + +class Camera(Config): + marginTop: c_uint16 + marginRight: c_uint16 + marginBottom: c_uint16 + marginLeft: c_uint16 + format: c_uint8 # noqa + step: c_uint8 + target_value: c_uint8 + + +class Stream(Config): + on: c_uint8 + format: c_uint8 # noqa + transport: c_uint8 + + +configs = [ + Camera(name='camera', group='aideck_HIMAX', header="!CAM"), + Stream(name='stream', group='aideck_stream', header="!STR"), +] + +inputs = [] diff --git a/protocol/templates/crazyflie.aideck_p.h.j2 b/protocol/templates/crazyflie.aideck_p.h.j2 new file mode 100644 index 0000000..4581e58 --- /dev/null +++ b/protocol/templates/crazyflie.aideck_p.h.j2 @@ -0,0 +1,50 @@ +{%- from "macros.j2" import typedef, log, value, cb, check, param -%} +/* + This file was autogenerate on {{date}} from the following Python config: + + {{source | indent(2)}} +*/ + +#define HEADER_LENGTH {{config.header_length}} +#define INPUT_NUMBER {{ (config.inputs + config.configs) | count}} + +typedef struct { + const char *header; + uint8_t size; + void (*callback)(void *); + bool valid; +} input_t; + +{%- for data_t in config.configs %} +// --- config {{data_t.name}} +{{ typedef(data_t) }} +{{ log(data_t) }} +{{- value(data_t) -}} +{{- cb(data_t) -}} +{% endfor -%} + +{%- for data_t in config.inputs %} +// --- input {{data_t.name}} +{{ typedef(data_t) }} +{{ log(data_t) }} +// To be implemented +static void {{data_t.set_cb}}({{data_t.typename}} *); +{{- cb(data_t) -}} +{% endfor -%} + +static input_t config.inputs[INPUT_NUMBER] = { + {{(config.configs + config.inputs) | map('input') | join(",\n") | indent(2)}} +}; +void update_config(void *data) +{ +{%- for data_t in config.configs -%} + {{- check(data_t) | indent(2) -}} +{%- endfor %} +} +{% for group, cs in config.configs | groupby('group') %} +PARAM_GROUP_START({{group}}) +{%- for data_t in cs -%} +{{- param(data_t) -}} +{%- endfor %} +PARAM_GROUP_STOP({{group}}) +{% endfor %} diff --git a/protocol/templates/gap._protocol.c.j2 b/protocol/templates/gap._protocol.c.j2 new file mode 100644 index 0000000..48b99db --- /dev/null +++ b/protocol/templates/gap._protocol.c.j2 @@ -0,0 +1,46 @@ +{%- from "macros.j2" import typedef -%} + +#include "protocol.h" + +#define HEADER_LENGTH 4 +#define UART_RX_PROTOCOL +static PI_L2 uint8_t header[HEADER_LENGTH]; +static pi_task_t task; + +{%- for data_t in config.configs + config.inputs %} +void send_{{data_t.name}}_config({{data_t.typename}} *config) +{ + if(!enabled) return; +#ifdef {{data_t.name | upper}}_HEADER + pi_uart_write(&device, {{data_t.name | upper}}_HEADER, HEADER_LENGTH); + pi_uart_write(&device, (uint8_t *)config, sizeof({{data_t.typename}})); +#endif +} +{% endfor %} + +static void received_header(void *arg) +{ +{% for data_t in config.configs %} +#ifdef {{data_t.name | upper}}_HEADER + if(memcmp(header, {{data_t.name | upper}}_HEADER, HEADER_LENGTH) == 0) + { + PI_L2 uint8_t rx_buffer[sizeof({{data_t.typename}})]; + if(pi_uart_read(&device, rx_buffer, sizeof({{data_t.typename}}))) + { + LOG_ERROR("Failed to read {{data_t.name}} config\n"); + } + else{ + set_{{data_t.name}}_config(({{data_t.typename}} *)rx_buffer); + } + goto: done + } +#endif +{% endfor %} + done: pi_uart_read_async(&device, header, HEADER_LENGTH, &task); +} + +static void start_rx_protocol() +{ + pi_task_callback(&task, (void *) received_header, &task); + pi_uart_read_async(&device, header, HEADER_LENGTH, &task); +} diff --git a/protocol/templates/gap.protocol.h.j2 b/protocol/templates/gap.protocol.h.j2 new file mode 100644 index 0000000..a4d30ec --- /dev/null +++ b/protocol/templates/gap.protocol.h.j2 @@ -0,0 +1,16 @@ +{% from "macros.j2" import typedef, cb, check, param -%} +#include + +{%- for data_t in config.configs %} +{{ typedef(data_t) }} +#define {{data_t.name | upper}}_HEADER "{{data_t.header_value}}" +void send_{{data_t.name}}_config({{data_t.typename}} *config); +// To be implemented if {{data_t.name | upper}}_HEADER is defined +void set_{{data_t.name}}_config({{data_t.typename}} *config); +{% endfor -%} + +{%- for data_t in config.inputs %} +{{ typedef(data_t) }} +#define {{data_t.name | upper}}_HEADER "{{data_t.header_value}}" +void send_{{data_t.name}}({{data_t.typename}} *config); +{% endfor -%} diff --git a/protocol/templates/macros.j2 b/protocol/templates/macros.j2 new file mode 100644 index 0000000..289ab51 --- /dev/null +++ b/protocol/templates/macros.j2 @@ -0,0 +1,53 @@ +{% macro typedef(data_t) %} +typedef struct { +{%- for name, type in data_t.__annotations__.items() %} + {{type | c}} {{name}}; +{%- endfor %} +} __attribute__((packed)) {{data_t.typename}}; +{% endmacro %} + +{% macro log(data_t) %} +void log_{{data_t.name}}({{data_t.typename}} *value) +{ + DEBUG_PRINT("{{data_t.__annotations__.items() | map('f') | join(', ')}}\n", value->{{data_t.__annotations__.keys() | join(', value->')}}); +} +{% endmacro %} + +{% macro value(data_t) %} +static struct { + {{data_t.typename}} value, dvalue; + const char *header; +} {{data_t.value}} = { .header= "{{data_t.header_value | safe}}" }; +{% endmacro %} + +{% macro param(data_t) %} +{%- for name, type in data_t.__annotations__.items() %} +PARAM_ADD(PARAM_{{type | cf}}, {{name}}, &({{data_t.value}}.value.{{name}})) +{%- endfor -%} +{% endmacro %} + +{% macro cb(data_t) %} +static void {{data_t.cb}}(void *buffer) +{ + {{data_t.typename}} *value = ({{data_t.typename}} *)buffer; + {%- if data_t is not config %} + {{data_t.set_cb}}(value); + {%- else %} + {{data_t.value}}.value = {{data_t.value}}.dvalue = *value; + DEBUG_PRINT("GAP has updated {{data_t.name}} config\n"); + log_{{data_t.name}}(&({{data_t.value}}.value)); + {%- endif %} +} +{% endmacro %} + +{% macro check(data_t) %} +if(memcmp(&({{data_t.value}}.value), &({{data_t.value}}.dvalue), sizeof({{data_t.typename}}))) +{ + DEBUG_PRINT("Will request GAP to update {{data_t.name}} config\n"); + log_{{data_t.name}}(&({{data_t.value}}.value)); + uart1SendData(HEADER_LENGTH, (uint8_t *) {{data_t.value}}.header); + uart1SendData(sizeof({{data_t.typename}}), (uint8_t *)&({{data_t.value}}.value)); + {{data_t.value}}.dvalue = {{data_t.value}}.value; + return; +} +{% endmacro %}