├── Kalman
├── html
│ ├── favicon.ico
│ ├── error.html
│ ├── main.css
│ ├── main.js
│ └── root.html
├── main
│ ├── idf_component.yml
│ ├── parameter.h
│ ├── CMakeLists.txt
│ ├── component.mk
│ ├── Kconfig.projbuild
│ ├── udp_trans.c
│ ├── main.cpp
│ ├── web_client.c
│ ├── wifi.c
│ ├── lsm6ds3.cpp
│ └── web_server.c
└── CMakeLists.txt
├── components
├── websocket
│ ├── component.mk
│ ├── CMakeLists.txt
│ ├── Kconfig
│ ├── include
│ │ ├── websocket_server.h
│ │ └── websocket.h
│ ├── websocket.c
│ ├── README.md
│ └── websocket_server.c
├── I2Cdev
│ ├── CMakeLists.txt
│ ├── component.mk
│ ├── I2Cdev.h
│ └── I2Cdev.cpp
├── KalmanFilter
│ ├── CMakeLists.txt
│ ├── component.mk
│ ├── Kalman.h
│ └── Kalman.cpp
├── MadgwickAHRS
│ ├── CMakeLists.txt
│ ├── component.mk
│ ├── MadgwickAHRS.h
│ └── MadgwickAHRS.cpp
├── LSM6DS3
│ ├── CMakeLists.txt
│ ├── component.mk
│ ├── LSM6DS3.h
│ └── LSM6DS3.cpp
└── README.md
├── Madgwick
├── html
│ ├── favicon.ico
│ ├── error.html
│ ├── main.css
│ ├── main.js
│ └── root.html
├── main
│ ├── idf_component.yml
│ ├── parameter.h
│ ├── CMakeLists.txt
│ ├── component.mk
│ ├── Kconfig.projbuild
│ ├── udp_trans.c
│ ├── main.cpp
│ ├── web_client.c
│ ├── lsm6ds3.cpp
│ ├── wifi.c
│ └── web_server.c
└── CMakeLists.txt
├── LICENSE
└── README.md
/Kalman/html/favicon.ico:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/nopnop2002/esp-idf-lsm6ds3/HEAD/Kalman/html/favicon.ico
--------------------------------------------------------------------------------
/components/websocket/component.mk:
--------------------------------------------------------------------------------
1 | # websocket component makefile
2 | # all files are in default positions
--------------------------------------------------------------------------------
/Madgwick/html/favicon.ico:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/nopnop2002/esp-idf-lsm6ds3/HEAD/Madgwick/html/favicon.ico
--------------------------------------------------------------------------------
/components/I2Cdev/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | idf_component_register(SRCS "I2Cdev.cpp"
2 | PRIV_REQUIRES driver
3 | INCLUDE_DIRS "."
4 | )
5 |
--------------------------------------------------------------------------------
/components/KalmanFilter/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | idf_component_register(SRCS "Kalman.cpp"
2 | PRIV_REQUIRES esp_timer
3 | INCLUDE_DIRS ".")
4 |
--------------------------------------------------------------------------------
/components/MadgwickAHRS/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | idf_component_register(SRCS "MadgwickAHRS.cpp"
2 | PRIV_REQUIRES esp_timer
3 | INCLUDE_DIRS ".")
4 |
--------------------------------------------------------------------------------
/components/websocket/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | set(COMPONENT_SRCS "websocket.c" "websocket_server.c")
2 | set(COMPONENT_ADD_INCLUDEDIRS "./include")
3 | set(COMPONENT_REQUIRES lwip mbedtls)
4 | register_component()
--------------------------------------------------------------------------------
/components/LSM6DS3/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | idf_component_register(SRCS "LSM6DS3.cpp"
2 | INCLUDE_DIRS "."
3 | PRIV_REQUIRES driver esp_timer
4 | REQUIRES I2Cdev)
5 |
--------------------------------------------------------------------------------
/Kalman/main/idf_component.yml:
--------------------------------------------------------------------------------
1 | ## IDF Component Manager Manifest File
2 | dependencies:
3 | espressif/mdns:
4 | version: "^1.0.3"
5 | rules:
6 | - if: "idf_version >=5.0"
7 | espressif/cjson:
8 | version: "^1.7.0"
9 | rules:
10 | - if: "idf_version >=6.0"
11 |
--------------------------------------------------------------------------------
/Madgwick/main/idf_component.yml:
--------------------------------------------------------------------------------
1 | ## IDF Component Manager Manifest File
2 | dependencies:
3 | espressif/mdns:
4 | version: "^1.0.3"
5 | rules:
6 | - if: "idf_version >=5.0"
7 | espressif/cjson:
8 | version: "^1.7.0"
9 | rules:
10 | - if: "idf_version >=6.0"
11 |
--------------------------------------------------------------------------------
/Kalman/main/parameter.h:
--------------------------------------------------------------------------------
1 | typedef struct {
2 | uint16_t port;
3 | char ipv4[20]; // xxx.xxx.xxx.xxx
4 | } PARAMETER_t;
5 |
6 |
7 | typedef struct {
8 | float quatx;
9 | float quaty;
10 | float quatz;
11 | float quatw;
12 | float roll;
13 | float pitch;
14 | float yaw;
15 | } POSE_t;
16 |
17 |
--------------------------------------------------------------------------------
/Madgwick/main/parameter.h:
--------------------------------------------------------------------------------
1 | typedef struct {
2 | uint16_t port;
3 | char ipv4[20]; // xxx.xxx.xxx.xxx
4 | } PARAMETER_t;
5 |
6 |
7 | typedef struct {
8 | float quatx;
9 | float quaty;
10 | float quatz;
11 | float quatw;
12 | float roll;
13 | float pitch;
14 | float yaw;
15 | } POSE_t;
16 |
17 |
--------------------------------------------------------------------------------
/Kalman/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | # The following lines of boilerplate have to be in your project's CMakeLists
2 | # in this exact order for cmake to work correctly
3 | cmake_minimum_required(VERSION 3.5)
4 |
5 | set(EXTRA_COMPONENT_DIRS ../components/)
6 |
7 | include($ENV{IDF_PATH}/tools/cmake/project.cmake)
8 | project(LSM6DS3)
9 |
--------------------------------------------------------------------------------
/Kalman/html/error.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | ESP32 Error 404
5 |
6 |
7 |
8 |
9 |
10 |
11 | Error 404
12 | Unknown page. Return home.
13 |
14 |
15 |
16 |
17 |
--------------------------------------------------------------------------------
/Madgwick/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | # The following lines of boilerplate have to be in your project's CMakeLists
2 | # in this exact order for cmake to work correctly
3 | cmake_minimum_required(VERSION 3.5)
4 |
5 | set(EXTRA_COMPONENT_DIRS ../components/)
6 |
7 | include($ENV{IDF_PATH}/tools/cmake/project.cmake)
8 | project(LSM6DS3)
9 |
--------------------------------------------------------------------------------
/Madgwick/html/error.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 | ESP32 Error 404
5 |
6 |
7 |
8 |
9 |
10 |
11 | Error 404
12 | Unknown page. Return home.
13 |
14 |
15 |
16 |
17 |
--------------------------------------------------------------------------------
/Kalman/html/main.css:
--------------------------------------------------------------------------------
1 | /*
2 | Your favorite style is defined here.
3 | */
4 | #canvas1{
5 | position: fixed;
6 | left: 10px;
7 | padding: 0px;
8 | }
9 | #canvas2{
10 | position: fixed;
11 | left: 450px;
12 | padding: 0px;
13 | }
14 | #canvas3{
15 | position: fixed;
16 | left: 450px;
17 | padding: 0px;
18 | }
19 |
20 |
--------------------------------------------------------------------------------
/Madgwick/html/main.css:
--------------------------------------------------------------------------------
1 | /*
2 | Your favorite style is defined here.
3 | */
4 | #canvas1{
5 | position: fixed;
6 | left: 10px;
7 | padding: 0px;
8 | }
9 | #canvas2{
10 | position: fixed;
11 | left: 450px;
12 | padding: 0px;
13 | }
14 | #canvas3{
15 | position: fixed;
16 | left: 450px;
17 | padding: 0px;
18 | }
19 |
20 |
--------------------------------------------------------------------------------
/Kalman/main/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | idf_component_register(SRCS "main.cpp" "lsm6ds3.cpp" "wifi.c" "udp_trans.c" "web_server.c" "web_client.c"
2 | INCLUDE_DIRS "."
3 | EMBED_FILES "../html/error.html"
4 | "../html/favicon.ico"
5 | "../html/main.js"
6 | "../html/root.html"
7 | "../html/main.css")
8 |
--------------------------------------------------------------------------------
/Madgwick/main/CMakeLists.txt:
--------------------------------------------------------------------------------
1 | idf_component_register(SRCS "main.cpp" "lsm6ds3.cpp" "wifi.c" "udp_trans.c" "web_server.c" "web_client.c"
2 | INCLUDE_DIRS "."
3 | EMBED_FILES "../html/error.html"
4 | "../html/favicon.ico"
5 | "../html/main.js"
6 | "../html/root.html"
7 | "../html/main.css")
8 |
--------------------------------------------------------------------------------
/Kalman/main/component.mk:
--------------------------------------------------------------------------------
1 | #
2 | # Main component makefile.
3 | #
4 | # This Makefile can be left empty. By default, it will take the sources in the
5 | # src/ directory, compile them and link them into lib(subdirectory_name).a
6 | # in the build directory. This behaviour is entirely configurable,
7 | # please read the ESP-IDF documents if you need to do this.
8 | #
9 |
--------------------------------------------------------------------------------
/Madgwick/main/component.mk:
--------------------------------------------------------------------------------
1 | #
2 | # Main component makefile.
3 | #
4 | # This Makefile can be left empty. By default, it will take the sources in the
5 | # src/ directory, compile them and link them into lib(subdirectory_name).a
6 | # in the build directory. This behaviour is entirely configurable,
7 | # please read the ESP-IDF documents if you need to do this.
8 | #
9 |
--------------------------------------------------------------------------------
/components/I2Cdev/component.mk:
--------------------------------------------------------------------------------
1 | #
2 | # Main component makefile.
3 | #
4 | # This Makefile can be left empty. By default, it will take the sources in the
5 | # src/ directory, compile them and link them into lib(subdirectory_name).a
6 | # in the build directory. This behaviour is entirely configurable,
7 | # please read the ESP-IDF documents if you need to do this.
8 | #
9 |
10 | COMPONENT_ADD_INCLUDEDIRS=.
--------------------------------------------------------------------------------
/components/LSM6DS3/component.mk:
--------------------------------------------------------------------------------
1 | #
2 | # Main component makefile.
3 | #
4 | # This Makefile can be left empty. By default, it will take the sources in the
5 | # src/ directory, compile them and link them into lib(subdirectory_name).a
6 | # in the build directory. This behaviour is entirely configurable,
7 | # please read the ESP-IDF documents if you need to do this.
8 | #
9 |
10 | COMPONENT_ADD_INCLUDEDIRS=.
--------------------------------------------------------------------------------
/components/KalmanFilter/component.mk:
--------------------------------------------------------------------------------
1 | #
2 | # Main component makefile.
3 | #
4 | # This Makefile can be left empty. By default, it will take the sources in the
5 | # src/ directory, compile them and link them into lib(subdirectory_name).a
6 | # in the build directory. This behaviour is entirely configurable,
7 | # please read the ESP-IDF documents if you need to do this.
8 | #
9 |
10 | COMPONENT_ADD_INCLUDEDIRS=.
--------------------------------------------------------------------------------
/components/MadgwickAHRS/component.mk:
--------------------------------------------------------------------------------
1 | #
2 | # Main component makefile.
3 | #
4 | # This Makefile can be left empty. By default, it will take the sources in the
5 | # src/ directory, compile them and link them into lib(subdirectory_name).a
6 | # in the build directory. This behaviour is entirely configurable,
7 | # please read the ESP-IDF documents if you need to do this.
8 | #
9 |
10 | COMPONENT_ADD_INCLUDEDIRS=.
--------------------------------------------------------------------------------
/components/README.md:
--------------------------------------------------------------------------------
1 | # KalmanFilter
2 | I used [this](https://github.com/TKJElectronics/KalmanFilter) Kalman Filter by TKJ Electronics.
3 |
4 | # MadgwickAHRS
5 | I based it on [this](https://github.com/arduino-libraries/MadgwickAHRS).
6 | The original is for Arduino IDE.
7 | Arduino IDE is a single-tasking development environment, so you can call functions periodically.
8 | On the other hand, ESP-IDF is a multi-tasking development environment and cannot call functions periodically.
9 | There will always be a slight time lag.
10 | For this filter, the sampling period (time difference from the previous execution) is very important.
11 | The original is fixed at 1.0 second, but I changed this to a parameter.
12 |
13 |
14 | # I2Cdev library collection
15 | I used [this](https://github.com/jrowberg/i2cdevlib/tree/master/Arduino) I2Cdev library collection by Jeff Rowberg.
16 | However, I2Cdev has been rewritten for ESP-IDF.
17 |
18 | # websocket
19 | I used [this](https://github.com/Molorius/esp32-websocket) ESP-IDF WebSocket Component.
20 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | MIT License
2 |
3 | Copyright (c) 2023 nopnop2002
4 |
5 | Permission is hereby granted, free of charge, to any person obtaining a copy
6 | of this software and associated documentation files (the "Software"), to deal
7 | in the Software without restriction, including without limitation the rights
8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 |
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 |
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 |
--------------------------------------------------------------------------------
/Kalman/main/Kconfig.projbuild:
--------------------------------------------------------------------------------
1 | menu "Application Configuration"
2 |
3 | config ESP_WIFI_SSID
4 | string "WiFi SSID"
5 | default "myssid"
6 | help
7 | SSID (network name) for the example to connect to.
8 |
9 | config ESP_WIFI_PASSWORD
10 | string "WiFi Password"
11 | default "mypassword"
12 | help
13 | WiFi password (WPA or WPA2) for the example to use.
14 |
15 | config ESP_MAXIMUM_RETRY
16 | int "Maximum retry"
17 | default 5
18 | help
19 | Set the Maximum retry to avoid station reconnecting to the AP unlimited when the AP is really inexistent.
20 |
21 | config GPIO_RANGE_MAX
22 | int
23 | default 33 if IDF_TARGET_ESP32
24 | default 46 if IDF_TARGET_ESP32S2
25 | default 48 if IDF_TARGET_ESP32S3
26 | default 18 if IDF_TARGET_ESP32C2
27 | default 19 if IDF_TARGET_ESP32C3
28 | default 30 if IDF_TARGET_ESP32C6
29 |
30 | config GPIO_SCL
31 | int "SCL GPIO number"
32 | range 0 GPIO_RANGE_MAX
33 | default 22 if IDF_TARGET_ESP32
34 | default 12 if IDF_TARGET_ESP32S2 || IDF_TARGET_ESP32S3
35 | default 5 # C3 and others
36 | help
37 | GPIO number (IOxx) to I2C SCL.
38 |
39 | config GPIO_SDA
40 | int "SDA GPIO number"
41 | range 0 GPIO_RANGE_MAX
42 | default 21 if IDF_TARGET_ESP32
43 | default 11 if IDF_TARGET_ESP32S2 || IDF_TARGET_ESP32S3
44 | default 4 # C3 and others
45 | help
46 | GPIO number (IOxx) of SDA.
47 |
48 | config I2C_ADDR
49 | hex "I2C Address"
50 | range 0x6A 0x6B
51 | default 0x6A
52 | help
53 | I2C Address.
54 |
55 | endmenu
56 |
--------------------------------------------------------------------------------
/Madgwick/main/Kconfig.projbuild:
--------------------------------------------------------------------------------
1 | menu "Application Configuration"
2 |
3 | config ESP_WIFI_SSID
4 | string "WiFi SSID"
5 | default "myssid"
6 | help
7 | SSID (network name) for the example to connect to.
8 |
9 | config ESP_WIFI_PASSWORD
10 | string "WiFi Password"
11 | default "mypassword"
12 | help
13 | WiFi password (WPA or WPA2) for the example to use.
14 |
15 | config ESP_MAXIMUM_RETRY
16 | int "Maximum retry"
17 | default 5
18 | help
19 | Set the Maximum retry to avoid station reconnecting to the AP unlimited when the AP is really inexistent.
20 |
21 | config GPIO_RANGE_MAX
22 | int
23 | default 33 if IDF_TARGET_ESP32
24 | default 46 if IDF_TARGET_ESP32S2
25 | default 48 if IDF_TARGET_ESP32S3
26 | default 18 if IDF_TARGET_ESP32C2
27 | default 19 if IDF_TARGET_ESP32C3
28 | default 30 if IDF_TARGET_ESP32C6
29 |
30 | config GPIO_SCL
31 | int "SCL GPIO number"
32 | range 0 GPIO_RANGE_MAX
33 | default 22 if IDF_TARGET_ESP32
34 | default 12 if IDF_TARGET_ESP32S2 || IDF_TARGET_ESP32S3
35 | default 5 # C3 and others
36 | help
37 | GPIO number (IOxx) to I2C SCL.
38 |
39 | config GPIO_SDA
40 | int "SDA GPIO number"
41 | range 0 GPIO_RANGE_MAX
42 | default 21 if IDF_TARGET_ESP32
43 | default 11 if IDF_TARGET_ESP32S2 || IDF_TARGET_ESP32S3
44 | default 4 # C3 and others
45 | help
46 | GPIO number (IOxx) of SDA.
47 |
48 | config I2C_ADDR
49 | hex "I2C Address"
50 | range 0x6A 0x6B
51 | default 0x6A
52 | help
53 | I2C Address.
54 |
55 | endmenu
56 |
--------------------------------------------------------------------------------
/components/websocket/Kconfig:
--------------------------------------------------------------------------------
1 |
2 | menu "WebSocket Server"
3 |
4 | config WEBSOCKET_SERVER_MAX_CLIENTS
5 | int "Max clients"
6 | range 1 1000
7 | default 20
8 | help
9 | Maximum number of clients that the WebSocket
10 | server can handle at a time.
11 |
12 | config WEBSOCKET_SERVER_QUEUE_SIZE
13 | int "Queue read size"
14 | range 1 100
15 | default 10
16 | help
17 | Size of the queue to deal with incoming
18 | WebSocket messages. The queue holds the
19 | connection, not the actual message.
20 |
21 | config WEBSOCKET_SERVER_QUEUE_TIMEOUT
22 | int "Queue timeout"
23 | range 0 10000
24 | default 30
25 | help
26 | Timeout for adding new connections to the
27 | read queue.
28 |
29 | config WEBSOCKET_SERVER_TASK_STACK_DEPTH
30 | int "Stack depth"
31 | range 3000 20000
32 | default 6000
33 | help
34 | Stack depth for the WebSocket server. The task
35 | handles reads.
36 |
37 | config WEBSOCKET_SERVER_TASK_PRIORITY
38 | int "Priority"
39 | range 1 20
40 | default 5
41 | help
42 | Priority for the WebSocket server. The task
43 | handles reads.
44 |
45 | config WEBSOCKET_SERVER_PINNED
46 | bool "Server pinned to core"
47 | default false
48 | help
49 | Pin the WebSocket server task to a specific core.
50 | The task handles reads.
51 |
52 | config WEBSOCKET_SERVER_PINNED_CORE
53 | int "Pinned core"
54 | depends on WEBSOCKET_SERVER_PINNED
55 | range 0 1
56 | default 0
57 | help
58 | Core that the WebSocket server is pinned to.
59 | The task handles reads.
60 |
61 | endmenu
62 |
--------------------------------------------------------------------------------
/Kalman/main/udp_trans.c:
--------------------------------------------------------------------------------
1 | /* UDP Transmitter
2 |
3 | This example code is in the Public Domain (or CC0 licensed, at your option.)
4 |
5 | Unless required by applicable law or agreed to in writing, this
6 | software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
7 | CONDITIONS OF ANY KIND, either express or implied.
8 | */
9 | #include
10 | #include "freertos/FreeRTOS.h"
11 | #include "freertos/timers.h"
12 | #include "esp_system.h"
13 | #include "esp_log.h"
14 | #include "lwip/sockets.h"
15 |
16 | #include "parameter.h"
17 |
18 | extern QueueHandle_t xQueueTrans;
19 |
20 | static const char *TAG = "SEND";
21 |
22 | // UDP Send Task
23 | void udp_trans(void *pvParameters) {
24 | ESP_LOGI(TAG, "Start");
25 | #if 0
26 | PARAMETER_t *task_parameter = pvParameters;
27 | PARAMETER_t param;
28 | memcpy((char *)¶m, task_parameter, sizeof(PARAMETER_t));
29 | ESP_LOGI(TAG, "Start:param.port=%d param.ipv4=[%s]", param.port, param.ipv4);
30 | #endif
31 |
32 | struct sockaddr_in addr;
33 | memset(&addr, 0, sizeof(addr));
34 | addr.sin_family = AF_INET;
35 | //addr.sin_port = htons(param.port);
36 | addr.sin_port = htons(5005); // port of pyteapot.py
37 | addr.sin_addr.s_addr = htonl(INADDR_BROADCAST); /* send message to 255.255.255.255 */
38 | //addr.sin_addr.s_addr = inet_addr("255.255.255.255"); /* send message to 255.255.255.255 */
39 | //addr.sin_addr.s_addr = inet_addr(param.ipv4);
40 |
41 | /* create the socket */
42 | int fd;
43 | int ret;
44 | fd = lwip_socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP ); // Create a UDP socket.
45 | LWIP_ASSERT("fd >= 0", fd >= 0);
46 |
47 | POSE_t pose;
48 | char buffer[64];
49 | while(1) {
50 | if(xQueueReceive(xQueueTrans, &pose, portMAX_DELAY)) {
51 | ESP_LOGD(TAG, "pose=%f %f %f", pose.roll, pose.pitch, pose.yaw);
52 | //sprintf(buffer, "y168.8099yp12.7914pr-11.8401r");
53 | int buflen = sprintf(buffer, "y%fyp%fpr%fr", pose.yaw, pose.pitch, pose.roll);
54 | ret = lwip_sendto(fd, buffer, buflen, 0, (struct sockaddr *)&addr, sizeof(addr));
55 | LWIP_ASSERT("ret == buflen", ret == buflen);
56 | ESP_LOGD(TAG, "lwip_sendto ret=%d",ret);
57 | } else {
58 | ESP_LOGE(TAG, "xQueueReceive fail");
59 | break;
60 | }
61 | }
62 |
63 | /* close socket. Don't reach here. */
64 | ret = lwip_close(fd);
65 | LWIP_ASSERT("ret == 0", ret == 0);
66 | vTaskDelete( NULL );
67 |
68 | }
69 |
70 |
--------------------------------------------------------------------------------
/Madgwick/main/udp_trans.c:
--------------------------------------------------------------------------------
1 | /* UDP Transmitter
2 |
3 | This example code is in the Public Domain (or CC0 licensed, at your option.)
4 |
5 | Unless required by applicable law or agreed to in writing, this
6 | software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
7 | CONDITIONS OF ANY KIND, either express or implied.
8 | */
9 | #include
10 | #include "freertos/FreeRTOS.h"
11 | #include "freertos/timers.h"
12 | #include "esp_system.h"
13 | #include "esp_log.h"
14 | #include "lwip/sockets.h"
15 |
16 | #include "parameter.h"
17 |
18 | extern QueueHandle_t xQueueTrans;
19 |
20 | static const char *TAG = "SEND";
21 |
22 | // UDP Send Task
23 | void udp_trans(void *pvParameters) {
24 | ESP_LOGI(TAG, "Start");
25 | #if 0
26 | PARAMETER_t *task_parameter = pvParameters;
27 | PARAMETER_t param;
28 | memcpy((char *)¶m, task_parameter, sizeof(PARAMETER_t));
29 | ESP_LOGI(TAG, "Start:param.port=%d param.ipv4=[%s]", param.port, param.ipv4);
30 | #endif
31 |
32 | struct sockaddr_in addr;
33 | memset(&addr, 0, sizeof(addr));
34 | addr.sin_family = AF_INET;
35 | //addr.sin_port = htons(param.port);
36 | addr.sin_port = htons(5005); // port of pyteapot.py
37 | addr.sin_addr.s_addr = htonl(INADDR_BROADCAST); /* send message to 255.255.255.255 */
38 | //addr.sin_addr.s_addr = inet_addr("255.255.255.255"); /* send message to 255.255.255.255 */
39 | //addr.sin_addr.s_addr = inet_addr(param.ipv4);
40 |
41 | /* create the socket */
42 | int fd;
43 | int ret;
44 | fd = lwip_socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP ); // Create a UDP socket.
45 | LWIP_ASSERT("fd >= 0", fd >= 0);
46 |
47 | POSE_t pose;
48 | char buffer[64];
49 | while(1) {
50 | if(xQueueReceive(xQueueTrans, &pose, portMAX_DELAY)) {
51 | ESP_LOGD(TAG, "pose=%f %f %f", pose.roll, pose.pitch, pose.yaw);
52 | //sprintf(buffer, "y168.8099yp12.7914pr-11.8401r");
53 | int buflen = sprintf(buffer, "y%fyp%fpr%fr", pose.yaw, pose.pitch, pose.roll);
54 | ret = lwip_sendto(fd, buffer, buflen, 0, (struct sockaddr *)&addr, sizeof(addr));
55 | LWIP_ASSERT("ret == buflen", ret == buflen);
56 | ESP_LOGD(TAG, "lwip_sendto ret=%d",ret);
57 | } else {
58 | ESP_LOGE(TAG, "xQueueReceive fail");
59 | break;
60 | }
61 | }
62 |
63 | /* close socket. Don't reach here. */
64 | ret = lwip_close(fd);
65 | LWIP_ASSERT("ret == 0", ret == 0);
66 | vTaskDelete( NULL );
67 |
68 | }
69 |
70 |
--------------------------------------------------------------------------------
/components/KalmanFilter/Kalman.h:
--------------------------------------------------------------------------------
1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
2 |
3 | This software may be distributed and modified under the terms of the GNU
4 | General Public License version 2 (GPL2) as published by the Free Software
5 | Foundation and appearing in the file GPL2.TXT included in the packaging of
6 | this file. Please note that GPL2 Section 2[b] requires that all works based
7 | on this software must also be made publicly available under the terms of
8 | the GPL2 ("Copyleft").
9 |
10 | Contact information
11 | -------------------
12 |
13 | Kristian Lauszus, TKJ Electronics
14 | Web : http://www.tkjelectronics.com
15 | e-mail : kristianl@tkjelectronics.com
16 | */
17 |
18 | #ifndef _Kalman_h_
19 | #define _Kalman_h_
20 |
21 | class Kalman {
22 | public:
23 | Kalman();
24 |
25 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
26 | float getAngle(float newAngle, float newRate, float dt);
27 |
28 | void setAngle(float angle); // Used to set angle, this should be set as the starting angle
29 | float getRate(); // Return the unbiased rate
30 |
31 | /* These are used to tune the Kalman filter */
32 | void setQangle(float Q_angle);
33 | /**
34 | * setQbias(float Q_bias)
35 | * Default value (0.003f) is in Kalman.cpp.
36 | * Raise this to follow input more closely,
37 | * lower this to smooth result of kalman filter.
38 | */
39 | void setQbias(float Q_bias);
40 | void setRmeasure(float R_measure);
41 |
42 | float getQangle();
43 | float getQbias();
44 | float getRmeasure();
45 |
46 | private:
47 | /* Kalman filter variables */
48 | float Q_angle; // Process noise variance for the accelerometer
49 | float Q_bias; // Process noise variance for the gyro bias
50 | float R_measure; // Measurement noise variance - this is actually the variance of the measurement noise
51 |
52 | float angle; // The angle calculated by the Kalman filter - part of the 2x1 state vector
53 | float bias; // The gyro bias calculated by the Kalman filter - part of the 2x1 state vector
54 | float rate; // Unbiased rate calculated from the rate and the calculated bias - you have to call getAngle to update the rate
55 |
56 | float P[2][2]; // Error covariance matrix - This is a 2x2 matrix
57 | };
58 |
59 | #endif
60 |
--------------------------------------------------------------------------------
/Kalman/main/main.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include "freertos/FreeRTOS.h"
3 | #include "freertos/task.h"
4 | #include "freertos/queue.h"
5 | #include "freertos/message_buffer.h"
6 | #include "esp_log.h"
7 | #include "mdns.h"
8 | #include "I2Cdev.h"
9 |
10 | #include "parameter.h"
11 |
12 | #include "websocket_server.h"
13 |
14 | MessageBufferHandle_t xMessageBufferToClient;
15 |
16 | static const char *TAG = "MAIN";
17 | static const char *MDNS_HOSTNAME = "esp32";
18 |
19 | QueueHandle_t xQueueTrans;
20 |
21 | extern "C" {
22 | void app_main(void);
23 | }
24 |
25 | #ifdef __cplusplus
26 | extern "C" {
27 | #endif
28 | void start_wifi(void);
29 | void start_mdns(void);
30 | int ws_server_start(void);
31 | void udp_trans(void *pvParameters);
32 | void server_task(void *pvParameters);
33 | void client_task(void *pvParameters);
34 | #ifdef __cplusplus
35 | }
36 | #endif
37 |
38 | void lsm6ds3(void *pvParameters);
39 |
40 | void start_mdns(void)
41 | {
42 | //initialize mDNS
43 | ESP_ERROR_CHECK( mdns_init() );
44 | //set mDNS hostname (required if you want to advertise services)
45 | ESP_ERROR_CHECK( mdns_hostname_set(MDNS_HOSTNAME) );
46 | ESP_LOGI(TAG, "mdns hostname set to: [%s]", MDNS_HOSTNAME);
47 |
48 | //initialize service
49 | ESP_ERROR_CHECK( mdns_service_add(NULL, "_http", "_tcp", 80, NULL, 0) );
50 |
51 | #if 0
52 | //set default mDNS instance name
53 | ESP_ERROR_CHECK( mdns_instance_name_set("ESP32 with mDNS") );
54 | #endif
55 | }
56 |
57 | void app_main(void)
58 | {
59 | // Initialize WiFi
60 | start_wifi();
61 |
62 | // Initialize mDNS
63 | start_mdns();
64 |
65 | // Initialize i2c
66 | I2Cdev::initialize(400000);
67 |
68 | // Create Queue
69 | xQueueTrans = xQueueCreate(10, sizeof(POSE_t));
70 | configASSERT( xQueueTrans );
71 |
72 | // Create Message Buffer
73 | xMessageBufferToClient = xMessageBufferCreate(1024);
74 | configASSERT( xMessageBufferToClient );
75 |
76 | /* Get the local IP address */
77 | esp_netif_ip_info_t ip_info;
78 | ESP_ERROR_CHECK(esp_netif_get_ip_info(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"), &ip_info));
79 | char cparam0[64];
80 | sprintf(cparam0, IPSTR, IP2STR(&ip_info.ip));
81 | ESP_LOGI(TAG, "cparam0=[%s]", cparam0);
82 |
83 | // Start web socket server
84 | ws_server_start();
85 |
86 | // Start web server
87 | xTaskCreate(&server_task, "SERVER", 1024*3, (void *)cparam0, 5, NULL);
88 |
89 | // Start web client
90 | xTaskCreate(&client_task, "CLIENT", 1024*3, (void *)0x011, 5, NULL);
91 |
92 | // Start imu task
93 | xTaskCreate(&lsm6ds3, "IMU", 1024*8, NULL, 5, NULL);
94 |
95 | // Start udp task
96 | xTaskCreate(&udp_trans, "UDP", 1024*3, NULL, 5, NULL);
97 |
98 | vTaskDelay(100);
99 | }
100 |
--------------------------------------------------------------------------------
/Madgwick/main/main.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include "freertos/FreeRTOS.h"
3 | #include "freertos/task.h"
4 | #include "freertos/queue.h"
5 | #include "freertos/message_buffer.h"
6 | #include "esp_log.h"
7 | #include "mdns.h"
8 | #include "I2Cdev.h"
9 |
10 | #include "parameter.h"
11 |
12 | #include "websocket_server.h"
13 |
14 | MessageBufferHandle_t xMessageBufferToClient;
15 |
16 | static const char *TAG = "MAIN";
17 | static const char *MDNS_HOSTNAME = "esp32";
18 |
19 | QueueHandle_t xQueueTrans;
20 |
21 | extern "C" {
22 | void app_main(void);
23 | }
24 |
25 | #ifdef __cplusplus
26 | extern "C" {
27 | #endif
28 | void start_wifi(void);
29 | void start_mdns(void);
30 | int ws_server_start(void);
31 | void udp_trans(void *pvParameters);
32 | void server_task(void *pvParameters);
33 | void client_task(void *pvParameters);
34 | #ifdef __cplusplus
35 | }
36 | #endif
37 |
38 | void lsm6ds3(void *pvParameters);
39 |
40 | void start_mdns(void)
41 | {
42 | //initialize mDNS
43 | ESP_ERROR_CHECK( mdns_init() );
44 | //set mDNS hostname (required if you want to advertise services)
45 | ESP_ERROR_CHECK( mdns_hostname_set(MDNS_HOSTNAME) );
46 | ESP_LOGI(TAG, "mdns hostname set to: [%s]", MDNS_HOSTNAME);
47 |
48 | //initialize service
49 | ESP_ERROR_CHECK( mdns_service_add(NULL, "_http", "_tcp", 80, NULL, 0) );
50 |
51 | #if 0
52 | //set default mDNS instance name
53 | ESP_ERROR_CHECK( mdns_instance_name_set("ESP32 with mDNS") );
54 | #endif
55 | }
56 |
57 | void app_main(void)
58 | {
59 | // Initialize WiFi
60 | start_wifi();
61 |
62 | // Initialize mDNS
63 | start_mdns();
64 |
65 | // Initialize i2c
66 | I2Cdev::initialize(400000);
67 |
68 | // Create Queue
69 | xQueueTrans = xQueueCreate(10, sizeof(POSE_t));
70 | configASSERT( xQueueTrans );
71 |
72 | // Create Message Buffer
73 | xMessageBufferToClient = xMessageBufferCreate(1024);
74 | configASSERT( xMessageBufferToClient );
75 |
76 | /* Get the local IP address */
77 | esp_netif_ip_info_t ip_info;
78 | ESP_ERROR_CHECK(esp_netif_get_ip_info(esp_netif_get_handle_from_ifkey("WIFI_STA_DEF"), &ip_info));
79 | char cparam0[64];
80 | sprintf(cparam0, IPSTR, IP2STR(&ip_info.ip));
81 | ESP_LOGI(TAG, "cparam0=[%s]", cparam0);
82 |
83 | // Start web socket server
84 | ws_server_start();
85 |
86 | // Start web server
87 | xTaskCreate(&server_task, "SERVER", 1024*3, (void *)cparam0, 5, NULL);
88 |
89 | // Start web client
90 | xTaskCreate(&client_task, "CLIENT", 1024*3, (void *)0x011, 5, NULL);
91 |
92 | // Start imu task
93 | xTaskCreate(&lsm6ds3, "IMU", 1024*8, NULL, 5, NULL);
94 |
95 | // Start udp task
96 | xTaskCreate(&udp_trans, "UDP", 1024*3, NULL, 5, NULL);
97 |
98 | vTaskDelay(100);
99 | }
100 |
--------------------------------------------------------------------------------
/components/LSM6DS3/LSM6DS3.h:
--------------------------------------------------------------------------------
1 | /*
2 | This file is part of the Arduino_LSM6DS3 library.
3 | Copyright (c) 2019 Arduino SA. All rights reserved.
4 |
5 | This library is free software; you can redistribute it and/or
6 | modify it under the terms of the GNU Lesser General Public
7 | License as published by the Free Software Foundation; either
8 | version 2.1 of the License, or (at your option) any later version.
9 |
10 | This library is distributed in the hope that it will be useful,
11 | but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 | Lesser General Public License for more details.
14 |
15 | You should have received a copy of the GNU Lesser General Public
16 | License along with this library; if not, write to the Free Software
17 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
18 | */
19 |
20 | #ifndef _LSM6DS3_H_
21 | #define _LSM6DS3_H_
22 |
23 | #include "I2Cdev.h"
24 |
25 | #define LSM6DS3_ADDRESS 0x6A
26 |
27 | #define LSM6DS3_WHO_AM_I_REG 0X0F
28 | #define LSM6DS3_CTRL1_XL 0X10
29 | #define LSM6DS3_CTRL2_G 0X11
30 |
31 | #define LSM6DS3_STATUS_REG 0X1E
32 |
33 | #define LSM6DS3_CTRL6_C 0X15
34 | #define LSM6DS3_CTRL7_G 0X16
35 | #define LSM6DS3_CTRL8_XL 0X17
36 |
37 | #define LSM6DS3_OUTX_L_G 0X22
38 | #define LSM6DS3_OUTX_H_G 0X23
39 | #define LSM6DS3_OUTY_L_G 0X24
40 | #define LSM6DS3_OUTY_H_G 0X25
41 | #define LSM6DS3_OUTZ_L_G 0X26
42 | #define LSM6DS3_OUTZ_H_G 0X27
43 |
44 | #define LSM6DS3_OUTX_L_XL 0X28
45 | #define LSM6DS3_OUTX_H_XL 0X29
46 | #define LSM6DS3_OUTY_L_XL 0X2A
47 | #define LSM6DS3_OUTY_H_XL 0X2B
48 | #define LSM6DS3_OUTZ_L_XL 0X2C
49 | #define LSM6DS3_OUTZ_H_XL 0X2D
50 |
51 |
52 |
53 | class LSM6DS3 {
54 | public:
55 | LSM6DS3(uint8_t slaveAddress);
56 |
57 | int begin();
58 | void end();
59 |
60 | // Accelerometer
61 | virtual int readAcceleration(float& x, float& y, float& z); // Results are in g (earth gravity).
62 | virtual float accelerationSampleRate(); // Sampling rate of the sensor.
63 | virtual int accelerationAvailable(); // Check for available data from accelerometer
64 |
65 | // Gyroscope
66 | virtual int readGyroscope(float& x, float& y, float& z); // Results are in degrees/second.
67 | virtual float gyroscopeSampleRate(); // Sampling rate of the sensor.
68 | virtual int gyroscopeAvailable(); // Check for available data from gyroscope
69 |
70 |
71 | protected:
72 | int readRegister(uint8_t address);
73 | int readRegisters(uint8_t address, uint8_t* data, size_t length);
74 | int writeRegister(uint8_t address, uint8_t value);
75 |
76 |
77 | private:
78 | uint8_t devAddr;
79 | };
80 |
81 | #endif /* _LSM6DS3_H_ */
82 |
83 |
--------------------------------------------------------------------------------
/components/MadgwickAHRS/MadgwickAHRS.h:
--------------------------------------------------------------------------------
1 | //=============================================================================================
2 | // MadgwickAHRS.h
3 | //=============================================================================================
4 | //
5 | // Implementation of Madgwick's IMU and AHRS algorithms.
6 | // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
7 | //
8 | // From the x-io website "Open-source resources available on this website are
9 | // provided under the GNU General Public Licence unless an alternative licence
10 | // is provided in source."
11 | //
12 | // Date Author Notes
13 | // 29/09/2011 SOH Madgwick Initial release
14 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
15 | //
16 | //=============================================================================================
17 | #ifndef MadgwickAHRS_h
18 | #define MadgwickAHRS_h
19 | #include
20 |
21 | //--------------------------------------------------------------------------------------------
22 | // Variable declaration
23 | class Madgwick{
24 | private:
25 | static float invSqrt(float x);
26 | float beta; // algorithm gain
27 | float q0;
28 | float q1;
29 | float q2;
30 | float q3; // quaternion of sensor frame relative to auxiliary frame
31 | float invSampleFreq;
32 | float roll;
33 | float pitch;
34 | float yaw;
35 | char anglesComputed;
36 | void computeAngles();
37 |
38 | //-------------------------------------------------------------------------------------------
39 | // Function declarations
40 | public:
41 | Madgwick(void);
42 | void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
43 | void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt);
44 | void updateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt);
45 | //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
46 | //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);};
47 | //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
48 | float getRoll() {
49 | if (!anglesComputed) computeAngles();
50 | return roll * 57.29578f;
51 | }
52 | float getPitch() {
53 | if (!anglesComputed) computeAngles();
54 | return pitch * 57.29578f;
55 | }
56 | float getYaw() {
57 | if (!anglesComputed) computeAngles();
58 | return yaw * 57.29578f + 180.0f;
59 | }
60 | float getRollRadians() {
61 | if (!anglesComputed) computeAngles();
62 | return roll;
63 | }
64 | float getPitchRadians() {
65 | if (!anglesComputed) computeAngles();
66 | return pitch;
67 | }
68 | float getYawRadians() {
69 | if (!anglesComputed) computeAngles();
70 | return yaw;
71 | }
72 | };
73 | #endif
74 |
75 |
--------------------------------------------------------------------------------
/Kalman/main/web_client.c:
--------------------------------------------------------------------------------
1 | /*
2 | Example using WEB Socket.
3 | This example code is in the Public Domain (or CC0 licensed, at your option.)
4 | Unless required by applicable law or agreed to in writing, this
5 | software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
6 | CONDITIONS OF ANY KIND, either express or implied.
7 | */
8 |
9 | #include
10 | #include
11 | #include
12 | #include "freertos/FreeRTOS.h"
13 | #include "freertos/task.h"
14 | #include "freertos/queue.h"
15 | #include "freertos/event_groups.h"
16 | #include "freertos/message_buffer.h"
17 | #include "esp_log.h"
18 | #include "cJSON.h"
19 |
20 | static const char *TAG = "client_task";
21 |
22 | #include "websocket_server.h"
23 |
24 | extern MessageBufferHandle_t xMessageBufferToClient;
25 |
26 | void client_task(void* pvParameters) {
27 | int32_t task_parameter = (int32_t)pvParameters;
28 | ESP_LOGI(TAG,"Starting. task_parameter=0x%"PRIx32, task_parameter);
29 |
30 | char meter1[8];
31 | memset(meter1, 0, sizeof(meter1));
32 | if (task_parameter & 0x0001) strcpy(meter1, "ROLL");
33 | char meter2[8];
34 | memset(meter2, 0, sizeof(meter2));
35 | if (task_parameter & 0x0010) strcpy(meter2, "PITCH");
36 | char meter3[8];
37 | memset(meter3, 0, sizeof(meter3));
38 | //if (task_parameter & 0x0100) strcpy(meter3, "YAW");
39 | if (task_parameter & 0x0100) strcpy(meter3, "HEADING");
40 |
41 | char cRxBuffer[512];
42 | char DEL = 0x04;
43 | char outBuffer[64];
44 |
45 | while (1) {
46 | size_t readBytes = xMessageBufferReceive(xMessageBufferToClient, cRxBuffer, sizeof(cRxBuffer), portMAX_DELAY );
47 | ESP_LOGD(TAG, "readBytes=%d", readBytes);
48 | ESP_LOGD(TAG, "cRxBuffer=[%.*s]", readBytes, cRxBuffer);
49 | cJSON *root = cJSON_Parse(cRxBuffer);
50 | if (cJSON_GetObjectItem(root, "id")) {
51 | char *id = cJSON_GetObjectItem(root,"id")->valuestring;
52 | ESP_LOGD(TAG, "id=[%s]",id);
53 |
54 | if ( strcmp (id, "init") == 0) {
55 | sprintf(outBuffer,"HEAD%cEuler Display using ESP32", DEL);
56 | ESP_LOGD(TAG, "outBuffer=[%s]", outBuffer);
57 | ws_server_send_text_all(outBuffer,strlen(outBuffer));
58 |
59 | sprintf(outBuffer,"METER%c%s%c%s%c%s", DEL,meter1,DEL,meter2,DEL,meter3);
60 | ESP_LOGD(TAG, "outBuffer=[%s]", outBuffer);
61 | ws_server_send_text_all(outBuffer,strlen(outBuffer));
62 | } // end if
63 |
64 | if ( strcmp (id, "data-request") == 0) {
65 | double roll = cJSON_GetObjectItem(root,"roll")->valuedouble;
66 | double pitch = cJSON_GetObjectItem(root,"pitch")->valuedouble;
67 | double yaw = cJSON_GetObjectItem(root,"yaw")->valuedouble;
68 | ESP_LOGD(TAG,"roll=%f pitch=%f yaw=%f", roll, pitch, yaw);
69 |
70 | sprintf(outBuffer,"DATA%c%f%c%f%c%f", DEL, roll, DEL, pitch, DEL, yaw);
71 | ESP_LOGD(TAG, "outBuffer=[%s]", outBuffer);
72 | ws_server_send_text_all(outBuffer,strlen(outBuffer));
73 |
74 | ESP_LOGD(TAG,"free_size:%d %d", heap_caps_get_free_size(MALLOC_CAP_8BIT), heap_caps_get_free_size(MALLOC_CAP_32BIT));
75 |
76 | } // end if
77 | } // end if
78 |
79 | // Delete a cJSON structure
80 | cJSON_Delete(root);
81 |
82 | } // end while
83 |
84 | // Never reach here
85 | vTaskDelete(NULL);
86 | }
87 |
--------------------------------------------------------------------------------
/Madgwick/main/web_client.c:
--------------------------------------------------------------------------------
1 | /*
2 | Example using WEB Socket.
3 | This example code is in the Public Domain (or CC0 licensed, at your option.)
4 | Unless required by applicable law or agreed to in writing, this
5 | software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
6 | CONDITIONS OF ANY KIND, either express or implied.
7 | */
8 |
9 | #include
10 | #include
11 | #include
12 | #include "freertos/FreeRTOS.h"
13 | #include "freertos/task.h"
14 | #include "freertos/queue.h"
15 | #include "freertos/event_groups.h"
16 | #include "freertos/message_buffer.h"
17 | #include "esp_log.h"
18 | #include "cJSON.h"
19 |
20 | static const char *TAG = "client_task";
21 |
22 | #include "websocket_server.h"
23 |
24 | extern MessageBufferHandle_t xMessageBufferToClient;
25 |
26 | void client_task(void* pvParameters) {
27 | int32_t task_parameter = (int32_t)pvParameters;
28 | ESP_LOGI(TAG,"Starting. task_parameter=0x%"PRIx32, task_parameter);
29 |
30 | char meter1[8];
31 | memset(meter1, 0, sizeof(meter1));
32 | if (task_parameter & 0x0001) strcpy(meter1, "ROLL");
33 | char meter2[8];
34 | memset(meter2, 0, sizeof(meter2));
35 | if (task_parameter & 0x0010) strcpy(meter2, "PITCH");
36 | char meter3[8];
37 | memset(meter3, 0, sizeof(meter3));
38 | //if (task_parameter & 0x0100) strcpy(meter3, "YAW");
39 | if (task_parameter & 0x0100) strcpy(meter3, "HEADING");
40 |
41 | char cRxBuffer[512];
42 | char DEL = 0x04;
43 | char outBuffer[64];
44 |
45 | while (1) {
46 | size_t readBytes = xMessageBufferReceive(xMessageBufferToClient, cRxBuffer, sizeof(cRxBuffer), portMAX_DELAY );
47 | ESP_LOGD(TAG, "readBytes=%d", readBytes);
48 | ESP_LOGD(TAG, "cRxBuffer=[%.*s]", readBytes, cRxBuffer);
49 | cJSON *root = cJSON_Parse(cRxBuffer);
50 | if (cJSON_GetObjectItem(root, "id")) {
51 | char *id = cJSON_GetObjectItem(root,"id")->valuestring;
52 | ESP_LOGD(TAG, "id=[%s]",id);
53 |
54 | if ( strcmp (id, "init") == 0) {
55 | sprintf(outBuffer,"HEAD%cEuler Display using ESP32", DEL);
56 | ESP_LOGD(TAG, "outBuffer=[%s]", outBuffer);
57 | ws_server_send_text_all(outBuffer,strlen(outBuffer));
58 |
59 | sprintf(outBuffer,"METER%c%s%c%s%c%s", DEL,meter1,DEL,meter2,DEL,meter3);
60 | ESP_LOGD(TAG, "outBuffer=[%s]", outBuffer);
61 | ws_server_send_text_all(outBuffer,strlen(outBuffer));
62 | } // end if
63 |
64 | if ( strcmp (id, "data-request") == 0) {
65 | double roll = cJSON_GetObjectItem(root,"roll")->valuedouble;
66 | double pitch = cJSON_GetObjectItem(root,"pitch")->valuedouble;
67 | double yaw = cJSON_GetObjectItem(root,"yaw")->valuedouble;
68 | ESP_LOGD(TAG,"roll=%f pitch=%f yaw=%f", roll, pitch, yaw);
69 |
70 | sprintf(outBuffer,"DATA%c%f%c%f%c%f", DEL, roll, DEL, pitch, DEL, yaw);
71 | ESP_LOGD(TAG, "outBuffer=[%s]", outBuffer);
72 | ws_server_send_text_all(outBuffer,strlen(outBuffer));
73 |
74 | ESP_LOGD(TAG,"free_size:%d %d", heap_caps_get_free_size(MALLOC_CAP_8BIT), heap_caps_get_free_size(MALLOC_CAP_32BIT));
75 |
76 | } // end if
77 | } // end if
78 |
79 | // Delete a cJSON structure
80 | cJSON_Delete(root);
81 |
82 | } // end while
83 |
84 | // Never reach here
85 | vTaskDelete(NULL);
86 | }
87 |
--------------------------------------------------------------------------------
/Kalman/html/main.js:
--------------------------------------------------------------------------------
1 | //document.getElementById("datetime").innerHTML = "WebSocket is not connected";
2 |
3 | var websocket = new WebSocket('ws://'+location.hostname+'/');
4 | var meter1 = 0;
5 | var meter2 = 0;
6 | var meter3 = 0;
7 | var roll = 0.0;
8 | var yaw = 0.0;
9 | var pitch = 0.0;
10 | var deg2rad = 0.0174533;
11 |
12 | function sendText(name) {
13 | console.log('sendText');
14 | var data = {};
15 | data["id"] = name;
16 | console.log('data=', data);
17 | json_data = JSON.stringify(data);
18 | console.log('json_data=' + json_data);
19 | websocket.send(json_data);
20 | }
21 |
22 | websocket.onopen = function(evt) {
23 | console.log('WebSocket connection opened');
24 | var data = {};
25 | data["id"] = "init";
26 | console.log('data=', data);
27 | json_data = JSON.stringify(data);
28 | console.log('json_data=' + json_data);
29 | websocket.send(json_data);
30 | //document.getElementById("datetime").innerHTML = "WebSocket is connected!";
31 | }
32 |
33 | websocket.onmessage = function(evt) {
34 | var msg = evt.data;
35 | console.log("msg=" + msg);
36 | var values = msg.split('\4'); // \4 is EOT
37 | //console.log("values=" + values);
38 | switch(values[0]) {
39 | case 'HEAD':
40 | console.log("HEAD values[1]=" + values[1]);
41 | var h1 = document.getElementById( 'header' );
42 | h1.textContent = values[1];
43 | break;
44 |
45 | case 'METER':
46 | //console.log("gauge1=" + Object.keys(gauge1.options));
47 | //console.log("gauge1.options.units=" + gauge1.options.units);
48 | console.log("METER values[1]=" + values[1]);
49 | console.log("METER values[2]=" + values[2]);
50 | console.log("METER values[3]=" + values[3]);
51 | if (values[1] != "") {
52 | gauge1.options.title = values[1];
53 | document.getElementById("canvas1").style.display = "inline-block";
54 | meter1 = 1;
55 | }
56 | if (values[2] != "") {
57 | gauge2.options.title = values[2];
58 | document.getElementById("canvas2").style.display = "inline-block";
59 | meter2 = 1;
60 | }
61 | if (values[3] != "") {
62 | gauge3.options.title = values[3];
63 | document.getElementById("canvas3").style.display = "inline-block";
64 | meter3 = 1;
65 | }
66 | break;
67 |
68 | case 'DATA':
69 | console.log("DATA values[1]=" + values[1]);
70 | var val1 = parseFloat(values[1]);
71 | gauge1.value = val1;
72 | gauge1.update({ valueText: val1.toFixed(1) });
73 | roll = val1 * deg2rad;
74 | if (meter2) {
75 | console.log("DATA values[2]=" + values[2]);
76 | var val2 = parseFloat(values[2]);
77 | gauge2.value = val2;
78 | gauge2.update({ valueText: val2.toFixed(1) });
79 | pitch = val2 * deg2rad;
80 | }
81 | if (meter3) {
82 | console.log("DATA values[3]=" + values[3]);
83 | var val3 = parseFloat(values[3]);
84 | gauge3.value = val3;
85 | gauge3.update({ valueText: val3.toFixed(1) });
86 | yaw = val3 * deg2rad * -1.0;
87 | }
88 | break;
89 |
90 | default:
91 | break;
92 | }
93 | }
94 |
95 | websocket.onclose = function(evt) {
96 | console.log('Websocket connection closed');
97 | //document.getElementById("datetime").innerHTML = "WebSocket closed";
98 | }
99 |
100 | websocket.onerror = function(evt) {
101 | console.log('Websocket error: ' + evt);
102 | //document.getElementById("datetime").innerHTML = "WebSocket error????!!!1!!";
103 | }
104 |
--------------------------------------------------------------------------------
/Madgwick/html/main.js:
--------------------------------------------------------------------------------
1 | //document.getElementById("datetime").innerHTML = "WebSocket is not connected";
2 |
3 | var websocket = new WebSocket('ws://'+location.hostname+'/');
4 | var meter1 = 0;
5 | var meter2 = 0;
6 | var meter3 = 0;
7 | var roll = 0.0;
8 | var yaw = 0.0;
9 | var pitch = 0.0;
10 | var deg2rad = 0.0174533;
11 |
12 | function sendText(name) {
13 | console.log('sendText');
14 | var data = {};
15 | data["id"] = name;
16 | console.log('data=', data);
17 | json_data = JSON.stringify(data);
18 | console.log('json_data=' + json_data);
19 | websocket.send(json_data);
20 | }
21 |
22 | websocket.onopen = function(evt) {
23 | console.log('WebSocket connection opened');
24 | var data = {};
25 | data["id"] = "init";
26 | console.log('data=', data);
27 | json_data = JSON.stringify(data);
28 | console.log('json_data=' + json_data);
29 | websocket.send(json_data);
30 | //document.getElementById("datetime").innerHTML = "WebSocket is connected!";
31 | }
32 |
33 | websocket.onmessage = function(evt) {
34 | var msg = evt.data;
35 | console.log("msg=" + msg);
36 | var values = msg.split('\4'); // \4 is EOT
37 | //console.log("values=" + values);
38 | switch(values[0]) {
39 | case 'HEAD':
40 | console.log("HEAD values[1]=" + values[1]);
41 | var h1 = document.getElementById( 'header' );
42 | h1.textContent = values[1];
43 | break;
44 |
45 | case 'METER':
46 | //console.log("gauge1=" + Object.keys(gauge1.options));
47 | //console.log("gauge1.options.units=" + gauge1.options.units);
48 | console.log("METER values[1]=" + values[1]);
49 | console.log("METER values[2]=" + values[2]);
50 | console.log("METER values[3]=" + values[3]);
51 | if (values[1] != "") {
52 | gauge1.options.title = values[1];
53 | document.getElementById("canvas1").style.display = "inline-block";
54 | meter1 = 1;
55 | }
56 | if (values[2] != "") {
57 | gauge2.options.title = values[2];
58 | document.getElementById("canvas2").style.display = "inline-block";
59 | meter2 = 1;
60 | }
61 | if (values[3] != "") {
62 | gauge3.options.title = values[3];
63 | document.getElementById("canvas3").style.display = "inline-block";
64 | meter3 = 1;
65 | }
66 | break;
67 |
68 | case 'DATA':
69 | console.log("DATA values[1]=" + values[1]);
70 | var val1 = parseFloat(values[1]);
71 | gauge1.value = val1;
72 | gauge1.update({ valueText: val1.toFixed(1) });
73 | roll = val1 * deg2rad;
74 | if (meter2) {
75 | console.log("DATA values[2]=" + values[2]);
76 | var val2 = parseFloat(values[2]);
77 | gauge2.value = val2;
78 | gauge2.update({ valueText: val2.toFixed(1) });
79 | pitch = val2 * deg2rad;
80 | }
81 | if (meter3) {
82 | console.log("DATA values[3]=" + values[3]);
83 | var val3 = parseFloat(values[3]);
84 | gauge3.value = val3;
85 | gauge3.update({ valueText: val3.toFixed(1) });
86 | yaw = val3 * deg2rad * -1.0;
87 | }
88 | break;
89 |
90 | default:
91 | break;
92 | }
93 | }
94 |
95 | websocket.onclose = function(evt) {
96 | console.log('Websocket connection closed');
97 | //document.getElementById("datetime").innerHTML = "WebSocket closed";
98 | }
99 |
100 | websocket.onerror = function(evt) {
101 | console.log('Websocket error: ' + evt);
102 | //document.getElementById("datetime").innerHTML = "WebSocket error????!!!1!!";
103 | }
104 |
--------------------------------------------------------------------------------
/components/KalmanFilter/Kalman.cpp:
--------------------------------------------------------------------------------
1 | /* Copyright (C) 2012 Kristian Lauszus, TKJ Electronics. All rights reserved.
2 |
3 | This software may be distributed and modified under the terms of the GNU
4 | General Public License version 2 (GPL2) as published by the Free Software
5 | Foundation and appearing in the file GPL2.TXT included in the packaging of
6 | this file. Please note that GPL2 Section 2[b] requires that all works based
7 | on this software must also be made publicly available under the terms of
8 | the GPL2 ("Copyleft").
9 |
10 | Contact information
11 | -------------------
12 |
13 | Kristian Lauszus, TKJ Electronics
14 | Web : http://www.tkjelectronics.com
15 | e-mail : kristianl@tkjelectronics.com
16 | */
17 |
18 | #include "Kalman.h"
19 |
20 | Kalman::Kalman() {
21 | /* We will set the variables like so, these can also be tuned by the user */
22 | Q_angle = 0.001f;
23 | Q_bias = 0.003f;
24 | R_measure = 0.03f;
25 |
26 | angle = 0.0f; // Reset the angle
27 | bias = 0.0f; // Reset bias
28 |
29 | P[0][0] = 0.0f; // Since we assume that the bias is 0 and we know the starting angle (use setAngle), the error covariance matrix is set like so - see: http://en.wikipedia.org/wiki/Kalman_filter#Example_application.2C_technical
30 | P[0][1] = 0.0f;
31 | P[1][0] = 0.0f;
32 | P[1][1] = 0.0f;
33 | };
34 |
35 | // The angle should be in degrees and the rate should be in degrees per second and the delta time in seconds
36 | float Kalman::getAngle(float newAngle, float newRate, float dt) {
37 | // KasBot V2 - Kalman filter module - http://www.x-firm.com/?page_id=145
38 | // Modified by Kristian Lauszus
39 | // See my blog post for more information: http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it
40 |
41 | // Discrete Kalman filter time update equations - Time Update ("Predict")
42 | // Update xhat - Project the state ahead
43 | /* Step 1 */
44 | rate = newRate - bias;
45 | angle += dt * rate;
46 |
47 | // Update estimation error covariance - Project the error covariance ahead
48 | /* Step 2 */
49 | P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
50 | P[0][1] -= dt * P[1][1];
51 | P[1][0] -= dt * P[1][1];
52 | P[1][1] += Q_bias * dt;
53 |
54 | // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
55 | // Calculate Kalman gain - Compute the Kalman gain
56 | /* Step 4 */
57 | float S = P[0][0] + R_measure; // Estimate error
58 | /* Step 5 */
59 | float K[2]; // Kalman gain - This is a 2x1 vector
60 | K[0] = P[0][0] / S;
61 | K[1] = P[1][0] / S;
62 |
63 | // Calculate angle and bias - Update estimate with measurement zk (newAngle)
64 | /* Step 3 */
65 | float y = newAngle - angle; // Angle difference
66 | /* Step 6 */
67 | angle += K[0] * y;
68 | bias += K[1] * y;
69 |
70 | // Calculate estimation error covariance - Update the error covariance
71 | /* Step 7 */
72 | float P00_temp = P[0][0];
73 | float P01_temp = P[0][1];
74 |
75 | P[0][0] -= K[0] * P00_temp;
76 | P[0][1] -= K[0] * P01_temp;
77 | P[1][0] -= K[1] * P00_temp;
78 | P[1][1] -= K[1] * P01_temp;
79 |
80 | return angle;
81 | };
82 |
83 | void Kalman::setAngle(float angle) { this->angle = angle; }; // Used to set angle, this should be set as the starting angle
84 | float Kalman::getRate() { return this->rate; }; // Return the unbiased rate
85 |
86 | /* These are used to tune the Kalman filter */
87 | void Kalman::setQangle(float Q_angle) { this->Q_angle = Q_angle; };
88 | void Kalman::setQbias(float Q_bias) { this->Q_bias = Q_bias; };
89 | void Kalman::setRmeasure(float R_measure) { this->R_measure = R_measure; };
90 |
91 | float Kalman::getQangle() { return this->Q_angle; };
92 | float Kalman::getQbias() { return this->Q_bias; };
93 | float Kalman::getRmeasure() { return this->R_measure; };
94 |
--------------------------------------------------------------------------------
/components/LSM6DS3/LSM6DS3.cpp:
--------------------------------------------------------------------------------
1 | /*
2 | This file is part of the Arduino_LSM6DS3 library.
3 | Copyright (c) 2019 Arduino SA. All rights reserved.
4 |
5 | This library is free software; you can redistribute it and/or
6 | modify it under the terms of the GNU Lesser General Public
7 | License as published by the Free Software Foundation; either
8 | version 2.1 of the License, or (at your option) any later version.
9 |
10 | This library is distributed in the hope that it will be useful,
11 | but WITHOUT ANY WARRANTY; without even the implied warranty of
12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13 | Lesser General Public License for more details.
14 |
15 | You should have received a copy of the GNU Lesser General Public
16 | License along with this library; if not, write to the Free Software
17 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
18 | */
19 |
20 | #include "LSM6DS3.h"
21 |
22 | LSM6DS3::LSM6DS3(uint8_t slaveAddress) {
23 | devAddr = slaveAddress;
24 | }
25 |
26 | int LSM6DS3::begin()
27 | {
28 | if (!(readRegister(LSM6DS3_WHO_AM_I_REG) == 0x6C || readRegister(LSM6DS3_WHO_AM_I_REG) == 0x69)) {
29 | end();
30 | return 0;
31 | }
32 |
33 | // Set the Accelerometer control register to work at 104 Hz, 4 g,and in bypass mode and enable ODR/4
34 | // low pass filter (check figure9 of LSM6DS3's datasheet)
35 | writeRegister(LSM6DS3_CTRL1_XL, 0x4A);
36 |
37 | #if 0
38 | //set the gyroscope control register to work at 104 Hz, 2000 dps and in bypass mode
39 | writeRegister(LSM6DS3_CTRL2_G, 0x4C);
40 | #endif
41 | //set the gyroscope control register to work at 104 Hz, 250 dps and in bypass mode
42 | writeRegister(LSM6DS3_CTRL2_G, 0x40);
43 |
44 | // set gyroscope power mode to high performance and bandwidth to 16 MHz
45 | writeRegister(LSM6DS3_CTRL7_G, 0x00);
46 |
47 | // Set the ODR config register to ODR/4
48 | writeRegister(LSM6DS3_CTRL8_XL, 0x09);
49 |
50 | return 1;
51 | }
52 |
53 | void LSM6DS3::end()
54 | {
55 | writeRegister(LSM6DS3_CTRL2_G, 0x00);
56 | writeRegister(LSM6DS3_CTRL1_XL, 0x00);
57 | }
58 |
59 | int LSM6DS3::readAcceleration(float& x, float& y, float& z)
60 | {
61 | int16_t data[3];
62 |
63 | if (!readRegisters(LSM6DS3_OUTX_L_XL, (uint8_t*)data, sizeof(data))) {
64 | //x = NAN;
65 | //y = NAN;
66 | //z = NAN;
67 |
68 | return 0;
69 | }
70 |
71 | #if 0
72 | x = data[0] * 4.0 / 32768.0;
73 | y = data[1] * 4.0 / 32768.0;
74 | z = data[2] * 4.0 / 32768.0;
75 | #endif
76 | x = data[0] / 8192.0;
77 | y = data[1] / 8192.0;
78 | z = data[2] / 8192.0;
79 |
80 | return 1;
81 | }
82 |
83 | int LSM6DS3::accelerationAvailable()
84 | {
85 | if (readRegister(LSM6DS3_STATUS_REG) & 0x01) {
86 | return 1;
87 | }
88 |
89 | return 0;
90 | }
91 |
92 | float LSM6DS3::accelerationSampleRate()
93 | {
94 | return 104.0F;
95 | }
96 |
97 | int LSM6DS3::readGyroscope(float& x, float& y, float& z)
98 | {
99 | int16_t data[3];
100 |
101 | if (!readRegisters(LSM6DS3_OUTX_L_G, (uint8_t*)data, sizeof(data))) {
102 | //x = NAN;
103 | //y = NAN;
104 | //z = NAN;
105 |
106 | return 0;
107 | }
108 |
109 | #if 0
110 | x = data[0] * 2000.0 / 32768.0;
111 | y = data[1] * 2000.0 / 32768.0;
112 | z = data[2] * 2000.0 / 32768.0;
113 | #endif
114 | x = data[0] / 131.0;
115 | y = data[1] / 131.0;
116 | z = data[2] / 131.0;
117 |
118 | return 1;
119 | }
120 |
121 | int LSM6DS3::gyroscopeAvailable()
122 | {
123 | if (readRegister(LSM6DS3_STATUS_REG) & 0x02) {
124 | return 1;
125 | }
126 |
127 | return 0;
128 | }
129 |
130 | float LSM6DS3::gyroscopeSampleRate()
131 | {
132 | return 104.0F;
133 | }
134 |
135 | int LSM6DS3::readRegister(uint8_t address)
136 | {
137 | uint8_t value;
138 |
139 | if (readRegisters(address, &value, sizeof(value)) != 1) {
140 | return -1;
141 | }
142 |
143 | return value;
144 | }
145 |
146 | int LSM6DS3::readRegisters(uint8_t address, uint8_t* data, size_t length)
147 | {
148 | I2Cdev::readBytes(devAddr, address, length, data);
149 | return 1;
150 | }
151 |
152 | int LSM6DS3::writeRegister(uint8_t address, uint8_t value)
153 | {
154 | I2Cdev::writeByte(devAddr, address, value);
155 | return 1;
156 | }
157 |
--------------------------------------------------------------------------------
/components/I2Cdev/I2Cdev.h:
--------------------------------------------------------------------------------
1 | // I2Cdev library collection - Main I2C device class
2 | // Abstracts bit and byte I2C R/W functions into a convenient class
3 | // EFM32 stub port by Nicolas Baldeck
4 | // Based on Arduino's I2Cdev by Jeff Rowberg
5 | //
6 | // Changelog:
7 | // 2023-03-09 - Add some functions by nopnop2002
8 | // 2015-01-02 - Initial release
9 |
10 |
11 | /* ============================================
12 | I2Cdev device library code is placed under the MIT license
13 | Copyright (c) 2015 Jeff Rowberg, Nicolas Baldeck
14 |
15 | Permission is hereby granted, free of charge, to any person obtaining a copy
16 | of this software and associated documentation files (the "Software"), to deal
17 | in the Software without restriction, including without limitation the rights
18 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
19 | copies of the Software, and to permit persons to whom the Software is
20 | furnished to do so, subject to the following conditions:
21 |
22 | The above copyright notice and this permission notice shall be included in
23 | all copies or substantial portions of the Software.
24 |
25 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
26 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
27 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
28 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
29 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
30 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
31 | THE SOFTWARE.
32 | ===============================================
33 | */
34 |
35 | #ifndef _I2CDEV_H_
36 | #define _I2CDEV_H_
37 |
38 | #include
39 |
40 | #define I2CDEV_DEFAULT_READ_TIMEOUT 1000
41 |
42 | class I2Cdev {
43 | public:
44 | I2Cdev();
45 |
46 | static void initialize(int clkSpeed);
47 | static void enable(bool isEnabled);
48 |
49 | static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
50 | static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
51 | static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
52 | static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
53 | static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
54 | static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
55 | static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
56 | static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout, void *wireObj=0);
57 |
58 | static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj=0);
59 | static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data, void *wireObj=0);
60 | static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj=0);
61 | static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data, void *wireObj=0);
62 | static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj=0);
63 | static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj=0);
64 | static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, void *wireObj=0);
65 | static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, void *wireObj=0);
66 |
67 | static uint16_t readTimeout;
68 |
69 | //private:
70 | static void SelectRegister(uint8_t dev, uint8_t reg);
71 | //static I2C_TransferReturn_TypeDef transfer(I2C_TransferSeq_TypeDef *seq, uint16_t timeout=I2Cdev::readTimeout);
72 | };
73 |
74 | #endif /* _I2CDEV_H_ */
75 |
--------------------------------------------------------------------------------
/components/websocket/include/websocket_server.h:
--------------------------------------------------------------------------------
1 |
2 | /*
3 | esp32-websocket - a websocket component on esp-idf
4 | Copyright (C) 2019 Blake Felt - blake.w.felt@gmail.com
5 |
6 | This program is free software: you can redistribute it and/or modify
7 | it under the terms of the GNU General Public License as published by
8 | the Free Software Foundation, either version 3 of the License, or
9 | (at your option) any later version.
10 |
11 | This program is distributed in the hope that it will be useful,
12 | but WITHOUT ANY WARRANTY; without even the implied warranty of
13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 | GNU General Public License for more details.
15 |
16 | You should have received a copy of the GNU General Public License
17 | along with this program. If not, see .
18 | */
19 |
20 | #ifdef __cplusplus
21 | extern "C" {
22 | #endif
23 |
24 | #ifndef WEBSOCKET_SERVER_H
25 | #define WEBSOCKET_SERVER_H
26 |
27 | #include "websocket.h"
28 |
29 | #define WEBSOCKET_SERVER_MAX_CLIENTS CONFIG_WEBSOCKET_SERVER_MAX_CLIENTS
30 | #define WEBSOCKET_SERVER_QUEUE_SIZE CONFIG_WEBSOCKET_SERVER_QUEUE_SIZE
31 | #define WEBSOCKET_SERVER_QUEUE_TIMEOUT CONFIG_WEBSOCKET_SERVER_QUEUE_TIMEOUT
32 | #define WEBSOCKET_SERVER_TASK_STACK_DEPTH CONFIG_WEBSOCKET_SERVER_TASK_STACK_DEPTH
33 | #define WEBSOCKET_SERVER_TASK_PRIORITY CONFIG_WEBSOCKET_SERVER_TASK_PRIORITY
34 | #define WEBSOCKET_SERVER_PINNED CONFIG_WEBSOCKET_SERVER_PINNED
35 | #if WEBSOCKET_SERVER_PINNED
36 | #define WEBSOCKET_SERVER_PINNED_CORE CONFIG_WEBSOCKET_SERVER_PINNED_CORE
37 | #endif
38 |
39 | // starts the server
40 | int ws_server_start();
41 |
42 | // ends the server
43 | int ws_server_stop();
44 |
45 | // adds a client, returns the client's number in the server
46 | int ws_server_add_client(struct netconn* conn,
47 | char* msg,
48 | uint16_t len,
49 | char* url,
50 | void (*callback)(uint8_t num,
51 | WEBSOCKET_TYPE_t type,
52 | char* msg,
53 | uint64_t len));
54 |
55 | int ws_server_add_client_protocol(struct netconn* conn,
56 | char* msg,
57 | uint16_t len,
58 | char* url,
59 | char* protocol,
60 | void (*callback)(uint8_t num,
61 | WEBSOCKET_TYPE_t type,
62 | char* msg,
63 | uint64_t len));
64 |
65 | int ws_server_len_url(char* url); // returns the number of connected clients to url
66 | int ws_server_len_all(); // returns the total number of connected clients
67 |
68 | int ws_server_remove_client(int num); // removes the client with the set number
69 | int ws_server_remove_clients(char* url); // removes all clients connected to the specified url
70 | int ws_server_remove_all(); // removes all clients from the server
71 |
72 | int ws_server_send_text_client(int num,char* msg,uint64_t len); // send text to client with the set number
73 | int ws_server_send_text_clients(char* url,char* msg,uint64_t len); // sends text to all clients with the set number
74 | int ws_server_send_text_all(char* msg,uint64_t len); // sends text to all clients
75 |
76 | int ws_server_send_bin_client(int num,char* msg,uint64_t len);
77 | int ws_server_send_bin_clients(char* url,char* msg,uint64_t len);
78 | int ws_server_send_bin_all(char* msg,uint64_t len);
79 |
80 | // these versions can be sent from the callback ONLY
81 |
82 | int ws_server_send_text_client_from_callback(int num,char* msg,uint64_t len); // send text to client with the set number
83 | int ws_server_send_text_clients_from_callback(char* url,char* msg,uint64_t len); // sends text to all clients with the set number
84 | int ws_server_send_text_all_from_callback(char* msg,uint64_t len); // sends text to all clients
85 |
86 | int ws_server_send_bin_client_from_callback(int num,char* msg,uint64_t len); //sends binary to client with the set number
87 | int ws_server_send_bin_clients_from_callback(char* url,char* msg,uint64_t len); // sends binary to all clients with the set number
88 | int ws_server_send_bin_all_from_callback(char* msg,uint64_t len); // sends binary to all clients
89 |
90 | int ws_server_ping(); // sends a ping to all connected clients
91 |
92 | #endif
93 |
94 | #ifdef __cplusplus
95 | }
96 | #endif
97 |
--------------------------------------------------------------------------------
/components/websocket/include/websocket.h:
--------------------------------------------------------------------------------
1 |
2 | /*
3 | esp32-websocket - a websocket component on esp-idf
4 | Copyright (C) 2019 Blake Felt - blake.w.felt@gmail.com
5 |
6 | This program is free software: you can redistribute it and/or modify
7 | it under the terms of the GNU General Public License as published by
8 | the Free Software Foundation, either version 3 of the License, or
9 | (at your option) any later version.
10 |
11 | This program is distributed in the hope that it will be useful,
12 | but WITHOUT ANY WARRANTY; without even the implied warranty of
13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 | GNU General Public License for more details.
15 |
16 | You should have received a copy of the GNU General Public License
17 | along with this program. If not, see .
18 | */
19 |
20 | #ifdef __cplusplus
21 | extern "C" {
22 | #endif
23 |
24 | #ifndef WEBSOCKET_H
25 | #define WEBSOCKET_H
26 |
27 | #include "lwip/api.h"
28 |
29 | // the different codes for the callbacks
30 | typedef enum {
31 | WEBSOCKET_CONNECT,
32 | WEBSOCKET_DISCONNECT_EXTERNAL, // the other side disconnected
33 | WEBSOCKET_DISCONNECT_INTERNAL, // the esp32 disconnected
34 | WEBSOCKET_DISCONNECT_ERROR, // disconnect due to error
35 | WEBSOCKET_TEXT,
36 | WEBSOCKET_BIN,
37 | WEBSOCKET_PING,
38 | WEBSOCKET_PONG
39 | } WEBSOCKET_TYPE_t;
40 |
41 | // websocket operation codes
42 | typedef enum {
43 | WEBSOCKET_OPCODE_CONT = 0x0,
44 | WEBSOCKET_OPCODE_TEXT = 0x1,
45 | WEBSOCKET_OPCODE_BIN = 0x2,
46 | WEBSOCKET_OPCODE_CLOSE = 0x8,
47 | WEBSOCKET_OPCODE_PING = 0x9,
48 | WEBSOCKET_OPCODE_PONG = 0xA
49 | } WEBSOCKET_OPCODES_t;
50 |
51 | // the header, useful for creating and quickly passing to functions
52 | typedef struct {
53 | union {
54 | struct {
55 | uint16_t LEN:7; // bits 0.. 6
56 | uint16_t MASK:1; // bit 7
57 | uint16_t OPCODE:4; // bits 8.. 11
58 | uint16_t :3; // bits 12.. 14 reserved
59 | uint16_t FIN:1; // bit 15
60 | } bit;
61 | struct {
62 | uint16_t ONE:8; // bits 0.. 7
63 | uint16_t ZERO:8; // bits 8.. 15
64 | } pos;
65 | } param; // the initial parameters of the header
66 | uint64_t length; // actual message length
67 | union {
68 | char part[4]; // the mask, array
69 | uint32_t full; // the mask, all 32 bits
70 | } key; // masking key
71 | bool received; // was a message successfully received?
72 | } ws_header_t;
73 |
74 | // a client, with space for a server callback or a client callback (depending on use)
75 | typedef struct {
76 | struct netconn* conn; // the connection
77 | char* url; // the associated url, null terminated
78 | char* protocol; // the associated protocol, null terminated
79 | bool ping; // did we send a ping?
80 | WEBSOCKET_OPCODES_t last_opcode; // the previous opcode
81 | char* contin; // any continuation piece
82 | bool contin_text; // is the continue a binary or text?
83 | uint64_t len; // length of continuation
84 | uint32_t unfinished; // sometimes netconn doesn't read a full frame, treated similarly to a continuation frame
85 | void (*ccallback)(WEBSOCKET_TYPE_t type,char* msg,uint64_t len); // client callback
86 | void (*scallback)(uint8_t num,WEBSOCKET_TYPE_t type,char* msg,uint64_t len); // server callback
87 | } ws_client_t;
88 |
89 | // returns the populated client struct
90 | // does not send any header, assumes the proper handshake has already occurred
91 | // ccallback = callback for client (userspace)
92 | // scallback = callback for server (userspace)
93 | ws_client_t ws_connect_client(struct netconn* conn,
94 | char* url,
95 | void (*ccallback)(WEBSOCKET_TYPE_t type,char* msg,uint64_t len),
96 | void (*scallback)(uint8_t num,WEBSOCKET_TYPE_t type,char* msg,uint64_t len)
97 | );
98 | void ws_disconnect_client(ws_client_t* client,bool mask);
99 | bool ws_is_connected(ws_client_t client); // returns 1 if connected, status updates after send/read/connect/disconnect
100 | int ws_send(ws_client_t* client,WEBSOCKET_OPCODES_t opcode,char* msg,uint64_t len,bool mask); // sends message. this function performs the masking
101 | char* ws_read(ws_client_t* client,ws_header_t* header); // unmasks and returns message. populates header.
102 | char* ws_hash_handshake(char* key,uint8_t len); // returns string of output
103 |
104 | #endif // ifndef WEBSOCKET_H
105 |
106 | #ifdef __cplusplus
107 | }
108 | #endif
109 |
--------------------------------------------------------------------------------
/Madgwick/main/lsm6ds3.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 |
4 | #include "freertos/FreeRTOS.h"
5 | #include "freertos/task.h"
6 | #include "freertos/queue.h"
7 | #include "freertos/message_buffer.h"
8 | #include "esp_timer.h"
9 | #include "esp_log.h"
10 | #include "esp_err.h"
11 | #include "cJSON.h"
12 |
13 | #include "parameter.h"
14 |
15 | extern QueueHandle_t xQueueTrans;
16 | extern MessageBufferHandle_t xMessageBufferToClient;
17 |
18 | static const char *TAG = "IMU";
19 |
20 | // I2Cdev and LSM303DLHC must be installed as libraries, or else the .cpp/.h files
21 | // for both classes must be in the include path of your project
22 | #include "I2Cdev.h"
23 |
24 | #include "LSM6DS3.h"
25 |
26 | LSM6DS3 imu(CONFIG_I2C_ADDR);
27 |
28 | // Source: https://github.com/arduino-libraries/MadgwickAHRS
29 | #include "MadgwickAHRS.h"
30 |
31 | #define RAD_TO_DEG (180.0/M_PI)
32 | #define DEG_TO_RAD 0.0174533
33 |
34 | // Arduino macro
35 | #define micros() (unsigned long) (esp_timer_get_time())
36 | #define delay(ms) esp_rom_delay_us(ms*1000)
37 |
38 | Madgwick madgwick;
39 |
40 | void _getMotion6(double *_ax, double *_ay, double *_az, double *_gx, double *_gy, double *_gz) {
41 | float ax=0.0, ay=0.0, az=0.0;
42 | float gx=0.0, gy=0.0, gz=0.0;
43 | #if 0
44 | if (imu.accelerationAvailable()) {
45 | imu.readAcceleration(ax, ay, az);
46 | }
47 | if (imu.gyroscopeAvailable()) {
48 | imu.readGyroscope(gx, gy, gz);
49 | }
50 | #endif
51 | while(1) {
52 | if (imu.accelerationAvailable()) break;
53 | vTaskDelay(1);
54 | }
55 | imu.readAcceleration(ax, ay, az);
56 | while(1) {
57 | if (imu.gyroscopeAvailable()) break;
58 | vTaskDelay(1);
59 | }
60 | imu.readGyroscope(gx, gy, gz);
61 |
62 | *_ax = ax;
63 | *_ay = ay;
64 | *_az = az;
65 | *_gx = gx;
66 | *_gy = gy;
67 | *_gz = gz;
68 | }
69 |
70 | // Get time in seconds since boot
71 | // Compatible with ROS's time.toSec() function
72 | double TimeToSec() {
73 | int64_t _time = esp_timer_get_time(); // Get time in microseconds since boot
74 | double __time = (double)_time / 1000000;
75 | return __time;
76 | }
77 |
78 | void lsm6ds3(void *pvParameters){
79 | // Initialize device
80 | if (imu.begin() == 0) {
81 | ESP_LOGE(TAG, "Connection fail");
82 | vTaskDelete(NULL);
83 | }
84 |
85 | int elasped = 0;
86 | double last_time_ = TimeToSec();
87 |
88 | bool initialized = false;
89 | float initial_roll = 0.0;
90 | float initial_pitch = 0.0;
91 | float initial_yaw = 0.0;
92 |
93 | // It takes time for the estimated value to stabilize.
94 | // It need about 4Sec.
95 | int initial_period = 400;
96 |
97 | while(1){
98 | double ax=0.0, ay=0.0, az=0.0;
99 | double gx=0.0, gy=0.0, gz=0.0;
100 | _getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
101 | //printf("%f %f %f - %f %f %f\n", ax, ay, az, gx, gy, gz);
102 |
103 | // Get the elapsed time from the previous
104 | float dt = (TimeToSec() - last_time_);
105 | ESP_LOGD(TAG, "dt=%f",dt);
106 | last_time_ = TimeToSec();
107 |
108 | // Get Euler
109 | madgwick.updateIMU(gx, gy, gz, ax, ay, az, dt);
110 | float roll = madgwick.getRoll();
111 | float pitch = madgwick.getPitch();
112 | float yaw = madgwick.getYaw();
113 | ESP_LOGD(TAG, "roll=%f pitch=%f yaw=%f", roll, pitch, yaw);
114 |
115 |
116 | /* Print Data every 10 times */
117 | if (elasped > initial_period) {
118 | // Set the first data
119 | if (!initialized) {
120 | initial_roll = roll;
121 | initial_pitch = pitch;
122 | initial_yaw = yaw;
123 | initialized = true;
124 | initial_period = 10;
125 | }
126 |
127 | // Send UDP packet
128 | float _roll = roll-initial_roll;
129 | float _pitch = pitch-initial_pitch;
130 | float _yaw = yaw-initial_yaw;
131 | ESP_LOGD(TAG, "roll:%f pitch=%f yaw=%f", roll, pitch, yaw);
132 | ESP_LOGI(TAG, "roll:%f pitch=%f yaw=%f", _roll, _pitch, _yaw);
133 |
134 | POSE_t pose;
135 | pose.roll = _roll;
136 | pose.pitch = _pitch;
137 | pose.yaw = 0.0;
138 | if (xQueueSend(xQueueTrans, &pose, 100) != pdPASS ) {
139 | ESP_LOGE(pcTaskGetName(NULL), "xQueueSend fail");
140 | }
141 |
142 | // Send WEB request
143 | cJSON *request;
144 | request = cJSON_CreateObject();
145 | cJSON_AddStringToObject(request, "id", "data-request");
146 | cJSON_AddNumberToObject(request, "roll", _roll);
147 | cJSON_AddNumberToObject(request, "pitch", _pitch);
148 | cJSON_AddNumberToObject(request, "yaw", 0.0);
149 | char *my_json_string = cJSON_Print(request);
150 | ESP_LOGD(TAG, "my_json_string\n%s",my_json_string);
151 | size_t xBytesSent = xMessageBufferSend(xMessageBufferToClient, my_json_string, strlen(my_json_string), 100);
152 | if (xBytesSent != strlen(my_json_string)) {
153 | ESP_LOGE(TAG, "xMessageBufferSend fail");
154 | }
155 | cJSON_Delete(request);
156 | cJSON_free(my_json_string);
157 |
158 | vTaskDelay(1);
159 | elasped = 0;
160 | }
161 | elasped++;
162 | vTaskDelay(1);
163 | }
164 |
165 | // Never reach here
166 | vTaskDelete(NULL);
167 | }
168 |
--------------------------------------------------------------------------------
/Kalman/main/wifi.c:
--------------------------------------------------------------------------------
1 | /* WiFi station Example
2 |
3 | This example code is in the Public Domain (or CC0 licensed, at your option.)
4 |
5 | Unless required by applicable law or agreed to in writing, this
6 | software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
7 | CONDITIONS OF ANY KIND, either express or implied.
8 | */
9 |
10 | #include
11 | #include "freertos/FreeRTOS.h"
12 | #include "freertos/task.h"
13 | #include "freertos/event_groups.h"
14 | #include "esp_system.h"
15 | #include "esp_wifi.h"
16 | #include "esp_event.h"
17 | #include "esp_log.h"
18 | #include "nvs_flash.h"
19 |
20 | /* FreeRTOS event group to signal when we are connected*/
21 | static EventGroupHandle_t s_wifi_event_group;
22 |
23 | /* The event group allows multiple bits for each event, but we only care about two events:
24 | * - we are connected to the AP with an IP
25 | * - we failed to connect after the maximum amount of retries */
26 | #define WIFI_CONNECTED_BIT BIT0
27 | #define WIFI_FAIL_BIT BIT1
28 |
29 | static const char *TAG = "wifi station";
30 |
31 | static int s_retry_num = 0;
32 |
33 | static void event_handler(void* arg, esp_event_base_t event_base, int32_t event_id, void* event_data)
34 | {
35 | if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
36 | esp_wifi_connect();
37 | } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
38 | if (s_retry_num < CONFIG_ESP_MAXIMUM_RETRY) {
39 | esp_wifi_connect();
40 | s_retry_num++;
41 | ESP_LOGI(TAG, "retry to connect to the AP");
42 | } else {
43 | xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT);
44 | }
45 | ESP_LOGI(TAG,"connect to the AP fail");
46 | } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
47 | ip_event_got_ip_t* event = (ip_event_got_ip_t*) event_data;
48 | ESP_LOGI(TAG, "got ip:" IPSTR, IP2STR(&event->ip_info.ip));
49 | s_retry_num = 0;
50 | xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
51 | }
52 | }
53 |
54 | esp_err_t wifi_init_sta(void)
55 | {
56 | s_wifi_event_group = xEventGroupCreate();
57 |
58 | ESP_ERROR_CHECK(esp_netif_init());
59 | ESP_ERROR_CHECK(esp_event_loop_create_default());
60 | esp_netif_create_default_wifi_sta();
61 |
62 | wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
63 | ESP_ERROR_CHECK(esp_wifi_init(&cfg));
64 |
65 | esp_event_handler_instance_t instance_any_id;
66 | esp_event_handler_instance_t instance_got_ip;
67 | ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT,
68 | ESP_EVENT_ANY_ID,
69 | &event_handler,
70 | NULL,
71 | &instance_any_id));
72 | ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT,
73 | IP_EVENT_STA_GOT_IP,
74 | &event_handler,
75 | NULL,
76 | &instance_got_ip));
77 |
78 | wifi_config_t wifi_config = {
79 | .sta = {
80 | .ssid = CONFIG_ESP_WIFI_SSID,
81 | .password = CONFIG_ESP_WIFI_PASSWORD,
82 | /* Setting a password implies station will connect to all security modes including WEP/WPA.
83 | * However these modes are deprecated and not advisable to be used. Incase your Access point
84 | * doesn't support WPA2, these mode can be enabled by commenting below line */
85 | .threshold.authmode = WIFI_AUTH_WPA2_PSK,
86 |
87 | .pmf_cfg = {
88 | .capable = true,
89 | .required = false
90 | },
91 | },
92 | };
93 | ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));
94 | ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA) );
95 | ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config) );
96 | ESP_ERROR_CHECK(esp_wifi_start() );
97 |
98 | /* Waiting until either the connection is established (WIFI_CONNECTED_BIT) or connection failed for the maximum
99 | * number of re-tries (WIFI_FAIL_BIT). The bits are set by event_handler() (see above) */
100 | esp_err_t ret_value = ESP_OK;
101 | EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group,
102 | WIFI_CONNECTED_BIT | WIFI_FAIL_BIT,
103 | pdFALSE,
104 | pdFALSE,
105 | portMAX_DELAY);
106 |
107 | /* xEventGroupWaitBits() returns the bits before the call returned, hence we can test which event actually
108 | * happened. */
109 | if (bits & WIFI_CONNECTED_BIT) {
110 | ESP_LOGI(TAG, "connected to ap SSID:%s password:%s", CONFIG_ESP_WIFI_SSID, CONFIG_ESP_WIFI_PASSWORD);
111 | } else if (bits & WIFI_FAIL_BIT) {
112 | ESP_LOGI(TAG, "Failed to connect to SSID:%s, password:%s", CONFIG_ESP_WIFI_SSID, CONFIG_ESP_WIFI_PASSWORD);
113 | ret_value = ESP_FAIL;
114 | } else {
115 | ESP_LOGE(TAG, "UNEXPECTED EVENT");
116 | ret_value = ESP_FAIL;
117 | }
118 |
119 | /* The event will not be processed after unregister */
120 | ESP_ERROR_CHECK(esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, instance_got_ip));
121 | ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id));
122 | vEventGroupDelete(s_wifi_event_group);
123 | return ret_value;
124 | }
125 |
126 | void wifi_status(void *pvParameters)
127 | {
128 | ESP_LOGI(pcTaskGetName(0), "Start");
129 |
130 | wifi_ap_record_t ap_info;
131 | while(1) {
132 | esp_err_t ret = esp_wifi_sta_get_ap_info(&ap_info);
133 | ESP_LOGI(pcTaskGetName(0), "esp_wifi_sta_get_ap_info=%d", ret);
134 | if (ret != ESP_OK) break;
135 | vTaskDelay(1000);
136 | }
137 | esp_restart();
138 | }
139 |
140 | void start_wifi(void)
141 | {
142 | // Initialize NVS
143 | ESP_LOGI(TAG, "Start");
144 | esp_err_t ret = nvs_flash_init();
145 | if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
146 | ESP_ERROR_CHECK(nvs_flash_erase());
147 | ret = nvs_flash_init();
148 | }
149 | ESP_ERROR_CHECK(ret);
150 |
151 | //Initialize WiFi
152 | ESP_ERROR_CHECK(wifi_init_sta());
153 | ESP_LOGI(TAG, "Finish");
154 | }
155 |
--------------------------------------------------------------------------------
/Madgwick/main/wifi.c:
--------------------------------------------------------------------------------
1 | /* WiFi station Example
2 |
3 | This example code is in the Public Domain (or CC0 licensed, at your option.)
4 |
5 | Unless required by applicable law or agreed to in writing, this
6 | software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
7 | CONDITIONS OF ANY KIND, either express or implied.
8 | */
9 |
10 | #include
11 | #include "freertos/FreeRTOS.h"
12 | #include "freertos/task.h"
13 | #include "freertos/event_groups.h"
14 | #include "esp_system.h"
15 | #include "esp_wifi.h"
16 | #include "esp_event.h"
17 | #include "esp_log.h"
18 | #include "nvs_flash.h"
19 |
20 | /* FreeRTOS event group to signal when we are connected*/
21 | static EventGroupHandle_t s_wifi_event_group;
22 |
23 | /* The event group allows multiple bits for each event, but we only care about two events:
24 | * - we are connected to the AP with an IP
25 | * - we failed to connect after the maximum amount of retries */
26 | #define WIFI_CONNECTED_BIT BIT0
27 | #define WIFI_FAIL_BIT BIT1
28 |
29 | static const char *TAG = "wifi station";
30 |
31 | static int s_retry_num = 0;
32 |
33 | static void event_handler(void* arg, esp_event_base_t event_base, int32_t event_id, void* event_data)
34 | {
35 | if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
36 | esp_wifi_connect();
37 | } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
38 | if (s_retry_num < CONFIG_ESP_MAXIMUM_RETRY) {
39 | esp_wifi_connect();
40 | s_retry_num++;
41 | ESP_LOGI(TAG, "retry to connect to the AP");
42 | } else {
43 | xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT);
44 | }
45 | ESP_LOGI(TAG,"connect to the AP fail");
46 | } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
47 | ip_event_got_ip_t* event = (ip_event_got_ip_t*) event_data;
48 | ESP_LOGI(TAG, "got ip:" IPSTR, IP2STR(&event->ip_info.ip));
49 | s_retry_num = 0;
50 | xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
51 | }
52 | }
53 |
54 | esp_err_t wifi_init_sta(void)
55 | {
56 | s_wifi_event_group = xEventGroupCreate();
57 |
58 | ESP_ERROR_CHECK(esp_netif_init());
59 | ESP_ERROR_CHECK(esp_event_loop_create_default());
60 | esp_netif_create_default_wifi_sta();
61 |
62 | wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
63 | ESP_ERROR_CHECK(esp_wifi_init(&cfg));
64 |
65 | esp_event_handler_instance_t instance_any_id;
66 | esp_event_handler_instance_t instance_got_ip;
67 | ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT,
68 | ESP_EVENT_ANY_ID,
69 | &event_handler,
70 | NULL,
71 | &instance_any_id));
72 | ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT,
73 | IP_EVENT_STA_GOT_IP,
74 | &event_handler,
75 | NULL,
76 | &instance_got_ip));
77 |
78 | wifi_config_t wifi_config = {
79 | .sta = {
80 | .ssid = CONFIG_ESP_WIFI_SSID,
81 | .password = CONFIG_ESP_WIFI_PASSWORD,
82 | /* Setting a password implies station will connect to all security modes including WEP/WPA.
83 | * However these modes are deprecated and not advisable to be used. Incase your Access point
84 | * doesn't support WPA2, these mode can be enabled by commenting below line */
85 | .threshold.authmode = WIFI_AUTH_WPA2_PSK,
86 |
87 | .pmf_cfg = {
88 | .capable = true,
89 | .required = false
90 | },
91 | },
92 | };
93 | ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));
94 | ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA) );
95 | ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config) );
96 | ESP_ERROR_CHECK(esp_wifi_start() );
97 |
98 | /* Waiting until either the connection is established (WIFI_CONNECTED_BIT) or connection failed for the maximum
99 | * number of re-tries (WIFI_FAIL_BIT). The bits are set by event_handler() (see above) */
100 | esp_err_t ret_value = ESP_OK;
101 | EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group,
102 | WIFI_CONNECTED_BIT | WIFI_FAIL_BIT,
103 | pdFALSE,
104 | pdFALSE,
105 | portMAX_DELAY);
106 |
107 | /* xEventGroupWaitBits() returns the bits before the call returned, hence we can test which event actually
108 | * happened. */
109 | if (bits & WIFI_CONNECTED_BIT) {
110 | ESP_LOGI(TAG, "connected to ap SSID:%s password:%s", CONFIG_ESP_WIFI_SSID, CONFIG_ESP_WIFI_PASSWORD);
111 | } else if (bits & WIFI_FAIL_BIT) {
112 | ESP_LOGI(TAG, "Failed to connect to SSID:%s, password:%s", CONFIG_ESP_WIFI_SSID, CONFIG_ESP_WIFI_PASSWORD);
113 | ret_value = ESP_FAIL;
114 | } else {
115 | ESP_LOGE(TAG, "UNEXPECTED EVENT");
116 | ret_value = ESP_FAIL;
117 | }
118 |
119 | /* The event will not be processed after unregister */
120 | ESP_ERROR_CHECK(esp_event_handler_instance_unregister(IP_EVENT, IP_EVENT_STA_GOT_IP, instance_got_ip));
121 | ESP_ERROR_CHECK(esp_event_handler_instance_unregister(WIFI_EVENT, ESP_EVENT_ANY_ID, instance_any_id));
122 | vEventGroupDelete(s_wifi_event_group);
123 | return ret_value;
124 | }
125 |
126 | void wifi_status(void *pvParameters)
127 | {
128 | ESP_LOGI(pcTaskGetName(0), "Start");
129 |
130 | wifi_ap_record_t ap_info;
131 | while(1) {
132 | esp_err_t ret = esp_wifi_sta_get_ap_info(&ap_info);
133 | ESP_LOGI(pcTaskGetName(0), "esp_wifi_sta_get_ap_info=%d", ret);
134 | if (ret != ESP_OK) break;
135 | vTaskDelay(1000);
136 | }
137 | esp_restart();
138 | }
139 |
140 | void start_wifi(void)
141 | {
142 | // Initialize NVS
143 | ESP_LOGI(TAG, "Start");
144 | esp_err_t ret = nvs_flash_init();
145 | if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
146 | ESP_ERROR_CHECK(nvs_flash_erase());
147 | ret = nvs_flash_init();
148 | }
149 | ESP_ERROR_CHECK(ret);
150 |
151 | //Initialize WiFi
152 | ESP_ERROR_CHECK(wifi_init_sta());
153 | ESP_LOGI(TAG, "Finish");
154 | }
155 |
--------------------------------------------------------------------------------
/Kalman/html/root.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 | Euler Meter
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
204 |
205 |
206 |
207 |
208 |
209 |
--------------------------------------------------------------------------------
/Madgwick/html/root.html:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
7 |
8 | Euler Meter
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 |
17 |
18 |
19 |
20 |
21 |
22 |
23 |
24 |
204 |
205 |
206 |
207 |
208 |
209 |
--------------------------------------------------------------------------------
/Kalman/main/lsm6ds3.cpp:
--------------------------------------------------------------------------------
1 | #include
2 | #include
3 |
4 | #include "freertos/FreeRTOS.h"
5 | #include "freertos/task.h"
6 | #include "freertos/queue.h"
7 | #include "freertos/message_buffer.h"
8 | #include "esp_timer.h"
9 | #include "esp_log.h"
10 | #include "esp_err.h"
11 | #include "cJSON.h"
12 |
13 | #include "parameter.h"
14 |
15 | extern QueueHandle_t xQueueTrans;
16 | extern MessageBufferHandle_t xMessageBufferToClient;
17 |
18 | static const char *TAG = "IMU";
19 |
20 | // I2Cdev and LSM303DLHC must be installed as libraries, or else the .cpp/.h files
21 | // for both classes must be in the include path of your project
22 | #include "I2Cdev.h"
23 |
24 | #include "LSM6DS3.h"
25 |
26 | LSM6DS3 imu(CONFIG_I2C_ADDR);
27 |
28 | // Source: https://github.com/TKJElectronics/KalmanFilter
29 | #include "Kalman.h"
30 |
31 | #define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead
32 | #define RAD_TO_DEG (180.0/M_PI)
33 | #define DEG_TO_RAD 0.0174533
34 |
35 | // Arduino macro
36 | #define micros() (unsigned long) (esp_timer_get_time())
37 | #define delay(ms) esp_rom_delay_us(ms*1000)
38 |
39 | Kalman kalmanX; // Create the Kalman instances
40 | Kalman kalmanY;
41 |
42 | void _getMotion6(double *_ax, double *_ay, double *_az, double *_gx, double *_gy, double *_gz) {
43 | float ax=0.0, ay=0.0, az=0.0;
44 | float gx=0.0, gy=0.0, gz=0.0;
45 | #if 0
46 | if (imu.accelerationAvailable()) {
47 | imu.readAcceleration(ax, ay, az);
48 | }
49 | if (imu.gyroscopeAvailable()) {
50 | imu.readGyroscope(gx, gy, gz);
51 | }
52 | #endif
53 | while(1) {
54 | if (imu.accelerationAvailable()) break;
55 | vTaskDelay(1);
56 | }
57 | imu.readAcceleration(ax, ay, az);
58 | while(1) {
59 | if (imu.gyroscopeAvailable()) break;
60 | vTaskDelay(1);
61 | }
62 | imu.readGyroscope(gx, gy, gz);
63 |
64 | *_ax = ax;
65 | *_ay = ay;
66 | *_az = az;
67 | *_gx = gx;
68 | *_gy = gy;
69 | *_gz = gz;
70 | }
71 |
72 | void getRollPitch(double accX, double accY, double accZ, double *roll, double *pitch) {
73 | // atan2 outputs the value of - to (radians) - see http://en.wikipedia.org/wiki/Atan2
74 | // It is then converted from radians to degrees
75 | #ifdef RESTRICT_PITCH // Eq. 25 and 26
76 | *roll = atan2(accY, accZ) * RAD_TO_DEG;
77 | *pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
78 | #else // Eq. 28 and 29
79 | *roll = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
80 | *pitch = atan2(-accX, accZ) * RAD_TO_DEG;
81 | #endif
82 | }
83 |
84 | void lsm6ds3(void *pvParameters){
85 | // Initialize device
86 | if (imu.begin() == 0) {
87 | ESP_LOGE(TAG, "Connection fail");
88 | vTaskDelete(NULL);
89 | }
90 |
91 | // Set Kalman and gyro starting angle
92 | double accX, accY, accZ;
93 | double gyroX, gyroY, gyroZ;
94 | double roll, pitch; // Roll and pitch are calculated using the accelerometer
95 | double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
96 |
97 | _getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ);
98 | getRollPitch(accX, accY, accZ, &roll, &pitch);
99 | kalAngleX = roll;
100 | kalAngleY = pitch;
101 | kalmanX.setAngle(roll); // Set starting angle
102 | kalmanY.setAngle(pitch);
103 | uint32_t timer = micros();
104 |
105 | int elasped = 0;
106 | bool initialized = false;
107 | double initial_kalAngleX = 0.0;
108 | double initial_kalAngleY = 0.0;
109 |
110 | while(1){
111 | _getMotion6(&accX, &accY, &accZ, &gyroX, &gyroY, &gyroZ);
112 | //printf("%f %f %f - %f %f %f\n", accX, accY, accZ, gyroX, gyroY, gyroZ);
113 | getRollPitch(accX, accY, accZ, &roll, &pitch);
114 |
115 | double dt = (double)(micros() - timer) / 1000000; // Calculate delta time
116 | timer = micros();
117 |
118 | /* Roll and pitch estimation */
119 | double gyroXrate = gyroX;
120 | double gyroYrate = gyroY;
121 |
122 | #ifdef RESTRICT_PITCH
123 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
124 | if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
125 | kalmanX.setAngle(roll);
126 | kalAngleX = roll;
127 | } else
128 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
129 |
130 |
131 | if (abs(kalAngleX) > 90)
132 | gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
133 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
134 | #else
135 | // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
136 | if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
137 | kalmanY.setAngle(pitch);
138 | kalAngleY = pitch;
139 | } else
140 | kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
141 |
142 | if (abs(kalAngleY) > 90)
143 | gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
144 | kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
145 | #endif
146 |
147 | /* Print Data every 10 times */
148 | if (elasped > 10) {
149 | // Set the first data
150 | if (!initialized) {
151 | initial_kalAngleX = kalAngleX;
152 | initial_kalAngleY = kalAngleY;
153 | initialized = true;
154 | }
155 |
156 | #if 0
157 | printf("roll:%f", roll); printf(" ");
158 | printf("kalAngleX:%f", kalAngleX); printf(" ");
159 | printf("initial_kalAngleX:%f", initial_kalAngleX); printf(" ");
160 | printf("kalAngleX-initial_kalAngleX:%f", kalAngleX-initial_kalAngleX); printf(" ");
161 | printf("\n");
162 |
163 | printf("pitch:%f", pitch); printf(" ");
164 | printf("kalAngleY:%f", kalAngleY); printf(" ");
165 | printf("initial_kalAngleY:%f", initial_kalAngleY); printf(" ");
166 | printf("kalAngleY-initial_kalAngleY: %f", kalAngleY-initial_kalAngleY); printf(" ");
167 | printf("\n");
168 | #endif
169 |
170 | // Send UDP packet
171 | float _roll = kalAngleX-initial_kalAngleX;
172 | float _pitch = kalAngleY-initial_kalAngleY;
173 | ESP_LOGI(TAG, "roll:%f pitch=%f", _roll, _pitch);
174 |
175 | POSE_t pose;
176 | pose.roll = _roll;
177 | pose.pitch = _pitch;
178 | pose.yaw = 0.0;
179 | if (xQueueSend(xQueueTrans, &pose, 100) != pdPASS ) {
180 | ESP_LOGE(pcTaskGetName(NULL), "xQueueSend fail");
181 | }
182 |
183 | // Send WEB request
184 | cJSON *request;
185 | request = cJSON_CreateObject();
186 | cJSON_AddStringToObject(request, "id", "data-request");
187 | cJSON_AddNumberToObject(request, "roll", _roll);
188 | cJSON_AddNumberToObject(request, "pitch", _pitch);
189 | cJSON_AddNumberToObject(request, "yaw", 0.0);
190 | char *my_json_string = cJSON_Print(request);
191 | ESP_LOGD(TAG, "my_json_string\n%s",my_json_string);
192 | size_t xBytesSent = xMessageBufferSend(xMessageBufferToClient, my_json_string, strlen(my_json_string), 100);
193 | if (xBytesSent != strlen(my_json_string)) {
194 | ESP_LOGE(TAG, "xMessageBufferSend fail");
195 | }
196 | cJSON_Delete(request);
197 | cJSON_free(my_json_string);
198 |
199 | vTaskDelay(1);
200 | elasped = 0;
201 | }
202 | elasped++;
203 | vTaskDelay(1);
204 | }
205 |
206 | // Never reach here
207 | vTaskDelete(NULL);
208 | }
209 |
--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
1 | # esp-idf-lsm6ds3
2 | A demo showing the pose of the lsm6ds3 6DoF IMU sensor in 3D using esp-idf.
3 |
4 | You can use the Kalman filter or the Madgwick filter to estimate the Euler angle.
5 | Euler angles are roll, pitch and yaw.
6 | It's very intuitive and easy to understand.
7 | However, since LSM6DS3 is a 6DoF IMU, YAW estimation is not possible.
8 | 
9 | You can view like this.
10 | 
11 |
12 | # Software requiment
13 | ESP-IDF V5.0 or later.
14 | ESP-IDF V4.4 release branch reached EOL in July 2024.
15 | ESP-IDF V5.1 is required when using ESP32-C6.
16 |
17 | # Hardware requirements
18 | LSM6DS3 Accelerometer Gyroscope module 6 Dof inertial Measurement Sensors.
19 |
20 | # Wireing
21 | |LSM6DS3||ESP32|ESP32-S2/S3|ESP32-C2/C3/C6||
22 | |:-:|:-:|:-:|:-:|:-:|:-:|
23 | |VIN|--|N/C|N/C|N/C||
24 | |3V3|--|3.3V|3.3V|3.3V||
25 | |GND|--|GND|GND|GND||
26 | |SCL|--|GPIO22|GPIO12|GPIO5|(*1)|
27 | |SDA|--|GPIO21|GPIO11|GPIO4|(*1)|
28 | |CS|--|3.3V|3.3V|3.3V|Use i2c|
29 | |SAO|--|GND/3.3V|GND/3.3V|GND/3.3V|(*2)|
30 |
31 | (*1)You can change it to any pin using menuconfig.
32 |
33 | (*2)Choosing an i2c address.
34 | GND:i2c address is 0x6A.
35 | 3.3V:i2c address is 0x6B.
36 |
37 |
38 | # Get Euler angles from lsm6ds3 using Kalman filter
39 | ```
40 | git clone https://github.com/nopnop2002/esp-idf-lsm6ds3
41 | cd esp-idf-lsm6ds3/Kalman
42 | idf.py set-target {esp32/esp32s2/esp32s3/esp32c2/esp32c3/esp32c6}
43 | idf.py menuconfig
44 | idf.py flash
45 | ```
46 |
47 | ### Configuration
48 | 
49 | 
50 |
51 |
52 | # Get Euler angles from lsm6ds3 using Madgwick filter
53 | ```
54 | git clone https://github.com/nopnop2002/esp-idf-lsm6ds3
55 | cd esp-idf-lsm6ds3/Madgwick
56 | idf.py set-target {esp32/esp32s2/esp32s3/esp32c2/esp32c3/esp32c6}
57 | idf.py menuconfig
58 | idf.py flash
59 | ```
60 |
61 | ### Configuration
62 | 
63 | 
64 |
65 | # View Euler angles with built-in web server
66 | ESP32 acts as a web server.
67 | I used [this](https://github.com/Molorius/esp32-websocket) component.
68 | This component can communicate directly with the browser.
69 | Enter the following in the address bar of your web browser.
70 | ```
71 | http:://{IP of ESP32}/
72 | or
73 | http://esp32.local/
74 | ```
75 |
76 | 
77 |
78 | WEB pages are stored in the html folder.
79 | I used [this](https://canvas-gauges.com/) for gauge display.
80 | I used [this](https://threejs.org/) for 3D display.
81 | You can change the design and color according to your preference like this.
82 | 
83 |
84 | # View Euler angles using PyTeapot
85 | You can view Euler angles using [this](https://github.com/thecountoftuscany/PyTeapot-Quaternion-Euler-cube-rotation) tool.
86 | It works as a UDP display server.
87 | This is a great application.
88 |
89 | ```
90 | +-------------+ +-------------+ +-------------+
91 | | | | | | |
92 | | IMU |--(i2c)-->| ESP32 |--(UDP)-->| pyteapot.py |
93 | | | | | | |
94 | +-------------+ +-------------+ +-------------+
95 | ```
96 |
97 | ### Installation for Linux
98 | ```
99 | $ python3 --version
100 | Python 3.11.2
101 | $ sudo apt install python3-pip python3-setuptools
102 | $ python3 -m pip install -U pip
103 | $ python3 -m pip install pygame
104 | $ python3 -m pip install PyOpenGL PyOpenGL_accelerate
105 | $ git clone https://github.com/thecountoftuscany/PyTeapot-Quaternion-Euler-cube-rotation
106 | $ cd PyTeapot-Quaternion-Euler-cube-rotation
107 | $ python3 pyteapot.py
108 | ```
109 | The posture of your sensor is displayed.
110 | 
111 |
112 | ### Installation for Windows
113 | Install Git for Windows from [here](https://gitforwindows.org/).
114 | Install Python Releases for Windows from [here](https://www.python.org/downloads/windows/).
115 | Open Git Bash and run:
116 | ```
117 | $ python --version
118 | Python 3.11.9
119 | $ python -m pip install -U pip
120 | $ python -m pip install pygame
121 | $ python -m pip install PyOpenGL PyOpenGL_accelerate
122 | $ git clone https://github.com/thecountoftuscany/PyTeapot-Quaternion-Euler-cube-rotation
123 | $ cd PyTeapot-Quaternion-Euler-cube-rotation
124 | $ python pyteapot.py
125 | ```
126 | 
127 |
128 |
129 | # View Euler angles using panda3d library
130 | You can view Euler angles using [this](https://www.panda3d.org/) library.
131 | It works as a UDP display server.
132 |
133 | ```
134 | +-------------+ +-------------+ +-------------+
135 | | | | | | |
136 | | IMU |--(ic2)-->| ESP32 |--(UDP)-->| panda.py |
137 | | | | | | |
138 | +-------------+ +-------------+ +-------------+
139 | ```
140 |
141 | ### Installation for Linux
142 | ```
143 | $ python3 --version
144 | Python 3.11.2
145 | $ sudo apt install python3-pip python3-setuptools
146 | $ python3 -m pip install -U pip
147 | $ python3 -m pip install panda3d
148 | $ git clone https://github.com/nopnop2002/esp-idf-mpu6050-dmp
149 | $ cd esp-idf-mpu6050-dmp/panda3d
150 | $ python3 panda.py --help
151 | usage: panda.py [-h] [--model {jet,biplain,707,fa18}]
152 |
153 | options:
154 | -h, --help show this help message and exit
155 | --model {jet,biplain,707,fa18}
156 | ```
157 | 
158 |
159 | ### Installation for Windows
160 | Install Git for Windows from [here](https://gitforwindows.org/).
161 | Install Python Releases for Windows from [here](https://www.python.org/downloads/windows/).
162 | Open Git Bash and run:
163 | ```
164 | $ python --version
165 | Python 3.11.9
166 | $ python -m pip install -U pip
167 | $ python -m pip install panda3d
168 | $ git clone https://github.com/nopnop2002/esp-idf-mpu6050-dmp
169 | $ cd esp-idf-mpu6050-dmp/panda3d
170 | $ python panda.py --help
171 | usage: panda.py [-h] [--model {jet,biplain,707,fa18}]
172 |
173 | options:
174 | -h, --help show this help message and exit
175 | --model {jet,biplain,707,fa18}
176 | ```
177 | 
178 |
179 | ### How to use
180 | See [here](https://github.com/nopnop2002/esp-idf-mpu6050-dmp/blob/main/panda3d/README.md)
181 |
182 |
183 |
--------------------------------------------------------------------------------
/Kalman/main/web_server.c:
--------------------------------------------------------------------------------
1 | /*
2 | Example using WEB Socket.
3 | This example code is in the Public Domain (or CC0 licensed, at your option.)
4 | Unless required by applicable law or agreed to in writing, this
5 | software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
6 | CONDITIONS OF ANY KIND, either express or implied.
7 | */
8 |
9 | #include
10 | #include
11 | #include
12 | #include "freertos/FreeRTOS.h"
13 | #include "freertos/task.h"
14 | #include "freertos/queue.h"
15 | #include "freertos/message_buffer.h"
16 | #include "esp_log.h"
17 |
18 | #include "websocket_server.h"
19 |
20 | static QueueHandle_t client_queue;
21 | extern MessageBufferHandle_t xMessageBufferToClient;
22 |
23 | const static int client_queue_size = 10;
24 |
25 | // handles websocket events
26 | void websocket_callback(uint8_t num,WEBSOCKET_TYPE_t type,char* msg,uint64_t len) {
27 | const static char* TAG = "websocket_callback";
28 | //int value;
29 |
30 | switch(type) {
31 | case WEBSOCKET_CONNECT:
32 | ESP_LOGI(TAG,"client %i connected!",num);
33 | break;
34 | case WEBSOCKET_DISCONNECT_EXTERNAL:
35 | ESP_LOGI(TAG,"client %i sent a disconnect message",num);
36 | break;
37 | case WEBSOCKET_DISCONNECT_INTERNAL:
38 | ESP_LOGI(TAG,"client %i was disconnected",num);
39 | break;
40 | case WEBSOCKET_DISCONNECT_ERROR:
41 | ESP_LOGI(TAG,"client %i was disconnected due to an error",num);
42 | break;
43 | case WEBSOCKET_TEXT:
44 | if(len) { // if the message length was greater than zero
45 | ESP_LOGI(TAG, "got message length %i: %s", (int)len, msg);
46 | size_t xBytesSent = xMessageBufferSendFromISR(xMessageBufferToClient, msg, len, NULL);
47 | if (xBytesSent != len) {
48 | ESP_LOGE(TAG, "xMessageBufferSend fail");
49 | }
50 | }
51 | break;
52 | case WEBSOCKET_BIN:
53 | ESP_LOGI(TAG,"client %i sent binary message of size %"PRIu32":\n%s",num,(uint32_t)len,msg);
54 | break;
55 | case WEBSOCKET_PING:
56 | ESP_LOGI(TAG,"client %i pinged us with message of size %"PRIu32":\n%s",num,(uint32_t)len,msg);
57 | break;
58 | case WEBSOCKET_PONG:
59 | ESP_LOGI(TAG,"client %i responded to the ping",num);
60 | break;
61 | }
62 | }
63 |
64 | // serves any clients
65 | static void http_server(struct netconn *conn) {
66 | const static char* TAG = "http_server";
67 | const static char HTML_HEADER[] = "HTTP/1.1 200 OK\nContent-type: text/html\n\n";
68 | const static char ERROR_HEADER[] = "HTTP/1.1 404 Not Found\nContent-type: text/html\n\n";
69 | const static char JS_HEADER[] = "HTTP/1.1 200 OK\nContent-type: text/javascript\n\n";
70 | const static char CSS_HEADER[] = "HTTP/1.1 200 OK\nContent-type: text/css\n\n";
71 | //const static char PNG_HEADER[] = "HTTP/1.1 200 OK\nContent-type: image/png\n\n";
72 | const static char ICO_HEADER[] = "HTTP/1.1 200 OK\nContent-type: image/x-icon\n\n";
73 | //const static char PDF_HEADER[] = "HTTP/1.1 200 OK\nContent-type: application/pdf\n\n";
74 | //const static char EVENT_HEADER[] = "HTTP/1.1 200 OK\nContent-Type: text/event-stream\nCache-Control: no-cache\nretry: 3000\n\n";
75 | struct netbuf* inbuf;
76 | static char* buf;
77 | static uint16_t buflen;
78 | static err_t err;
79 |
80 | // default page
81 | extern const uint8_t root_html_start[] asm("_binary_root_html_start");
82 | extern const uint8_t root_html_end[] asm("_binary_root_html_end");
83 | const uint32_t root_html_len = root_html_end - root_html_start;
84 |
85 | // main.js
86 | extern const uint8_t main_js_start[] asm("_binary_main_js_start");
87 | extern const uint8_t main_js_end[] asm("_binary_main_js_end");
88 | const uint32_t main_js_len = main_js_end - main_js_start;
89 |
90 | // main.css
91 | extern const uint8_t main_css_start[] asm("_binary_main_css_start");
92 | extern const uint8_t main_css_end[] asm("_binary_main_css_end");
93 | const uint32_t main_css_len = main_css_end - main_css_start;
94 |
95 | // favicon.ico
96 | extern const uint8_t favicon_ico_start[] asm("_binary_favicon_ico_start");
97 | extern const uint8_t favicon_ico_end[] asm("_binary_favicon_ico_end");
98 | const uint32_t favicon_ico_len = favicon_ico_end - favicon_ico_start;
99 |
100 | // error page
101 | extern const uint8_t error_html_start[] asm("_binary_error_html_start");
102 | extern const uint8_t error_html_end[] asm("_binary_error_html_end");
103 | const uint32_t error_html_len = error_html_end - error_html_start;
104 |
105 | netconn_set_recvtimeout(conn,1000); // allow a connection timeout of 1 second
106 | ESP_LOGI(TAG,"reading from client...");
107 | err = netconn_recv(conn, &inbuf);
108 | ESP_LOGI(TAG,"read from client");
109 | if(err==ERR_OK) {
110 | netbuf_data(inbuf, (void**)&buf, &buflen);
111 | if(buf) {
112 |
113 | ESP_LOGD(TAG, "buf=[%s]", buf);
114 | // default page
115 | if (strstr(buf,"GET / ")
116 | && !strstr(buf,"Upgrade: websocket")) {
117 | ESP_LOGI(TAG,"Sending /");
118 | netconn_write(conn, HTML_HEADER, sizeof(HTML_HEADER)-1,NETCONN_NOCOPY);
119 | netconn_write(conn, root_html_start,root_html_len,NETCONN_NOCOPY);
120 | netconn_close(conn);
121 | netconn_delete(conn);
122 | netbuf_delete(inbuf);
123 | }
124 |
125 | // default page websocket
126 | else if(strstr(buf,"GET / ")
127 | && strstr(buf,"Upgrade: websocket")) {
128 | ESP_LOGI(TAG,"Requesting websocket on /");
129 | ws_server_add_client(conn,buf,buflen,"/",websocket_callback);
130 | netbuf_delete(inbuf);
131 | }
132 |
133 | else if(strstr(buf,"GET /main.js ")) {
134 | ESP_LOGI(TAG,"Sending /main.js");
135 | netconn_write(conn, JS_HEADER, sizeof(JS_HEADER)-1,NETCONN_NOCOPY);
136 | netconn_write(conn, main_js_start, main_js_len,NETCONN_NOCOPY);
137 | netconn_close(conn);
138 | netconn_delete(conn);
139 | netbuf_delete(inbuf);
140 | }
141 |
142 | else if(strstr(buf,"GET /main.css ")) {
143 | ESP_LOGI(TAG,"Sending /main.css");
144 | netconn_write(conn, CSS_HEADER, sizeof(CSS_HEADER)-1,NETCONN_NOCOPY);
145 | netconn_write(conn, main_css_start, main_css_len,NETCONN_NOCOPY);
146 | netconn_close(conn);
147 | netconn_delete(conn);
148 | netbuf_delete(inbuf);
149 | }
150 |
151 | else if(strstr(buf,"GET /favicon.ico ")) {
152 | ESP_LOGI(TAG,"Sending favicon.ico");
153 | netconn_write(conn,ICO_HEADER,sizeof(ICO_HEADER)-1,NETCONN_NOCOPY);
154 | netconn_write(conn,favicon_ico_start,favicon_ico_len,NETCONN_NOCOPY);
155 | netconn_close(conn);
156 | netconn_delete(conn);
157 | netbuf_delete(inbuf);
158 | }
159 |
160 | else if(strstr(buf,"GET /")) {
161 | ESP_LOGE(TAG,"Unknown request, sending error page: %s",buf);
162 | netconn_write(conn, ERROR_HEADER, sizeof(ERROR_HEADER)-1,NETCONN_NOCOPY);
163 | netconn_write(conn, error_html_start, error_html_len,NETCONN_NOCOPY);
164 | netconn_close(conn);
165 | netconn_delete(conn);
166 | netbuf_delete(inbuf);
167 | }
168 |
169 | else {
170 | ESP_LOGE(TAG,"Unknown request");
171 | netconn_close(conn);
172 | netconn_delete(conn);
173 | netbuf_delete(inbuf);
174 | }
175 | }
176 | else {
177 | ESP_LOGI(TAG,"Unknown request (empty?...)");
178 | netconn_close(conn);
179 | netconn_delete(conn);
180 | netbuf_delete(inbuf);
181 | }
182 | }
183 | else { // if err==ERR_OK
184 | ESP_LOGI(TAG,"error on read, closing connection");
185 | netconn_close(conn);
186 | netconn_delete(conn);
187 | netbuf_delete(inbuf);
188 | }
189 | }
190 |
191 | // receives clients from queue, handles them
192 | void server_handle_task(void* pvParameters) {
193 | const static char* TAG = "server_handle_task";
194 | struct netconn* conn;
195 | ESP_LOGI(TAG,"Starting");
196 | for(;;) {
197 | xQueueReceive(client_queue,&conn,portMAX_DELAY);
198 | if(!conn) continue;
199 | http_server(conn);
200 | }
201 | vTaskDelete(NULL);
202 | }
203 |
204 | // handles clients when they first connect. passes to a queue
205 | void server_task(void* pvParameters) {
206 | const static char* TAG = "server_task";
207 | char *task_parameter = (char *)pvParameters;
208 | ESP_LOGI(TAG, "Start task_parameter=%s", task_parameter);
209 | char url[64];
210 | sprintf(url, "http://%s", task_parameter);
211 | ESP_LOGI(TAG, "Starting server on %s", url);
212 |
213 | struct netconn *conn, *newconn;
214 | static err_t err;
215 | client_queue = xQueueCreate(client_queue_size,sizeof(struct netconn*));
216 | configASSERT( client_queue );
217 |
218 |
219 | UBaseType_t PriorityGet = uxTaskPriorityGet(NULL);
220 | ESP_LOGI(TAG, "PriorityGet=%d", PriorityGet);
221 | xTaskCreate(&server_handle_task, "server_handle", 1024*3, NULL, PriorityGet, NULL);
222 |
223 |
224 | conn = netconn_new(NETCONN_TCP);
225 | netconn_bind(conn,NULL,80);
226 | netconn_listen(conn);
227 | ESP_LOGI(TAG,"server listening");
228 | do {
229 | err = netconn_accept(conn, &newconn);
230 | ESP_LOGI(TAG,"new client");
231 | if(err == ERR_OK) {
232 | xQueueSendToBack(client_queue,&newconn,portMAX_DELAY);
233 | //http_server(newconn);
234 | }
235 | } while(err == ERR_OK);
236 | netconn_close(conn);
237 | netconn_delete(conn);
238 | ESP_LOGE(TAG,"task ending, rebooting board");
239 | esp_restart();
240 | }
241 |
--------------------------------------------------------------------------------
/Madgwick/main/web_server.c:
--------------------------------------------------------------------------------
1 | /*
2 | Example using WEB Socket.
3 | This example code is in the Public Domain (or CC0 licensed, at your option.)
4 | Unless required by applicable law or agreed to in writing, this
5 | software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
6 | CONDITIONS OF ANY KIND, either express or implied.
7 | */
8 |
9 | #include
10 | #include
11 | #include
12 | #include "freertos/FreeRTOS.h"
13 | #include "freertos/task.h"
14 | #include "freertos/queue.h"
15 | #include "freertos/message_buffer.h"
16 | #include "esp_log.h"
17 |
18 | #include "websocket_server.h"
19 |
20 | static QueueHandle_t client_queue;
21 | extern MessageBufferHandle_t xMessageBufferToClient;
22 |
23 | const static int client_queue_size = 10;
24 |
25 | // handles websocket events
26 | void websocket_callback(uint8_t num,WEBSOCKET_TYPE_t type,char* msg,uint64_t len) {
27 | const static char* TAG = "websocket_callback";
28 | //int value;
29 |
30 | switch(type) {
31 | case WEBSOCKET_CONNECT:
32 | ESP_LOGI(TAG,"client %i connected!",num);
33 | break;
34 | case WEBSOCKET_DISCONNECT_EXTERNAL:
35 | ESP_LOGI(TAG,"client %i sent a disconnect message",num);
36 | break;
37 | case WEBSOCKET_DISCONNECT_INTERNAL:
38 | ESP_LOGI(TAG,"client %i was disconnected",num);
39 | break;
40 | case WEBSOCKET_DISCONNECT_ERROR:
41 | ESP_LOGI(TAG,"client %i was disconnected due to an error",num);
42 | break;
43 | case WEBSOCKET_TEXT:
44 | if(len) { // if the message length was greater than zero
45 | ESP_LOGI(TAG, "got message length %i: %s", (int)len, msg);
46 | size_t xBytesSent = xMessageBufferSendFromISR(xMessageBufferToClient, msg, len, NULL);
47 | if (xBytesSent != len) {
48 | ESP_LOGE(TAG, "xMessageBufferSend fail");
49 | }
50 | }
51 | break;
52 | case WEBSOCKET_BIN:
53 | ESP_LOGI(TAG,"client %i sent binary message of size %"PRIu32":\n%s",num,(uint32_t)len,msg);
54 | break;
55 | case WEBSOCKET_PING:
56 | ESP_LOGI(TAG,"client %i pinged us with message of size %"PRIu32":\n%s",num,(uint32_t)len,msg);
57 | break;
58 | case WEBSOCKET_PONG:
59 | ESP_LOGI(TAG,"client %i responded to the ping",num);
60 | break;
61 | }
62 | }
63 |
64 | // serves any clients
65 | static void http_server(struct netconn *conn) {
66 | const static char* TAG = "http_server";
67 | const static char HTML_HEADER[] = "HTTP/1.1 200 OK\nContent-type: text/html\n\n";
68 | const static char ERROR_HEADER[] = "HTTP/1.1 404 Not Found\nContent-type: text/html\n\n";
69 | const static char JS_HEADER[] = "HTTP/1.1 200 OK\nContent-type: text/javascript\n\n";
70 | const static char CSS_HEADER[] = "HTTP/1.1 200 OK\nContent-type: text/css\n\n";
71 | //const static char PNG_HEADER[] = "HTTP/1.1 200 OK\nContent-type: image/png\n\n";
72 | const static char ICO_HEADER[] = "HTTP/1.1 200 OK\nContent-type: image/x-icon\n\n";
73 | //const static char PDF_HEADER[] = "HTTP/1.1 200 OK\nContent-type: application/pdf\n\n";
74 | //const static char EVENT_HEADER[] = "HTTP/1.1 200 OK\nContent-Type: text/event-stream\nCache-Control: no-cache\nretry: 3000\n\n";
75 | struct netbuf* inbuf;
76 | static char* buf;
77 | static uint16_t buflen;
78 | static err_t err;
79 |
80 | // default page
81 | extern const uint8_t root_html_start[] asm("_binary_root_html_start");
82 | extern const uint8_t root_html_end[] asm("_binary_root_html_end");
83 | const uint32_t root_html_len = root_html_end - root_html_start;
84 |
85 | // main.js
86 | extern const uint8_t main_js_start[] asm("_binary_main_js_start");
87 | extern const uint8_t main_js_end[] asm("_binary_main_js_end");
88 | const uint32_t main_js_len = main_js_end - main_js_start;
89 |
90 | // main.css
91 | extern const uint8_t main_css_start[] asm("_binary_main_css_start");
92 | extern const uint8_t main_css_end[] asm("_binary_main_css_end");
93 | const uint32_t main_css_len = main_css_end - main_css_start;
94 |
95 | // favicon.ico
96 | extern const uint8_t favicon_ico_start[] asm("_binary_favicon_ico_start");
97 | extern const uint8_t favicon_ico_end[] asm("_binary_favicon_ico_end");
98 | const uint32_t favicon_ico_len = favicon_ico_end - favicon_ico_start;
99 |
100 | // error page
101 | extern const uint8_t error_html_start[] asm("_binary_error_html_start");
102 | extern const uint8_t error_html_end[] asm("_binary_error_html_end");
103 | const uint32_t error_html_len = error_html_end - error_html_start;
104 |
105 | netconn_set_recvtimeout(conn,1000); // allow a connection timeout of 1 second
106 | ESP_LOGI(TAG,"reading from client...");
107 | err = netconn_recv(conn, &inbuf);
108 | ESP_LOGI(TAG,"read from client");
109 | if(err==ERR_OK) {
110 | netbuf_data(inbuf, (void**)&buf, &buflen);
111 | if(buf) {
112 |
113 | ESP_LOGD(TAG, "buf=[%s]", buf);
114 | // default page
115 | if (strstr(buf,"GET / ")
116 | && !strstr(buf,"Upgrade: websocket")) {
117 | ESP_LOGI(TAG,"Sending /");
118 | netconn_write(conn, HTML_HEADER, sizeof(HTML_HEADER)-1,NETCONN_NOCOPY);
119 | netconn_write(conn, root_html_start,root_html_len,NETCONN_NOCOPY);
120 | netconn_close(conn);
121 | netconn_delete(conn);
122 | netbuf_delete(inbuf);
123 | }
124 |
125 | // default page websocket
126 | else if(strstr(buf,"GET / ")
127 | && strstr(buf,"Upgrade: websocket")) {
128 | ESP_LOGI(TAG,"Requesting websocket on /");
129 | ws_server_add_client(conn,buf,buflen,"/",websocket_callback);
130 | netbuf_delete(inbuf);
131 | }
132 |
133 | else if(strstr(buf,"GET /main.js ")) {
134 | ESP_LOGI(TAG,"Sending /main.js");
135 | netconn_write(conn, JS_HEADER, sizeof(JS_HEADER)-1,NETCONN_NOCOPY);
136 | netconn_write(conn, main_js_start, main_js_len,NETCONN_NOCOPY);
137 | netconn_close(conn);
138 | netconn_delete(conn);
139 | netbuf_delete(inbuf);
140 | }
141 |
142 | else if(strstr(buf,"GET /main.css ")) {
143 | ESP_LOGI(TAG,"Sending /main.css");
144 | netconn_write(conn, CSS_HEADER, sizeof(CSS_HEADER)-1,NETCONN_NOCOPY);
145 | netconn_write(conn, main_css_start, main_css_len,NETCONN_NOCOPY);
146 | netconn_close(conn);
147 | netconn_delete(conn);
148 | netbuf_delete(inbuf);
149 | }
150 |
151 | else if(strstr(buf,"GET /favicon.ico ")) {
152 | ESP_LOGI(TAG,"Sending favicon.ico");
153 | netconn_write(conn,ICO_HEADER,sizeof(ICO_HEADER)-1,NETCONN_NOCOPY);
154 | netconn_write(conn,favicon_ico_start,favicon_ico_len,NETCONN_NOCOPY);
155 | netconn_close(conn);
156 | netconn_delete(conn);
157 | netbuf_delete(inbuf);
158 | }
159 |
160 | else if(strstr(buf,"GET /")) {
161 | ESP_LOGE(TAG,"Unknown request, sending error page: %s",buf);
162 | netconn_write(conn, ERROR_HEADER, sizeof(ERROR_HEADER)-1,NETCONN_NOCOPY);
163 | netconn_write(conn, error_html_start, error_html_len,NETCONN_NOCOPY);
164 | netconn_close(conn);
165 | netconn_delete(conn);
166 | netbuf_delete(inbuf);
167 | }
168 |
169 | else {
170 | ESP_LOGE(TAG,"Unknown request");
171 | netconn_close(conn);
172 | netconn_delete(conn);
173 | netbuf_delete(inbuf);
174 | }
175 | }
176 | else {
177 | ESP_LOGI(TAG,"Unknown request (empty?...)");
178 | netconn_close(conn);
179 | netconn_delete(conn);
180 | netbuf_delete(inbuf);
181 | }
182 | }
183 | else { // if err==ERR_OK
184 | ESP_LOGI(TAG,"error on read, closing connection");
185 | netconn_close(conn);
186 | netconn_delete(conn);
187 | netbuf_delete(inbuf);
188 | }
189 | }
190 |
191 | // receives clients from queue, handles them
192 | void server_handle_task(void* pvParameters) {
193 | const static char* TAG = "server_handle_task";
194 | struct netconn* conn;
195 | ESP_LOGI(TAG,"Starting");
196 | for(;;) {
197 | xQueueReceive(client_queue,&conn,portMAX_DELAY);
198 | if(!conn) continue;
199 | http_server(conn);
200 | }
201 | vTaskDelete(NULL);
202 | }
203 |
204 | // handles clients when they first connect. passes to a queue
205 | void server_task(void* pvParameters) {
206 | const static char* TAG = "server_task";
207 | char *task_parameter = (char *)pvParameters;
208 | ESP_LOGI(TAG, "Start task_parameter=%s", task_parameter);
209 | char url[64];
210 | sprintf(url, "http://%s", task_parameter);
211 | ESP_LOGI(TAG, "Starting server on %s", url);
212 |
213 | struct netconn *conn, *newconn;
214 | static err_t err;
215 | client_queue = xQueueCreate(client_queue_size,sizeof(struct netconn*));
216 | configASSERT( client_queue );
217 |
218 |
219 | UBaseType_t PriorityGet = uxTaskPriorityGet(NULL);
220 | ESP_LOGI(TAG, "PriorityGet=%d", PriorityGet);
221 | xTaskCreate(&server_handle_task, "server_handle", 1024*3, NULL, PriorityGet, NULL);
222 |
223 |
224 | conn = netconn_new(NETCONN_TCP);
225 | netconn_bind(conn,NULL,80);
226 | netconn_listen(conn);
227 | ESP_LOGI(TAG,"server listening");
228 | do {
229 | err = netconn_accept(conn, &newconn);
230 | ESP_LOGI(TAG,"new client");
231 | if(err == ERR_OK) {
232 | xQueueSendToBack(client_queue,&newconn,portMAX_DELAY);
233 | //http_server(newconn);
234 | }
235 | } while(err == ERR_OK);
236 | netconn_close(conn);
237 | netconn_delete(conn);
238 | ESP_LOGE(TAG,"task ending, rebooting board");
239 | esp_restart();
240 | }
241 |
--------------------------------------------------------------------------------
/components/MadgwickAHRS/MadgwickAHRS.cpp:
--------------------------------------------------------------------------------
1 | //=============================================================================================
2 | // MadgwickAHRS.c
3 | //=============================================================================================
4 | //
5 | // Implementation of Madgwick's IMU and AHRS algorithms.
6 | // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
7 | //
8 | // From the x-io website "Open-source resources available on this website are
9 | // provided under the GNU General Public Licence unless an alternative licence
10 | // is provided in source."
11 | //
12 | // Date Author Notes
13 | // 29/09/2011 SOH Madgwick Initial release
14 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load
15 | // 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
16 | //
17 | //=============================================================================================
18 |
19 | //-------------------------------------------------------------------------------------------
20 | // Header files
21 |
22 | #include "MadgwickAHRS.h"
23 | #include
24 |
25 | //-------------------------------------------------------------------------------------------
26 | // Definitions
27 |
28 | #define sampleFreqDef 512.0f // sample frequency in Hz
29 | #define betaDef 0.1f // 2 * proportional gain
30 |
31 |
32 | //============================================================================================
33 | // Functions
34 |
35 | //-------------------------------------------------------------------------------------------
36 | // AHRS algorithm update
37 |
38 | Madgwick::Madgwick() {
39 | beta = betaDef;
40 | q0 = 1.0f;
41 | q1 = 0.0f;
42 | q2 = 0.0f;
43 | q3 = 0.0f;
44 | invSampleFreq = 1.0f / sampleFreqDef;
45 | anglesComputed = 0;
46 | }
47 |
48 | void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) {
49 | float recipNorm;
50 | float s0, s1, s2, s3;
51 | float qDot1, qDot2, qDot3, qDot4;
52 | float hx, hy;
53 | float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
54 |
55 | // Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
56 | if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
57 | updateIMU(gx, gy, gz, ax, ay, az, dt);
58 | return;
59 | }
60 |
61 | // Convert gyroscope degrees/sec to radians/sec
62 | gx *= 0.0174533f;
63 | gy *= 0.0174533f;
64 | gz *= 0.0174533f;
65 |
66 | // Rate of change of quaternion from gyroscope
67 | qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
68 | qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
69 | qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
70 | qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
71 |
72 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
73 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
74 |
75 | // Normalise accelerometer measurement
76 | recipNorm = invSqrt(ax * ax + ay * ay + az * az);
77 | ax *= recipNorm;
78 | ay *= recipNorm;
79 | az *= recipNorm;
80 |
81 | // Normalise magnetometer measurement
82 | recipNorm = invSqrt(mx * mx + my * my + mz * mz);
83 | mx *= recipNorm;
84 | my *= recipNorm;
85 | mz *= recipNorm;
86 |
87 | // Auxiliary variables to avoid repeated arithmetic
88 | _2q0mx = 2.0f * q0 * mx;
89 | _2q0my = 2.0f * q0 * my;
90 | _2q0mz = 2.0f * q0 * mz;
91 | _2q1mx = 2.0f * q1 * mx;
92 | _2q0 = 2.0f * q0;
93 | _2q1 = 2.0f * q1;
94 | _2q2 = 2.0f * q2;
95 | _2q3 = 2.0f * q3;
96 | _2q0q2 = 2.0f * q0 * q2;
97 | _2q2q3 = 2.0f * q2 * q3;
98 | q0q0 = q0 * q0;
99 | q0q1 = q0 * q1;
100 | q0q2 = q0 * q2;
101 | q0q3 = q0 * q3;
102 | q1q1 = q1 * q1;
103 | q1q2 = q1 * q2;
104 | q1q3 = q1 * q3;
105 | q2q2 = q2 * q2;
106 | q2q3 = q2 * q3;
107 | q3q3 = q3 * q3;
108 |
109 | // Reference direction of Earth's magnetic field
110 | hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
111 | hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
112 | _2bx = sqrtf(hx * hx + hy * hy);
113 | _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
114 | _4bx = 2.0f * _2bx;
115 | _4bz = 2.0f * _2bz;
116 |
117 | // Gradient decent algorithm corrective step
118 | s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
119 | s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
120 | s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
121 | s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
122 | recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
123 | s0 *= recipNorm;
124 | s1 *= recipNorm;
125 | s2 *= recipNorm;
126 | s3 *= recipNorm;
127 |
128 | // Apply feedback step
129 | qDot1 -= beta * s0;
130 | qDot2 -= beta * s1;
131 | qDot3 -= beta * s2;
132 | qDot4 -= beta * s3;
133 | }
134 |
135 | // Integrate rate of change of quaternion to yield quaternion
136 | //q0 += qDot1 * invSampleFreq;
137 | //q1 += qDot2 * invSampleFreq;
138 | //q2 += qDot3 * invSampleFreq;
139 | //q3 += qDot4 * invSampleFreq;
140 | q0 += qDot1 * dt;
141 | q1 += qDot2 * dt;
142 | q2 += qDot3 * dt;
143 | q3 += qDot4 * dt;
144 |
145 | // Normalise quaternion
146 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
147 | q0 *= recipNorm;
148 | q1 *= recipNorm;
149 | q2 *= recipNorm;
150 | q3 *= recipNorm;
151 | anglesComputed = 0;
152 | }
153 |
154 | //-------------------------------------------------------------------------------------------
155 | // IMU algorithm update
156 |
157 | void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az, float dt) {
158 | float recipNorm;
159 | float s0, s1, s2, s3;
160 | float qDot1, qDot2, qDot3, qDot4;
161 | float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
162 |
163 | // Convert gyroscope degrees/sec to radians/sec
164 | gx *= 0.0174533f;
165 | gy *= 0.0174533f;
166 | gz *= 0.0174533f;
167 |
168 | // Rate of change of quaternion from gyroscope
169 | qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
170 | qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
171 | qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
172 | qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
173 |
174 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
175 | if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
176 |
177 | // Normalise accelerometer measurement
178 | recipNorm = invSqrt(ax * ax + ay * ay + az * az);
179 | ax *= recipNorm;
180 | ay *= recipNorm;
181 | az *= recipNorm;
182 |
183 | // Auxiliary variables to avoid repeated arithmetic
184 | _2q0 = 2.0f * q0;
185 | _2q1 = 2.0f * q1;
186 | _2q2 = 2.0f * q2;
187 | _2q3 = 2.0f * q3;
188 | _4q0 = 4.0f * q0;
189 | _4q1 = 4.0f * q1;
190 | _4q2 = 4.0f * q2;
191 | _8q1 = 8.0f * q1;
192 | _8q2 = 8.0f * q2;
193 | q0q0 = q0 * q0;
194 | q1q1 = q1 * q1;
195 | q2q2 = q2 * q2;
196 | q3q3 = q3 * q3;
197 |
198 | // Gradient decent algorithm corrective step
199 | s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
200 | s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
201 | s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
202 | s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
203 | recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
204 | s0 *= recipNorm;
205 | s1 *= recipNorm;
206 | s2 *= recipNorm;
207 | s3 *= recipNorm;
208 |
209 | // Apply feedback step
210 | qDot1 -= beta * s0;
211 | qDot2 -= beta * s1;
212 | qDot3 -= beta * s2;
213 | qDot4 -= beta * s3;
214 | }
215 |
216 | // Integrate rate of change of quaternion to yield quaternion
217 | //q0 += qDot1 * invSampleFreq;
218 | //q1 += qDot2 * invSampleFreq;
219 | //q2 += qDot3 * invSampleFreq;
220 | //q3 += qDot4 * invSampleFreq;
221 | q0 += qDot1 * dt;
222 | q1 += qDot2 * dt;
223 | q2 += qDot3 * dt;
224 | q3 += qDot4 * dt;
225 |
226 | // Normalise quaternion
227 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
228 | q0 *= recipNorm;
229 | q1 *= recipNorm;
230 | q2 *= recipNorm;
231 | q3 *= recipNorm;
232 | anglesComputed = 0;
233 | }
234 |
235 | //-------------------------------------------------------------------------------------------
236 | // Fast inverse square-root
237 | // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
238 |
239 | float Madgwick::invSqrt(float x) {
240 | float halfx = 0.5f * x;
241 | float y = x;
242 | long i = *(long*)&y;
243 | i = 0x5f3759df - (i>>1);
244 | y = *(float*)&i;
245 | y = y * (1.5f - (halfx * y * y));
246 | y = y * (1.5f - (halfx * y * y));
247 | return y;
248 | }
249 |
250 | //-------------------------------------------------------------------------------------------
251 |
252 | void Madgwick::computeAngles()
253 | {
254 | roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
255 | pitch = asinf(-2.0f * (q1*q3 - q0*q2));
256 | yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
257 | anglesComputed = 1;
258 | }
259 |
260 |
261 |
--------------------------------------------------------------------------------
/components/websocket/websocket.c:
--------------------------------------------------------------------------------
1 |
2 | /*
3 | esp32-websocket - a websocket component on esp-idf
4 | Copyright (C) 2019 Blake Felt - blake.w.felt@gmail.com
5 |
6 | This program is free software: you can redistribute it and/or modify
7 | it under the terms of the GNU General Public License as published by
8 | the Free Software Foundation, either version 3 of the License, or
9 | (at your option) any later version.
10 |
11 | This program is distributed in the hope that it will be useful,
12 | but WITHOUT ANY WARRANTY; without even the implied warranty of
13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 | GNU General Public License for more details.
15 |
16 | You should have received a copy of the GNU General Public License
17 | along with this program. If not, see .
18 | */
19 |
20 | #include "websocket.h"
21 | #include "lwip/tcp.h" // for the netconn structure
22 | #include "esp_system.h" // for esp_random
23 | #include "mbedtls/base64.h"
24 | #include "mbedtls/sha1.h"
25 | #include
26 |
27 | ws_client_t ws_connect_client(struct netconn* conn,
28 | char* url,
29 | void (*ccallback)(WEBSOCKET_TYPE_t type,char* msg,uint64_t len),
30 | void (*scallback)(uint8_t num,WEBSOCKET_TYPE_t type,char* msg,uint64_t len)
31 | ) {
32 | ws_client_t client;
33 | client.conn = conn;
34 | client.url = url;
35 | client.ping = 0;
36 | client.last_opcode = 0;
37 | client.contin = NULL;
38 | client.len = 0;
39 | client.unfinished = 0;
40 | client.ccallback = ccallback;
41 | client.scallback = scallback;
42 | return client;
43 | }
44 |
45 | void ws_disconnect_client(ws_client_t* client,bool mask) {
46 | ws_send(client,WEBSOCKET_OPCODE_CLOSE,NULL,0,mask); // tell the client to close
47 | if(client->conn) {
48 | client->conn->callback = NULL; // shut off the callback
49 | netconn_close(client->conn);
50 | netconn_delete(client->conn);
51 | client->conn = NULL;
52 | }
53 | client->url = NULL;
54 | client->last_opcode = 0;
55 | if(client->len) {
56 | if(client->contin)
57 | free(client->contin);
58 | client->len = 0;
59 | }
60 | client->ccallback = NULL;
61 | client->scallback = NULL;
62 | }
63 |
64 | bool ws_is_connected(ws_client_t client) {
65 | if(client.conn)
66 | return 1;
67 | return 0;
68 | }
69 |
70 | static void ws_generate_mask(ws_header_t* header) {
71 | header->param.bit.MASK = 1;
72 | header->key.full = esp_random(); // generate a random 32 bit number
73 | }
74 |
75 | static void ws_encrypt_decrypt(char* msg,ws_header_t header) {
76 | if(header.param.bit.MASK) {
77 | for(uint64_t i=0; i> 8) & 0xFF;
129 | out[3] = (len ) & 0xFF;
130 | pos = 4;
131 | }
132 | if(header.param.bit.LEN == 127) {
133 | //memcpy(&out[2],&len,8);
134 | out[2] = (len >> 56) & 0xFF;
135 | out[3] = (len >> 48) & 0xFF;
136 | out[4] = (len >> 40) & 0xFF;
137 | out[5] = (len >> 32) & 0xFF;
138 | out[6] = (len >> 24) & 0xFF;
139 | out[7] = (len >> 16) & 0xFF;
140 | out[8] = (len >> 8) & 0xFF;
141 | out[9] = (len) & 0xFF;
142 | pos = 10;
143 | }
144 |
145 | if(mask) {
146 | out[pos] = header.key.part[0]; pos++;
147 | out[pos] = header.key.part[1]; pos++;
148 | out[pos] = header.key.part[2]; pos++;
149 | out[pos] = header.key.part[3]; pos++;
150 | memcpy(&out[pos],encrypt,len); // put in the encrypted message
151 | free(encrypt);
152 | }
153 | else {
154 | memcpy(&out[pos],msg,len);
155 | }
156 |
157 | ret = netconn_write(client->conn,out,true_len,NETCONN_COPY); // finally! send it.
158 | free(out); // free the entire message
159 | return ret;
160 | }
161 |
162 | char* ws_read(ws_client_t* client,ws_header_t* header) {
163 | char* ret;
164 | char* append;
165 | err_t err;
166 | struct netbuf* inbuf;
167 | struct netbuf* inbuf2;
168 | char* buf;
169 | char* buf2;
170 | uint16_t len;
171 | uint16_t len2;
172 | uint64_t pos;
173 | uint64_t cont_len;
174 | uint64_t cont_pos;
175 |
176 | // if we read from this previously (not cont frames), stop reading
177 | if(client->unfinished) {
178 | client->unfinished--;
179 | return NULL;
180 | }
181 |
182 | err = netconn_recv(client->conn,&inbuf);
183 | if(err != ERR_OK) return NULL;
184 | netbuf_data(inbuf,(void**)&buf, &len);
185 | if(!buf) return NULL;
186 |
187 | // get the header
188 | header->param.pos.ZERO = buf[0];
189 | header->param.pos.ONE = buf[1];
190 |
191 | // get the message length
192 | pos = 2;
193 | if(header->param.bit.LEN <= 125) {
194 | header->length = header->param.bit.LEN;
195 | }
196 | else if(header->param.bit.LEN == 126) {
197 | header->length = buf[2] << 8 | buf[3];
198 | pos = 4;
199 | }
200 | else { // LEN = 127
201 | header->length = (uint64_t)buf[2] << 56 | (uint64_t)buf[3] << 48
202 | | (uint64_t)buf[4] << 40 | (uint64_t)buf[5] << 32
203 | | (uint64_t)buf[6] << 24 | (uint64_t)buf[7] << 16
204 | | (uint64_t)buf[8] << 8 | (uint64_t)buf[9];
205 | pos = 10;
206 | }
207 |
208 | if(header->param.bit.MASK) {
209 | memcpy(&(header->key.full),&buf[pos],4); // extract the key
210 | pos += 4;
211 | }
212 |
213 | ret = malloc(header->length+1); // allocate memory, plus a byte
214 | if(!ret) {
215 | netbuf_delete(inbuf);
216 | header->received = 0;
217 | return NULL;
218 | }
219 |
220 | cont_len = len-pos; // get the actual length
221 | memcpy(ret,&buf[pos],header->length); // allocate the total memory
222 | cont_pos = cont_len; // get the initial position
223 | // netconn gives messages in pieces, so we need to get those (different than OPCODE_CONT)
224 | while(cont_len < header->length) { // while the actual length is less than the header stated
225 | err = netconn_recv(client->conn,&inbuf2);
226 | if(err != ERR_OK) {
227 | netbuf_delete(inbuf2);
228 | free(ret);
229 | client->unfinished = 0;
230 | header->received = 0;
231 | return NULL;
232 | }
233 | netbuf_data(inbuf2,(void**)&buf2, &len2);
234 | // Prevent catastrophic failure due to memory leakage
235 | if(cont_len + len2 > header->length) {
236 | netbuf_delete(inbuf2);
237 | free(ret);
238 | client->unfinished = 0;
239 | header->received = 0;
240 | return NULL;
241 | }
242 | memcpy(&ret[cont_pos],buf2,len2);
243 | cont_pos += len2;
244 | if(!buf2) {
245 | client->unfinished = 0;
246 | header->received = 0;
247 | }
248 | netbuf_delete(inbuf2);
249 | client->unfinished++;
250 | cont_len += len2;
251 | }
252 |
253 | ret[header->length] = '\0'; // end string
254 | ws_encrypt_decrypt(ret,*header); // unencrypt, if necessary
255 |
256 | if(header->param.bit.FIN == 0) { // if the message isn't done
257 | if((header->param.bit.OPCODE == WEBSOCKET_OPCODE_CONT) &&
258 | ((client->last_opcode==WEBSOCKET_OPCODE_BIN) || (client->last_opcode==WEBSOCKET_OPCODE_TEXT))) {
259 | cont_len = header->length + client->len;
260 | append = malloc(cont_len);
261 | memcpy(append,client->contin,client->len);
262 | memcpy(&append[client->len],ret,header->length);
263 | free(client->contin);
264 | client->contin = malloc(cont_len);
265 | client->len = cont_len;
266 |
267 | free(append);
268 | free(ret);
269 | netbuf_delete(inbuf);
270 | //free(buf);
271 | return NULL;
272 | }
273 | else if((header->param.bit.OPCODE==WEBSOCKET_OPCODE_BIN) || (header->param.bit.OPCODE==WEBSOCKET_OPCODE_TEXT)) {
274 | if(client->len) {
275 | free(client->contin);
276 | }
277 | client->contin = malloc(header->length);
278 | memcpy(client->contin,ret,header->length);
279 | client->len = header->length;
280 | client->last_opcode = header->param.bit.OPCODE;
281 |
282 | free(ret);
283 | netbuf_delete(inbuf);
284 | //free(buf);
285 | return NULL;
286 | }
287 | else { // there shouldn't be another FIN code....
288 | free(ret);
289 | netbuf_delete(inbuf);
290 | //free(buf);
291 | return NULL;
292 | }
293 | }
294 | client->last_opcode = header->param.bit.OPCODE;
295 | if(inbuf) netbuf_delete(inbuf);
296 | header->received = 1;
297 | return ret;
298 | }
299 |
300 | char* ws_hash_handshake(char* handshake,uint8_t len) {
301 | const char hash[] = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11";
302 | const uint8_t hash_len = sizeof(hash);
303 | char* ret;
304 | char key[64];
305 | unsigned char sha1sum[20];
306 | unsigned int ret_len;
307 |
308 | if(!len) return NULL;
309 | ret = malloc(32);
310 |
311 | memcpy(key,handshake,len);
312 | strlcpy(&key[len],hash,sizeof(key));
313 | mbedtls_sha1((unsigned char*)key,len+hash_len-1,sha1sum);
314 | mbedtls_base64_encode(NULL, 0, &ret_len, sha1sum, 20);
315 | if(!mbedtls_base64_encode((unsigned char*)ret,32,&ret_len,sha1sum,20)) {
316 | ret[ret_len] = '\0';
317 | return ret;
318 | }
319 | free(ret);
320 | return NULL;
321 | }
322 |
--------------------------------------------------------------------------------
/components/websocket/README.md:
--------------------------------------------------------------------------------
1 |
2 | By Blake Felt - blake.w.felt@gmail.com
3 |
4 | ESP32 WebSocket
5 | ==================
6 |
7 | A component for WebSockets on ESP-IDF using lwip netconn.
8 | For an example, see https://github.com/Molorius/ESP32-Examples.
9 |
10 | To add to a project, type:
11 | `git submodule add https://github.com/Molorius/esp32-websocket.git components/websocket`
12 | into the base directory of your project.
13 |
14 | Some configuration options for the Server can be found in menuconfig in:
15 | Component config ---> WebSocket Server
16 |
17 | This presently only has the WebSocket server code working, but client code will be added in the future (the groundwork is there).
18 |
19 | The code only allows one WebSocket server at a time, but this merely handles all incoming reads. New connections are added externally, so this can be used to hold various WebSocket connections.
20 |
21 | While this can theoretically handle very large messages, hardware constraints (RAM) limits the size of messages. I highly recommend not using more than 5000 bytes per message, but no constraint is in place for this.
22 |
23 | Any suggestions or fixes are gladly appreciated.
24 |
25 | Table of Contents
26 | =================
27 | * [Enumerations](#enumerations)
28 | * [WEBSOCKET_TYPE_t](#enum-websocket_type_t)
29 | * [Functions](#functions)
30 | * [ws_server_start](#int-ws_server_start)
31 | * [ws_server_stop](#int-ws_server_stop)
32 | * [ws_server_add_client](#int-ws_server_add_clientstruct-netconn-connchar-msguint16_t-lenchar-urlvoid-callback)
33 | * [ws_server_add_client_protocol](#int-ws_server_add_client_protocolstruct-netconn-connchar-msguint16_t-lenchar-urlchar-protocolvoid-callback)
34 | * [ws_server_len_url](#int-ws_server_len_urlchar-url)
35 | * [ws_server_len_all](#int-ws_server_len_all)
36 | * [ws_server_remove_client](#int-ws_server_remove_clientint-num)
37 | * [ws_server_remove_clients](#int-ws_server_remove_clientschar-url)
38 | * [ws_server_remove_all](#int-ws_server_remove_all)
39 | * [ws_server_send_text_client](#int-ws_server_send_text_clientint-numchar-msguint64_t-len)
40 | * [ws_server_send_text_clients](#int-ws_server_send_text_clientschar-urlchar-msguint64_t-len)
41 | * [ws_server_send_text_all](#int-ws_server_send_text_allchar-msguint64_t-len)
42 | * [ws_server_send_text_client_from_callback](#int-ws_server_send_text_client_from_callbackint-numchar-msguint64_t-len)
43 | * [ws_server_send_text_clients_from_callback](#int-ws_server_send_text_clients_from_callbackchar-urlchar-msguint64_t-len)
44 | * [ws_server_send_text_all_from_callback](#int-ws_server_send_text_all_from_callbackchar-msguint64_t-len)
45 | * [ws_server_send_bin_client_from_callback](#int-ws_server_send_bin_client_from_callbackint-numchar-msguint64_t-len)
46 | * [ws_server_send_bin_clients_from_callback](#int-ws_server_send_bin_clients_from_callbackchar-urlchar-msguint64_t-len)
47 | * [ws_server_send_bin_all_from_callback](#int-ws_server_send_bin_all_from_callbackchar-msguint64_t-len)
48 |
49 | Enumerations
50 | ============
51 |
52 | enum WEBSOCKET_TYPE_t
53 | ---------------------
54 |
55 | The different types of WebSocket events.
56 |
57 | *Values*
58 | * `WEBSOCKET_CONNECT`: A new client has successfully connected.
59 | * `WEBSOCKET_DISCONNECT_EXTERNAL`: The other side sent a disconnect message.
60 | * `WEBSOCKET_DISCONNECT_INTERNAL`: The esp32 server sent a disconnect message.
61 | * `WEBSOCKET_DISCONNECT_ERROR`: Disconnect due to a connection error.
62 | * `WEBSOCKET_TEXT`: Incoming text.
63 | * `WEBSOCKET_BIN`: Incoming binary.
64 | * `WEBSOCKET_PING`: The other side sent a ping message.
65 | * `WEBSOCKET_PONG`: The other side successfully replied to our ping.
66 |
67 | Functions
68 | =========
69 |
70 | int ws_server_start()
71 | ---------------------
72 |
73 | Starts the WebSocket Server. Use this function before attempting any
74 | sort of transmission or adding a client.
75 |
76 | *Returns*
77 | * 1: successful start
78 | * 0: server already running
79 |
80 | int ws_server_stop()
81 | --------------------
82 |
83 | Stops the WebSocket Server. New clients can still be added and
84 | messages can be sent, but new messages will not be received.
85 |
86 | *Returns*
87 | * 1: successful stop
88 | * 0: server was not running before
89 |
90 | int ws_server_add_client(struct netconn* conn,char* msg,uint16_t len,char* url,void *callback)
91 | ----------------------------------------------------------------------------------------------
92 |
93 | Adds a client to the WebSocket Server handler and performs the necessary handshake.
94 |
95 | *Parameters*
96 | * `conn`: the lwip netconn connection.
97 | * `msg`: the entire incoming request message to join the server. Necessary for the handshake.
98 | * `len`: the length of `msg`.
99 | * `url`: the NULL-terminated url. Used to keep track of clients, not required.
100 | * `callback`: the callback that is used to run WebSocket events. This must be with parameters(uint8_t num,WEBSOCKET_TYPE_t type,char* msg,uint64_t len) where "num" is the client number, "type" is the event type, "msg" is the incoming message, and "len" is the message length. The callback itself is optional.
101 |
102 | *Returns*
103 | * -2: not enough information in `msg` to perform handshake.
104 | * -1: server full, or connection issue.
105 | * 0 or greater: connection number
106 |
107 | int ws_server_add_client_protocol(struct netconn* conn,char* msg,uint16_t len,char* url,char* protocol,void *callback)
108 | ----------------------------------------------------------------------------------------------------------------------
109 |
110 | Adds a client to the WebSocket Server handler and performs the necessary handshake. Will also send
111 | the specified protocol.
112 |
113 | *Parameters*
114 | * `conn`: the lwip netconn connection.
115 | * `msg`: the entire incoming request message to join the server. Necessary for the handshake.
116 | * `len`: the length of `msg`.
117 | * `url`: the NULL-terminated url. Used to keep track of clients, not required.
118 | * `protocol`: the NULL-terminated protocol. This will be sent to the client in the header.
119 | * `callback`: the callback that is used to run WebSocket events. This must be with parameters(uint8_t num,WEBSOCKET_TYPE_t type,char* msg,uint64_t len) where "num" is the client number, "type" is the event type, "msg" is the incoming message, and "len" is the message length. The callback itself is optional.
120 |
121 | *Returns*
122 | * -2: not enough information in `msg` to perform handshake.
123 | * -1: server full, or connection issue.
124 | * 0 or greater: connection number
125 |
126 | int ws_server_len_url(char* url)
127 | --------------------------------
128 |
129 | Returns the number of clients connected to the specified URL.
130 |
131 | *Parameters*
132 | * `url`: the NULL-terminated string of the desired URL.
133 |
134 | *Returns*
135 | * The number of clients connected to the specified URL.
136 |
137 | int ws_server_len_all()
138 | -----------------------
139 |
140 | *Returns*
141 | * The number of connected clients.
142 |
143 | int ws_server_remove_client(int num)
144 | ------------------------------------
145 |
146 | Removes the desired client.
147 |
148 | *Parameters*
149 | * `num`: the client number
150 |
151 | *Returns*
152 | * 0: not a valid client number
153 | * 1: client disconnected
154 |
155 | int ws_server_remove_clients(char* url)
156 | ---------------------------------------
157 |
158 | Removes all clients connect to the desired URL.
159 |
160 | *Parameters*
161 | * `url`: the NULL-terminated URL.
162 |
163 | *Returns*
164 | * The number of clients that were disconnected.
165 |
166 | int ws_server_remove_all()
167 | --------------------------
168 |
169 | Removes all clients from server.
170 |
171 | *Returns*
172 | * The number of clients that were disconnected.
173 |
174 | int ws_server_send_text_client(int num,char* msg,uint64_t len)
175 | --------------------------------------------------------------
176 |
177 | Sends the desired message to the client.
178 |
179 | *Parameters*
180 | * `num`: the client's number.
181 | * `msg`: the desired message.
182 | * `len`: the length of the message.
183 |
184 | *Returns*
185 | * 0: message not sent properly
186 | * 1: message sent
187 |
188 | int ws_server_send_text_clients(char* url,char* msg,uint64_t len)
189 | -----------------------------------------------------------------
190 |
191 | Sends the message to clients connected to the desired URL.
192 |
193 | *Parameters*
194 | * `url`: the NULL-terminated URL.
195 | * `msg`: the desired message.
196 | * `len`: the length of the message.
197 |
198 | *Returns*
199 | * The number of clients that the message was sent to.
200 |
201 | int ws_server_send_text_all(char* msg,uint64_t len)
202 | ---------------------------------------------------
203 |
204 | Sends the message to all connected clients.
205 |
206 | *Parameters*
207 | * `msg`: the desired message
208 | * `len`: the length of the message
209 |
210 | *Returns*
211 | * The number of clients that the message was sent to.
212 |
213 | int ws_server_send_text_client_from_callback(int num,char* msg,uint64_t len)
214 | ----------------------------------------------------------------------------
215 |
216 | Sends the desired message to the client. Only use this inside the callback function.
217 |
218 | *Parameters*
219 | * `num`: the client's number.
220 | * `msg`: the desired message.
221 | * `len`: the length of the message.
222 |
223 | *Returns*
224 | * 0: message not sent properly
225 | * 1: message sent
226 |
227 | int ws_server_send_text_clients_from_callback(char* url,char* msg,uint64_t len)
228 | -------------------------------------------------------------------------------
229 |
230 | Sends the message to clients connected to the desired URL. Only use this inside the callback function.
231 |
232 | *Parameters*
233 | * `url`: the NULL-terminated URL.
234 | * `msg`: the desired message.
235 | * `len`: the length of the message.
236 |
237 | *Returns*
238 | * The number of clients that the message was sent to.
239 |
240 | int ws_server_send_text_all_from_callback(char* msg,uint64_t len)
241 | -----------------------------------------------------------------
242 |
243 | Sends the message to all connected clients. Only use this inside the callback function.
244 |
245 | *Parameters*
246 | * `msg`: the desired message
247 | * `len`: the length of the message
248 |
249 | *Returns*
250 | * The number of clients that the message was sent to.
251 |
252 | int ws_server_send_bin_client_from_callback(int num,char* msg,uint64_t len)
253 | ----------------------------------------------------------------------------
254 |
255 | Sends the desired binary message to the client. Only use this inside the callback function.
256 |
257 | *Parameters*
258 | * `num`: the client's number.
259 | * `msg`: the desired message.
260 | * `len`: the length of the message.
261 |
262 | *Returns*
263 | * 0: message not sent properly
264 | * 1: message sent
265 |
266 | int ws_server_send_bin_clients_from_callback(char* url,char* msg,uint64_t len)
267 | -------------------------------------------------------------------------------
268 |
269 | Sends the binary message to clients connected to the desired URL. Only use this inside the callback function.
270 |
271 | *Parameters*
272 | * `url`: the NULL-terminated URL.
273 | * `msg`: the desired message.
274 | * `len`: the length of the message.
275 |
276 | *Returns*
277 | * The number of clients that the message was sent to.
278 |
279 | int ws_server_send_bin_all_from_callback(char* msg,uint64_t len)
280 | -----------------------------------------------------------------
281 |
282 | Sends a binary message to all connected clients. Only use this inside the callback function.
283 |
284 | *Parameters*
285 | * `msg`: the desired message
286 | * `len`: the length of the message
287 |
288 | *Returns*
289 | * The number of clients that the message was sent to.
290 |
--------------------------------------------------------------------------------
/components/websocket/websocket_server.c:
--------------------------------------------------------------------------------
1 |
2 | /*
3 | esp32-websocket - a websocket component on esp-idf
4 | Copyright (C) 2019 Blake Felt - blake.w.felt@gmail.com
5 |
6 | This program is free software: you can redistribute it and/or modify
7 | it under the terms of the GNU General Public License as published by
8 | the Free Software Foundation, either version 3 of the License, or
9 | (at your option) any later version.
10 |
11 | This program is distributed in the hope that it will be useful,
12 | but WITHOUT ANY WARRANTY; without even the implied warranty of
13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 | GNU General Public License for more details.
15 |
16 | You should have received a copy of the GNU General Public License
17 | along with this program. If not, see .
18 | */
19 |
20 | #include "websocket_server.h"
21 | #include "freertos/FreeRTOS.h"
22 | #include "freertos/semphr.h"
23 | #include "freertos/task.h"
24 | #include "freertos/queue.h"
25 | #include
26 |
27 | static SemaphoreHandle_t xwebsocket_mutex; // to lock the client array
28 | static QueueHandle_t xwebsocket_queue; // to hold the clients that send messages
29 | static ws_client_t clients[WEBSOCKET_SERVER_MAX_CLIENTS]; // holds list of clients
30 | static TaskHandle_t xtask; // the task itself
31 |
32 | static void background_callback(struct netconn* conn, enum netconn_evt evt,u16_t len) {
33 | switch(evt) {
34 | case NETCONN_EVT_RCVPLUS:
35 | xQueueSendToBack(xwebsocket_queue,&conn,WEBSOCKET_SERVER_QUEUE_TIMEOUT);
36 | break;
37 | default:
38 | break;
39 | }
40 | }
41 |
42 | static void handle_read(uint8_t num) {
43 | ws_header_t header;
44 | char* msg;
45 |
46 | header.received = 0;
47 | msg = ws_read(&clients[num],&header);
48 |
49 | if(!header.received) {
50 | if(msg) free(msg);
51 | return;
52 | }
53 |
54 | switch(clients[num].last_opcode) {
55 | case WEBSOCKET_OPCODE_CONT:
56 | break;
57 | case WEBSOCKET_OPCODE_BIN:
58 | clients[num].scallback(num,WEBSOCKET_BIN,msg,header.length);
59 | break;
60 | case WEBSOCKET_OPCODE_TEXT:
61 | clients[num].scallback(num,WEBSOCKET_TEXT,msg,header.length);
62 | break;
63 | case WEBSOCKET_OPCODE_PING:
64 | ws_send(&clients[num],WEBSOCKET_OPCODE_PONG,msg,header.length,0);
65 | clients[num].scallback(num,WEBSOCKET_PING,msg,header.length);
66 | break;
67 | case WEBSOCKET_OPCODE_PONG:
68 | if(clients[num].ping) {
69 | clients[num].scallback(num,WEBSOCKET_PONG,NULL,0);
70 | clients[num].ping = 0;
71 | }
72 | break;
73 | case WEBSOCKET_OPCODE_CLOSE:
74 | clients[num].scallback(num,WEBSOCKET_DISCONNECT_EXTERNAL,NULL,0);
75 | ws_disconnect_client(&clients[num], 0);
76 | break;
77 | default:
78 | break;
79 | }
80 | if(msg) free(msg);
81 | }
82 |
83 | static void ws_server_task(void* pvParameters) {
84 | struct netconn* conn;
85 |
86 | xwebsocket_mutex = xSemaphoreCreateMutex();
87 | xwebsocket_queue = xQueueCreate(WEBSOCKET_SERVER_QUEUE_SIZE, sizeof(struct netconn*));
88 |
89 | // initialize all clients
90 | for(int i=0;icallback = background_callback;
201 | netconn_write(conn,handshake,strlen(handshake),NETCONN_COPY);
202 |
203 | for(int i=0;i
4 | // Based on Arduino's I2Cdev by Jeff Rowberg
5 | //
6 | // Changelog:
7 | // 2015-01-02 - Initial release
8 |
9 |
10 | /* ============================================
11 | I2Cdev device library code is placed under the MIT license
12 | Copyright (c) 2015 Jeff Rowberg, Nicolas Baldeck
13 |
14 | Permission is hereby granted, free of charge, to any person obtaining a copy
15 | of this software and associated documentation files (the "Software"), to deal
16 | in the Software without restriction, including without limitation the rights
17 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
18 | copies of the Software, and to permit persons to whom the Software is
19 | furnished to do so, subject to the following conditions:
20 |
21 | The above copyright notice and this permission notice shall be included in
22 | all copies or substantial portions of the Software.
23 |
24 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
25 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
26 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
27 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
28 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
29 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
30 | THE SOFTWARE.
31 | ===============================================
32 | */
33 |
34 | #include
35 | #include
36 | #include
37 | #include
38 | #include "sdkconfig.h"
39 |
40 | #include "I2Cdev.h"
41 |
42 | #define I2C_NUM I2C_NUM_0
43 |
44 | #undef ESP_ERROR_CHECK
45 | #define ESP_ERROR_CHECK(x) do { esp_err_t rc = (x); if (rc != ESP_OK) { ESP_LOGE("err", "esp_err_t = %d", rc); /*assert(0 && #x);*/} } while(0);
46 |
47 | /** Default constructor.
48 | */
49 | I2Cdev::I2Cdev() {
50 | }
51 |
52 | /** Initialize I2C0
53 | */
54 | void I2Cdev::initialize(int clkSpeed) {
55 | i2c_config_t conf;
56 | conf.mode = I2C_MODE_MASTER;
57 | conf.sda_io_num = (gpio_num_t)CONFIG_GPIO_SDA;
58 | conf.scl_io_num = (gpio_num_t)CONFIG_GPIO_SCL;
59 | conf.sda_pullup_en = GPIO_PULLUP_ENABLE;
60 | conf.scl_pullup_en = GPIO_PULLUP_ENABLE;
61 | //conf.master.clk_speed = 400000;
62 | conf.master.clk_speed = clkSpeed;
63 | conf.clk_flags = 0;
64 | ESP_ERROR_CHECK(i2c_param_config(I2C_NUM_0, &conf));
65 | ESP_ERROR_CHECK(i2c_driver_install(I2C_NUM_0, I2C_MODE_MASTER, 0, 0, 0));
66 | }
67 |
68 | /** Enable or disable I2C
69 | * @param isEnabled true = enable, false = disable
70 | */
71 | void I2Cdev::enable(bool isEnabled) {
72 |
73 | }
74 |
75 | /** Default timeout value for read operations.
76 | */
77 | uint16_t I2Cdev::readTimeout = I2CDEV_DEFAULT_READ_TIMEOUT;
78 | /** Read a single bit from an 8-bit device register.
79 | * @param devAddr I2C slave device address
80 | * @param regAddr Register regAddr to read from
81 | * @param bitNum Bit position to read (0-7)
82 | * @param data Container for single bit value
83 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
84 | * @return Status of read operation (true = success)
85 | */
86 | int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout, void *wireObj) {
87 | uint8_t b;
88 | uint8_t count = readByte(devAddr, regAddr, &b, timeout, wireObj);
89 | *data = b & (1 << bitNum);
90 | return count;
91 | }
92 |
93 | /** Read a single bit from a 16-bit device register.
94 | * @param devAddr I2C slave device address
95 | * @param regAddr Register regAddr to read from
96 | * @param bitNum Bit position to read (0-15)
97 | * @param data Container for single bit value
98 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
99 | * @return Status of read operation (true = success)
100 | */
101 | int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout, void *wireObj) {
102 | uint16_t b;
103 | uint8_t count = readWord(devAddr, regAddr, &b, timeout, wireObj);
104 | *data = b & (1 << bitNum);
105 | return count;
106 | }
107 |
108 | /** Read multiple bits from an 8-bit device register.
109 | * @param devAddr I2C slave device address
110 | * @param regAddr Register regAddr to read from
111 | * @param bitStart First bit position to read (0-7)
112 | * @param length Number of bits to read (not more than 8)
113 | * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
114 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
115 | * @return Status of read operation (true = success)
116 | */
117 | int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout, void *wireObj) {
118 | // 01101001 read byte
119 | // 76543210 bit numbers
120 | // xxx args: bitStart=4, length=3
121 | // 010 masked
122 | // -> 010 shifted
123 | uint8_t count, b;
124 | if ((count = readByte(devAddr, regAddr, &b, timeout, wireObj)) != 0) {
125 | uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
126 | b &= mask;
127 | b >>= (bitStart - length + 1);
128 | *data = b;
129 | }
130 | return count;
131 | }
132 |
133 | /** Read multiple bits from a 16-bit device register.
134 | * @param devAddr I2C slave device address
135 | * @param regAddr Register regAddr to read from
136 | * @param bitStart First bit position to read (0-15)
137 | * @param length Number of bits to read (not more than 16)
138 | * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
139 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
140 | * @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
141 | */
142 | int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout, void *wireObj) {
143 | // 1101011001101001 read byte
144 | // fedcba9876543210 bit numbers
145 | // xxx args: bitStart=12, length=3
146 | // 010 masked
147 | // -> 010 shifted
148 | uint8_t count;
149 | uint16_t w;
150 | if ((count = readWord(devAddr, regAddr, &w, timeout, wireObj)) != 0) {
151 | uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
152 | w &= mask;
153 | w >>= (bitStart - length + 1);
154 | *data = w;
155 | }
156 | return count;
157 | }
158 |
159 | /** Read single byte from an 8-bit device register.
160 | * @param devAddr I2C slave device address
161 | * @param regAddr Register regAddr to read from
162 | * @param data Container for byte value read from device
163 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
164 | * @return Status of read operation (true = success)
165 | */
166 | int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout, void *wireObj) {
167 | return readBytes(devAddr, regAddr, 1, data, timeout, wireObj);
168 | }
169 |
170 | /** Read multiple bytes from an 8-bit device register.
171 | * @param devAddr I2C slave device address
172 | * @param regAddr First register regAddr to read from
173 | * @param length Number of bytes to read
174 | * @param data Buffer to store read data in
175 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
176 | * @return I2C_TransferReturn_TypeDef http://downloads.energymicro.com/documentation/doxygen/group__I2C.html
177 | */
178 | int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout, void *wireObj) {
179 | i2c_cmd_handle_t cmd;
180 | SelectRegister(devAddr, regAddr);
181 |
182 | cmd = i2c_cmd_link_create();
183 | ESP_ERROR_CHECK(i2c_master_start(cmd));
184 | ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_READ, 1));
185 |
186 | if(length>1)
187 | ESP_ERROR_CHECK(i2c_master_read(cmd, data, length-1, I2C_MASTER_ACK));
188 |
189 | ESP_ERROR_CHECK(i2c_master_read_byte(cmd, data+length-1, I2C_MASTER_NACK));
190 |
191 | ESP_ERROR_CHECK(i2c_master_stop(cmd));
192 | ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS));
193 | i2c_cmd_link_delete(cmd);
194 |
195 | return length;
196 | }
197 |
198 | /** Write single word to a 16-bit device register.
199 | * @param devAddr I2C slave device address
200 | * @param regAddr Register address to write to
201 | * @param data New word value to write
202 | * @return Status of operation (true = success)
203 | */
204 | bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj){
205 | uint8_t data1[] = {(uint8_t)(data>>8), (uint8_t)(data & 0xff)};
206 | writeBytes(devAddr, regAddr, 2, data1, wireObj);
207 | return true;
208 | }
209 |
210 | /** Write multiple words to a 16-bit device register.
211 | * @param devAddr I2C slave device address
212 | * @param regAddr First register address to write to
213 | * @param length Number of words to write
214 | * @param data Buffer to copy new data from
215 | * @return Status of operation (true = success)
216 | */
217 | bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, void *wireObj) {
218 | for (int _index=0;_index>8), (uint8_t)(data[_index] & 0xff)};
221 | writeBytes(devAddr, _regAddr, 2, data1, wireObj);
222 | }
223 | return true;
224 | }
225 |
226 |
227 | void I2Cdev::SelectRegister(uint8_t dev, uint8_t reg){
228 | i2c_cmd_handle_t cmd;
229 |
230 | cmd = i2c_cmd_link_create();
231 | ESP_ERROR_CHECK(i2c_master_start(cmd));
232 | ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (dev << 1) | I2C_MASTER_WRITE, 1));
233 | ESP_ERROR_CHECK(i2c_master_write_byte(cmd, reg, 1));
234 | ESP_ERROR_CHECK(i2c_master_stop(cmd));
235 | ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS));
236 | i2c_cmd_link_delete(cmd);
237 | }
238 |
239 | /** write a single bit in an 8-bit device register.
240 | * @param devAddr I2C slave device address
241 | * @param regAddr Register regAddr to write to
242 | * @param bitNum Bit position to write (0-7)
243 | * @param value New bit value to write
244 | * @return Status of operation (true = success)
245 | */
246 | bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj) {
247 | uint8_t b;
248 | readByte(devAddr, regAddr, &b, I2Cdev::readTimeout, wireObj);
249 | b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
250 | return writeByte(devAddr, regAddr, b, wireObj);
251 | }
252 |
253 | /** write a single bit in a 16-bit device register.
254 | * @param devAddr I2C slave device address
255 | * @param regAddr Register regAddr to write to
256 | * @param bitNum Bit position to write (0-15)
257 | * @param value New bit value to write
258 | * @return Status of operation (true = success)
259 | */
260 | bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data, void *wireObj) {
261 | uint16_t w;
262 | readWord(devAddr, regAddr, &w, I2Cdev::readTimeout, wireObj);
263 | w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
264 | return writeWord(devAddr, regAddr, w, wireObj);
265 | }
266 |
267 |
268 | /** Write multiple bits in an 8-bit device register.
269 | * @param devAddr I2C slave device address
270 | * @param regAddr Register regAddr to write to
271 | * @param bitStart First bit position to write (0-7)
272 | * @param length Number of bits to write (not more than 8)
273 | * @param data Right-aligned value to write
274 | * @return Status of operation (true = success)
275 | */
276 | bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj) {
277 | // 010 value to write
278 | // 76543210 bit numbers
279 | // xxx args: bitStart=4, length=3
280 | // 00011100 mask byte
281 | // 10101111 original value (sample)
282 | // 10100011 original & ~mask
283 | // 10101011 masked | value
284 | uint8_t b = 0;
285 | if (readByte(devAddr, regAddr, &b, I2Cdev::readTimeout, wireObj) != 0) {
286 | uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
287 | data <<= (bitStart - length + 1); // shift data into correct position
288 | data &= mask; // zero all non-important bits in data
289 | b &= ~(mask); // zero all important bits in existing byte
290 | b |= data; // combine data with existing byte
291 | return writeByte(devAddr, regAddr, b, wireObj);
292 | } else {
293 | return false;
294 | }
295 | }
296 |
297 | /** Write multiple bits in a 16-bit device register.
298 | * @param devAddr I2C slave device address
299 | * @param regAddr Register regAddr to write to
300 | * @param bitStart First bit position to write (0-15)
301 | * @param length Number of bits to write (not more than 16)
302 | * @param data Right-aligned value to write
303 | * @return Status of operation (true = success)
304 | */
305 | bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data, void *wireObj) {
306 | // 010 value to write
307 | // fedcba9876543210 bit numbers
308 | // xxx args: bitStart=12, length=3
309 | // 0001110000000000 mask word
310 | // 1010111110010110 original value (sample)
311 | // 1010001110010110 original & ~mask
312 | // 1010101110010110 masked | value
313 | uint16_t w;
314 | if (readWord(devAddr, regAddr, &w, I2Cdev::readTimeout, wireObj) != 0) {
315 | uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
316 | data <<= (bitStart - length + 1); // shift data into correct position
317 | data &= mask; // zero all non-important bits in data
318 | w &= ~(mask); // zero all important bits in existing word
319 | w |= data; // combine data with existing word
320 | return writeWord(devAddr, regAddr, w, wireObj);
321 | } else {
322 | return false;
323 | }
324 | }
325 |
326 | /** Write single byte to an 8-bit device register.
327 | * @param devAddr I2C slave device address
328 | * @param regAddr Register address to write to
329 | * @param data New byte value to write
330 | * @return Status of operation (true = success)
331 | */
332 | bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj) {
333 | i2c_cmd_handle_t cmd;
334 |
335 | cmd = i2c_cmd_link_create();
336 | ESP_ERROR_CHECK(i2c_master_start(cmd));
337 | ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_WRITE, 1));
338 | ESP_ERROR_CHECK(i2c_master_write_byte(cmd, regAddr, 1));
339 | ESP_ERROR_CHECK(i2c_master_write_byte(cmd, data, 1));
340 | ESP_ERROR_CHECK(i2c_master_stop(cmd));
341 | ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS));
342 | i2c_cmd_link_delete(cmd);
343 |
344 | return true;
345 | }
346 |
347 | /** Write single byte to an 8-bit device register.
348 | * @param devAddr I2C slave device address
349 | * @param regAddr Register address to write to
350 | * @param length Number of bytes to write
351 | * @param data Array of bytes to write
352 | * @return Status of operation (true = success)
353 | */
354 | bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, void *wireObj){
355 | i2c_cmd_handle_t cmd;
356 |
357 | cmd = i2c_cmd_link_create();
358 | ESP_ERROR_CHECK(i2c_master_start(cmd));
359 | ESP_ERROR_CHECK(i2c_master_write_byte(cmd, (devAddr << 1) | I2C_MASTER_WRITE, 1));
360 | ESP_ERROR_CHECK(i2c_master_write_byte(cmd, regAddr, 1));
361 | ESP_ERROR_CHECK(i2c_master_write(cmd, data, length-1, 0));
362 | ESP_ERROR_CHECK(i2c_master_write_byte(cmd, data[length-1], 1));
363 | ESP_ERROR_CHECK(i2c_master_stop(cmd));
364 | ESP_ERROR_CHECK(i2c_master_cmd_begin(I2C_NUM, cmd, 1000/portTICK_PERIOD_MS));
365 | i2c_cmd_link_delete(cmd);
366 | return true;
367 | }
368 |
369 |
370 | /** Read single word from a 16-bit device register.
371 | * @param devAddr I2C slave device address
372 | * @param regAddr Register regAddr to read from
373 | * @param data Container for word value read from device
374 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
375 | * @return Status of read operation (true = success)
376 | */
377 | int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout, void *wireObj){
378 | uint8_t msb[2] = {0,0};
379 | readBytes(devAddr, regAddr, 2, msb, timeout, wireObj);
380 | *data = (int16_t)((msb[0] << 8) | msb[1]);
381 | return 0;
382 | }
383 |
384 | /** Read multiple words from a 16-bit device register.
385 | * @param devAddr I2C slave device address
386 | * @param regAddr First register regAddr to read from
387 | * @param length Number of words to read
388 | * @param data Buffer to store read data in
389 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
390 | * @return Number of words read (-1 indicates failure)
391 | */
392 | int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout, void *wireObj) {
393 | uint8_t msb[2] = {0,0};
394 | for (int _index=0;_index