Skip to content

Instantly share code, notes, and snippets.

@squirelo
Last active January 23, 2020 15:53
Show Gist options
  • Save squirelo/7f17b5271148652d9df1a1d05d1dea57 to your computer and use it in GitHub Desktop.
Save squirelo/7f17b5271148652d9df1a1d05d1dea57 to your computer and use it in GitHub Desktop.
#include <WiFi.h>
#include <WebSocketsServer.h>
#include <Husarnet.h>
#include <ArduinoJson.h>
#include <SparkFunMPU9250-DMP.h>
/* =============== config section start =============== */
#define INTERRUPT_PIN 19
// WiFi credentials
#define NUM_NETWORKS 2
// Add your networks credentials here
const char* ssidTab[NUM_NETWORKS] = {
"wifi-ssid-one",
"wifi-ssid-two",
};
const char* passwordTab[NUM_NETWORKS] = {
"wifi-pass-one",
"wifi-pass-two",
};
// Husarnet credentials
const char* hostName = "box3desp32"; //this will be the name of the 1st ESP32 device at https://app.husarnet.com
/* to get your join code go to https://app.husarnet.com
-> select network
-> click "Add element"
-> select "join code" tab
Keep it secret!
*/
const char* husarnetJoinCode = "xxxxxxxxxxxxxxxxxxxxxx";
/* =============== config section end =============== */
#define HTTP_PORT 8000
#define WEBSOCKET_PORT 8001
WebSocketsServer webSocket = WebSocketsServer(WEBSOCKET_PORT);
HusarnetServer server(HTTP_PORT);
StaticJsonBuffer<200> jsonBufferTx;
JsonObject& rootTx = jsonBufferTx.createObject();
const char* html =
#include "html.h"
;
bool wsconnected = false;
MPU9250_DMP imu;
void onWebSocketEvent(uint8_t num, WStype_t type, uint8_t* payload, size_t length) {
switch (type) {
case WStype_DISCONNECTED:
{
wsconnected = false;
Serial.printf("[%u] Disconnected\r\n", num);
}
break;
case WStype_CONNECTED:
{
wsconnected = true;
Serial.printf("\r\n[%u] Connection from Husarnet \r\n", num);
}
break;
case WStype_TEXT:
{
Serial.printf("[%u] Text:\r\n", num);
for (int i = 0; i < length; i++) {
Serial.printf("%c", (char)(*(payload + i)));
}
Serial.println();
}
break;
case WStype_BIN:
case WStype_ERROR:
case WStype_FRAGMENT_TEXT_START:
case WStype_FRAGMENT_BIN_START:
case WStype_FRAGMENT:
case WStype_FRAGMENT_FIN:
default:
break;
}
}
void taskWifi( void * parameter );
void taskHTTP( void * parameter );
void taskStatus( void * parameter );
SemaphoreHandle_t sem;
SemaphoreHandle_t mtx;
const signed char orientationDefault[9] = { 0, 1, 0, 0, 0, 1, 1, 0, 0 };
void setup()
{
pinMode(INTERRUPT_PIN, INPUT_PULLUP);
Serial.begin(115200);
if (imu.begin() != INV_SUCCESS) {
while (1) {
Serial.println("Unable to communicate with MPU-9250");
Serial.println("Check connections, and try again.");
Serial.println();
delay(5000);
}
}
imu.enableInterrupt();
imu.setIntLevel(INT_ACTIVE_LOW);
imu.setIntLatched(INT_LATCHED);
imu.dmpBegin(DMP_FEATURE_6X_LP_QUAT | // Enable 6-axis quat
DMP_FEATURE_GYRO_CAL, // Use gyro calibration
10); // Set DMP FIFO rate to 10 Hz
imu.dmpSetOrientation(orientationDefault);
sem = xSemaphoreCreateCounting( 10, 0 );
mtx = xSemaphoreCreateMutex();
xSemaphoreGive( mtx );
xTaskCreatePinnedToCore(
taskWifi, /* Task function. */
"taskWifi", /* String with name of task. */
20000, /* Stack size in bytes. */
NULL, /* Parameter passed as input of the task */
2, /* Priority of the task. */
NULL, /* Task handle. */
0); /* Core where the task should run */
xTaskCreatePinnedToCore(
taskHTTP, /* Task function. */
"taskHTTP", /* String with name of task. */
20000, /* Stack size in bytes. */
NULL, /* Parameter passed as input of the task */
2, /* Priority of the task. */
NULL, /* Task handle. */
0); /* Core where the task should run */
xTaskCreatePinnedToCore(
taskStatus, /* Task function. */
"taskStatus", /* String with name of task. */
20000, /* Stack size in bytes. */
NULL, /* Parameter passed as input of the task */
2, /* Priority of the task. */
NULL, /* Task handle. */
1); /* Core where the task should run */
}
void taskWifi( void * parameter ) {
bool initialSetup = 0;
while (1) {
for (int i = 0; i < NUM_NETWORKS; i++) {
delay(500);
Serial.println();
Serial.print("Connecting to ");
Serial.print(ssidTab[i]);
WiFi.begin(ssidTab[i], passwordTab[i]);
for (int j = 0; j < 10; j++) {
if (WiFi.status() != WL_CONNECTED) {
delay(500);
Serial.print(".");
} else {
if (initialSetup == 0) {
server.begin();
Husarnet.join(husarnetJoinCode, hostName);
Husarnet.start();
webSocket.begin();
webSocket.onEvent(onWebSocketEvent);
initialSetup = 1;
}
Serial.println("done");
Serial.print("IP address: ");
Serial.println(WiFi.localIP());
while (WiFi.status() == WL_CONNECTED) {
xSemaphoreTake(sem, 5);
if (xSemaphoreTake(mtx, 5) == pdTRUE) {
webSocket.loop();
xSemaphoreGive( mtx );
}
}
}
}
}
}
}
void taskHTTP( void * parameter )
{
String header;
while (1) {
while (WiFi.status() != WL_CONNECTED) {
delay(500);
}
delay(10);
HusarnetClient client = server.available();
if (client) {
Serial.println("New Client.");
String currentLine = "";
Serial.printf("connected: %d\r\n", client.connected());
while (client.connected()) {
if (client.available()) {
char c = client.read();
Serial.write(c);
header += c;
if (c == '\n') {
if (currentLine.length() == 0) {
client.println("HTTP/1.1 200 OK");
client.println("Content-type:text/html");
client.println("Connection: close");
client.println();
client.println(html);
break;
} else {
currentLine = "";
}
} else if (c != '\r') {
currentLine += c;
}
}
}
header = "";
client.stop();
Serial.println("Client disconnected.");
Serial.println("");
} else {
delay(200);
}
}
}
void taskStatus( void * parameter )
{
String output;
unsigned short fifoCnt;
inv_error_t result;
portMUX_TYPE myMutex = portMUX_INITIALIZER_UNLOCKED;
while (1) {
if ( digitalRead(INTERRUPT_PIN) == LOW ) {
taskENTER_CRITICAL(&myMutex);
fifoCnt = imu.fifoAvailable();
taskEXIT_CRITICAL(&myMutex);
if ( fifoCnt > 0) {
taskENTER_CRITICAL(&myMutex);
result = imu.dmpUpdateFifo();
taskEXIT_CRITICAL(&myMutex);
if ( result == INV_SUCCESS) {
imu.computeEulerAngles();
output = "";
float q0 = imu.calcQuat(imu.qw);
float q1 = imu.calcQuat(imu.qx);
float q2 = imu.calcQuat(imu.qy);
float q3 = imu.calcQuat(imu.qz);
//rootTx["roll"] = imu.roll;
//rootTx["pitch"] = imu.pitch;
//rootTx["yaw"] = imu.yaw;
rootTx["q0"] = q0;
rootTx["q1"] = q1;
rootTx["q2"] = q2;
rootTx["q3"] = q3;
rootTx.printTo(output);
//Serial.print(F("Sending: "));
//Serial.println(output);
if (wsconnected == true) {
if (xSemaphoreTake(mtx, 0) == pdTRUE) {
webSocket.sendTXT(0, output);
xSemaphoreGive( sem );
xSemaphoreGive( mtx );
}
}
}
} else {
Serial.println("false interrupt");
delay(20);
}
} else {
delay(20);
}
}
}
void loop()
{
Serial.printf("loop() running on core %d\r\n", xPortGetCoreID());
while (1) {
delay(5000);
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment