Darkages/SpotMicroESP32-Leika
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

🪄 Major refactoring to use ESP32 template

Browse Source 
This includes the change to use https://github.com/runeharlyk/ESP32-rapid-development-template.git as a freeRTos template
This commit is contained in:
Rune Harlyk
2023-08-02 22:26:09 +02:00
committed by Rune Daugaard Harlyk
parent 4c7b8954eb
commit 4282055be0
42 changed files with 2169 additions and 1065 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/AsyncJpegStream.cpp
+128
View File
@@ -0,0 +1,128 @@
#include <AsyncJpegStream.h>
static const char* STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
static const char* STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
static const char* STREAM_PART = "Content-Type: %s\r\nContent-Length: %u\r\n\r\n";
static const char * JPG_CONTENT_TYPE = "image/jpeg";
AsyncJpegStreamResponse::AsyncJpegStreamResponse(){
_callback = nullptr;
_code = 200;
_contentLength = 0;
_contentType = STREAM_CONTENT_TYPE;
_sendContentLength = false;
_chunked = true;
_index = 0;
_jpg_buf_len = 0;
_jpg_buf = NULL;
lastAsyncRequest = 0;
memset(&_frame, 0, sizeof(camera_frame_t));
}
AsyncJpegStreamResponse::~AsyncJpegStreamResponse(){
if(_frame.fb){
if(_frame.fb->format != PIXFORMAT_JPEG){
free(_jpg_buf);
}
esp_camera_fb_return(_frame.fb);
}
}
/*bool AsyncJpegStreamResponse::_sourceValid() {
return true;
}*/
size_t AsyncJpegStreamResponse::_fillBuffer(uint8_t *buf, size_t maxLen) {
size_t ret = _content(buf, maxLen, _index);
if(ret != RESPONSE_TRY_AGAIN){
_index += ret;
}
return ret;
}
size_t AsyncJpegStreamResponse::_content(uint8_t *buffer, size_t maxLen, size_t index){
if(!_frame.fb || _frame.index == _jpg_buf_len) {
if(index && _frame.fb){
uint64_t end = (uint64_t)micros();
int fp = (end - lastAsyncRequest) / 1000;
log_printf("Size: %uKB, Time: %ums (%.1ffps)\n", _jpg_buf_len/1024, fp);
lastAsyncRequest = end;
if(_frame.fb->format != PIXFORMAT_JPEG){
free(_jpg_buf);
}
esp_camera_fb_return(_frame.fb);
_frame.fb = NULL;
_jpg_buf_len = 0;
_jpg_buf = NULL;
}
if(maxLen < (strlen(STREAM_BOUNDARY) + strlen(STREAM_PART) + strlen(JPG_CONTENT_TYPE) + 8)){
//log_w("Not enough space for headers");
return RESPONSE_TRY_AGAIN;
}
//get frame
_frame.index = 0;
_frame.fb = esp_camera_fb_get();
if (_frame.fb == NULL) {
log_e("Camera frame failed");
return 0;
}
if(_frame.fb->format != PIXFORMAT_JPEG){
unsigned long st = millis();
bool jpeg_converted = frame2jpg(_frame.fb, 80, &_jpg_buf, &_jpg_buf_len);
if(!jpeg_converted){
log_e("JPEG compression failed");
esp_camera_fb_return(_frame.fb);
_frame.fb = NULL;
_jpg_buf_len = 0;
_jpg_buf = NULL;
return 0;
}
log_i("JPEG: %lums, %uB", millis() - st, _jpg_buf_len);
} else {
_jpg_buf_len = _frame.fb->len;
_jpg_buf = _frame.fb->buf;
}
//send boundary
size_t blen = 0;
if(index){
blen = strlen(STREAM_BOUNDARY);
memcpy(buffer, STREAM_BOUNDARY, blen);
buffer += blen;
}
//send header
size_t hlen = sprintf((char *)buffer, STREAM_PART, JPG_CONTENT_TYPE, _jpg_buf_len);
buffer += hlen;
//send frame
hlen = maxLen - hlen - blen;
if(hlen > _jpg_buf_len){
maxLen -= hlen - _jpg_buf_len;
hlen = _jpg_buf_len;
}
memcpy(buffer, _jpg_buf, hlen);
_frame.index += hlen;
return maxLen;
}
size_t available = _jpg_buf_len - _frame.index;
if(maxLen > available){
maxLen = available;
}
memcpy(buffer, _jpg_buf+_frame.index, maxLen);
_frame.index += maxLen;
return maxLen;
}
void streamJpg(AsyncWebServerRequest *request){
AsyncJpegStreamResponse *response = new AsyncJpegStreamResponse();
if(!response){
request->send(501);
return;
}
request->send(response);
}
src/camera.cpp
+43
View File
@@ -0,0 +1,43 @@
#include <servo.h>
#include <camera_pins.h>
#include <esp_camera.h>
esp_err_t InitializeCamera(){
camera_config_t camera_config;
camera_config.ledc_channel = LEDC_CHANNEL_0;
camera_config.ledc_timer = LEDC_TIMER_0;
camera_config.pin_d0 = Y2_GPIO_NUM;
camera_config.pin_d1 = Y3_GPIO_NUM;
camera_config.pin_d2 = Y4_GPIO_NUM;
camera_config.pin_d3 = Y5_GPIO_NUM;
camera_config.pin_d4 = Y6_GPIO_NUM;
camera_config.pin_d5 = Y7_GPIO_NUM;
camera_config.pin_d6 = Y8_GPIO_NUM;
camera_config.pin_d7 = Y9_GPIO_NUM;
camera_config.pin_xclk = XCLK_GPIO_NUM;
camera_config.pin_pclk = PCLK_GPIO_NUM;
camera_config.pin_vsync = VSYNC_GPIO_NUM;
camera_config.pin_href = HREF_GPIO_NUM;
camera_config.pin_sscb_sda = SIOD_GPIO_NUM;
camera_config.pin_sscb_scl = SIOC_GPIO_NUM;
camera_config.pin_pwdn = PWDN_GPIO_NUM;
camera_config.pin_reset = RESET_GPIO_NUM;
camera_config.xclk_freq_hz = 20000000;
camera_config.pixel_format = PIXFORMAT_JPEG;
if(psramFound()){
camera_config.frame_size = FRAMESIZE_SVGA;
camera_config.jpeg_quality = 10;
camera_config.fb_count = 2;
} else {
camera_config.frame_size = FRAMESIZE_SVGA;
camera_config.jpeg_quality = 12;
camera_config.fb_count = 1;
}
log_i("Initializing camera");
esp_err_t err = esp_camera_init(&camera_config);
if (err != ESP_OK) log_e("Camera probe failed with error 0x%x", err);
return err;
}
src/deviceconfig.cpp
+55
View File
@@ -0,0 +1,55 @@
#include <HTTPClient.h>
#include <UrlEncode.h>
#include <globals.h>
#include <deviceconfig.h>
#include <secrets.h>
DRAM_ATTR std::unique_ptr<DeviceConfig> g_ptrDeviceConfig;
DRAM_ATTR size_t g_DeviceConfigJSONBufferSize = 0;
void DeviceConfig::SaveToJSON()
{
g_ptrJSONWriter->FlagWriter(writerIndex);
}
DeviceConfig::DeviceConfig()
{
// Add SettingSpec for additional settings to this list
settingSpecs.emplace_back(
NAME_OF(ntpServer),
"NTP server address",
"The hostname or IP address of the NTP server to be used for time synchronization.",
SettingSpec::SettingType::String
);
settingSpecs.emplace_back(
NAME_OF(useMetric),
"Use metric system",
"A boolean that indicates if unit should be shown in metric ('true'/1) or imperial ('false'/0) format.",
SettingSpec::SettingType::Boolean
);
log_i("about to write");
writerIndex = g_ptrJSONWriter->RegisterWriter(
[this]() { SaveToJSONFile(DEVICE_CONFIG_FILE, g_DeviceConfigJSONBufferSize, *this); }
);
std::unique_ptr<AllocatedJsonDocument> pJsonDoc(nullptr);
if (LoadJSONFile(DEVICE_CONFIG_FILE, g_DeviceConfigJSONBufferSize, pJsonDoc))
{
log_i("Loading DeviceConfig from JSON");
DeserializeFromJSON(pJsonDoc->as<JsonObjectConst>(), true);
}
else
{
log_w("DeviceConfig could not be loaded from JSON, using defaults");
// Set default for additional settings in this code
ntpServer = DEFAULT_NTP_SERVER;
SaveToJSON();
}
}
src/ik_task.cpp
-139
View File
@@ -1,139 +0,0 @@
#include <ik_task.h>
static const char* TAG = "IK TASK";
position_t spot_position = {.omega=0,.phi=0,.psi=0,.xm=-40,.ym=-170, .zm=0, .set=1};
position_t goal_position = {0,};
int16_t servo_angles[4][3] = {{90, 150, 0}, {90, 30, 180}, {90, 150, 0}, {90, 30, 180}};
int16_t servo_angles_goal[4][3] = {0,};
void set_orientation_cb(int16_t omega, int16_t phi, int16_t psi, int16_t xm, int16_t ym, int16_t zm) {
goal_position.omega = omega;
goal_position.phi = phi;
goal_position.psi = psi;
goal_position.xm = xm;
goal_position.ym = ym;
goal_position.zm = zm;
goal_position.set = true;
}
void reset_position() {
set_orientation_cb(0, 0, 0, 0, 0, 0);
}
void set_leg_servos() {
for (int l = 0; l<4; l++) {
// for (int l = 1; l<2; l++) {
for (int s=0;s<3;s++) {
if (servo_angles[l][s] != servo_angles_goal[l][s]) {
servo_angles[l][s] = servo_angles_goal[l][s];
set_servo(l*3 + s, servo_angles[l][s]);
}
}
}
}
void iterate_to_position() {
ESP_LOGI(TAG, "GOAL (%f,%f,%f - %f,%f,%f)", goal_position.omega, goal_position.phi, goal_position.psi, goal_position.xm, goal_position.ym, goal_position.zm);
do {
spot_position.set = false;
int diff = 0;
if (goal_position.omega != spot_position.omega) {
diff = goal_position.omega - spot_position.omega;
if (abs(diff) < MOTION_STEP_ANGLE) {
spot_position.omega = goal_position.omega ;
} else {
diff = diff < 0 ? -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
spot_position.omega += diff;
spot_position.set = true;
}
}
if (goal_position.phi != spot_position.phi) {
diff = goal_position.phi - spot_position.phi;
if (abs(diff) < MOTION_STEP_ANGLE) {
spot_position.phi = goal_position.phi ;
} else {
diff = diff < 0 ? -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
spot_position.phi += diff;
spot_position.set = true;
}
}
if (goal_position.psi != spot_position.psi) {
diff = goal_position.psi - spot_position.psi;
if (abs(diff) < MOTION_STEP_ANGLE) {
spot_position.psi = goal_position.psi ;
} else {
diff = diff < 0 ? -MOTION_STEP_ANGLE : MOTION_STEP_ANGLE;
spot_position.psi += diff;
spot_position.set = true;
}
}
if (goal_position.xm != spot_position.xm) {
diff = goal_position.xm - spot_position.xm;
if (abs(diff) < MOTION_STEP_MOVEMENT) {
spot_position.xm = goal_position.xm ;
} else {
diff = diff < 0 ? -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
spot_position.xm += diff;
spot_position.set = true;
}
}
if (goal_position.ym != spot_position.ym) {
diff = goal_position.ym - spot_position.ym;
if (abs(diff) < MOTION_STEP_MOVEMENT) {
spot_position.ym = goal_position.ym ;
} else {
diff = diff < 0 ? -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
spot_position.ym += diff;
spot_position.set = true;
}
}
if (goal_position.zm != spot_position.zm) {
diff = goal_position.zm - spot_position.zm;
if (abs(diff) < MOTION_STEP_MOVEMENT) {
spot_position.zm = goal_position.zm ;
} else {
diff = diff < 0 ? -MOTION_STEP_MOVEMENT : MOTION_STEP_MOVEMENT;
spot_position.zm += diff;
spot_position.set = true;
}
}
ESP_LOGI(TAG, "CURRENT (%f,%f,%f - %f,%f,%f) %d", spot_position.omega, spot_position.phi, spot_position.psi, spot_position.xm, spot_position.ym, spot_position.zm, spot_position.set);
esp_err_t ret = spot_IK(spot_position.omega*DEGREES2RAD, spot_position.phi*DEGREES2RAD, spot_position.psi*DEGREES2RAD, spot_position.xm, spot_position.ym, spot_position.zm, servo_angles_goal);
ESP_LOGD(TAG, "Valid IK %d", ret==ESP_OK);
if (ret == ESP_OK) {
//print_int_matrix((int16_t*) servo_angles_goal, 4, 3, "servo_angles_goal", false);
set_leg_servos();
}
} while (spot_position.set);
//set_new_orientation_act_value((int16_t) spot_position.omega, (int16_t) spot_position.phi, (int16_t) spot_position.psi, (int16_t) spot_position.xm, (int16_t) spot_position.ym, (int16_t) spot_position.zm);
}
void task_ik(void *ignore){
ESP_LOGI(TAG, "Executing on core %d", xPortGetCoreID());
esp_err_t ret;
reset_position();
for(;;) {
vTaskDelay(100 / portTICK_RATE_MS);
if (goal_position.set) {
goal_position.set = false;
iterate_to_position();
}
}
vTaskDelete(NULL);
}
src/jsonserializer.cpp
+177
View File
@@ -0,0 +1,177 @@
#include <globals.h>
#include <SPIFFS.h>
#include <jsonserializer.h>
#include <taskmanager.h>
DRAM_ATTR std::unique_ptr<JSONWriter> g_ptrJSONWriter = nullptr;
bool LoadJSONFile(const char *fileName, size_t& bufferSize, std::unique_ptr<AllocatedJsonDocument>& pJsonDoc)
{
bool jsonReadSuccessful = false;
File file = SPIFFS.open(fileName);
if (file)
{
if (file.size() > 0)
{
log_i("Attempting to read JSON file %s", fileName);
if (bufferSize == 0)
bufferSize = std::max((size_t)JSON_BUFFER_BASE_SIZE, file.size());
// Loop is here to deal with out of memory conditions
while(true)
{
pJsonDoc.reset(new AllocatedJsonDocument(bufferSize));
DeserializationError error = deserializeJson(*pJsonDoc, file);
if (error == DeserializationError::NoMemory)
{
pJsonDoc.reset(nullptr);
file.seek(0);
bufferSize += JSON_BUFFER_INCREMENT;
log_w("Out of memory reading JSON from file %s - increasing buffer to %zu bytes", fileName, bufferSize);
}
else if (error == DeserializationError::Ok)
{
jsonReadSuccessful = true;
break;
}
else
{
log_w("Error with code %d occurred while deserializing JSON from file %s", to_value(error.code()), fileName);
break;
}
}
}
file.close();
}
return jsonReadSuccessful;
}
void SerializeWithBufferSize(std::unique_ptr<AllocatedJsonDocument>& pJsonDoc, size_t& bufferSize, std::function<bool(JsonObject&)> serializationFunction)
{
// Loop is here to deal with out of memory conditions
while(true)
{
pJsonDoc.reset(new AllocatedJsonDocument(bufferSize));
JsonObject jsonObject = pJsonDoc->to<JsonObject>();
if (serializationFunction(jsonObject))
break;
pJsonDoc.reset(nullptr);
bufferSize += JSON_BUFFER_INCREMENT;
log_w("Out of memory serializing object - increasing buffer to %zu bytes", bufferSize);
}
}
bool SaveToJSONFile(const char *fileName, size_t& bufferSize, IJSONSerializable& object)
{
if (bufferSize == 0)
bufferSize = JSON_BUFFER_BASE_SIZE;
std::unique_ptr<AllocatedJsonDocument> pJsonDoc(nullptr);
SerializeWithBufferSize(pJsonDoc, bufferSize, [&object](JsonObject& jsonObject) { return object.SerializeToJSON(jsonObject); });
SPIFFS.remove(fileName);
File file = SPIFFS.open(fileName, FILE_WRITE);
if (!file)
{
log_e("Unable to open file %s to write JSON!", fileName);
return false;
}
size_t bytesWritten = serializeJson(*pJsonDoc, file);
log_i("Number of bytes written to JSON file %s: %d", fileName, bytesWritten);
file.flush();
file.close();
if (bytesWritten == 0)
{
log_e("Unable to write JSON to file %s!", fileName);
SPIFFS.remove(fileName);
return false;
}
/*
file = SPIFFS.open(fileName);
if (file)
{
while (file.available())
Serial.write(file.read());
file.close();
}
*/
return true;
}
bool RemoveJSONFile(const char *fileName)
{
return SPIFFS.remove(fileName);
}
size_t JSONWriter::RegisterWriter(std::function<void()> writer)
{
// Add the writer with its flag unset
writers.emplace_back(writer);
return writers.size() - 1;
}
void JSONWriter::FlagWriter(size_t index)
{
// Check if we received a valid writer index
if (index >= writers.size())
return;
writers[index].flag = true;
latestFlagMs = millis();
g_TaskManager.NotifyJSONWriterThread();
}
// JSONWriterTaskEntry
//
// Invoke functions that write serialized JSON objects to SPIFFS at request, with some delay
void IRAM_ATTR JSONWriterTaskEntry(void *)
{
for(;;)
{
TickType_t notifyWait = portMAX_DELAY;
for (;;)
{
// Wait until we're woken up by a writer being flagged, or until we've reached the hold point
ulTaskNotifyTake(pdTRUE, notifyWait);
if (!g_ptrJSONWriter)
continue;
unsigned long holdUntil = g_ptrJSONWriter->latestFlagMs + JSON_WRITER_DELAY;
unsigned long now = millis();
if (now >= holdUntil)
break;
notifyWait = pdMS_TO_TICKS(holdUntil - now);
}
for (auto &entry : g_ptrJSONWriter->writers)
{
// Unset flag before we do the actual write. This makes that we don't miss another flag raise if it happens while writing
if (entry.flag.exchange(false))
entry.writer();
}
}
}
src/main.cpp
+27 -6
View File
@@ -1,11 +1,32 @@
#include <spot.h>
#include <globals.h>
#include <deviceconfig.h>
Spot spot;
void setup(){
spot.boot();
ESPTaskManager g_TaskManager;
#if USE_WIFI && USE_WEBSERVER
DRAM_ATTR CWebServer g_WebServer;
#endif
void setup() {
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
Serial.begin(BAUDRATE);
log_i("Booting");
SPIFFS.begin();
Wire.begin(SDA, SCL);
InitializeCamera();
g_TaskManager.begin();
g_TaskManager.StartThreads();
g_ptrJSONWriter = std::make_unique<JSONWriter>();
g_ptrDeviceConfig = std::make_unique<DeviceConfig>();
g_ptrServo = std::make_unique<Servo>();
g_ptrServo->begin();
g_ptrServo->setOscillatorFrequency(SERVO_OSCILLATOR_FREQUENCY);
g_ptrServo->setPWMFreq(SERVO_FREQ);
g_ptrServo->updateServos();
}
void loop(){
spot.handle();
void loop() {
delay(200);
}
src/movement.cpp
+79
View File
@@ -0,0 +1,79 @@
#include <featureflags.h>
#if USE_MPU
#include <MPU6050_light.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_HMC5883_U.h>
#endif
#if USE_POWER_BUTTON
#include <Bounce2.h>
#endif
#if USE_POWER_BUTTON
Bounce2::Button PowerButton;
#endif
#if USE_MPU
DRAM_ATTR MPU6050 g_imu(Wire);
DRAM_ATTR Adafruit_HMC5883_Unified g_mag = Adafruit_HMC5883_Unified(12345);
#endif
void beginMag() {
if(!g_mag.begin()) log_w("Failed to initialize HMC5883L");
}
void beginImu() {
byte status = g_imu.begin();
if(status != 0) log_w("MPU initialize failed");
delay(100);
g_imu.calcOffsets(true,true);
}
float getHeading() {
sensors_event_t event;
g_mag.getEvent(&event);
float heading = atan2(event.magnetic.y, event.magnetic.x);
float declinationAngle = 0.22;
heading += declinationAngle;
if(heading < 0) heading += 2*PI;
if(heading > 2*PI) heading -= 2*PI;
return heading * 180/M_PI;
}
float getTemp() {
return g_imu.getTemp();
}
float getAngleX() {
return g_imu.getAngleX();
}
float getAngleY() {
return g_imu.getAngleX();
}
float getAngleZ() {
return g_imu.getAngleZ();
}
void IRAM_ATTR MovementHandlingLoopEntry(void *) {
TickType_t notifyWait = 0;
beginMag();
beginImu();
for (;;) {
g_imu.update();
ulTaskNotifyTake(pdTRUE, notifyWait);
// #if USE_POWER_BUTTON
// PowerButton.update();
// if (PowerButton.pressed()) {
// log_i("Power Button Pressed");
// g_ptrServo->toggleState();
// }
// #endif
notifyWait = pdMS_TO_TICKS(100);
}
}
src/network.cpp
+96
View File
@@ -0,0 +1,96 @@
#include <globals.h>
#include <deviceconfig.h>
#if USE_WIFI && USE_WEBSERVER
extern DRAM_ATTR CWebServer g_WebServer;
#endif
#if USE_WIFI
bool ConnectToWiFi(uint cRetries) {
static bool bPreviousConnection = false;
if (WiFi.isConnected()) return true;
log_i("Connection to wifi");
WiFi.disconnect();
WiFi.mode(WIFI_STA);
WiFi.setHostname(HOSTNAME);
for (uint iPass = 0; iPass < cRetries; iPass++) {
log_i("Pass %u of %u: Connecting to Wifi SSID: \"%s\" - ESP32 Free Memory: %u, PSRAM:%u, PSRAM Free: %u\n",
iPass + 1, cRetries, SSID, ESP.getFreeHeap(), ESP.getPsramSize(), ESP.getFreePsram());
WiFi.begin(SSID, PASS);
delay(4000 + iPass * 1000);
if (WiFi.isConnected()) {
log_i("Connected to AP with BSSID: %s\n", WiFi.BSSIDstr().c_str());
break;
}
}
// Additional Services onwwards reliant on network so close if not up.
if (false == WiFi.isConnected()) {
log_i("Giving up on WiFi\n");
return false;
}
log_i("Received IP: %s", WiFi.localIP().toString().c_str());
// If we were connected before, network-dependent services will have been started already
if (bPreviousConnection)
return true;
#if USE_OTA
//debugI("Publishing OTA...");
SetupOTA(String(cszHostname));
#endif
#if USE_NTP
//debugI("Setting Clock...");
//NTPTimeClient::UpdateClockFromWeb(&g_Udp);
#endif
#if USE_WEBSERVER
g_WebServer.begin();
#endif
#if USE_MDNS
if(MDNS.begin(HOSTNAME)){
MDNS.addService("http", "tcp", HTTP_PORT);
}
#endif
bPreviousConnection = true;
return true;
}
void IRAM_ATTR NetworkHandlingLoopEntry(void *) {
unsigned long lastWifiCheck = 0;
unsigned long checkWiFiEveryMs = 1000;
TickType_t notifyWait = 0;
for (;;) {
ulTaskNotifyTake(pdTRUE, notifyWait);
if ( millis() - lastWifiCheck > checkWiFiEveryMs) {
if (WiFi.isConnected() == false && ConnectToWiFi(5) == false) {
log_e("Cannot Connect to Wifi!");
#if WAIT_FOR_WIFI
log_e("Rebooting in 5 seconds due to no Wifi available.");
delay(5000);
throw new std::runtime_error("Rebooting due to no Wifi available.");
#endif
}
}
#if USE_WEBSERVER
g_WebServer.loop();
#endif
notifyWait = pdMS_TO_TICKS(1000);
}
}
#endif // ENABLE_WIFI
src/servo.cpp
+2 -37
View File
@@ -1,38 +1,3 @@
#include <Adafruit_PWMServoDriver.h>
#include <servo.h>
#include "servo.h"
#include "config.h"
#include "servo_config.h"
static const char* TAG = "SERVO";
Adafruit_PWMServoDriver pwm = Adafruit_PWMServoDriver(0x40);
const int16_t servo_min[12] = {153,118,138,121,116,125,131,150,148,130,158,165};
const float servo_conversion[12] = {2.011111,2.011111,2.000000,2.050000,1.966667,2.027778,2.038889,1.677778,1.622222,2.027778,1.927778,1.650000};
const int8_t servo_invert[12] = {1,0,1, 0,1,0, 0,0,1, 1,1,0};
const float theta_range[3][2] = {{-M_PI / 3, M_PI/3}, {-2 * M_PI/3, M_PI/3}, {0, M_PI}};
esp_err_t disable_servos(){
ESP_LOGI(TAG, "Disabling servos");
pwm.setPWM(0, 0, 0);
return ESP_OK;
}
esp_err_t setup_pwm_controller(){
pwm.begin();
pwm.setOscillatorFrequency(SERVO_OSCILLATOR_FREQUENCY);
pwm.setPWMFreq(SERVO_FREQ);
return ESP_OK;
}
esp_err_t set_servo(uint8_t id, uint16_t angle) {
esp_err_t ret;
uint16_t pulse = (uint16_t) (0.5 + servo_min[id] + (angle * servo_conversion[id]));
ESP_LOGI(TAG, "setPWM of servo %d, %d degrees -> Pulse %d", id, angle, pulse);
ret = pwm.setPWM(id, 0, pulse);
if (ret == ESP_OK) return ESP_OK;
else return ESP_FAIL;
}
DRAM_ATTR std::unique_ptr<Servo> g_ptrServo;
src/spot.cpp
-241
View File
@@ -1,241 +0,0 @@
#include <spot.h>
websocket_message wsm;
Spot::Spot()
: _events(EVENTSOURCE_PATH)
, _ws(WEBSOCKET_PATH)
, _server(HTTP_PORT)
, _display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, SCREEN_RESET)
, _mpu(Wire)
, _leftUss(USS_LEFT, USS_LEFT, USS_MAX_DISTANCE)
, _rightUss(USS_RIGHT, USS_RIGHT, USS_MAX_DISTANCE)
, _pwm(0x40)
{ }
esp_err_t Spot::boot(){
log_i("Booting...");
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0);
SPIFFS.begin();
Wire.begin(SDA, SCL);
_initialize_button();
_initialize_display();
_initialize_mpu();
_initialize_camera();
initialize_wifi();
_initialize_arduino_oat();
_initialize_server();
log_i("Done booting");
return ESP_OK;
}
void Spot::handle(){
if(USE_CAPTIVE_PORTAL) _dnsServer.processNextRequest();
ArduinoOTA.handle();
_ws.cleanupClients();
broadcast_data();
}
esp_err_t Spot::initialize_wifi(){
if(USE_CAPTIVE_PORTAL) _initialize_captive_portal();
else _initialize_wifi_connection();
if(MDNS.begin(HOSTNAME)){
MDNS.addService("http", "tcp", HTTP_PORT);
}
if(USE_CAPTIVE_PORTAL) display_ip_and_ssid(&_display, WiFi.softAPIP().toString(), HOSTNAME);
else display_ip_and_ssid(&_display, WiFi.localIP().toString(), SSID);
return ESP_OK;
}
uint8_t Spot::cpu_temperature(){
return (temprature_sens_read() - 32) / 1.8;
}
esp_err_t Spot::broadcast_data(){
if(millis() - _last_broadcast < 50) return ESP_OK;
_mpu.update();
size_t numContent = 14;
float content[numContent] = {
WiFi.RSSI(),
_mpu.getTemp(),
_mpu.getAngleX(),
_mpu.getAngleY(),
_mpu.getAngleZ(),
cpu_temperature(),
_leftUss.ping_cm(),
_rightUss.ping_cm(),
ESP.getFreeHeap(),
ESP.getFreePsram(),
ESP.getMinFreeHeap(),
ESP.getMinFreePsram(),
ESP.getMaxAllocHeap(),
ESP.getMaxAllocPsram(),
};
uint8_t* buf = (uint8_t*) &content;
size_t buf_len = sizeof(buf);
_ws.binaryAll(buf, buf_len * numContent);
_last_broadcast = millis();
return ESP_OK;
}
esp_err_t Spot::_initialize_camera(){
camera_config_t camera_config;
camera_config.ledc_channel = LEDC_CHANNEL_0;
camera_config.ledc_timer = LEDC_TIMER_0;
camera_config.pin_d0 = Y2_GPIO_NUM;
camera_config.pin_d1 = Y3_GPIO_NUM;
camera_config.pin_d2 = Y4_GPIO_NUM;
camera_config.pin_d3 = Y5_GPIO_NUM;
camera_config.pin_d4 = Y6_GPIO_NUM;
camera_config.pin_d5 = Y7_GPIO_NUM;
camera_config.pin_d6 = Y8_GPIO_NUM;
camera_config.pin_d7 = Y9_GPIO_NUM;
camera_config.pin_xclk = XCLK_GPIO_NUM;
camera_config.pin_pclk = PCLK_GPIO_NUM;
camera_config.pin_vsync = VSYNC_GPIO_NUM;
camera_config.pin_href = HREF_GPIO_NUM;
camera_config.pin_sscb_sda = SIOD_GPIO_NUM;
camera_config.pin_sscb_scl = SIOC_GPIO_NUM;
camera_config.pin_pwdn = PWDN_GPIO_NUM;
camera_config.pin_reset = RESET_GPIO_NUM;
camera_config.xclk_freq_hz = 20000000;
camera_config.pixel_format = PIXFORMAT_JPEG;
if(psramFound()){
camera_config.frame_size = FRAMESIZE_SVGA;
camera_config.jpeg_quality = 10;
camera_config.fb_count = 2;
} else {
camera_config.frame_size = FRAMESIZE_SVGA;
camera_config.jpeg_quality = 12;
camera_config.fb_count = 1;
}
log_i("Initializing camera");
esp_err_t err = esp_camera_init(&camera_config);
if (err != ESP_OK) log_i("Camera probe failed with error 0x%x", err);
return err;
}
esp_err_t Spot::_initialize_wifi_connection(){
log_i("Connecting to wifi");
WiFi.begin(SSID, PASS);
int8_t timeout = 0;
while (WiFi.status() != WL_CONNECTED) {
delay(500);
timeout++;
if(timeout > 15) {
_initialize_captive_portal();
return ESP_FAIL;
}
}
log_i("Connected successfully");
return ESP_OK;
}
esp_err_t Spot::_initialize_arduino_oat(){
log_i("Starting ArduinoOTA");
ArduinoOTA.onStart([&]() { _events.send("Update Start", "ota"); });
ArduinoOTA.onEnd([&]() { _events.send("Update End", "ota"); });
ArduinoOTA.onProgress([&](unsigned int progress, unsigned int total) {
char p[32];
sprintf(p, "Progress: %u%%\n", (progress/(total/100)));
_events.send(p, "ota");
});
ArduinoOTA.onError([&](ota_error_t error) {
if(error == OTA_AUTH_ERROR) _events.send("Auth Failed", "ota");
else if(error == OTA_BEGIN_ERROR) _events.send("Begin Failed", "ota");
else if(error == OTA_CONNECT_ERROR) _events.send("Connect Failed", "ota");
else if(error == OTA_RECEIVE_ERROR) _events.send("Recieve Failed", "ota");
else if(error == OTA_END_ERROR) _events.send("End Failed", "ota");
});
ArduinoOTA.setHostname(HOSTNAME);
ArduinoOTA.begin();
return ESP_OK;
}
esp_err_t Spot::_initialize_captive_portal(){
log_i("Starting captive portal");
WiFi.softAP(HOSTNAME);
_dnsServer.start(53, "*", WiFi.softAPIP());
return ESP_OK;
}
esp_err_t Spot::_initialize_server(){
log_i("Starting webserver");
_ws.onEvent(onWsEvent);
_server.addHandler(&_ws);
_server.on("/stream", HTTP_GET, streamJpg);
_server.serveStatic("/", SPIFFS, "/").setDefaultFile("index.html");
_server.begin();
return ESP_OK;
}
esp_err_t Spot::_initialize_display(){
log_i("Initializing display");
if(!_display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
log_w("SSD1306 allocation failed");
return ESP_FAIL;
}
_display.display();
delay(200);
_display.clearDisplay();
return ESP_OK;
}
esp_err_t Spot::_initialize_mpu(){
log_i("Initializing MPU");
byte status = _mpu.begin();
if(status != 0) {
log_w("MPU initialize failed");
return ESP_FAIL;
}
log_i("Calculating offsets, do not move MPU6050");
delay(1000);
log_i("Cone calculating offsets");
_mpu.calcOffsets(true,true);
return ESP_OK;
}
esp_err_t Spot::_initialize_pwm_controller(){
log_i("Initializing PWM controller");
_pwm.begin();
_pwm.setOscillatorFrequency(SERVO_OSCILLATOR_FREQUENCY);
_pwm.setPWMFreq(SERVO_FREQ);
return ESP_OK;
}
esp_err_t Spot::_initialize_button() {
pinMode(BUTTON_LED, OUTPUT);
digitalWrite(BUTTON_LED, HIGH);
//pinMode(BUTTON, INPUT);
return ESP_OK;
}
void display_ip_and_ssid(Adafruit_SSD1306* display, String ip, const char* ssid) {
display->setTextColor(WHITE, BLACK);
display->setTextSize(1);
int16_t x1 = 0;
int16_t y1 = 0;
uint16_t h = 0;
uint16_t w = 0;
display->getTextBounds(ssid, 0, 0, &x1, &y1, &w, &h);
display->setCursor(SCREEN_WIDTH/2 - w/2, SCREEN_HEIGHT/2-8 - h/2);
display->println(ssid);
display->getTextBounds(ip, 0, 0, &x1, &y1, &w, &h);
display->setCursor(SCREEN_WIDTH/2 - w/2, SCREEN_HEIGHT/2+8 - h/2);
display->println(ip);
display->display();
}
src/spot_ik.cpp
-264
View File
@@ -1,264 +0,0 @@
#include "spot_ik.h"
#include "IK_config.h"
#include "esp_dsp.h"
#include "math.h"
// https://github.com/maartenweyn/SpotMicro_ESP32/blob/master/code/esp-idf/ik_test/main/spot_ik.c
static const char* TAG = "IK";
#define RAD2DEGREES 57.295779513082321 // 180 / PI
#define DEGREES2RAD 0.017453292519943
// float orientation[3] = {M_PI / 8, M_PI / 8, 0}; //A 3x1 arry with Spot's Roll, Pitch, Yaw angles - omega, phi, psi
// float position[3] = {0, 0, 0}; //A 3x1 array with Spot's X, Y, Z coordinates
static float Rx[4][4] = {{1, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 1}}; // X-axis rotation matrix
static float Ry[4][4] = {{0, 0, 0, 0}, {0, 1, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 1}}; // Y-axis rotation matrix
static float Rz[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 1, 0}, {0, 0, 0, 1}}; // Z-axis rotation matrix
static float Rxyz[4][4] = {0,};
static float Tm[4][4] = {0,}; // Transformation Matrix
static float T[4][4] = {{0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}, {0, 0, 0, 0}}; //Translation <Atrix
static float Trb[4][4] = {0,}; //Tranformation Matrix Right Back
static float Trf[4][4] = {0,}; //Tranformation Matrix Right Front
static float Tlb[4][4] = {0,}; //Tranformation Matrix Left Back
static float Tlf[4][4] = {0,}; //Tranformation Matrix Left Front
static const float Trb2[4][4] = {{0, 0, 1, -L/2}, {0, 1, 0, 0}, {-1, 0, 0, -W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Right Back
static const float Trf2[4][4] = {{0, 0, 1, L/2}, {0, 1, 0, 0}, {-1, 0, 0, -W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Right Front
static const float Tlb2[4][4] = {{0, 0, 1, -L/2}, {0, 1, 0, 0}, {-1, 0, 0, W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Left Back
static const float Tlf2[4][4] = {{0, 0, 1, L/2}, {0, 1, 0, 0}, {-1, 0, 0, W/2}, {0, 0, 0, 1}}; //Fixed part of Tranformation Matrix Left Front
static float Lp[4][4] = {{80, -130, 100, 1}, {80, -130, -100, 1}, {-130, -130, 100, 1}, {-130, -130, -100, 1}};
static const float Ix[4][4] = {{-1,0,0,0},{0,1,0,0},{0,0,1,0},{0,0,0,1}};
static esp_err_t inverse(float a[4][4], float b[4][4]) {
float s0 = a[0][0] * a[1][1] - a[1][0] * a[0][1];
float s1 = a[0][0] * a[1][2] - a[1][0] * a[0][2];
float s2 = a[0][0] * a[1][3] - a[1][0] * a[0][3];
float s3 = a[0][1] * a[1][2] - a[1][1] * a[0][2];
float s4 = a[0][1] * a[1][3] - a[1][1] * a[0][3];
float s5 = a[0][2] * a[1][3] - a[1][2] * a[0][3];
float c5 = a[2][2] * a[3][3] - a[3][2] * a[2][3];
float c4 = a[2][1] * a[3][3] - a[3][1] * a[2][3];
float c3 = a[2][1] * a[3][2] - a[3][1] * a[2][2];
float c2 = a[2][0] * a[3][3] - a[3][0] * a[2][3];
float c1 = a[2][0] * a[3][2] - a[3][0] * a[2][2];
float c0 = a[2][0] * a[3][1] - a[3][0] * a[2][1];
// Should check for 0 determinant
float det = (s0 * c5 - s1 * c4 + s2 * c3 + s3 * c2 - s4 * c1 + s5 * c0);
if (det == 0.0) return ESP_FAIL;
float invdet = 1.0 / det;
b[0][0] = ( a[1][1] * c5 - a[1][2] * c4 + a[1][3] * c3) * invdet;
b[0][1] = (-a[0][1] * c5 + a[0][2] * c4 - a[0][3] * c3) * invdet;
b[0][2] = ( a[3][1] * s5 - a[3][2] * s4 + a[3][3] * s3) * invdet;
b[0][3] = (-a[2][1] * s5 + a[2][2] * s4 - a[2][3] * s3) * invdet;
b[1][0] = (-a[1][0] * c5 + a[1][2] * c2 - a[1][3] * c1) * invdet;
b[1][1] = ( a[0][0] * c5 - a[0][2] * c2 + a[0][3] * c1) * invdet;
b[1][2] = (-a[3][0] * s5 + a[3][2] * s2 - a[3][3] * s1) * invdet;
b[1][3] = ( a[2][0] * s5 - a[2][2] * s2 + a[2][3] * s1) * invdet;
b[2][0] = ( a[1][0] * c4 - a[1][1] * c2 + a[1][3] * c0) * invdet;
b[2][1] = (-a[0][0] * c4 + a[0][1] * c2 - a[0][3] * c0) * invdet;
b[2][2] = ( a[3][0] * s4 - a[3][1] * s2 + a[3][3] * s0) * invdet;
b[2][3] = (-a[2][0] * s4 + a[2][1] * s2 - a[2][3] * s0) * invdet;
b[3][0] = (-a[1][0] * c3 + a[1][1] * c1 - a[1][2] * c0) * invdet;
b[3][1] = ( a[0][0] * c3 - a[0][1] * c1 + a[0][2] * c0) * invdet;
b[3][2] = (-a[3][0] * s3 + a[3][1] * s1 - a[3][2] * s0) * invdet;
b[3][3] = ( a[2][0] * s3 - a[2][1] * s1 + a[2][2] * s0) * invdet;
return ESP_OK;
}
void print_matrix(float * matrix, int n, int m, char* name) {
printf("Matrix %s:\n", name);
for (int i=0;i<n;i++) {
for (int j=0;j<m;j++)
printf("%.2f ", matrix[i*m + j]);
printf("\n");
}
}
void print_int_matrix(int16_t * matrix, int n, int m, char* name, uint8_t newlines) {
printf("Matrix %s:\n", name);
for (int i=0;i<n;i++) {
for (int j=0;j<m;j++)
printf("%3d ", matrix[i*m + j]);
if (newlines) printf("\n");
}
if (!newlines) printf("\n");
}
esp_err_t body_IK(float omega, float phi, float psi, float xm, float ym, float zm) {
float cos_omega = cos(omega);
float sin_omega = sin(omega);
float cos_phi = cos(phi);
float sin_phi = sin(phi);
float cos_psi = cos(psi);
float sin_psi = sin(psi);
Rx[1][1] = cos_omega;
Rx[1][2] = -sin_omega;
Rx[2][1] = sin_omega;
Rx[2][2] = cos_omega;
Ry[0][0] = cos_phi;
Ry[0][2] = sin_phi;
Ry[2][0] = -sin_phi;
Ry[2][2] = cos_phi;
Rz[0][0] = cos_psi;
Rz[0][1] = -sin_psi;
Rz[1][0] = sin_psi;
Rz[1][1] = cos_psi;
float Rxy[4][4];
dspm_mult_f32_ae32((float*) Rx, (float*) Ry, (float*) Rxy, 4, 4, 4);
dspm_mult_f32_ae32((float*) Rxy, (float*) Rz, (float*) Rxyz, 4, 4, 4);
// print_matrix((float*) Rx, 4, 4, "Rx");
// print_matrix((float*) Ry, 4, 4, "Ry");
// print_matrix((float*) Rz, 4, 4, "Rz");
// print_matrix((float*) Rxyz, 4, 4, "Rxyz");
T[0][3] = xm;
T[1][3] = ym;
T[2][3] = zm;
dsps_add_f32_ae32((float*) T, (float*) Rxyz, (float*) Tm, 16, 1, 1, 1);
// print_matrix((float*) Tm, 4, 4, "Tm");
dspm_mult_f32_ae32((float*) Tm, (float*) Trb2, (float*) Trb, 4, 4, 4);
dspm_mult_f32_ae32((float*) Tm, (float*) Trf2, (float*) Trf, 4, 4, 4);
dspm_mult_f32_ae32((float*) Tm, (float*) Tlf2, (float*) Tlf, 4, 4, 4);
dspm_mult_f32_ae32((float*) Tm, (float*) Tlb2, (float*) Tlb, 4, 4, 4);
// print_matrix((float*) Trb, 4, 4, "Trb");
// print_matrix((float*) Trf, 4, 4, "Trf");
// print_matrix((float*) Tlf, 4, 4, "Tlf");
// print_matrix((float*) Tlb, 4, 4, "Tlb");
return ESP_OK;
}
esp_err_t leg_IK(float* p, uint8_t leg_id, int16_t* servo_angles) {
//float x = (leg_id == LEG_RF || leg_id == LEG_RB) ? -p[0] : p[0];
// Rxy Lenght of shoulder-point on x/y plane only
float Rxy2 = p[0]*p[0] + p[1]*p[1] - L1L1;
if (Rxy2 < 0) Rxy2 = 0;
float Rxy = sqrt(Rxy2);
// Dxy Distance we need to cover in xy plane
float Dxy = Rxy - L2;
// Dxyz 3D Distance we need to cover
float Dxyz = sqrt(Dxy*Dxy+p[2]*p[2]);
// ESP_LOGD(tag, "IK Rxy,Dxy,Dxyz %.2f %.2f %.2f", Rxy, Dxy, Dxyz);
// The angle we need to cover - the angle already covered because of the offset of the servo
float theta1 = -atan2(p[1], p[0]) - atan2(Rxy, -L1);
if (isnan(theta1)) return -1;
// ESP_LOGD(tag, "T1 %.2f = (- %.2f - %.2f)", theta1, -atan2(p[1], p[0]), atan2(Rxy, -L1));
float d=(Dxyz*Dxyz - L3L3 - L4L4)/(LL34);
float theta3 = acos(d);
if (isnan(theta3)) return -3;
float theta2 = atan2(p[2], Dxy) - atan2(L4 * sin(theta3), L3 + L4 * cos(theta3));
if (isnan(theta2)) return -2;
// ESP_LOGD(tag, "IK %.2f - atan2( %.2f, %.2f) (%.2f)", atan2(p[2], Dxy), L4 * sin(theta3), L3 + L4 * cos(theta3), atan2(L4 * sin(theta3), L3 + L4 * cos(theta3)));
if (servo_invert[leg_id*3]) {
servo_angles[0] = 90 - (int16_t) (0.5 + theta1 * RAD2DEGREES);
} else {
servo_angles[0] = 90 + (int16_t) (0.5 + theta1 * RAD2DEGREES);
}
if (servo_invert[leg_id*3 + 1]) {
servo_angles[1] = 120 + (int16_t) (0.5 + theta2 * RAD2DEGREES);
} else {
servo_angles[1] = 60 - (int16_t) (0.5 + theta2 * RAD2DEGREES);
}
if (servo_invert[leg_id*3 + 2]) {
servo_angles[2] = 180 - (int16_t) (0.5 + theta3 * RAD2DEGREES);
} else {
servo_angles[2] = (int16_t) (0.5 + theta3 * RAD2DEGREES);
}
if (servo_angles[1] < 0) servo_angles[1] += 360;
if (servo_angles[1] > 360) servo_angles[1] -= 360;
// ESP_LOGD(tag, "IK T2 %.2f -> %d", theta2, servo_angles[1]);
// ESP_LOGD(tag, "IK D %.2f, T3 %.2f -> %d", d, theta3, servo_angles[2]);
if (theta1 < theta_range[0][0] || theta1 > theta_range[0][1]) return -4;
if (theta2 < theta_range[1][0] || theta2 > theta_range[1][1]) return -5;
if (theta3 < theta_range[2][0] || theta3 > theta_range[2][1]) return -6;
return ESP_OK;
}
esp_err_t spot_IK(float omega, float phi, float psi, float xm, float ym, float zm, int16_t servoangles[4][3]) {
esp_err_t ret = ESP_OK;
body_IK(omega, phi, psi, xm, ym, zm);
float inv[4][4];
float Q1[4][4];
float Q[4];
ret += inverse(Tlf, inv);
dspm_mult_f32_ae32((float*) inv, (float*) Lp[0], (float*) Q, 4, 4, 1);
// print_matrix((float*) Q, 1, 4, "Q LF");
ret += leg_IK((float*) Q, LEG_LF, (int16_t*) servoangles[0]);
//printf("leg_IK return %d\n", ret);
//print_int_matrix((int16_t*) servo_angles[0], 1, 3, "servo_angles LF", 1);
// printf("\n\nRF-----\n");
ret += inverse(Trf, inv);
// print_matrix((float*) Ix, 4, 4, "Ix");
// print_matrix((float*) inv, 4, 4, "inv Trf");
dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
// print_matrix((float*) Q1, 4, 4, "Q1 RF");
dspm_mult_f32_ae32((float*) Q1, (float*) Lp[1], (float*) Q, 4, 4, 1);
// print_matrix((float*) Q, 1, 4, "Q RF");
ret += leg_IK((float*) Q, LEG_RF, (int16_t*) servoangles[1]);
//printf("leg_IK return %d\n", ret);
//print_int_matrix((int16_t*) servo_angles[1], 1, 3, "servo_angles RF");
ret += inverse(Tlb, inv);
dspm_mult_f32_ae32((float*) inv, (float*) Lp[2], (float*) Q, 4, 4, 1);
// print_matrix((float*) Q, 1, 4, "Q LB");
ret += leg_IK((float*) Q, LEG_LB, (int16_t*) servoangles[2]);
//printf("leg_IK return %d\n", ret);
//print_int_matrix((int16_t*) servo_angles[2], 1, 3, "servo_angles LB");
// printf("\n\nRB-----\n");
ret += inverse(Trb, inv);
// print_matrix((float*) inv, 4, 4, "inv Trf");
dspm_mult_f32_ae32((float*) Ix, (float*) inv, (float*) Q1, 4, 4, 4);
// print_matrix((float*) Q1, 4, 4, "Q1 RF");
dspm_mult_f32_ae32((float*) Q1, (float*) Lp[3], (float*) Q, 4, 4, 1);
// print_matrix((float*) Q, 1, 4, "Q RB");
ret += leg_IK((float*) Q, LEG_RB, (int16_t*) servoangles[3]);
//printf("leg_IK return %d\n", ret);
//print_int_matrix((int16_t*) servo_angles[3], 1, 3, "servo_angles RB");
return ret;
}
src/webserver.cpp
+346
View File
@@ -0,0 +1,346 @@
#include "globals.h"
#include "webserver.h"
// Maps settings for which a validator is available to the invocation thereof
const std::map<String, CWebServer::ValueValidator> CWebServer::settingValidators
{};
std::vector<SettingSpec> CWebServer::deviceSettingSpecs{};
// Member function template specialzations
// Push param that represents a bool. Values considered true are text "true" and any whole number not equal to 0
template<>
bool CWebServer::PushPostParamIfPresent<bool>(AsyncWebServerRequest * pRequest, const String &paramName, ValueSetter<bool> setter)
{
return PushPostParamIfPresent<bool>(pRequest, paramName, setter, [](AsyncWebParameter * param) constexpr
{
const String& value = param->value();
return value == "true" || strtol(value.c_str(), NULL, 10);
});
}
// Push param that represents a size_t
template<>
bool CWebServer::PushPostParamIfPresent<size_t>(AsyncWebServerRequest * pRequest, const String &paramName, ValueSetter<size_t> setter)
{
return PushPostParamIfPresent<size_t>(pRequest, paramName, setter, [](AsyncWebParameter * param) constexpr
{
return strtoul(param->value().c_str(), NULL, 10);
});
}
// Add CORS header to and send JSON response
template<>
void CWebServer::AddCORSHeaderAndSendResponse<AsyncJsonResponse>(AsyncWebServerRequest * pRequest, AsyncJsonResponse * pResponse)
{
pResponse->setLength();
AddCORSHeaderAndSendResponse<AsyncWebServerResponse>(pRequest, pResponse);
}
// Member function implementations
bool CWebServer::IsPostParamTrue(AsyncWebServerRequest * pRequest, const String & paramName)
{
bool returnValue = false;
PushPostParamIfPresent<bool>(pRequest, paramName, [&returnValue](auto value) { returnValue = value; return true; });
return returnValue;
}
void CWebServer::GetStatistics(AsyncWebServerRequest * pRequest)
{
log_v("GetStatistics");
auto response = new AsyncJsonResponse(false, JSON_BUFFER_BASE_SIZE);
auto j = response->getRoot();
j["HEAP_SIZE"] = _staticStats.HeapSize;
j["HEAP_FREE"] = ESP.getFreeHeap();
j["HEAP_MIN"] = ESP.getMinFreeHeap();
j["DMA_SIZE"] = _staticStats.DmaHeapSize;
j["DMA_FREE"] = heap_caps_get_free_size(MALLOC_CAP_DMA);
j["DMA_MIN"] = heap_caps_get_largest_free_block(MALLOC_CAP_DMA);
j["PSRAM_SIZE"] = _staticStats.PsramSize;
j["PSRAM_FREE"] = ESP.getFreePsram();
j["PSRAM_MIN"] = ESP.getMinFreePsram();
j["CHIP_MODEL"] = _staticStats.ChipModel;
j["CHIP_CORES"] = _staticStats.ChipCores;
j["CHIP_SPEED"] = _staticStats.CpuFreqMHz;
j["PROG_SIZE"] = _staticStats.SketchSize;
j["CODE_SIZE"] = _staticStats.SketchSize;
j["CODE_FREE"] = _staticStats.FreeSketchSpace;
j["FLASH_SIZE"] = _staticStats.FlashChipSize;
j["CPU_USED"] = g_TaskManager.GetCPUUsagePercent();
j["CPU_USED_CORE0"] = g_TaskManager.GetCPUUsagePercent(0);
j["CPU_USED_CORE1"] = g_TaskManager.GetCPUUsagePercent(1);
AddCORSHeaderAndSendResponse(pRequest, response);
}
void CWebServer::SendSettingSpecsResponse(AsyncWebServerRequest * pRequest, const std::vector<SettingSpec> & settingSpecs)
{
static size_t jsonBufferSize = JSON_BUFFER_BASE_SIZE;
bool bufferOverflow;
do
{
bufferOverflow = false;
auto response = std::make_unique<AsyncJsonResponse>(false, jsonBufferSize);
auto jsonArray = response->getRoot().to<JsonArray>();
for (auto& spec : settingSpecs)
{
auto specObject = jsonArray.createNestedObject();
StaticJsonDocument<384> jsonDoc;
jsonDoc["name"] = spec.Name;
jsonDoc["friendlyName"] = spec.FriendlyName;
jsonDoc["description"] = spec.Description;
jsonDoc["type"] = to_value(spec.Type);
jsonDoc["typeName"] = spec.ToName(spec.Type);
if (!specObject.set(jsonDoc.as<JsonObjectConst>()))
{
bufferOverflow = true;
jsonBufferSize += JSON_BUFFER_INCREMENT;
log_v("JSON response buffer overflow! Increased buffer to %zu bytes", jsonBufferSize);
break;
}
}
if (!bufferOverflow)
AddCORSHeaderAndSendResponse(pRequest, response.release());
} while (bufferOverflow);
}
const std::vector<SettingSpec> & CWebServer::LoadDeviceSettingSpecs()
{
if (deviceSettingSpecs.size() == 0)
{
auto deviceConfigSpecs = g_ptrDeviceConfig->GetSettingSpecs();
deviceSettingSpecs.insert(deviceSettingSpecs.end(), deviceConfigSpecs.begin(), deviceConfigSpecs.end());
}
return deviceSettingSpecs;
}
void CWebServer::GetSettingSpecs(AsyncWebServerRequest * pRequest)
{
SendSettingSpecsResponse(pRequest, LoadDeviceSettingSpecs());
}
// Responds with current config, excluding any sensitive values
void CWebServer::GetSettings(AsyncWebServerRequest * pRequest)
{
log_v("GetSettings");
auto response = new AsyncJsonResponse(false, JSON_BUFFER_BASE_SIZE);
response->addHeader("Server","NightDriverStrip");
auto root = response->getRoot();
JsonObject jsonObject = root.to<JsonObject>();
// We get the serialized JSON for the device config, without any sensitive values
g_ptrDeviceConfig->SerializeToJSON(jsonObject, false);
AddCORSHeaderAndSendResponse(pRequest, response);
}
// Support function that silently sets whatever settings are included in the request passed.
// Composing a response is left to the invoker!
void CWebServer::SetSettingsIfPresent(AsyncWebServerRequest * pRequest)
{
PushPostParamIfPresent<String>(pRequest, DeviceConfig::NTPServerTag, SET_VALUE(g_ptrDeviceConfig->SetNTPServer(value)));
}
// Set settings and return resulting config
void CWebServer::SetSettings(AsyncWebServerRequest * pRequest)
{
log_v("SetSettings");
SetSettingsIfPresent(pRequest);
// We return the current config in response
GetSettings(pRequest);
}
// Save the posted file to SPIFFS
void CWebServer::SaveFile(AsyncWebServerRequest* request, String filename, size_t index, uint8_t* data, size_t len, bool final)
{
log_v("SaveFile");
}
#if USE_OAT
// Update the current firmware
void CWebServer::UpdateFileSystemImage(AsyncWebServerRequest *request, String filename, size_t index, uint8_t *data, size_t len, bool final)
{
log_v("UpdateFileSystemImage");
HandleUpdate(request, filename, index, data, len, final, (ESP.getFreeSketchSpace() - 0x1000) & 0xFFFFF000, U_FLASH);
}
// Update the current firmware
void CWebServer::UpdateFirmware(AsyncWebServerRequest *request, String filename, size_t index, uint8_t *data, size_t len, bool final)
{
log_v("UpdateFirmware");
HandleUpdate(request, filename, index, data, len, final, UPDATE_SIZE_UNKNOWN, U_SPIFFS);
}
// Handles updating the filesystem image or firmware
void CWebServer::HandleUpdate(AsyncWebServerRequest* request, String filename, size_t index, uint8_t* data, size_t len, bool final, size_t upload_size, uint8_t upload_index) {
if (!index) {
log_i("Update Start: %s\n", filename.c_str());
log_i("Uploading: %d", upload_index);
if (!Update.begin(upload_size, upload_index)) {
Update.printError(Serial);
}
}
if (!Update.hasError()) {
if (Update.write(data, len) != len) {
Update.printError(Serial);
}
}
if (final) {
if (Update.end(true)) {
log_i("Update Success: %uB\n", index + len);
}
else {
Update.printError(Serial);
}
}
}
// Ensures the request gets send to the client
void CWebServer::UpdateRequestHandler(AsyncWebServerRequest* request){
bool success = !Update.hasError();
AsyncWebServerResponse* response = request->beginResponse(200, "text/plain", success ? "OK" : "FAIL");
response->addHeader("Connection", "close");
request->send(response);
if (success) {
delay(250);
ESP.restart();
}
}
#endif
// Reset effect config, device config and/or the board itself
void CWebServer::Reset(AsyncWebServerRequest * pRequest)
{
if (IsPostParamTrue(pRequest, "deviceConfig"))
{
log_i("Removing DeviceConfig");
g_ptrDeviceConfig->RemovePersisted();
}
bool boardResetRequested = IsPostParamTrue(pRequest, "board");
AddCORSHeaderAndSendOKResponse(pRequest);
if (boardResetRequested)
{
log_w("Resetting device at API request!");
delay(1000); // Give the response a second to be sent
throw new std::runtime_error("Resetting device at API request");
}
}
String dir(void) {
String x = "<table>";
File root = SPIFFS.open("/");
if(!root) {
return "Failed to open directory";
}
if(!root.isDirectory()) {
return "Not a directory";
}
File file = root.openNextFile();
while(file) {
x += "<tr><td>"+String(file.name());
if(file.isDirectory()){
x += "<td>DIR";
} else {
x += "<td style='text-align:right'>"+String(file.size());
}
file = root.openNextFile();
}
x += "<tr><td>Occupied space<td style='text-align:right'>"+String(SPIFFS.usedBytes());
x += "<tr><td>Total space<td style='text-align:right'>"+String(SPIFFS.totalBytes());
return x+"</table>";
}
// Send Gzip SPA
void CWebServer::GzipSpa(AsyncWebServerRequest * pRequest)
{
if(!SPIFFS.exists("/index.html.gz")){
log_e("Gzipped SPA not found");
pRequest->send(404, "text/html", dir());
return;
}
AsyncWebServerResponse *response = pRequest->beginResponse(SPIFFS, "/index.html.gz", "text/html", false);
response->addHeader("Content-Encoding", "gzip");
response->addHeader("Cache-Control"," max-age=86400");
AddCORSHeaderAndSendResponse(pRequest, response);
}
enum Action {
SERVO_UPDATE,
};
void handleWebSocketJson(uint8_t* data) {
const uint8_t size = JSON_OBJECT_SIZE(1);
StaticJsonDocument<100> json;
DeserializationError err = deserializeJson(json, data);
if (err) {
Serial.print(F("deserializeJson() failed with code "));
Serial.println(err.c_str());
return;
}
const uint8_t action = json["action"];
if(action == 0) {
const uint8_t servoIndex = json["servo"];
const int16_t pwm = json["pwm"];
log_i("Moving servo:%u to pwm%u", servoIndex, pwm);
g_ptrServo->setPWM(servoIndex, 0, pwm);
}
}
void handleWebSocketBuffer(uint8_t* data) {
if(data[0] == 0) g_ptrServo->setBody(data[1], data[2], data[3], data[4], data[5], data[6]);
else if(data[0] == 1) {
log_i("About to update all servos");
int8_t* angles = (int8_t*)data+1;
g_ptrServo->SetAngles(angles);
}
}
void handleWebSocketMessage(void* arg, uint8_t* data, size_t len, AsyncWebSocket* server, AsyncWebSocketClient* client) {
AwsFrameInfo* info = (AwsFrameInfo*)arg;
if(info->final && info->index == 0 && info->len == len){
if(info->opcode == WS_TEXT) handleWebSocketJson(data);
else handleWebSocketBuffer(data);
}
}
// Reset effect config, device config and/or the board itself
void CWebServer::HandleWsMessage(AsyncWebSocket* server, AsyncWebSocketClient* client, AwsEventType type, void* arg, uint8_t* data, size_t len)
{
//if (type == WS_EVT_CONNECT) handleNewConnection(server, client);
/*else*/ if (type == WS_EVT_DISCONNECT) log_i("ws[%s][%u] disconnect\n", server->url(), client->id());
else if (type == WS_EVT_ERROR) log_i("ws[%s][%u] error(%u): %s\n", server->url(), client->id(), *((uint16_t*)arg), (char*)data);
else if (type == WS_EVT_PONG) log_i("ws[%s][%u] pong[%u]: %s\n", server->url(), client->id(), len, (len) ? (char*)data : "");
else if (type == WS_EVT_DATA) handleWebSocketMessage(arg, data, len, server, client);
}