From f23a5f8e5a639d167482c28fa55691a07011d2b1 Mon Sep 17 00:00:00 2001 From: Marcus Date: Sun, 12 Dec 2021 08:20:14 +0100 Subject: [PATCH 01/10] Reworked example to work with new ESP32 firmware * Uses FreeRTOS * Uses new protocol and not frame streamer * Uses openCV for viewing and decoding Bayer images * No JPEG support yet, only raw * Assumes color camera (Bayer encoded image) * Lots of debug stuff left * ~7 FPS raw thoughput to computer --- .gitignore | 3 + .../wifi_jpeg_streamer/Makefile | 10 +- .../wifi_jpeg_streamer/com.c | 296 ++++++++++++++ .../wifi_jpeg_streamer/com.h | 29 ++ .../wifi_jpeg_streamer/opencv-viewer.py | 116 ++++++ .../wifi_jpeg_streamer/routing_info.h | 40 ++ .../wifi_jpeg_streamer/test.c | 368 +++++++++++------- NINA/CMakeLists.txt | 2 - NINA/firmware/Makefile | 4 - NINA/firmware/main/Kconfig.projbuild | 26 -- .../main/ai-deck-jpeg-streamer-demo.c | 239 ------------ NINA/firmware/main/component.mk | 8 - NINA/firmware/main/spi.c | 138 ------- NINA/firmware/main/spi.h | 32 -- NINA/firmware/main/wifi.c | 199 ---------- NINA/firmware/main/wifi.h | 53 --- NINA/viewer.py | 145 ------- 17 files changed, 716 insertions(+), 992 deletions(-) create mode 100644 GAP8/test_functionalities/wifi_jpeg_streamer/com.c create mode 100644 GAP8/test_functionalities/wifi_jpeg_streamer/com.h create mode 100644 GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py create mode 100644 GAP8/test_functionalities/wifi_jpeg_streamer/routing_info.h delete mode 100644 NINA/CMakeLists.txt delete mode 100644 NINA/firmware/Makefile delete mode 100644 NINA/firmware/main/Kconfig.projbuild delete mode 100644 NINA/firmware/main/ai-deck-jpeg-streamer-demo.c delete mode 100644 NINA/firmware/main/component.mk delete mode 100644 NINA/firmware/main/spi.c delete mode 100644 NINA/firmware/main/spi.h delete mode 100644 NINA/firmware/main/wifi.c delete mode 100644 NINA/firmware/main/wifi.h delete mode 100644 NINA/viewer.py diff --git a/.gitignore b/.gitignore index 787084e..9270ede 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,6 @@ NINA/firmware/sdkconfig* # GAP8 build files GAP8/**/BUILD/* + +# Image streamer example WiFi credentials +GAP8/test_functionalities/wifi_jpeg_streamer/wifi_credentials.h diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile b/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile index 1c6886a..7002fdb 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile @@ -1,12 +1,14 @@ +io=uart +PMSIS_OS = freertos + APP = test -APP_SRCS += test.c +APP_SRCS += test.c com.c APP_CFLAGS += -O3 -g -APP_LDFLAGS += -lgaptools -lgaplib +APP_CFLAGS += -DconfigUSE_TIMERS=1 -DINCLUDE_xTimerPendFunctionCall=1 + RUNNER_CONFIG = $(CURDIR)/config.ini -streamer: - ./streamer include $(RULES_DIR)/pmsis_rules.mk diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/com.c b/GAP8/test_functionalities/wifi_jpeg_streamer/com.c new file mode 100644 index 0000000..0114e1a --- /dev/null +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/com.c @@ -0,0 +1,296 @@ + +#include "pmsis.h" +#include "com.h" + +#define max(a, b) \ + ( \ + { \ + __typeof__(a) _a = (a); \ + __typeof__(b) _b = (b); \ + _a > _b ? _a : _b; \ + }) + +#if 0 +#define DEBUG_PRINTF printf +#else +#define DEBUG_PRINTF(...) ((void) 0) +#endif /* DEBUG */ + +// Nina RTT (goes high when Nina is ready to talk) +#define CONFIG_NINA_GPIO_NINA_ACK 18 +#define CONFIG_NINA_GPIO_NINA_ACK_PAD PI_PAD_32_A13_TIMER0_CH1 +#define CONFIG_NINA_GPIO_NINA_ACK_PAD_FUNC PI_PAD_32_A13_GPIO_A18_FUNC1 + +// Structs for setting up interrut for the Nina RTT pin +static struct pi_gpio_conf cts_gpio_conf; +static pi_gpio_callback_t cb_gpio; + +// GAP8 RTT (pull high when we want to talk) +#define CONFIG_NINA_GPIO_NINA_NOTIF 3 +#define CONFIG_NINA_GPIO_NINA_NOTIF_PAD PI_PAD_15_B1_RF_PACTRL3 +#define CONFIG_NINA_GPIO_NINA_NOTIF_PAD_FUNC PI_PAD_15_B1_GPIO_A3_FUNC1 + +#define GPIO_HIGH ((uint32_t)1) +#define GPIO_LOW ((uint32_t)0) + +static pi_device_t spi_dev, nina_rtt_dev, gap8_rtt_dev; + +// Queues for interacting with COM layer +static QueueHandle_t txq = NULL; +static QueueHandle_t rxq = NULL; + +// To optimize sending the queue should fit at least one image +#define TXQ_SIZE (80) +#define RXQ_SIZE (5) + +static EventGroupHandle_t evGroup; +#define NINA_RTT_BIT (1 << 0) +#define TX_QUEUE_BIT (1 << 1) + +#define INITIAL_TRANSFER_SIZE (4) + +void vDataReadyISR(void *args) +{ + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + xEventGroupSetBitsFromISR(evGroup, NINA_RTT_BIT, &xHigherPriorityTaskWoken); + portYIELD_FROM_ISR(xHigherPriorityTaskWoken); +} + +static void setup_nina_rtt_pin(pi_device_t *device) +{ + // Configure Nina RTT + pi_gpio_conf_init(&cts_gpio_conf); + pi_open_from_conf(device, &cts_gpio_conf); + pi_gpio_open(device); + pi_gpio_pin_configure(device, CONFIG_NINA_GPIO_NINA_ACK, PI_GPIO_INPUT); + pi_gpio_pin_notif_configure(device, CONFIG_NINA_GPIO_NINA_ACK, PI_GPIO_NOTIF_RISE); + pi_pad_set_function(CONFIG_NINA_GPIO_NINA_ACK_PAD, CONFIG_NINA_GPIO_NINA_ACK_PAD_FUNC); + + // Set up interrupt + uint32_t gpio_mask = (1 << (CONFIG_NINA_GPIO_NINA_ACK & PI_GPIO_NUM_MASK)); // create pin mask + pi_gpio_callback_init(&cb_gpio, gpio_mask, vDataReadyISR, (void *)CONFIG_NINA_GPIO_NINA_ACK); // setup callback and handler + pi_gpio_callback_add(device, &cb_gpio); // Attach callback to gpio pin +} + +static void setup_gap8_rtt_pin(pi_device_t *device) +{ + // Set up GAP8 RTT + struct pi_gpio_conf gpio_conf; + pi_gpio_conf_init(&gpio_conf); + pi_open_from_conf(device, &gpio_conf); + pi_gpio_open(device); + pi_gpio_pin_configure(device, CONFIG_NINA_GPIO_NINA_NOTIF, PI_GPIO_OUTPUT); + pi_pad_set_function(CONFIG_NINA_GPIO_NINA_NOTIF_PAD, CONFIG_NINA_GPIO_NINA_NOTIF_PAD_FUNC); +} + +void set_gap8_rtt_pin(pi_device_t *device, uint32_t val) +{ + // Set the GAP8 RTT pin + if (pi_gpio_pin_write(device, CONFIG_NINA_GPIO_NINA_NOTIF, val)) + { + DEBUG_PRINTF("Could not set notification\n"); + pmsis_exit(-1); + } +} + +static void init_spi(pi_device_t *device) +{ + struct pi_spi_conf spi_conf = {0}; + + pi_spi_conf_init(&spi_conf); + spi_conf.wordsize = PI_SPI_WORDSIZE_8; + spi_conf.big_endian = 1; + spi_conf.max_baudrate = 10000000; + spi_conf.polarity = 0; + spi_conf.phase = 0; + spi_conf.itf = 1; // SPI1 + spi_conf.cs = 0; // CS0 + + pi_open_from_conf(device, &spi_conf); + + if (pi_spi_open(device)) + { + DEBUG_PRINTF("SPI open failed\n"); + pmsis_exit(-1); + } +} + +static uint32_t start; +static uint32_t end; + +void com_task(void *parameters) +{ + EventBits_t evBits; + uint8_t *rx_buff, *tx_buff; + + rx_buff = (uint8_t *)pmsis_l2_malloc((uint32_t)sizeof(packet_t)); + tx_buff = (uint8_t *)pmsis_l2_malloc((uint32_t)sizeof(packet_t)); + + if (rx_buff == 0) { + DEBUG_PRINTF("Could not allocate RX buffer\n"); + } + + if (tx_buff == 0) { + DEBUG_PRINTF("Could not allocate TX buffer\n"); + } + + + DEBUG_PRINTF("Starting com task\n"); + + while (1) + { + + // Check if we have more to send, if not then wait until we have or Nina wants to send + if (uxQueueMessagesWaiting(txq) == 0) { + DEBUG_PRINTF("Waiting for action!\n"); + // Wait for either TXQ or RTT from Nina + evBits = xEventGroupWaitBits(evGroup, + NINA_RTT_BIT | TX_QUEUE_BIT, + pdTRUE, // Clear bits before returning + pdFALSE, // Wait for any bit + portMAX_DELAY); + DEBUG_PRINTF("Unlocked\n"); + start = xTaskGetTickCount(); + } else { + // If we didn't unlock on the bits, then reset them + start = xTaskGetTickCount(); + evBits = 0; + DEBUG_PRINTF("Still has stuff to send\n"); + } + + if ((evBits & NINA_RTT_BIT) == NINA_RTT_BIT) + { + DEBUG_PRINTF("We were awakened by Nina RTT\n"); + } + + if (uxQueueMessagesWaiting(txq) > 0) + { + xQueueReceive(txq, tx_buff, 0); + DEBUG_PRINTF("Should send packet of size %i\n", ((packet_t *)tx_buff)->len); + } + else + { + memset(tx_buff, 0, sizeof(packet_t)); + } + + // Check if we have a package to send (all 0 otherwise) + if (((packet_t *)tx_buff)->len > 0) + { + set_gap8_rtt_pin(&gap8_rtt_dev, GPIO_HIGH); + // Check if Nina RTT was set at the same time, if not wait + if ((evBits & NINA_RTT_BIT) == 0) + { + DEBUG_PRINTF("Waiting for Nina RTT\n"); + xEventGroupWaitBits(evGroup, NINA_RTT_BIT, pdTRUE, pdFALSE, (TickType_t)portMAX_DELAY); + } else { + DEBUG_PRINTF("Nina RTT already high\n"); + } + } + //memset(rx_buff, 0x01, sizeof(packet_t)); + // There's a risk that we've been emptying the queue while another package has been + // pushed and set the event bit again, which will trigger this loop again. + // To avoid one extra read (that's not needed) double check here. + if ((evBits & NINA_RTT_BIT) == NINA_RTT_BIT || ((packet_t *)tx_buff)->len > 0) { + DEBUG_PRINTF("Initiating SPI tansfer\n"); + + pi_spi_transfer(&spi_dev, + tx_buff, + rx_buff, + INITIAL_TRANSFER_SIZE * 8, + PI_SPI_LINES_SINGLE | PI_SPI_CS_KEEP); + + int tx_len = ((packet_t *)tx_buff)->len; + int rx_len = ((packet_t *)rx_buff)->len; + + DEBUG_PRINTF("Should read %i bytes\n", rx_len); + + int sizeLeft = max(tx_len - INITIAL_TRANSFER_SIZE + 2, rx_len - INITIAL_TRANSFER_SIZE + 2); + + DEBUG_PRINTF("Transfer size left is %i\n", sizeLeft); + + // Set minumum size left, this works with 0 bytes as well + sizeLeft = max(0, sizeLeft); + + // We only support transfers which are multiples of 4 + if ((sizeLeft % 4) > 0) { + sizeLeft += (4-sizeLeft%4); // Pad upwards + } + + DEBUG_PRINTF("Sending %i bytes\n", sizeLeft); + + // Set GAP8 RTT low before we end the transfer + set_gap8_rtt_pin(&gap8_rtt_dev, GPIO_LOW); + + // Transfer the remaining bytes + pi_spi_transfer(&spi_dev, + &tx_buff[INITIAL_TRANSFER_SIZE], + &rx_buff[INITIAL_TRANSFER_SIZE], + sizeLeft * 8, + PI_SPI_LINES_SINGLE | PI_SPI_CS_AUTO); + + DEBUG_PRINTF("Read %i bytes\n", ((packet_t *)rx_buff)->len); + + end = xTaskGetTickCount(); + // These seems to take about 1-2 ms and we're sending about 80 pkgs / image + //printf("Transaction took %u ms\n", end-start); + + if (((packet_t *)rx_buff)->len > 0) + { + if (xQueueSend(rxq, ((packet_t *)rx_buff), (TickType_t)portMAX_DELAY) != pdPASS) + { + DEBUG_PRINTF("RX Queue full!\n"); + } else { + DEBUG_PRINTF("Queued packet\n"); + } + } + + // Do not wait for Nina RTT to go low, we trigger on rising edge anyway + } else { + // For debug + DEBUG_PRINTF("Spurious read\n"); + } + } +} + +void com_init() +{ + DEBUG_PRINTF("Initialize communication\n"); + + setup_gap8_rtt_pin(&gap8_rtt_dev); + init_spi(&spi_dev); + + txq = xQueueCreate(TXQ_SIZE, sizeof(packet_t)); + rxq = xQueueCreate(RXQ_SIZE, sizeof(packet_t)); + + if (txq == NULL || rxq == NULL) + { + pmsis_exit(1); + } + + evGroup = xEventGroupCreate(); + + BaseType_t xTask; + xTask = xTaskCreate(com_task, "com_task", configMINIMAL_STACK_SIZE * 6, + NULL, tskIDLE_PRIORITY + 1, NULL); + if (xTask != pdPASS) + { + DEBUG_PRINTF("COM task did not start !\n"); + pmsis_exit(-1); + } + + setup_nina_rtt_pin(&nina_rtt_dev); +} + +void com_read(packet_t *p) +{ + xQueueReceive(rxq, p, (TickType_t)portMAX_DELAY); +} + +void com_write(packet_t *p) +{ + start = xTaskGetTickCount(); + //printf("Will queue up packet\n"); + xQueueSend(txq, p, (TickType_t)portMAX_DELAY); + //printf("Have queued up packet!\n"); + xEventGroupSetBits(evGroup, TX_QUEUE_BIT); +} diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/com.h b/GAP8/test_functionalities/wifi_jpeg_streamer/com.h new file mode 100644 index 0000000..67849db --- /dev/null +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/com.h @@ -0,0 +1,29 @@ +#include + +#ifndef __COM_H__ +#define __COM_H__ + +#define MTU (1022) + +typedef struct +{ + uint16_t len; // Of data (max 1022) + uint8_t data[MTU]; +} __attribute__((packed)) packet_t; + +typedef struct +{ + uint16_t len; // Of data (max 1022) + uint8_t dst; // Bootloader is 0xFF + uint8_t src; + uint8_t data[MTU-2]; +} __attribute__((packed)) routed_packet_t; + +/* Initialize the communication */ +void com_init(); + +void com_read(packet_t * p); + +void com_write(packet_t * p); + +#endif \ No newline at end of file diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py b/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py new file mode 100644 index 0000000..6bde0bf --- /dev/null +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2021 Bitcraze AB +# +# AI-deck demo +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License along with +# this program; if not, write to the Free Software Foundation, Inc., 51 +# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. +# +# Demo for showing streamed JPEG images from the AI-deck example. +# +# By default this demo connects to the IP of the AI-deck example when in +# Access point mode. +# +# The demo works by opening a socket to the AI-deck, downloads a stream of +# JPEG images and looks for start/end-of-frame for the streamed JPEG images. +# Once an image has been fully downloaded it's rendered in the UI. +# +# Note that the demo firmware is continously streaming JPEG files so a single +# JPEG image is taken from the stream using the JPEG start-of-frame (0xFF 0xD8) +# and the end-of-frame (0xFF 0xD9). + +import argparse +import time +import socket,os,struct, time +import numpy as np + +# 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 JPEG streamer example') +parser.add_argument("-n", default="192.168.4.1", metavar="ip", help="AI-deck IP") +parser.add_argument("-p", type=int, default='5000', metavar="port", help="AI-deck port") +args = parser.parse_args() + +deck_port = args.p +deck_ip = args.n + +print("Connecting to socket on {}:{}...".format(deck_ip, deck_port)) +client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +client_socket.connect((deck_ip, deck_port)) +print("Socket connected") + +imgdata = None +data_buffer = bytearray() + +def rx_bytes(size): + data = bytearray() + while len(data) < size: + data.extend(client_socket.recv(size-len(data))) + return data + +import cv2 + +start = time.time() +count = 0 + +while(1): + # First get the info + packetInfoRaw = rx_bytes(4) + #print(packetInfoRaw) + [length, dst, src] = struct.unpack('{:02X})".format(length, src, dst)) + chunk = rx_bytes(length - 2) + imgStream.extend(chunk) + with open("img.raw", "wb") as f: + f.write(imgStream) + count = count + 1 + bayer_img = np.frombuffer(imgStream, dtype=np.uint8) + #img2 = np.fromfile("img.raw", dtype=np.uint8) + bayer_img.shape = (244, 324) + meanTimePerImage = (time.time()-start) / count + print("{}".format(meanTimePerImage)) + print("{}".format(1/meanTimePerImage)) + cv2.imshow('Bayer', bayer_img) + cv2.imshow('Color', cv2.cvtColor(bayer_img, cv2.COLOR_BayerBG2BGRA)) + cv2.waitKey(1) + + diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/routing_info.h b/GAP8/test_functionalities/wifi_jpeg_streamer/routing_info.h new file mode 100644 index 0000000..4497a9b --- /dev/null +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/routing_info.h @@ -0,0 +1,40 @@ +#pragma once + +#include + +// Targets where data can be routed +#define STM32 ((uint8_t) 0x00) +#define ESP32 ((uint8_t) 0x10) +#define GAP8 ((uint8_t) 0x20) + +// Functionality where data can be +// sent for each target. + +// System includes PM etc. +#define SYSTEM ((uint8_t) 0x00) +// Console messages +#define CONSOLE ((uint8_t) 0x01) +// CrazyRealTimeProtocol passthough +#define CRTP ((uint8_t) 0x02) +// WiFi control data (i.e connect and stuff) +#define WIFI_CTRL ((uint8_t) 0x05) +// WiFi data. Once connection is established this acts as passthough +#define WIFI_DATA ((uint8_t) 0x06) +// Application running on the target§ +#define APP ((uint8_t) 0x07) +// Test running on the target (SINK/SOURCE/ECHO) +#define TEST ((uint8_t) 0x0E) +// Bootloader running on target +#define BOOTLOADER ((uint8_t) 0x0F) + +#define MAKE_ROUTE(TARGET, FUNCTION) (TARGET | FUNCTION) +#define ROUTE_TARGET(ROUTE) (ROUTE & 0xF0) +#define ROUTE_FUNCTION(ROUTE) (ROUTE & 0x0F) + + +// Header used for routing, place after length (which is not part +// of routing) +typedef struct { + uint8_t dst; + uint8_t src; +} __attribute__((packed)) routable_packet_header_t; diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c index 51b27a5..f000f6a 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c @@ -1,58 +1,30 @@ +#include "pmsis.h" + #include "bsp/camera/himax.h" -#include "bsp/camera/mt9v034.h" -#include "bsp/transport/nina_w10.h" -#include "tools/frame_streamer.h" +#include "bsp/buffer.h" #include "stdio.h" -#if defined(CONFIG_GAPUINO) || defined(CONFIG_AI_DECK) -#define CAM_WIDTH 324 -#define CAM_HEIGHT 244 -#else -#define CAM_WIDTH 320 -#define CAM_HEIGHT 240 -#endif +#include "com.h" +#include "routing_info.h" + +// This file should be created manually and not committed (part of ignore) +// It should contain the following: +// static const char ssid[] = "YourSSID"; +// static const char passwd[] = "YourWiFiKey"; +#include "wifi_credentials.h" + +#define IMG_ORIENTATION 0x0101 +#define CAM_WIDTH 324 +#define CAM_HEIGHT 244 static pi_task_t task1; -static pi_task_t task2; static unsigned char *imgBuff0; -static unsigned char *imgBuff1; static struct pi_device camera; -static struct pi_device wifi; -static frame_streamer_t *streamer1; -static frame_streamer_t *streamer2; static pi_buffer_t buffer; -static pi_buffer_t buffer2; static volatile int stream1_done; -static volatile int stream2_done; - -static void streamer_handler(void *arg); - - -static void cam_handler(void *arg) -{ - pi_camera_control(&camera, PI_CAMERA_CMD_STOP, 0); - - stream1_done = 0; - stream2_done = 0; - - frame_streamer_send_async(streamer1, &buffer, pi_task_callback(&task1, streamer_handler, (void *)&stream1_done)); - - return; -} - - - -static void streamer_handler(void *arg) -{ - *(int *)arg = 1; - if (stream1_done) // && stream2_done) - { - pi_camera_capture_async(&camera, imgBuff0, CAM_WIDTH*CAM_HEIGHT, pi_task_callback(&task1, cam_handler, NULL)); - pi_camera_control(&camera, PI_CAMERA_CMD_START, 0); - } -} - +static EventGroupHandle_t evGroup; +#define CAPTURE_DONE_BIT (1 << 0) static int open_pi_camera_himax(struct pi_device *device) { @@ -66,155 +38,267 @@ static int open_pi_camera_himax(struct pi_device *device) if (pi_camera_open(device)) return -1; - - // rotate image + // rotate image pi_camera_control(&camera, PI_CAMERA_CMD_START, 0); - uint8_t set_value=3; + uint8_t set_value = 3; uint8_t reg_value; pi_camera_reg_set(&camera, IMG_ORIENTATION, &set_value); pi_time_wait_us(1000000); pi_camera_reg_get(&camera, IMG_ORIENTATION, ®_value); - if (set_value!=reg_value) + if (set_value != reg_value) { printf("Failed to rotate camera image\n"); return -1; } pi_camera_control(&camera, PI_CAMERA_CMD_STOP, 0); - + pi_camera_control(device, PI_CAMERA_CMD_AEG_INIT, 0); return 0; } +#define LED_PIN 2 +static pi_device_t led_gpio_dev; -static int open_pi_camera_mt9v034(struct pi_device *device) -{ - struct pi_mt9v034_conf cam_conf; - - pi_mt9v034_conf_init(&cam_conf); - - cam_conf.format = PI_CAMERA_QVGA; - - pi_open_from_conf(device, &cam_conf); - if (pi_camera_open(device)) - return -1; - +static int wifiConnected = 0; +static int wifiClientConnected = 0; - - return 0; +static routed_packet_t rxp; +void rx_task(void *parameters) +{ + while (1) + { + com_read(&rxp); + printf("Got something from the ESP!\n"); + + switch (rxp.data[0]) + { + case 0x21: + printf("Wifi is now connected\n"); + wifiConnected = 1; + break; + case 0x23: + printf("Wifi client connection status: %u\n", rxp.data[1]); + wifiClientConnected = rxp.data[1]; + break; + } + } } - - -static int open_camera(struct pi_device *device) +static void capture_done_cb(void *arg) { -#ifdef CONFIG_GAPOC_A - return open_pi_camera_mt9v034(device); -#endif -#if defined(CONFIG_GAPUINO) || defined(CONFIG_AI_DECK) - return open_pi_camera_himax(device); -#endif - return -1; + xEventGroupSetBits(evGroup, CAPTURE_DONE_BIT); } - -static int open_wifi(struct pi_device *device) +typedef struct { - struct pi_nina_w10_conf nina_conf; - - pi_nina_w10_conf_init(&nina_conf); + uint8_t magic; + uint16_t width; + uint16_t height; + uint8_t depth; + uint8_t type; + uint32_t size; +} __attribute__((packed)) img_header_t; + +static uint32_t start; +static uint32_t end; + +static routed_packet_t txp; +void camera_task(void *parameters) +{ + vTaskDelay(2000); + + printf("Sending wifi stuff...\n"); + txp.len = 2 + sizeof(ssid); + txp.dst = MAKE_ROUTE(ESP32, WIFI_CTRL); + txp.src = MAKE_ROUTE(GAP8, WIFI_CTRL); + txp.data[0] = 0x10; // Set SSID + memcpy(&txp.data[1], ssid, sizeof(ssid)); + com_write(&txp); + + txp.len = 2 + sizeof(passwd); + txp.data[0] = 0x11; // Set passwd + memcpy(&txp.data[1], passwd, sizeof(passwd)); + com_write(&txp); + + txp.len = 3; + txp.data[0] = 0x20; // Connect wifi + com_write(&txp); + + printf("Starting camera task...\n"); + uint32_t resolution = CAM_WIDTH * CAM_HEIGHT; + uint32_t imgSize = resolution * sizeof(unsigned char); + imgBuff0 = (unsigned char *)pmsis_l2_malloc(imgSize); + if (imgBuff0 == NULL) + { + printf("Failed to allocate Memory for Image \n"); + return 1; + } + printf("Allocated memory for image: %u bytes\n", imgSize); - nina_conf.ssid = ""; - nina_conf.passwd = ""; - nina_conf.ip_addr = "0.0.0.0"; - nina_conf.port = 5555; - pi_open_from_conf(device, &nina_conf); - if (pi_transport_open(device)) + if (open_pi_camera_himax(&camera)) + { + printf("Failed to open camera\n"); return -1; + } + printf("Camera is open\n"); - return 0; -} + pi_buffer_init(&buffer, PI_BUFFER_TYPE_L2, imgBuff0); + pi_buffer_set_format(&buffer, CAM_WIDTH, CAM_HEIGHT, 1, PI_BUFFER_FORMAT_GRAY); + pi_camera_control(&camera, PI_CAMERA_CMD_STOP, 0); + while (1) + { + if (wifiClientConnected == 1) + { + start = xTaskGetTickCount(); + //memset(imgBuff0, 0, imgSize); + pi_camera_capture_async(&camera, imgBuff0, resolution, pi_task_callback(&task1, capture_done_cb, NULL)); + pi_camera_control(&camera, PI_CAMERA_CMD_START, 0); + xEventGroupWaitBits(evGroup, CAPTURE_DONE_BIT, pdTRUE, pdFALSE, (TickType_t)portMAX_DELAY); + pi_camera_control(&camera, PI_CAMERA_CMD_STOP, 0); + end = xTaskGetTickCount(); + //printf("Captured in %u\n", end - start); + // printf("0x%08X%08X%08X %08X\n", + // (uint64_t) imgBuff0[0], + // (uint64_t) imgBuff0[7], + // (uint64_t) imgBuff0[15], + // (uint64_t) imgBuff0[5000]); + //pi_camera_control(&camera, PI_CAMERA_CMD_STOP, 0); + //printf("Captured image\n"); + + start = xTaskGetTickCount(); + + txp.dst = MAKE_ROUTE(ESP32, WIFI_DATA); + txp.src = MAKE_ROUTE(GAP8, WIFI_DATA); + + // First send information about the image + img_header_t *header = (img_header_t *)txp.data; + header->magic = 0xBC; + header->width = CAM_WIDTH; + header->height = CAM_HEIGHT; + header->depth = 1; + header->type = 0; + header->size = imgSize; + txp.len = sizeof(img_header_t) + 2; + com_write(&txp); + + int i = 0; + int part = 0; + int offset = 0; + int size = 0; + + do + { + offset = part * sizeof(txp.data); + size = sizeof(txp.data); + if (offset + size > imgSize) + { + size = imgSize - offset; + } + memcpy(txp.data, &imgBuff0[offset], sizeof(txp.data)); + //printf("Copied from %u (size is %u)\n", offset, size); + txp.len = size + 2; // + 2 for header + com_write(&txp); + part++; + } while (size == sizeof(txp.data)); + //printf("Finished sending image\n"); + end = xTaskGetTickCount(); + //printf("Sent in %u\n", end - start); + vTaskDelay(10); + } + else + { + //printf("Client is not connected, hold off\n"); + vTaskDelay(10); + } + } +} -static frame_streamer_t *open_streamer(char *name) +void hb_task(void *parameters) { - struct frame_streamer_conf frame_streamer_conf; + (void)parameters; + char *taskname = pcTaskGetName(NULL); - frame_streamer_conf_init(&frame_streamer_conf); + // Initialize the LED pin + pi_gpio_pin_configure(&led_gpio_dev, LED_PIN, PI_GPIO_OUTPUT); - frame_streamer_conf.transport = &wifi; - frame_streamer_conf.format = FRAME_STREAMER_FORMAT_JPEG; - frame_streamer_conf.width = CAM_WIDTH; - frame_streamer_conf.height = CAM_HEIGHT; - frame_streamer_conf.depth = 1; - frame_streamer_conf.name = name; + const TickType_t xDelay = 500 / portTICK_PERIOD_MS; - return frame_streamer_open(&frame_streamer_conf); -} -static pi_task_t led_task; -static int led_val = 0; -static struct pi_device gpio_device; -static void led_handle(void *arg) -{ - pi_gpio_pin_write(&gpio_device, 2, led_val); - led_val ^= 1; - pi_task_push_delayed_us(pi_task_callback(&led_task, led_handle, NULL), 500000); + while (1) + { + pi_gpio_pin_write(&led_gpio_dev, LED_PIN, 1); + vTaskDelay(xDelay); + pi_gpio_pin_write(&led_gpio_dev, LED_PIN, 0); + vTaskDelay(xDelay); + } } -int main() +void start_bootloader(void) { - printf("Entering main controller...\n"); + struct pi_uart_conf conf; + struct pi_device device; + pi_uart_conf_init(&conf); + conf.baudrate_bps = 115200; - pi_freq_set(PI_FREQ_DOMAIN_FC, 150000000); + pi_open_from_conf(&device, &conf); + if (pi_uart_open(&device)) + { + printf("[UART] open failed !\n"); + pmsis_exit(-1); + } - pi_gpio_pin_configure(&gpio_device, 2, PI_GPIO_OUTPUT); + printf("\nStarting up!\n"); + printf("FC at %u MHz\n", pi_freq_get(PI_FREQ_DOMAIN_FC) / 1000000); - pi_task_push_delayed_us(pi_task_callback(&led_task, led_handle, NULL), 500000); + printf("Starting up tasks...\n"); - imgBuff0 = (unsigned char *)pmsis_l2_malloc((CAM_WIDTH*CAM_HEIGHT)*sizeof(unsigned char)); - if (imgBuff0 == NULL) { - printf("Failed to allocate Memory for Image \n"); - return 1; - } - printf("Allocated Memory for Image\n"); + evGroup = xEventGroupCreate(); + + BaseType_t xTask; - if (open_camera(&camera)) + xTask = xTaskCreate(hb_task, "hb_task", configMINIMAL_STACK_SIZE * 2, + NULL, tskIDLE_PRIORITY + 1, NULL); + if (xTask != pdPASS) { - printf("Failed to open camera\n"); - return -1; + printf("HB task did not start !\n"); + pmsis_exit(-1); } - printf("Opened Camera\n"); + com_init(); + xTask = xTaskCreate(camera_task, "camera_task", configMINIMAL_STACK_SIZE * 4, + NULL, tskIDLE_PRIORITY + 1, NULL); - if (open_wifi(&wifi)) + if (xTask != pdPASS) { - printf("Failed to open wifi\n"); - return -1; + printf("Camera task did not start !\n"); + pmsis_exit(-1); } - printf("Opened WIFI\n"); - - - streamer1 = open_streamer("camera"); - if (streamer1 == NULL) - return -1; - - printf("Opened streamer\n"); - - pi_buffer_init(&buffer, PI_BUFFER_TYPE_L2, imgBuff0); - pi_buffer_set_format(&buffer, CAM_WIDTH, CAM_HEIGHT, 1, PI_BUFFER_FORMAT_GRAY); + xTask = xTaskCreate(rx_task, "rx_task", configMINIMAL_STACK_SIZE * 2, + NULL, tskIDLE_PRIORITY + 1, NULL); - pi_camera_control(&camera, PI_CAMERA_CMD_STOP, 0); - pi_camera_capture_async(&camera, imgBuff0, CAM_WIDTH*CAM_HEIGHT, pi_task_callback(&task1, cam_handler, NULL)); - pi_camera_control(&camera, PI_CAMERA_CMD_START, 0); - printf("6\n"); + if (xTask != pdPASS) + { + printf("RX task did not start !\n"); + pmsis_exit(-1); + } - while(1) + while (1) { pi_yield(); } - - return 0; } + +int main(void) +{ + pi_bsp_init(); + + // Increase the FC freq to 250 MHz + pi_freq_set(PI_FREQ_DOMAIN_FC, 250000000); + pi_pmu_voltage_set(PI_PMU_DOMAIN_FC, 1200); + + return pmsis_kickoff((void *)start_bootloader); +} \ No newline at end of file diff --git a/NINA/CMakeLists.txt b/NINA/CMakeLists.txt deleted file mode 100644 index 7c5b459..0000000 --- a/NINA/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -idf_component_register(SRCS "ai-deck-jpeg-streamer-demo.c" "wifi.c" "spi.c" - INCLUDE_DIRS ".") \ No newline at end of file diff --git a/NINA/firmware/Makefile b/NINA/firmware/Makefile deleted file mode 100644 index 94d63a6..0000000 --- a/NINA/firmware/Makefile +++ /dev/null @@ -1,4 +0,0 @@ -PROJECT_NAME := ai-deck-jpeg-streamer-demo - -include $(IDF_PATH)/make/project.mk - diff --git a/NINA/firmware/main/Kconfig.projbuild b/NINA/firmware/main/Kconfig.projbuild deleted file mode 100644 index 5de1c76..0000000 --- a/NINA/firmware/main/Kconfig.projbuild +++ /dev/null @@ -1,26 +0,0 @@ -menu "AI deck example Configuration" - - config USE_AS_AP - bool "Act as access-point" - default y - 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/firmware/main/ai-deck-jpeg-streamer-demo.c b/NINA/firmware/main/ai-deck-jpeg-streamer-demo.c deleted file mode 100644 index 239b343..0000000 --- a/NINA/firmware/main/ai-deck-jpeg-streamer-demo.c +++ /dev/null @@ -1,239 +0,0 @@ -/* - * 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. -*/ - -#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_event.h" -#include "esp_log.h" -#include "nvs_flash.h" -#include "esp_event_loop.h" -#include "soc/rtc_cntl_reg.h" -#include "esp_spi_flash.h" -#include "driver/gpio.h" - -#include "wifi.h" -#include "spi.h" - -/* GAP8 streamer packet type for JPEG related data */ -#define NINA_W10_CMD_SEND_PACKET 0x81 - -/* 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 GPIO */ -#define BLINK_GPIO 4 - -/* The period of the LED blinking */ -static uint32_t blink_period_ms = 500; - -/* Log tag for printouts */ -static const char *TAG = "demo"; - -/* GAP8 communication struct */ -typedef struct -{ - uint32_t type; /* Is 0x81 for JPEG related data */ - uint32_t size; /* Size of data to request */ - uint32_t info; /* Signals end-of-frame or header/footer packet*/ -} __attribute__((packed)) nina_req_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; - -/* Send JPEG data to client connected on WiFi */ -static void send_imagedata_to_client(uint32_t type, uint32_t size, uint32_t info, uint8_t *buffer) { - bool new_frame = (info == 1); - - if (wifi_is_socket_connected()) - { - - wifi_send_packet( (const char*) buffer, size); - - if (new_frame) { - wifi_send_packet( (const char*) &jpeg_footer, sizeof(jpeg_footer) ); - } - - if (new_frame) { - wifi_send_packet( (const char*) &jpeg_header, sizeof(jpeg_header) ); - } - } -} - -/* Handle data from JPEG stream */ -static void handle_jpeg_stream_data(uint32_t type, uint32_t size, uint32_t info, uint8_t *buffer) { - switch (state) { - case HEADER: - ESP_LOGI(TAG, "Setting JPEG header of %i bytes", size); - memcpy(&jpeg_header, (uint8_t*) buffer, sizeof(jpeg_header)); - state = FOOTER; - break; - case FOOTER: - ESP_LOGI(TAG, "Setting JPEG footer of %i bytes", size); - memcpy(&jpeg_footer, (uint8_t*) buffer, sizeof(jpeg_footer)); - state = DATA; - break; - case DATA: - // This could be dangerous if the last part of the image is 12 bytes.. - if (size != 12) { - send_imagedata_to_client(type, size, info, buffer); - } - break; - } -} - -/* Handle packet received from the GAP8 */ -static void handle_gap8_package(uint8_t *buffer) { - nina_req_t *req = (nina_req_t *)buffer; - - switch (req->type) - { - case NINA_W10_CMD_SEND_PACKET: - if (req->info <= 1) { - - uint32_t type = req->type; - uint32_t size = req->size; - uint32_t info = req->info; - - spi_read_data(&buffer, size*8); // Make sure transfer is word aligned - - handle_jpeg_stream_data(type, size, info, buffer); - } - break; - } -} - -/* Task for receiving JPEG data from GAP8 and sending to client via WiFi */ -static void img_sending_task(void *pvParameters) { - spi_init(); - - // Add a delay so it won't start sending images right at - // the same moment it's connecting with Wifi - vTaskDelay(pdMS_TO_TICKS(1000)); - - while (1) - { - int32_t datalength = spi_read_data(&spi_buffer, CMD_PACKET_SIZE); - if (datalength > 0) { - handle_gap8_package(spi_buffer); - } - } -} - -/* Task for handling WiFi state */ -static void wifi_task(void *pvParameters) { - wifi_bind_socket(); - while (1) { - blink_period_ms = 500; - wifi_wait_for_socket_connected(); - blink_period_ms = 100; - wifi_wait_for_disconnect(); - ESP_LOGI(TAG, "Client disconnected"); - } -} - -/* Main application and blinking of LED */ -void app_main() { - gpio_pad_select_gpio(BLINK_GPIO); - gpio_set_direction(BLINK_GPIO, GPIO_MODE_OUTPUT); - gpio_set_level(BLINK_GPIO, 1); - - ESP_ERROR_CHECK(nvs_flash_init()); - ESP_ERROR_CHECK(esp_event_loop_create_default()); - - wifi_init(WIFI_MODE, CONFIG_EXAMPLE_SSID, CONFIG_EXAMPLE_PASSWORD); - - xTaskCreate(img_sending_task, "img_sending_task", 4096, NULL, 5, NULL); - xTaskCreate(wifi_task, "wifi_task", 4096, NULL, 5, NULL); - - while(1) { - gpio_set_level(BLINK_GPIO, 0); - vTaskDelay(pdMS_TO_TICKS(blink_period_ms/2)); - gpio_set_level(BLINK_GPIO, 1); - vTaskDelay(pdMS_TO_TICKS(blink_period_ms/2)); - } -} diff --git a/NINA/firmware/main/component.mk b/NINA/firmware/main/component.mk deleted file mode 100644 index 61f8990..0000000 --- a/NINA/firmware/main/component.mk +++ /dev/null @@ -1,8 +0,0 @@ -# -# 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/firmware/main/spi.c b/NINA/firmware/main/spi.c deleted file mode 100644 index eb7aa57..0000000 --- a/NINA/firmware/main/spi.c +++ /dev/null @@ -1,138 +0,0 @@ -/* - * 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/firmware/main/wifi.c b/NINA/firmware/main/wifi.c deleted file mode 100644 index 52a5901..0000000 --- a/NINA/firmware/main/wifi.c +++ /dev/null @@ -1,199 +0,0 @@ -/* - * 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/firmware/main/wifi.h b/NINA/firmware/main/wifi.h deleted file mode 100644 index 4393b03..0000000 --- a/NINA/firmware/main/wifi.h +++ /dev/null @@ -1,53 +0,0 @@ -/* - * 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/viewer.py b/NINA/viewer.py deleted file mode 100644 index 1bf6aad..0000000 --- a/NINA/viewer.py +++ /dev/null @@ -1,145 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2020 Bitcraze AB -# -# AI-deck demo -# -# This program is free software; you can redistribute it and/or -# modify it under the terms of the GNU General Public License -# as published by the Free Software Foundation; either version 2 -# of the License, or (at your option) any later version. -# -# This program is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU General Public License for more details. -# You should have received a copy of the GNU General Public License along with -# this program; if not, write to the Free Software Foundation, Inc., 51 -# Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. -# -# Demo for showing streamed JPEG images from the AI-deck example. -# -# By default this demo connects to the IP of the AI-deck example when in -# Access point mode. -# -# The demo works by opening a socket to the AI-deck, downloads a stream of -# JPEG images and looks for start/end-of-frame for the streamed JPEG images. -# Once an image has been fully downloaded it's rendered in the UI. -# -# Note that the demo firmware is continously streaming JPEG files so a single -# JPEG image is taken from the stream using the JPEG start-of-frame (0xFF 0xD8) -# and the end-of-frame (0xFF 0xD9). - -import argparse -import gi -gi.require_version('Gtk', '3.0') -from gi.repository import Gtk, Gdk, GdkPixbuf, GLib -import threading -import time -import socket,os,struct - -deck_ip = None -deck_port = None - -class ImgThread(threading.Thread): - def __init__(self, callback): - threading.Thread.__init__(self, daemon=True) - self._callback = callback - - def run(self): - print("Connecting to socket on {}:{}...".format(deck_ip, deck_port)) - client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) - client_socket.connect((deck_ip, deck_port)) - print("Socket connected") - - imgdata = None - data_buffer = bytearray() - - while(1): - # Reveive image data from the AI-deck - data_buffer.extend(client_socket.recv(512)) - - # Look for start-of-frame and end-of-frame - start_idx = data_buffer.find(b"\xff\xd8") - end_idx = data_buffer.find(b"\xff\xd9") - - # At startup we might get an end before we get the first start, if - # that is the case then throw away the data before start - if end_idx > -1 and end_idx < start_idx: - data_buffer = data_buffer[start_idx:] - - # We have a start and an end of the image in the buffer now - if start_idx > -1 and end_idx > -1 and end_idx > start_idx: - # Pick out the image to render ... - imgdata = data_buffer[start_idx:end_idx + 2] - # .. and remove it from the buffer - data_buffer = data_buffer[end_idx + 2 :] - try: - self._callback(imgdata) - except gi.repository.GLib.Error: - print("Error rendering image") - -# UI for showing frames from AI-deck example -class FrameViewer(Gtk.Window): - - def __init__(self): - super(FrameViewer, self).__init__() - self.frame = None - self.init_ui() - self._start = None - self.set_default_size(374, 294) - - def init_ui(self): - self.override_background_color(Gtk.StateType.NORMAL, Gdk.RGBA(0, 0, 0, 1)) - self.set_border_width(20) - self.set_title("Connecting...") - self.frame = Gtk.Image() - f = Gtk.Fixed() - f.put(self.frame, 10, 10) - self.add(f) - self.connect("destroy", Gtk.main_quit) - self._thread = ImgThread(self._showframe) - self._thread.start() - - def _update_image(self, pix): - self.frame.set_from_pixbuf(pix) - - def _showframe(self, imgdata): - # Add FPS/img size to window title - if (self._start != None): - fps = 1 / (time.time() - self._start) - GLib.idle_add(self.set_title, "{:.1f} fps / {:.1f} kb".format(fps, len(imgdata)/1000)) - self._start = time.time() - - # Try to decode JPEG from the data sent from the stream - try: - img_loader = GdkPixbuf.PixbufLoader() - img_loader.write(imgdata) - img_loader.close() - pix = img_loader.get_pixbuf() - GLib.idle_add(self._update_image, pix) - except gi.repository.GLib.Error: - print("Could not set image!") - -# 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 JPEG streamer example') -parser.add_argument("-n", default="192.168.4.1", metavar="ip", help="AI-deck IP") -parser.add_argument("-p", type=int, default='5000', metavar="port", help="AI-deck port") -args = parser.parse_args() - -deck_port = args.p -deck_ip = args.n - -fw = FrameViewer() -fw.show_all() -Gtk.main() - - From 183497b9e3ada1436e4f33011a2cec4235fc5de1 Mon Sep 17 00:00:00 2001 From: Marcus Date: Mon, 13 Dec 2021 17:59:32 +0100 Subject: [PATCH 02/10] Added JPEG sending/receiving, needs cleanup --- .../wifi_jpeg_streamer/Makefile | 1 + .../wifi_jpeg_streamer/opencv-viewer.py | 35 +-- .../wifi_jpeg_streamer/test.c | 199 +++++++++++++----- 3 files changed, 174 insertions(+), 61 deletions(-) diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile b/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile index 7002fdb..efc3eba 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile @@ -7,6 +7,7 @@ APP_CFLAGS += -O3 -g APP_CFLAGS += -DconfigUSE_TIMERS=1 -DINCLUDE_xTimerPendFunctionCall=1 +CONFIG_GAP_LIB_JPEG = 1 RUNNER_CONFIG = $(CURDIR)/config.ini diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py b/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py index 6bde0bf..79fe52c 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py @@ -95,22 +95,31 @@ def rx_bytes(size): imgStream = bytearray() while len(imgStream) < size: - packetInfoRaw = rx_bytes(4) - [length, dst, src] = struct.unpack('{:02X})".format(length, src, dst)) - chunk = rx_bytes(length - 2) - imgStream.extend(chunk) - with open("img.raw", "wb") as f: - f.write(imgStream) + packetInfoRaw = rx_bytes(4) + [length, dst, src] = struct.unpack('{:02X})".format(length, src, dst)) + chunk = rx_bytes(length - 2) + imgStream.extend(chunk) + count = count + 1 - bayer_img = np.frombuffer(imgStream, dtype=np.uint8) - #img2 = np.fromfile("img.raw", dtype=np.uint8) - bayer_img.shape = (244, 324) meanTimePerImage = (time.time()-start) / count print("{}".format(meanTimePerImage)) print("{}".format(1/meanTimePerImage)) - cv2.imshow('Bayer', bayer_img) - cv2.imshow('Color', cv2.cvtColor(bayer_img, cv2.COLOR_BayerBG2BGRA)) - cv2.waitKey(1) + if format == 0: + with open("img.raw", "wb") as f: + f.write(imgStream) + bayer_img = np.frombuffer(imgStream, dtype=np.uint8) + #img2 = np.fromfile("img.raw", dtype=np.uint8) + bayer_img.shape = (244, 324) + cv2.imshow('Bayer', bayer_img) + cv2.imshow('Color', cv2.cvtColor(bayer_img, cv2.COLOR_BayerBG2BGRA)) + cv2.waitKey(1) + else: + with open("img.jpeg", "wb") as f: + f.write(imgStream) + nparr = np.frombuffer(imgStream, np.uint8) + decoded = cv2.imdecode(nparr,cv2.IMREAD_UNCHANGED) + cv2.imshow('JPEG', decoded) + cv2.waitKey(1) diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c index f000f6a..ccda560 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c @@ -2,6 +2,7 @@ #include "bsp/camera/himax.h" #include "bsp/buffer.h" +#include "gaplib/jpeg_encoder.h" #include "stdio.h" #include "com.h" @@ -21,7 +22,6 @@ static pi_task_t task1; static unsigned char *imgBuff0; static struct pi_device camera; static pi_buffer_t buffer; -static volatile int stream1_done; static EventGroupHandle_t evGroup; #define CAPTURE_DONE_BIT (1 << 0) @@ -70,12 +70,12 @@ void rx_task(void *parameters) while (1) { com_read(&rxp); - printf("Got something from the ESP!\n"); switch (rxp.data[0]) { case 0x21: - printf("Wifi is now connected\n"); + printf("Wifi connected (%u.%u.%u.%u)\n", rxp.data[1], rxp.data[2], + rxp.data[3], rxp.data[4]); wifiConnected = 1; break; case 0x23: @@ -104,7 +104,25 @@ typedef struct static uint32_t start; static uint32_t end; +static jpeg_encoder_t jpeg_encoder; + +typedef enum +{ + RAW_ENCODING = 1, + JPEG_ENCODING = 2 +} __attribute__((packed)) StreamerMode_t; + +pi_buffer_t header; +int headerSize; +pi_buffer_t footer; +int footerSize; +pi_buffer_t jpeg_data; +uint32_t jpegSize; + +static StreamerMode_t streamerMode = RAW_ENCODING; + static routed_packet_t txp; + void camera_task(void *parameters) { vTaskDelay(2000); @@ -144,69 +162,154 @@ void camera_task(void *parameters) } printf("Camera is open\n"); + struct jpeg_encoder_conf enc_conf; + jpeg_encoder_conf_init(&enc_conf); + enc_conf.width = CAM_WIDTH; + enc_conf.height = CAM_HEIGHT; + enc_conf.flags = 0; // Move this to the cluster + + if (jpeg_encoder_open(&jpeg_encoder, &enc_conf)) + { + printf("Failed initialize JPEG encoder\n"); + return -1; + } + + printf("JPEG encoder initialized\n"); + pi_buffer_init(&buffer, PI_BUFFER_TYPE_L2, imgBuff0); pi_buffer_set_format(&buffer, CAM_WIDTH, CAM_HEIGHT, 1, PI_BUFFER_FORMAT_GRAY); + header.size = 1024; + header.data = pmsis_l2_malloc(1024); + + footer.size = 10; + footer.data = pmsis_l2_malloc(10); + + // This must fit the full encoded JPEG + jpeg_data.size = 1024 * 15; + jpeg_data.data = pmsis_l2_malloc(1024 * 15); + + // Check malloc! + + jpeg_encoder_header(&jpeg_encoder, &header, &headerSize); + printf("JPEG header size is %u\n", headerSize); + jpeg_encoder_footer(&jpeg_encoder, &footer, &footerSize); + printf("JPEG footer size is %u\n", footerSize); + pi_camera_control(&camera, PI_CAMERA_CMD_STOP, 0); while (1) { if (wifiClientConnected == 1) { start = xTaskGetTickCount(); - //memset(imgBuff0, 0, imgSize); pi_camera_capture_async(&camera, imgBuff0, resolution, pi_task_callback(&task1, capture_done_cb, NULL)); pi_camera_control(&camera, PI_CAMERA_CMD_START, 0); xEventGroupWaitBits(evGroup, CAPTURE_DONE_BIT, pdTRUE, pdFALSE, (TickType_t)portMAX_DELAY); pi_camera_control(&camera, PI_CAMERA_CMD_STOP, 0); end = xTaskGetTickCount(); - //printf("Captured in %u\n", end - start); - // printf("0x%08X%08X%08X %08X\n", - // (uint64_t) imgBuff0[0], - // (uint64_t) imgBuff0[7], - // (uint64_t) imgBuff0[15], - // (uint64_t) imgBuff0[5000]); - //pi_camera_control(&camera, PI_CAMERA_CMD_STOP, 0); - //printf("Captured image\n"); - - start = xTaskGetTickCount(); + printf("Captured in %u ms\n", end - start); - txp.dst = MAKE_ROUTE(ESP32, WIFI_DATA); - txp.src = MAKE_ROUTE(GAP8, WIFI_DATA); - - // First send information about the image - img_header_t *header = (img_header_t *)txp.data; - header->magic = 0xBC; - header->width = CAM_WIDTH; - header->height = CAM_HEIGHT; - header->depth = 1; - header->type = 0; - header->size = imgSize; - txp.len = sizeof(img_header_t) + 2; - com_write(&txp); - - int i = 0; - int part = 0; - int offset = 0; - int size = 0; - - do + if (streamerMode == JPEG_ENCODING) { - offset = part * sizeof(txp.data); - size = sizeof(txp.data); - if (offset + size > imgSize) + //jpeg_encoder_process_async(&jpeg_encoder, &buffer, &jpeg_data, pi_task_callback(&task1, encoding_done_cb, NULL)); + //xEventGroupWaitBits(evGroup, JPEG_ENCODING_DONE_BIT, pdTRUE, pdFALSE, (TickType_t)portMAX_DELAY); + //jpeg_encoder_process_status(&jpegSize, NULL); + start = xTaskGetTickCount(); + jpeg_encoder_process(&jpeg_encoder, &buffer, &jpeg_data, &jpegSize); + end = xTaskGetTickCount(); + printf("Encoded in %u ms (size is %u)\n", end - start, jpegSize); + + txp.dst = MAKE_ROUTE(ESP32, WIFI_DATA); + txp.src = MAKE_ROUTE(GAP8, WIFI_DATA); + + uint32_t imgSize = headerSize + jpegSize + footerSize; + + // First send information about the image + img_header_t *imgHeader = (img_header_t *)txp.data; + imgHeader->magic = 0xBC; + imgHeader->width = CAM_WIDTH; + imgHeader->height = CAM_HEIGHT; + imgHeader->depth = 1; + imgHeader->type = 1; + imgHeader->size = imgSize; + txp.len = sizeof(img_header_t) + 2; + com_write(&txp); + + int i = 0; + int part = 0; + int offset = 0; + int size = 0; + + start = xTaskGetTickCount(); + // First send header + memcpy(txp.data, header.data, headerSize); + txp.len = headerSize + 2; // + 2 for header + com_write(&txp); + + do { - size = imgSize - offset; - } - memcpy(txp.data, &imgBuff0[offset], sizeof(txp.data)); - //printf("Copied from %u (size is %u)\n", offset, size); - txp.len = size + 2; // + 2 for header + offset = part * sizeof(txp.data); + size = sizeof(txp.data); + if (offset + size > jpegSize) + { + size = jpegSize - offset; + } + memcpy(txp.data, &jpeg_data.data[offset], sizeof(txp.data)); + //printf("Copied from %u (size is %u)\n", offset, size); + txp.len = size + 2; // + 2 for header + com_write(&txp); + part++; + } while (size == sizeof(txp.data)); + + memcpy(txp.data, footer.data, footerSize); + txp.len = footerSize + 2; // + 2 for header com_write(&txp); - part++; - } while (size == sizeof(txp.data)); - //printf("Finished sending image\n"); - end = xTaskGetTickCount(); - //printf("Sent in %u\n", end - start); - vTaskDelay(10); + + end = xTaskGetTickCount(); + printf("Sent in %u\n", end - start); + } + else + { + start = xTaskGetTickCount(); + + txp.dst = MAKE_ROUTE(ESP32, WIFI_DATA); + txp.src = MAKE_ROUTE(GAP8, WIFI_DATA); + + // First send information about the image + img_header_t *header = (img_header_t *)txp.data; + header->magic = 0xBC; + header->width = CAM_WIDTH; + header->height = CAM_HEIGHT; + header->depth = 1; + header->type = 0; + header->size = imgSize; + txp.len = sizeof(img_header_t) + 2; + com_write(&txp); + + int i = 0; + int part = 0; + int offset = 0; + int size = 0; + + do + { + offset = part * sizeof(txp.data); + size = sizeof(txp.data); + if (offset + size > imgSize) + { + size = imgSize - offset; + } + memcpy(txp.data, &imgBuff0[offset], sizeof(txp.data)); + //printf("Copied from %u (size is %u)\n", offset, size); + txp.len = size + 2; // + 2 for header + com_write(&txp); + part++; + } while (size == sizeof(txp.data)); + //printf("Finished sending image\n"); + end = xTaskGetTickCount(); + printf("Sent in %u\n", end - start); + vTaskDelay(10); + } } else { From 703808f8a83cfb04288a12cb47a9cd158a4db13d Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 14 Dec 2021 14:48:27 +0100 Subject: [PATCH 03/10] Updated protocol (CPX) --- .../wifi_jpeg_streamer/Makefile | 2 +- .../wifi_jpeg_streamer/com.h | 8 -- .../wifi_jpeg_streamer/cpx.c | 29 ++++++ .../wifi_jpeg_streamer/cpx.h | 41 ++++++++ .../wifi_jpeg_streamer/opencv-viewer.py | 6 +- .../wifi_jpeg_streamer/test.c | 96 ++++++++++--------- .../wifi_jpeg_streamer/wifi.h | 18 ++++ 7 files changed, 141 insertions(+), 59 deletions(-) create mode 100644 GAP8/test_functionalities/wifi_jpeg_streamer/cpx.c create mode 100644 GAP8/test_functionalities/wifi_jpeg_streamer/cpx.h create mode 100644 GAP8/test_functionalities/wifi_jpeg_streamer/wifi.h diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile b/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile index efc3eba..cfcf583 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile @@ -2,7 +2,7 @@ io=uart PMSIS_OS = freertos APP = test -APP_SRCS += test.c com.c +APP_SRCS += test.c com.c cpx.c APP_CFLAGS += -O3 -g APP_CFLAGS += -DconfigUSE_TIMERS=1 -DINCLUDE_xTimerPendFunctionCall=1 diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/com.h b/GAP8/test_functionalities/wifi_jpeg_streamer/com.h index 67849db..57da3d2 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/com.h +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/com.h @@ -11,14 +11,6 @@ typedef struct uint8_t data[MTU]; } __attribute__((packed)) packet_t; -typedef struct -{ - uint16_t len; // Of data (max 1022) - uint8_t dst; // Bootloader is 0xFF - uint8_t src; - uint8_t data[MTU-2]; -} __attribute__((packed)) routed_packet_t; - /* Initialize the communication */ void com_init(); diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/cpx.c b/GAP8/test_functionalities/wifi_jpeg_streamer/cpx.c new file mode 100644 index 0000000..d4e7549 --- /dev/null +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/cpx.c @@ -0,0 +1,29 @@ +#pragma once + +#include +#include "com.h" + +#include "cpx.h" + +static packet_t txp; +static packet_t rxp; + +// Return length of packet +uint32_t cpxReceivePacketBlocking(CPXPacket_t * packet) { + uint32_t size; + com_read(&rxp); + size = rxp.len; + memcpy(&packet->routing, rxp.data, size); + return size; +} + +void cpxSendPacketBlocking(CPXPacket_t * packet, uint32_t size) { + uint32_t wireLength = size + sizeof(CPXRouting_t); + txp.len = wireLength; + memcpy(txp.data, &packet->routing, wireLength); + com_write(&txp); +} + +bool cpxSendPacket(CPXPacket_t * packet, uint32_t timeoutInMS) { + // TODO +} \ No newline at end of file diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/cpx.h b/GAP8/test_functionalities/wifi_jpeg_streamer/cpx.h new file mode 100644 index 0000000..678feed --- /dev/null +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/cpx.h @@ -0,0 +1,41 @@ +#pragma once + +#include +#include + +#include "com.h" + +typedef enum { + STM32 = 1, + ESP32 = 2, + HOST = 3, + GAP8 = 4 +} __attribute__((packed)) CPXTarget_t; // Rename to Destination + +typedef enum { + SYSTEM = 1, + CONSOLE = 2, + CRTP = 3, + WIFI_CTRL = 4, + APP = 5, + TEST = 0x0E, + BOOTLOADER = 0x0F, +} __attribute__((packed)) CPXFunction_t; + +typedef struct { + uint8_t destination : 4; + uint8_t source : 4; + uint8_t function; +} __attribute__((packed)) CPXRouting_t; + +typedef struct { + CPXRouting_t routing; + uint8_t data[MTU-2]; +} __attribute__((packed)) CPXPacket_t; + +// Return length of packet +uint32_t cpxReceivePacketBlocking(CPXPacket_t * packet); + +void cpxSendPacketBlocking(CPXPacket_t * packet, uint32_t size); + +bool cpxSendPacket(CPXPacket_t * packet, uint32_t timeoutInMS); \ No newline at end of file diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py b/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py index 79fe52c..f517793 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/opencv-viewer.py @@ -75,10 +75,10 @@ def rx_bytes(size): # First get the info packetInfoRaw = rx_bytes(4) #print(packetInfoRaw) - [length, dst, src] = struct.unpack('0x{:02X}".format(routing & 0xF, routing >> 4)) + #print("Function is 0x{:02X}".format(function)) imgHeader = rx_bytes(length - 2) #print(imgHeader) diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c index ccda560..5981e09 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c @@ -5,8 +5,8 @@ #include "gaplib/jpeg_encoder.h" #include "stdio.h" -#include "com.h" -#include "routing_info.h" +#include "cpx.h" +#include "wifi.h" // This file should be created manually and not committed (part of ignore) // It should contain the following: @@ -64,24 +64,29 @@ static pi_device_t led_gpio_dev; static int wifiConnected = 0; static int wifiClientConnected = 0; -static routed_packet_t rxp; +static CPXPacket_t rxp; void rx_task(void *parameters) { while (1) { - com_read(&rxp); + cpxReceivePacketBlocking(&rxp); + + if (rxp.routing.function == WIFI_CTRL) { + WiFiCTRLPacket_t * wifiCtrl = (WiFiCTRLPacket_t*) rxp.data; + + switch (wifiCtrl->cmd) + { + case STATUS_WIFI_CONNECTED: + printf("Wifi connected (%u.%u.%u.%u)\n", wifiCtrl->data[0], wifiCtrl->data[1], + wifiCtrl->data[2], wifiCtrl->data[3]); + wifiConnected = 1; + break; + case STATUS_CLIENT_CONNECTED: + printf("Wifi client connection status: %u\n", wifiCtrl->data[0]); + wifiClientConnected = wifiCtrl->data[0]; + break; + } - switch (rxp.data[0]) - { - case 0x21: - printf("Wifi connected (%u.%u.%u.%u)\n", rxp.data[1], rxp.data[2], - rxp.data[3], rxp.data[4]); - wifiConnected = 1; - break; - case 0x23: - printf("Wifi client connection status: %u\n", rxp.data[1]); - wifiClientConnected = rxp.data[1]; - break; } } } @@ -121,28 +126,29 @@ uint32_t jpegSize; static StreamerMode_t streamerMode = RAW_ENCODING; -static routed_packet_t txp; +static CPXPacket_t txp; void camera_task(void *parameters) { vTaskDelay(2000); printf("Sending wifi stuff...\n"); - txp.len = 2 + sizeof(ssid); - txp.dst = MAKE_ROUTE(ESP32, WIFI_CTRL); - txp.src = MAKE_ROUTE(GAP8, WIFI_CTRL); - txp.data[0] = 0x10; // Set SSID - memcpy(&txp.data[1], ssid, sizeof(ssid)); - com_write(&txp); - - txp.len = 2 + sizeof(passwd); - txp.data[0] = 0x11; // Set passwd - memcpy(&txp.data[1], passwd, sizeof(passwd)); - com_write(&txp); - - txp.len = 3; - txp.data[0] = 0x20; // Connect wifi - com_write(&txp); + // Set up the routing for the WiFi CTRL packets + txp.routing.destination = ESP32; + rxp.routing.source = GAP8; + txp.routing.function = WIFI_CTRL; + + WiFiCTRLPacket_t * wifiCtrl = (WiFiCTRLPacket_t*) txp.data; + wifiCtrl->cmd = SET_SSID; + memcpy(wifiCtrl->data, ssid, sizeof(ssid)); + cpxSendPacketBlocking(&txp, sizeof(ssid) + 1); // Too large + + wifiCtrl->cmd = SET_KEY; + memcpy(wifiCtrl->data, passwd, sizeof(passwd)); + cpxSendPacketBlocking(&txp, sizeof(passwd) + 1); // Too large + + wifiCtrl->cmd = WIFI_CONNECT; + cpxSendPacketBlocking(&txp, sizeof(WiFiCTRLPacket_t)); // Too large printf("Starting camera task...\n"); uint32_t resolution = CAM_WIDTH * CAM_HEIGHT; @@ -219,8 +225,9 @@ void camera_task(void *parameters) end = xTaskGetTickCount(); printf("Encoded in %u ms (size is %u)\n", end - start, jpegSize); - txp.dst = MAKE_ROUTE(ESP32, WIFI_DATA); - txp.src = MAKE_ROUTE(GAP8, WIFI_DATA); + txp.routing.destination = HOST; + txp.routing.source = GAP8; + txp.routing.function = 0; // We don't care about this one for now uint32_t imgSize = headerSize + jpegSize + footerSize; @@ -232,8 +239,7 @@ void camera_task(void *parameters) imgHeader->depth = 1; imgHeader->type = 1; imgHeader->size = imgSize; - txp.len = sizeof(img_header_t) + 2; - com_write(&txp); + cpxSendPacketBlocking(&txp, sizeof(img_header_t)); int i = 0; int part = 0; @@ -243,8 +249,7 @@ void camera_task(void *parameters) start = xTaskGetTickCount(); // First send header memcpy(txp.data, header.data, headerSize); - txp.len = headerSize + 2; // + 2 for header - com_write(&txp); + cpxSendPacketBlocking(&txp, headerSize); do { @@ -256,14 +261,12 @@ void camera_task(void *parameters) } memcpy(txp.data, &jpeg_data.data[offset], sizeof(txp.data)); //printf("Copied from %u (size is %u)\n", offset, size); - txp.len = size + 2; // + 2 for header - com_write(&txp); + cpxSendPacketBlocking(&txp, size); part++; } while (size == sizeof(txp.data)); memcpy(txp.data, footer.data, footerSize); - txp.len = footerSize + 2; // + 2 for header - com_write(&txp); + cpxSendPacketBlocking(&txp, footerSize); end = xTaskGetTickCount(); printf("Sent in %u\n", end - start); @@ -272,8 +275,9 @@ void camera_task(void *parameters) { start = xTaskGetTickCount(); - txp.dst = MAKE_ROUTE(ESP32, WIFI_DATA); - txp.src = MAKE_ROUTE(GAP8, WIFI_DATA); + txp.routing.destination = HOST; + txp.routing.source = GAP8; + txp.routing.function = 0; // Not used yet // First send information about the image img_header_t *header = (img_header_t *)txp.data; @@ -283,8 +287,7 @@ void camera_task(void *parameters) header->depth = 1; header->type = 0; header->size = imgSize; - txp.len = sizeof(img_header_t) + 2; - com_write(&txp); + cpxSendPacketBlocking(&txp, sizeof(img_header_t)); int i = 0; int part = 0; @@ -301,8 +304,7 @@ void camera_task(void *parameters) } memcpy(txp.data, &imgBuff0[offset], sizeof(txp.data)); //printf("Copied from %u (size is %u)\n", offset, size); - txp.len = size + 2; // + 2 for header - com_write(&txp); + cpxSendPacketBlocking(&txp, size); part++; } while (size == sizeof(txp.data)); //printf("Finished sending image\n"); diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/wifi.h b/GAP8/test_functionalities/wifi_jpeg_streamer/wifi.h new file mode 100644 index 0000000..88946c4 --- /dev/null +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/wifi.h @@ -0,0 +1,18 @@ +#pragma once + +#include + +typedef enum { + SET_SSID = 0x10, + SET_KEY = 0x11, + + WIFI_CONNECT = 0x20, + + STATUS_WIFI_CONNECTED = 0x31, + STATUS_CLIENT_CONNECTED = 0x32 +} __attribute__((packed)) WiFiCTRLType; + +typedef struct { + WiFiCTRLType cmd; + uint8_t data[50]; +} __attribute__((packed)) WiFiCTRLPacket_t; \ No newline at end of file From e525cb0060b59fe89bdfb340f113fbf784001be2 Mon Sep 17 00:00:00 2001 From: Marcus Date: Mon, 20 Dec 2021 14:18:54 +0100 Subject: [PATCH 04/10] Added readme --- .../wifi_jpeg_streamer/README.md | 66 +++++++++++++++++++ .../wifi_jpeg_streamer/routing_info.h | 40 ----------- 2 files changed, 66 insertions(+), 40 deletions(-) create mode 100644 GAP8/test_functionalities/wifi_jpeg_streamer/README.md delete mode 100644 GAP8/test_functionalities/wifi_jpeg_streamer/routing_info.h diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/README.md b/GAP8/test_functionalities/wifi_jpeg_streamer/README.md new file mode 100644 index 0000000..713b86d --- /dev/null +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/README.md @@ -0,0 +1,66 @@ +# Image streaming example + +**Note:** This example uses a new protocol that is still not finished, so this is +still very much experimental and will change in the future before replacing the old example. + +This example will stream images from the AI-deck to a computer using WiFi. It can be set to +either stream raw data or JPEG compressed data. + +## How to use the example + +This example (and the ESP/Crazyflie firmware it needs) is still under development, so care needs to +be taken when running it. + +### GAP8 firmware + +Clone the GAP8 test firmware and select the correct branch: + +```text +git clone -b cf-link https://github.com/bitcraze/AIdeck_examples.git +``` + +Go into the example and create a file for the WiFi SSID/key: +```text +cd AIdeck_examples/GAP8/test_functionalities/wifi_jpeg_streamer +touch wifi_credentials.h +``` + +Edit this file to contain the setup for your local WiFi (note that this file is in the ```.gitignore```): +```c +static const char ssid[] = "YourSSID"; +static const char passwd[] = "YourWiFiKey"; +``` + +Build and flash the application. **Note:** Due to a bug in the Makefiles in the SDK you will need to use +our toolbelt to build/flash the application, or fix the Makefiles as described in [this issue](https://github.com/GreenWaves-Technologies/gap_sdk/issues/266). + +```text +docker pull bitcraze/aideck:4.8.0 +docker run --rm -it -v $PWD:/module/data/ --device /dev/ttyUSB0 --privileged -P bitcraze/aideck:4.8.0 /bin/bash -c 'export GAPY_OPENOCD_CABLE=interface/ftdi/olimex-arm-usb-tiny-h.cfg; source /gap_sdk/configs/ai_deck.sh; cd /module/data/; make clean all image flash' +``` + +### ESP32 firmware + +Checkout the ESP32 firmware, build in flash it. Note that this firmware requires the ESP IDF 4.3.1. + +```text +git checkout https://github.com/bitcraze/aideck-esp-firmware.git +cd aideck-esp-firmware +idf.py app bootloader +docker run --rm -it -v $PWD:/module/ --device /dev/ttyUSB0 --privileged -P bitcraze/aideck-nina /bin/bash -c "/openocd-esp32/bin/openocd -f interface/ftdi/olimex-arm-usb-tiny-h.cfg -f board/esp-wroom-32.cfg -c 'program_esp32 build/bootloader/bootloader.bin 0x1000 verify' -c 'program_esp32 build/aideck_esp.bin 0x10000 verify reset exit'" +``` + +### Crazyflie firmware + +If you're also connecting the Crazyflie you will need a new version of this firmware as well, otherwise the console +in the Crazyflie python client will fill up with garbage. + +This code is in PR [904](https://github.com/bitcraze/crazyflie-firmware/pull/904) in the crazyflie-firmware repository. + +Clone, build and flash it. + +```text +git clone -b cf-link-aideck https://github.com/bitcraze/crazyflie-firmware.git +cd crazyflie-firmware +make DEBUG=1 all && make cload +``` diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/routing_info.h b/GAP8/test_functionalities/wifi_jpeg_streamer/routing_info.h deleted file mode 100644 index 4497a9b..0000000 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/routing_info.h +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once - -#include - -// Targets where data can be routed -#define STM32 ((uint8_t) 0x00) -#define ESP32 ((uint8_t) 0x10) -#define GAP8 ((uint8_t) 0x20) - -// Functionality where data can be -// sent for each target. - -// System includes PM etc. -#define SYSTEM ((uint8_t) 0x00) -// Console messages -#define CONSOLE ((uint8_t) 0x01) -// CrazyRealTimeProtocol passthough -#define CRTP ((uint8_t) 0x02) -// WiFi control data (i.e connect and stuff) -#define WIFI_CTRL ((uint8_t) 0x05) -// WiFi data. Once connection is established this acts as passthough -#define WIFI_DATA ((uint8_t) 0x06) -// Application running on the target§ -#define APP ((uint8_t) 0x07) -// Test running on the target (SINK/SOURCE/ECHO) -#define TEST ((uint8_t) 0x0E) -// Bootloader running on target -#define BOOTLOADER ((uint8_t) 0x0F) - -#define MAKE_ROUTE(TARGET, FUNCTION) (TARGET | FUNCTION) -#define ROUTE_TARGET(ROUTE) (ROUTE & 0xF0) -#define ROUTE_FUNCTION(ROUTE) (ROUTE & 0x0F) - - -// Header used for routing, place after length (which is not part -// of routing) -typedef struct { - uint8_t dst; - uint8_t src; -} __attribute__((packed)) routable_packet_header_t; From b43338433ae339ff3c985515e7cb5816dc04496a Mon Sep 17 00:00:00 2001 From: Marcus Date: Mon, 20 Dec 2021 14:31:28 +0100 Subject: [PATCH 05/10] Updated readme with information on viewer --- .../wifi_jpeg_streamer/README.md | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/README.md b/GAP8/test_functionalities/wifi_jpeg_streamer/README.md index 713b86d..851b678 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/README.md +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/README.md @@ -64,3 +64,26 @@ git clone -b cf-link-aideck https://github.com/bitcraze/crazyflie-firmware.git cd crazyflie-firmware make DEBUG=1 all && make cload ``` + +### Viewer on the PC + +Once everything is flashed, power cycle the hardware and wait for it to connect to the WiFi. The IP +can either be found by have a USB<>UART adapter and listening either to the GAP8 (UART1 TX on expansion connector), +the ESP32 (IO1 on the expansion header) or checking the assigned IP on your router interface. + +The viewer needs openCV, it can be started by using the following command in the directory of the GAP8 example +code. + +```text +python opencv-viewer.py -n +``` + +**Note:** The openCV version installed byt the GAP8 SDK is not compatible with this example (you will get and error +about along the lines of: ```The function is not implemented. Rebuild the library with Windows, GTK+ 2.x or Cocoa support.```), so if you're +not using a virtual environment for the SDK you will have to do it for the example. Then run the following command +in the GAP8 example firmware folder. + +```text +virtualenv venv +venv/bin/activate +``` From e643bdc3ae94df164b0e1197ed4cfe93444fbb40 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Micha=C5=82=20Barci=C5=9B?= Date: Fri, 28 Jan 2022 15:31:20 +0400 Subject: [PATCH 06/10] git clone instead of git checkout --- GAP8/test_functionalities/wifi_jpeg_streamer/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/README.md b/GAP8/test_functionalities/wifi_jpeg_streamer/README.md index 851b678..ed989fd 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/README.md +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/README.md @@ -44,7 +44,7 @@ docker run --rm -it -v $PWD:/module/data/ --device /dev/ttyUSB0 --privileged -P Checkout the ESP32 firmware, build in flash it. Note that this firmware requires the ESP IDF 4.3.1. ```text -git checkout https://github.com/bitcraze/aideck-esp-firmware.git +git clone https://github.com/bitcraze/aideck-esp-firmware.git cd aideck-esp-firmware idf.py app bootloader docker run --rm -it -v $PWD:/module/ --device /dev/ttyUSB0 --privileged -P bitcraze/aideck-nina /bin/bash -c "/openocd-esp32/bin/openocd -f interface/ftdi/olimex-arm-usb-tiny-h.cfg -f board/esp-wroom-32.cfg -c 'program_esp32 build/bootloader/bootloader.bin 0x1000 verify' -c 'program_esp32 build/aideck_esp.bin 0x10000 verify reset exit'" From 19affd752f934997af03853cb8f0d5b479ca4862 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Tue, 1 Feb 2022 19:56:58 +0100 Subject: [PATCH 07/10] Add support for software access point mode This changes the definition of WIFI_CTRL_WIFI_CONNECT (now with an additional payload) and thus requires an updated NINA firmware as well. --- .../wifi_jpeg_streamer/README.md | 1 + .../wifi_jpeg_streamer/test.c | 16 ++++++++++------ .../wifi_jpeg_streamer/wifi.h | 12 ++++++------ 3 files changed, 17 insertions(+), 12 deletions(-) diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/README.md b/GAP8/test_functionalities/wifi_jpeg_streamer/README.md index ed989fd..6775600 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/README.md +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/README.md @@ -29,6 +29,7 @@ Edit this file to contain the setup for your local WiFi (note that this file is ```c static const char ssid[] = "YourSSID"; static const char passwd[] = "YourWiFiKey"; +static const bool use_soft_ap = false; ``` Build and flash the application. **Note:** Due to a bug in the Makefiles in the SDK you will need to use diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c index 5981e09..79f7b5d 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c @@ -12,6 +12,7 @@ // It should contain the following: // static const char ssid[] = "YourSSID"; // static const char passwd[] = "YourWiFiKey"; +// static const bool use_soft_ap = false; #include "wifi_credentials.h" #define IMG_ORIENTATION 0x0101 @@ -76,15 +77,17 @@ void rx_task(void *parameters) switch (wifiCtrl->cmd) { - case STATUS_WIFI_CONNECTED: + case WIFI_CTRL_STATUS_WIFI_CONNECTED: printf("Wifi connected (%u.%u.%u.%u)\n", wifiCtrl->data[0], wifiCtrl->data[1], wifiCtrl->data[2], wifiCtrl->data[3]); wifiConnected = 1; break; - case STATUS_CLIENT_CONNECTED: + case WIFI_CTRL_STATUS_CLIENT_CONNECTED: printf("Wifi client connection status: %u\n", wifiCtrl->data[0]); wifiClientConnected = wifiCtrl->data[0]; break; + default: + break; } } @@ -139,16 +142,17 @@ void camera_task(void *parameters) txp.routing.function = WIFI_CTRL; WiFiCTRLPacket_t * wifiCtrl = (WiFiCTRLPacket_t*) txp.data; - wifiCtrl->cmd = SET_SSID; + wifiCtrl->cmd = WIFI_CTRL_SET_SSID; memcpy(wifiCtrl->data, ssid, sizeof(ssid)); cpxSendPacketBlocking(&txp, sizeof(ssid) + 1); // Too large - wifiCtrl->cmd = SET_KEY; + wifiCtrl->cmd = WIFI_CTRL_SET_KEY; memcpy(wifiCtrl->data, passwd, sizeof(passwd)); cpxSendPacketBlocking(&txp, sizeof(passwd) + 1); // Too large - wifiCtrl->cmd = WIFI_CONNECT; - cpxSendPacketBlocking(&txp, sizeof(WiFiCTRLPacket_t)); // Too large + wifiCtrl->cmd = WIFI_CTRL_WIFI_CONNECT; + wifiCtrl->data[0] = use_soft_ap; + cpxSendPacketBlocking(&txp, 2); printf("Starting camera task...\n"); uint32_t resolution = CAM_WIDTH * CAM_HEIGHT; diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/wifi.h b/GAP8/test_functionalities/wifi_jpeg_streamer/wifi.h index 88946c4..8e44996 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/wifi.h +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/wifi.h @@ -3,13 +3,13 @@ #include typedef enum { - SET_SSID = 0x10, - SET_KEY = 0x11, + WIFI_CTRL_SET_SSID = 0x10, + WIFI_CTRL_SET_KEY = 0x11, - WIFI_CONNECT = 0x20, - - STATUS_WIFI_CONNECTED = 0x31, - STATUS_CLIENT_CONNECTED = 0x32 + WIFI_CTRL_WIFI_CONNECT = 0x20, + + WIFI_CTRL_STATUS_WIFI_CONNECTED = 0x31, + WIFI_CTRL_STATUS_CLIENT_CONNECTED = 0x32, } __attribute__((packed)) WiFiCTRLType; typedef struct { From d0c0c01323a43483c16941e52c02ff864e38abce Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 8 Mar 2022 11:28:45 +0100 Subject: [PATCH 08/10] Updated CPX/SPI communication with latest changes --- .../wifi_jpeg_streamer/com.c | 60 ++++++++++++++----- .../wifi_jpeg_streamer/cpx.h | 6 +- 2 files changed, 49 insertions(+), 17 deletions(-) diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/com.c b/GAP8/test_functionalities/wifi_jpeg_streamer/com.c index 0114e1a..29f84d9 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/com.c +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/com.c @@ -1,4 +1,31 @@ +/** + * ,---------, ____ _ __ + * | ,-^-, | / __ )(_) /_______________ _____ ___ + * | ( O ) | / __ / / __/ ___/ ___/ __ `/_ / / _ \ + * | / ,--´ | / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ + * +------` /_____/_/\__/\___/_/ \__,_/ /___/\___/ + * + * AI-deck GAP8 WiFi streamer example + * + * Copyright (C) 2022 Bitcraze AB + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, in version 3. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + * + * + * com.c - SPI interface for ESP32 communication + */ + #include "pmsis.h" #include "com.h" @@ -118,25 +145,22 @@ static void init_spi(pi_device_t *device) static uint32_t start; static uint32_t end; +static uint8_t rx_buff[sizeof(packet_t)]; +static uint8_t tx_buff[sizeof(packet_t)]; + void com_task(void *parameters) { EventBits_t evBits; - uint8_t *rx_buff, *tx_buff; + uint32_t startupESPRTTValue; - rx_buff = (uint8_t *)pmsis_l2_malloc((uint32_t)sizeof(packet_t)); - tx_buff = (uint8_t *)pmsis_l2_malloc((uint32_t)sizeof(packet_t)); + DEBUG_PRINTF("Starting com task\n"); - if (rx_buff == 0) { - DEBUG_PRINTF("Could not allocate RX buffer\n"); - } + pi_gpio_pin_read(&nina_rtt_dev, CONFIG_NINA_GPIO_NINA_ACK, &startupESPRTTValue); - if (tx_buff == 0) { - DEBUG_PRINTF("Could not allocate TX buffer\n"); + if (startupESPRTTValue > 0) { + xEventGroupSetBits(evGroup, NINA_RTT_BIT); } - - DEBUG_PRINTF("Starting com task\n"); - while (1) { @@ -216,6 +240,15 @@ void com_task(void *parameters) sizeLeft += (4-sizeLeft%4); // Pad upwards } + // Protect against the case where the ESP might signal + // on the RTT line that it wants to send, but actually has + // no length. Calling the SPI transfer function with size = 0 + // will corrupt the following transaction. Sending random data + // is ok, since the length is set to 0 and the ESP will ignore it. + if (sizeLeft == 0) { + sizeLeft = 4; + } + DEBUG_PRINTF("Sending %i bytes\n", sizeLeft); // Set GAP8 RTT low before we end the transfer @@ -230,10 +263,6 @@ void com_task(void *parameters) DEBUG_PRINTF("Read %i bytes\n", ((packet_t *)rx_buff)->len); - end = xTaskGetTickCount(); - // These seems to take about 1-2 ms and we're sending about 80 pkgs / image - //printf("Transaction took %u ms\n", end-start); - if (((packet_t *)rx_buff)->len > 0) { if (xQueueSend(rxq, ((packet_t *)rx_buff), (TickType_t)portMAX_DELAY) != pdPASS) @@ -264,6 +293,7 @@ void com_init() if (txq == NULL || rxq == NULL) { + printf("Could not allocate txq and/or rxq in com\n"); pmsis_exit(1); } diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/cpx.h b/GAP8/test_functionalities/wifi_jpeg_streamer/cpx.h index 678feed..80d978b 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/cpx.h +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/cpx.h @@ -23,8 +23,10 @@ typedef enum { } __attribute__((packed)) CPXFunction_t; typedef struct { - uint8_t destination : 4; - uint8_t source : 4; + uint8_t destination : 3; + uint8_t source : 3; + bool lastPacket : 1; + bool reserved : 1; uint8_t function; } __attribute__((packed)) CPXRouting_t; From 7f60f0b965d402585eba87ca15f33ee4163057f7 Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 8 Mar 2022 11:49:44 +0100 Subject: [PATCH 09/10] Changed example name at startup --- GAP8/test_functionalities/wifi_jpeg_streamer/test.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c index 5981e09..64e7ea7 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c @@ -354,7 +354,7 @@ void start_bootloader(void) pmsis_exit(-1); } - printf("\nStarting up!\n"); + printf("\n-- WiFi image streamer example --\n"); printf("FC at %u MHz\n", pi_freq_get(PI_FREQ_DOMAIN_FC) / 1000000); printf("Starting up tasks...\n"); From 710b8618bb56bb198e0fbc429ba3265d250abad4 Mon Sep 17 00:00:00 2001 From: Marcus Date: Tue, 8 Mar 2022 14:59:09 +0100 Subject: [PATCH 10/10] Updated example to only support AP and updated readme --- .gitignore | 3 - .../wifi_jpeg_streamer/Makefile | 4 ++ .../wifi_jpeg_streamer/README.md | 71 ++++--------------- .../wifi_jpeg_streamer/test.c | 23 +++--- 4 files changed, 25 insertions(+), 76 deletions(-) diff --git a/.gitignore b/.gitignore index 9270ede..787084e 100644 --- a/.gitignore +++ b/.gitignore @@ -4,6 +4,3 @@ NINA/firmware/sdkconfig* # GAP8 build files GAP8/**/BUILD/* - -# Image streamer example WiFi credentials -GAP8/test_functionalities/wifi_jpeg_streamer/wifi_credentials.h diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile b/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile index cfcf583..4e67eab 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/Makefile @@ -1,6 +1,10 @@ io=uart PMSIS_OS = freertos +ifeq ($(SETUP_WIFI_AP), 1) +APP_CFLAGS += -DSETUP_WIFI_AP +endif + APP = test APP_SRCS += test.c com.c cpx.c APP_CFLAGS += -O3 -g diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/README.md b/GAP8/test_functionalities/wifi_jpeg_streamer/README.md index 6775600..561b47c 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/README.md +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/README.md @@ -1,85 +1,40 @@ # Image streaming example -**Note:** This example uses a new protocol that is still not finished, so this is -still very much experimental and will change in the future before replacing the old example. - This example will stream images from the AI-deck to a computer using WiFi. It can be set to -either stream raw data or JPEG compressed data. +either stream raw data or JPEG compressed data. If the raw data is Bayer encoded the viewer +will colorize the image. ## How to use the example -This example (and the ESP/Crazyflie firmware it needs) is still under development, so care needs to -be taken when running it. - ### GAP8 firmware -Clone the GAP8 test firmware and select the correct branch: - -```text -git clone -b cf-link https://github.com/bitcraze/AIdeck_examples.git -``` - -Go into the example and create a file for the WiFi SSID/key: -```text -cd AIdeck_examples/GAP8/test_functionalities/wifi_jpeg_streamer -touch wifi_credentials.h -``` - -Edit this file to contain the setup for your local WiFi (note that this file is in the ```.gitignore```): -```c -static const char ssid[] = "YourSSID"; -static const char passwd[] = "YourWiFiKey"; -static const bool use_soft_ap = false; -``` - -Build and flash the application. **Note:** Due to a bug in the Makefiles in the SDK you will need to use -our toolbelt to build/flash the application, or fix the Makefiles as described in [this issue](https://github.com/GreenWaves-Technologies/gap_sdk/issues/266). +This example can be build to either set up the WiFi as an access point, or to let +another party handle the WiFi (like the Crazyflie). The parameter that controls this is +```SETUP_WIFI_AP``` and it is enabled by setting it to ```1```. ```text docker pull bitcraze/aideck:4.8.0 docker run --rm -it -v $PWD:/module/data/ --device /dev/ttyUSB0 --privileged -P bitcraze/aideck:4.8.0 /bin/bash -c 'export GAPY_OPENOCD_CABLE=interface/ftdi/olimex-arm-usb-tiny-h.cfg; source /gap_sdk/configs/ai_deck.sh; cd /module/data/; make clean all image flash' ``` -### ESP32 firmware +### ESP32/Crazyflie firmware -Checkout the ESP32 firmware, build in flash it. Note that this firmware requires the ESP IDF 4.3.1. - -```text -git clone https://github.com/bitcraze/aideck-esp-firmware.git -cd aideck-esp-firmware -idf.py app bootloader -docker run --rm -it -v $PWD:/module/ --device /dev/ttyUSB0 --privileged -P bitcraze/aideck-nina /bin/bash -c "/openocd-esp32/bin/openocd -f interface/ftdi/olimex-arm-usb-tiny-h.cfg -f board/esp-wroom-32.cfg -c 'program_esp32 build/bootloader/bootloader.bin 0x1000 verify' -c 'program_esp32 build/aideck_esp.bin 0x10000 verify reset exit'" -``` - -### Crazyflie firmware - -If you're also connecting the Crazyflie you will need a new version of this firmware as well, otherwise the console -in the Crazyflie python client will fill up with garbage. - -This code is in PR [904](https://github.com/bitcraze/crazyflie-firmware/pull/904) in the crazyflie-firmware repository. - -Clone, build and flash it. - -```text -git clone -b cf-link-aideck https://github.com/bitcraze/crazyflie-firmware.git -cd crazyflie-firmware -make DEBUG=1 all && make cload -``` +For the example to work you will need a recent version of the Crazyflie and ESP firmware. ### Viewer on the PC -Once everything is flashed, power cycle the hardware and wait for it to connect to the WiFi. The IP -can either be found by have a USB<>UART adapter and listening either to the GAP8 (UART1 TX on expansion connector), -the ESP32 (IO1 on the expansion header) or checking the assigned IP on your router interface. +Once everything is flashed, power cycle the hardware and wait for it to connect to the WiFi. If +you're connecting the AI-deck to an access point you can see the assigned IP in the Crazyflie client +console. If you're using the example as AP, then the IP of the AI-deck will be ```192.168.4.1```. -The viewer needs openCV, it can be started by using the following command in the directory of the GAP8 example -code. +The viewer needs openCV, it can be started by using the following command in the directory of +the GAP8 example code. ```text python opencv-viewer.py -n ``` -**Note:** The openCV version installed byt the GAP8 SDK is not compatible with this example (you will get and error +**Note:** The openCV version installed by the GAP8 SDK is not compatible with this example (you will get and error about along the lines of: ```The function is not implemented. Rebuild the library with Windows, GTK+ 2.x or Cocoa support.```), so if you're not using a virtual environment for the SDK you will have to do it for the example. Then run the following command in the GAP8 example firmware folder. diff --git a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c index 74b904a..096f179 100644 --- a/GAP8/test_functionalities/wifi_jpeg_streamer/test.c +++ b/GAP8/test_functionalities/wifi_jpeg_streamer/test.c @@ -8,13 +8,6 @@ #include "cpx.h" #include "wifi.h" -// This file should be created manually and not committed (part of ignore) -// It should contain the following: -// static const char ssid[] = "YourSSID"; -// static const char passwd[] = "YourWiFiKey"; -// static const bool use_soft_ap = false; -#include "wifi_credentials.h" - #define IMG_ORIENTATION 0x0101 #define CAM_WIDTH 324 #define CAM_HEIGHT 244 @@ -135,24 +128,24 @@ void camera_task(void *parameters) { vTaskDelay(2000); - printf("Sending wifi stuff...\n"); +#ifdef SETUP_WIFI_AP + static char ssid[] = "WiFi streaming example"; + printf("Setting up WiFi AP\n"); // Set up the routing for the WiFi CTRL packets txp.routing.destination = ESP32; rxp.routing.source = GAP8; txp.routing.function = WIFI_CTRL; WiFiCTRLPacket_t * wifiCtrl = (WiFiCTRLPacket_t*) txp.data; + wifiCtrl->cmd = WIFI_CTRL_SET_SSID; memcpy(wifiCtrl->data, ssid, sizeof(ssid)); - cpxSendPacketBlocking(&txp, sizeof(ssid) + 1); // Too large - - wifiCtrl->cmd = WIFI_CTRL_SET_KEY; - memcpy(wifiCtrl->data, passwd, sizeof(passwd)); - cpxSendPacketBlocking(&txp, sizeof(passwd) + 1); // Too large - + cpxSendPacketBlocking(&txp, sizeof(ssid)); // Too large + wifiCtrl->cmd = WIFI_CTRL_WIFI_CONNECT; - wifiCtrl->data[0] = use_soft_ap; + wifiCtrl->data[0] = 0x01; cpxSendPacketBlocking(&txp, 2); +#endif printf("Starting camera task...\n"); uint32_t resolution = CAM_WIDTH * CAM_HEIGHT;