From 8d13d2937767dfa8e17fbb5be1fe62d4b2416d30 Mon Sep 17 00:00:00 2001 From: 0x1abin <270995079@qq.com> Date: Wed, 11 Oct 2017 17:09:55 +0800 Subject: [PATCH] update v0.1.2 --- examples/Advanced/WiFiSetting/Parsing.cpp | 607 ++++++++++++++++++ examples/Advanced/WiFiSetting/WebServer.cpp | 525 +++++++++++++++ examples/Advanced/WiFiSetting/WebServer.h | 186 ++++++ examples/Advanced/WiFiSetting/WiFiSetting.ino | 229 +++++++ .../WiFiSetting/detail/RequestHandler.h | 19 + .../WiFiSetting/detail/RequestHandlersImpl.h | 153 +++++ examples/Basics/Display/Display.ino | 40 +- examples/Basics/FactoryTest/FactoryTest.ino | 124 ++-- .../LoRaDumpRegisters/LoRaDumpRegisters.ino | 30 + .../Modules/Lora/LoRaDuplex/LoRaDuplex.ino | 102 +++ .../LoRaDuplexCallback/LoRaDuplexCallback.ino | 102 +++ .../LoRaReceiverCallback.ino | 40 ++ .../Lora/LoRaSetSpread/LoRaSetSpread.ino | 83 +++ .../Lora/LoRaSetSyncWord/LoRaSetSyncWord.ino | 79 +++ .../Modules/MPU9250/GetRawData/GetRawData.ino | 47 -- .../MPU9250BasicAHRS/MPU9250BasicAHRS.ino | 420 ++++++++++++ extras/MPU9250.cpp | 169 +++++ extras/MPU9250.h | 236 +++++++ extras/i2c_MPU9250.h | 460 +++++++++++++ library.json | 2 +- library.properties | 2 +- src/M5Stack.cpp | 18 +- src/M5Stack.h | 13 +- src/utility/Display.cpp | 10 +- src/utility/MPU9250.cpp | 514 ++++++++++++--- src/utility/MPU9250.h | 319 +++++++-- src/utility/quaternionFilters.cpp | 234 +++++++ src/utility/quaternionFilters.h | 14 + 28 files changed, 4477 insertions(+), 300 deletions(-) create mode 100644 examples/Advanced/WiFiSetting/Parsing.cpp create mode 100644 examples/Advanced/WiFiSetting/WebServer.cpp create mode 100644 examples/Advanced/WiFiSetting/WebServer.h create mode 100644 examples/Advanced/WiFiSetting/WiFiSetting.ino create mode 100644 examples/Advanced/WiFiSetting/detail/RequestHandler.h create mode 100644 examples/Advanced/WiFiSetting/detail/RequestHandlersImpl.h create mode 100644 examples/Modules/Lora/LoRaDumpRegisters/LoRaDumpRegisters.ino create mode 100644 examples/Modules/Lora/LoRaDuplex/LoRaDuplex.ino create mode 100644 examples/Modules/Lora/LoRaDuplexCallback/LoRaDuplexCallback.ino create mode 100644 examples/Modules/Lora/LoRaReceiverCallback/LoRaReceiverCallback.ino create mode 100644 examples/Modules/Lora/LoRaSetSpread/LoRaSetSpread.ino create mode 100644 examples/Modules/Lora/LoRaSetSyncWord/LoRaSetSyncWord.ino delete mode 100644 examples/Modules/MPU9250/GetRawData/GetRawData.ino create mode 100644 examples/Modules/MPU9250/MPU9250BasicAHRS/MPU9250BasicAHRS.ino create mode 100644 extras/MPU9250.cpp create mode 100644 extras/MPU9250.h create mode 100644 extras/i2c_MPU9250.h create mode 100644 src/utility/quaternionFilters.cpp create mode 100644 src/utility/quaternionFilters.h diff --git a/examples/Advanced/WiFiSetting/Parsing.cpp b/examples/Advanced/WiFiSetting/Parsing.cpp new file mode 100644 index 00000000..a14e6ab4 --- /dev/null +++ b/examples/Advanced/WiFiSetting/Parsing.cpp @@ -0,0 +1,607 @@ +/* + Parsing.cpp - HTTP request parsing. + + Copyright (c) 2015 Ivan Grokhotkov. All rights reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + Modified 8 May 2015 by Hristo Gochkov (proper post and file upload handling) +*/ + +#include +#include "WiFiServer.h" +#include "WiFiClient.h" +#include "WebServer.h" + +//#define DEBUG_ESP_HTTP_SERVER +#ifdef DEBUG_ESP_PORT +#define DEBUG_OUTPUT DEBUG_ESP_PORT +#else +#define DEBUG_OUTPUT Serial +#endif + +static char* readBytesWithTimeout(WiFiClient& client, size_t maxLength, size_t& dataLength, int timeout_ms) +{ + char *buf = nullptr; + dataLength = 0; + while (dataLength < maxLength) { + int tries = timeout_ms; + size_t newLength; + while (!(newLength = client.available()) && tries--) delay(1); + if (!newLength) { + break; + } + if (!buf) { + buf = (char *) malloc(newLength + 1); + if (!buf) { + return nullptr; + } + } + else { + char* newBuf = (char *) realloc(buf, dataLength + newLength + 1); + if (!newBuf) { + free(buf); + return nullptr; + } + buf = newBuf; + } + client.readBytes(buf + dataLength, newLength); + dataLength += newLength; + buf[dataLength] = '\0'; + } + return buf; +} + +bool WebServer::_parseRequest(WiFiClient& client) { + // Read the first line of HTTP request + String req = client.readStringUntil('\r'); + client.readStringUntil('\n'); + //reset header value + for (int i = 0; i < _headerKeysCount; ++i) { + _currentHeaders[i].value =String(); + } + + // First line of HTTP request looks like "GET /path HTTP/1.1" + // Retrieve the "/path" part by finding the spaces + int addr_start = req.indexOf(' '); + int addr_end = req.indexOf(' ', addr_start + 1); + if (addr_start == -1 || addr_end == -1) { +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("Invalid request: "); + DEBUG_OUTPUT.println(req); +#endif + return false; + } + + String methodStr = req.substring(0, addr_start); + String url = req.substring(addr_start + 1, addr_end); + String versionEnd = req.substring(addr_end + 8); + _currentVersion = atoi(versionEnd.c_str()); + String searchStr = ""; + int hasSearch = url.indexOf('?'); + if (hasSearch != -1){ + searchStr = urlDecode(url.substring(hasSearch + 1)); + url = url.substring(0, hasSearch); + } + _currentUri = url; + _chunked = false; + + HTTPMethod method = HTTP_GET; + if (methodStr == "POST") { + method = HTTP_POST; + } else if (methodStr == "DELETE") { + method = HTTP_DELETE; + } else if (methodStr == "OPTIONS") { + method = HTTP_OPTIONS; + } else if (methodStr == "PUT") { + method = HTTP_PUT; + } else if (methodStr == "PATCH") { + method = HTTP_PATCH; + } + _currentMethod = method; + +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("method: "); + DEBUG_OUTPUT.print(methodStr); + DEBUG_OUTPUT.print(" url: "); + DEBUG_OUTPUT.print(url); + DEBUG_OUTPUT.print(" search: "); + DEBUG_OUTPUT.println(searchStr); +#endif + + //attach handler + RequestHandler* handler; + for (handler = _firstHandler; handler; handler = handler->next()) { + if (handler->canHandle(_currentMethod, _currentUri)) + break; + } + _currentHandler = handler; + + String formData; + // below is needed only when POST type request + if (method == HTTP_POST || method == HTTP_PUT || method == HTTP_PATCH || method == HTTP_DELETE){ + String boundaryStr; + String headerName; + String headerValue; + bool isForm = false; + bool isEncoded = false; + uint32_t contentLength = 0; + //parse headers + while(1){ + req = client.readStringUntil('\r'); + client.readStringUntil('\n'); + if (req == "") break;//no moar headers + int headerDiv = req.indexOf(':'); + if (headerDiv == -1){ + break; + } + headerName = req.substring(0, headerDiv); + headerValue = req.substring(headerDiv + 1); + headerValue.trim(); + _collectHeader(headerName.c_str(),headerValue.c_str()); + + #ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("headerName: "); + DEBUG_OUTPUT.println(headerName); + DEBUG_OUTPUT.print("headerValue: "); + DEBUG_OUTPUT.println(headerValue); + #endif + + if (headerName.equalsIgnoreCase("Content-Type")){ + if (headerValue.startsWith("text/plain")){ + isForm = false; + } else if (headerValue.startsWith("application/x-www-form-urlencoded")){ + isForm = false; + isEncoded = true; + } else if (headerValue.startsWith("multipart/")){ + boundaryStr = headerValue.substring(headerValue.indexOf('=')+1); + isForm = true; + } + } else if (headerName.equalsIgnoreCase("Content-Length")){ + contentLength = headerValue.toInt(); + } else if (headerName.equalsIgnoreCase("Host")){ + _hostHeader = headerValue; + } + } + + if (!isForm){ + size_t plainLength; + char* plainBuf = readBytesWithTimeout(client, contentLength, plainLength, HTTP_MAX_POST_WAIT); + if (plainLength < contentLength) { + free(plainBuf); + return false; + } + if (contentLength > 0) { + if (searchStr != "") searchStr += '&'; + if(isEncoded){ + //url encoded form + String decoded = urlDecode(plainBuf); + size_t decodedLen = decoded.length(); + memcpy(plainBuf, decoded.c_str(), decodedLen); + plainBuf[decodedLen] = 0; + searchStr += plainBuf; + } + _parseArguments(searchStr); + if(!isEncoded){ + //plain post json or other data + RequestArgument& arg = _currentArgs[_currentArgCount++]; + arg.key = "plain"; + arg.value = String(plainBuf); + } + + #ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("Plain: "); + DEBUG_OUTPUT.println(plainBuf); + #endif + free(plainBuf); + } + } + + if (isForm){ + _parseArguments(searchStr); + if (!_parseForm(client, boundaryStr, contentLength)) { + return false; + } + } + } else { + String headerName; + String headerValue; + //parse headers + while(1){ + req = client.readStringUntil('\r'); + client.readStringUntil('\n'); + if (req == "") break;//no moar headers + int headerDiv = req.indexOf(':'); + if (headerDiv == -1){ + break; + } + headerName = req.substring(0, headerDiv); + headerValue = req.substring(headerDiv + 2); + _collectHeader(headerName.c_str(),headerValue.c_str()); + + #ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("headerName: "); + DEBUG_OUTPUT.println(headerName); + DEBUG_OUTPUT.print("headerValue: "); + DEBUG_OUTPUT.println(headerValue); + #endif + + if (headerName.equalsIgnoreCase("Host")){ + _hostHeader = headerValue; + } + } + _parseArguments(searchStr); + } + client.flush(); + +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("Request: "); + DEBUG_OUTPUT.println(url); + DEBUG_OUTPUT.print(" Arguments: "); + DEBUG_OUTPUT.println(searchStr); +#endif + + return true; +} + +bool WebServer::_collectHeader(const char* headerName, const char* headerValue) { + for (int i = 0; i < _headerKeysCount; i++) { + if (_currentHeaders[i].key.equalsIgnoreCase(headerName)) { + _currentHeaders[i].value=headerValue; + return true; + } + } + return false; +} + +void WebServer::_parseArguments(String data) { +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("args: "); + DEBUG_OUTPUT.println(data); +#endif + if (_currentArgs) + delete[] _currentArgs; + _currentArgs = 0; + if (data.length() == 0) { + _currentArgCount = 0; + _currentArgs = new RequestArgument[1]; + return; + } + _currentArgCount = 1; + + for (int i = 0; i < (int)data.length(); ) { + i = data.indexOf('&', i); + if (i == -1) + break; + ++i; + ++_currentArgCount; + } +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("args count: "); + DEBUG_OUTPUT.println(_currentArgCount); +#endif + + _currentArgs = new RequestArgument[_currentArgCount+1]; + int pos = 0; + int iarg; + for (iarg = 0; iarg < _currentArgCount;) { + int equal_sign_index = data.indexOf('=', pos); + int next_arg_index = data.indexOf('&', pos); +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("pos "); + DEBUG_OUTPUT.print(pos); + DEBUG_OUTPUT.print("=@ "); + DEBUG_OUTPUT.print(equal_sign_index); + DEBUG_OUTPUT.print(" &@ "); + DEBUG_OUTPUT.println(next_arg_index); +#endif + if ((equal_sign_index == -1) || ((equal_sign_index > next_arg_index) && (next_arg_index != -1))) { +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("arg missing value: "); + DEBUG_OUTPUT.println(iarg); +#endif + if (next_arg_index == -1) + break; + pos = next_arg_index + 1; + continue; + } + RequestArgument& arg = _currentArgs[iarg]; + arg.key = data.substring(pos, equal_sign_index); + arg.value = data.substring(equal_sign_index + 1, next_arg_index); +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("arg "); + DEBUG_OUTPUT.print(iarg); + DEBUG_OUTPUT.print(" key: "); + DEBUG_OUTPUT.print(arg.key); + DEBUG_OUTPUT.print(" value: "); + DEBUG_OUTPUT.println(arg.value); +#endif + ++iarg; + if (next_arg_index == -1) + break; + pos = next_arg_index + 1; + } + _currentArgCount = iarg; +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("args count: "); + DEBUG_OUTPUT.println(_currentArgCount); +#endif + +} + +void WebServer::_uploadWriteByte(uint8_t b){ + if (_currentUpload.currentSize == HTTP_UPLOAD_BUFLEN){ + if(_currentHandler && _currentHandler->canUpload(_currentUri)) + _currentHandler->upload(*this, _currentUri, _currentUpload); + _currentUpload.totalSize += _currentUpload.currentSize; + _currentUpload.currentSize = 0; + } + _currentUpload.buf[_currentUpload.currentSize++] = b; +} + +uint8_t WebServer::_uploadReadByte(WiFiClient& client){ + int res = client.read(); + if(res == -1){ + while(!client.available() && client.connected()) + yield(); + res = client.read(); + } + return (uint8_t)res; +} + +bool WebServer::_parseForm(WiFiClient& client, String boundary, uint32_t len){ + (void) len; +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("Parse Form: Boundary: "); + DEBUG_OUTPUT.print(boundary); + DEBUG_OUTPUT.print(" Length: "); + DEBUG_OUTPUT.println(len); +#endif + String line; + int retry = 0; + do { + line = client.readStringUntil('\r'); + ++retry; + } while (line.length() == 0 && retry < 3); + + client.readStringUntil('\n'); + //start reading the form + if (line == ("--"+boundary)){ + RequestArgument* postArgs = new RequestArgument[32]; + int postArgsLen = 0; + while(1){ + String argName; + String argValue; + String argType; + String argFilename; + bool argIsFile = false; + + line = client.readStringUntil('\r'); + client.readStringUntil('\n'); + if (line.length() > 19 && line.substring(0, 19).equalsIgnoreCase("Content-Disposition")){ + int nameStart = line.indexOf('='); + if (nameStart != -1){ + argName = line.substring(nameStart+2); + nameStart = argName.indexOf('='); + if (nameStart == -1){ + argName = argName.substring(0, argName.length() - 1); + } else { + argFilename = argName.substring(nameStart+2, argName.length() - 1); + argName = argName.substring(0, argName.indexOf('"')); + argIsFile = true; +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("PostArg FileName: "); + DEBUG_OUTPUT.println(argFilename); +#endif + //use GET to set the filename if uploading using blob + if (argFilename == "blob" && hasArg("filename")) argFilename = arg("filename"); + } +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("PostArg Name: "); + DEBUG_OUTPUT.println(argName); +#endif + argType = "text/plain"; + line = client.readStringUntil('\r'); + client.readStringUntil('\n'); + if (line.length() > 12 && line.substring(0, 12).equalsIgnoreCase("Content-Type")){ + argType = line.substring(line.indexOf(':')+2); + //skip next line + client.readStringUntil('\r'); + client.readStringUntil('\n'); + } +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("PostArg Type: "); + DEBUG_OUTPUT.println(argType); +#endif + if (!argIsFile){ + while(1){ + line = client.readStringUntil('\r'); + client.readStringUntil('\n'); + if (line.startsWith("--"+boundary)) break; + if (argValue.length() > 0) argValue += "\n"; + argValue += line; + } +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("PostArg Value: "); + DEBUG_OUTPUT.println(argValue); + DEBUG_OUTPUT.println(); +#endif + + RequestArgument& arg = postArgs[postArgsLen++]; + arg.key = argName; + arg.value = argValue; + + if (line == ("--"+boundary+"--")){ +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.println("Done Parsing POST"); +#endif + break; + } + } else { + _currentUpload.status = UPLOAD_FILE_START; + _currentUpload.name = argName; + _currentUpload.filename = argFilename; + _currentUpload.type = argType; + _currentUpload.totalSize = 0; + _currentUpload.currentSize = 0; +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("Start File: "); + DEBUG_OUTPUT.print(_currentUpload.filename); + DEBUG_OUTPUT.print(" Type: "); + DEBUG_OUTPUT.println(_currentUpload.type); +#endif + if(_currentHandler && _currentHandler->canUpload(_currentUri)) + _currentHandler->upload(*this, _currentUri, _currentUpload); + _currentUpload.status = UPLOAD_FILE_WRITE; + uint8_t argByte = _uploadReadByte(client); +readfile: + while(argByte != 0x0D){ + if (!client.connected()) return _parseFormUploadAborted(); + _uploadWriteByte(argByte); + argByte = _uploadReadByte(client); + } + + argByte = _uploadReadByte(client); + if (!client.connected()) return _parseFormUploadAborted(); + if (argByte == 0x0A){ + argByte = _uploadReadByte(client); + if (!client.connected()) return _parseFormUploadAborted(); + if ((char)argByte != '-'){ + //continue reading the file + _uploadWriteByte(0x0D); + _uploadWriteByte(0x0A); + goto readfile; + } else { + argByte = _uploadReadByte(client); + if (!client.connected()) return _parseFormUploadAborted(); + if ((char)argByte != '-'){ + //continue reading the file + _uploadWriteByte(0x0D); + _uploadWriteByte(0x0A); + _uploadWriteByte((uint8_t)('-')); + goto readfile; + } + } + + uint8_t endBuf[boundary.length()]; + client.readBytes(endBuf, boundary.length()); + + if (strstr((const char*)endBuf, boundary.c_str()) != NULL){ + if(_currentHandler && _currentHandler->canUpload(_currentUri)) + _currentHandler->upload(*this, _currentUri, _currentUpload); + _currentUpload.totalSize += _currentUpload.currentSize; + _currentUpload.status = UPLOAD_FILE_END; + if(_currentHandler && _currentHandler->canUpload(_currentUri)) + _currentHandler->upload(*this, _currentUri, _currentUpload); +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("End File: "); + DEBUG_OUTPUT.print(_currentUpload.filename); + DEBUG_OUTPUT.print(" Type: "); + DEBUG_OUTPUT.print(_currentUpload.type); + DEBUG_OUTPUT.print(" Size: "); + DEBUG_OUTPUT.println(_currentUpload.totalSize); +#endif + line = client.readStringUntil(0x0D); + client.readStringUntil(0x0A); + if (line == "--"){ +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.println("Done Parsing POST"); +#endif + break; + } + continue; + } else { + _uploadWriteByte(0x0D); + _uploadWriteByte(0x0A); + _uploadWriteByte((uint8_t)('-')); + _uploadWriteByte((uint8_t)('-')); + uint32_t i = 0; + while(i < boundary.length()){ + _uploadWriteByte(endBuf[i++]); + } + argByte = _uploadReadByte(client); + goto readfile; + } + } else { + _uploadWriteByte(0x0D); + goto readfile; + } + break; + } + } + } + } + + int iarg; + int totalArgs = ((32 - postArgsLen) < _currentArgCount)?(32 - postArgsLen):_currentArgCount; + for (iarg = 0; iarg < totalArgs; iarg++){ + RequestArgument& arg = postArgs[postArgsLen++]; + arg.key = _currentArgs[iarg].key; + arg.value = _currentArgs[iarg].value; + } + if (_currentArgs) delete[] _currentArgs; + _currentArgs = new RequestArgument[postArgsLen]; + for (iarg = 0; iarg < postArgsLen; iarg++){ + RequestArgument& arg = _currentArgs[iarg]; + arg.key = postArgs[iarg].key; + arg.value = postArgs[iarg].value; + } + _currentArgCount = iarg; + if (postArgs) delete[] postArgs; + return true; + } +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.print("Error: line: "); + DEBUG_OUTPUT.println(line); +#endif + return false; +} + +String WebServer::urlDecode(const String& text) +{ + String decoded = ""; + char temp[] = "0x00"; + unsigned int len = text.length(); + unsigned int i = 0; + while (i < len) + { + char decodedChar; + char encodedChar = text.charAt(i++); + if ((encodedChar == '%') && (i + 1 < len)) + { + temp[2] = text.charAt(i++); + temp[3] = text.charAt(i++); + + decodedChar = strtol(temp, NULL, 16); + } + else { + if (encodedChar == '+') + { + decodedChar = ' '; + } + else { + decodedChar = encodedChar; // normal ascii char + } + } + decoded += decodedChar; + } + return decoded; +} + +bool WebServer::_parseFormUploadAborted(){ + _currentUpload.status = UPLOAD_FILE_ABORTED; + if(_currentHandler && _currentHandler->canUpload(_currentUri)) + _currentHandler->upload(*this, _currentUri, _currentUpload); + return false; +} diff --git a/examples/Advanced/WiFiSetting/WebServer.cpp b/examples/Advanced/WiFiSetting/WebServer.cpp new file mode 100644 index 00000000..0403ed4c --- /dev/null +++ b/examples/Advanced/WiFiSetting/WebServer.cpp @@ -0,0 +1,525 @@ +/* + WebServer.cpp - Dead simple web-server. + Supports only one simultaneous client, knows how to handle GET and POST. + + Copyright (c) 2014 Ivan Grokhotkov. All rights reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + Modified 8 May 2015 by Hristo Gochkov (proper post and file upload handling) +*/ + + +#include +#include +#include "WiFiServer.h" +#include "WiFiClient.h" +#include "WebServer.h" +#include "FS.h" +#include "detail/RequestHandlersImpl.h" + +//#define DEBUG_ESP_HTTP_SERVER +#ifdef DEBUG_ESP_PORT +#define DEBUG_OUTPUT DEBUG_ESP_PORT +#else +#define DEBUG_OUTPUT Serial +#endif + +const char * AUTHORIZATION_HEADER = "Authorization"; + +WebServer::WebServer(IPAddress addr, int port) +: _server(addr, port) +, _currentMethod(HTTP_ANY) +, _currentVersion(0) +, _currentStatus(HC_NONE) +, _statusChange(0) +, _currentHandler(0) +, _firstHandler(0) +, _lastHandler(0) +, _currentArgCount(0) +, _currentArgs(0) +, _headerKeysCount(0) +, _currentHeaders(0) +, _contentLength(0) +, _chunked(false) +{ +} + +WebServer::WebServer(int port) +: _server(port) +, _currentMethod(HTTP_ANY) +, _currentVersion(0) +, _currentStatus(HC_NONE) +, _statusChange(0) +, _currentHandler(0) +, _firstHandler(0) +, _lastHandler(0) +, _currentArgCount(0) +, _currentArgs(0) +, _headerKeysCount(0) +, _currentHeaders(0) +, _contentLength(0) +, _chunked(false) +{ +} + +WebServer::~WebServer() { + if (_currentHeaders) + delete[]_currentHeaders; + _headerKeysCount = 0; + RequestHandler* handler = _firstHandler; + while (handler) { + RequestHandler* next = handler->next(); + delete handler; + handler = next; + } + close(); +} + +void WebServer::begin() { + _currentStatus = HC_NONE; + _server.begin(); + if(!_headerKeysCount) + collectHeaders(0, 0); +} + +bool WebServer::authenticate(const char * username, const char * password){ + if(hasHeader(AUTHORIZATION_HEADER)){ + String authReq = header(AUTHORIZATION_HEADER); + if(authReq.startsWith("Basic")){ + authReq = authReq.substring(6); + authReq.trim(); + char toencodeLen = strlen(username)+strlen(password)+1; + char *toencode = new char[toencodeLen + 1]; + if(toencode == NULL){ + authReq = String(); + return false; + } + char *encoded = new char[base64_encode_expected_len(toencodeLen)+1]; + if(encoded == NULL){ + authReq = String(); + delete[] toencode; + return false; + } + sprintf(toencode, "%s:%s", username, password); + if(base64_encode_chars(toencode, toencodeLen, encoded) > 0 && authReq.equals(encoded)){ + authReq = String(); + delete[] toencode; + delete[] encoded; + return true; + } + delete[] toencode; + delete[] encoded; + } + authReq = String(); + } + return false; +} + +void WebServer::requestAuthentication(){ + sendHeader("WWW-Authenticate", "Basic realm=\"Login Required\""); + send(401); +} + +void WebServer::on(const String &uri, WebServer::THandlerFunction handler) { + on(uri, HTTP_ANY, handler); +} + +void WebServer::on(const String &uri, HTTPMethod method, WebServer::THandlerFunction fn) { + on(uri, method, fn, _fileUploadHandler); +} + +void WebServer::on(const String &uri, HTTPMethod method, WebServer::THandlerFunction fn, WebServer::THandlerFunction ufn) { + _addRequestHandler(new FunctionRequestHandler(fn, ufn, uri, method)); +} + +void WebServer::addHandler(RequestHandler* handler) { + _addRequestHandler(handler); +} + +void WebServer::_addRequestHandler(RequestHandler* handler) { + if (!_lastHandler) { + _firstHandler = handler; + _lastHandler = handler; + } + else { + _lastHandler->next(handler); + _lastHandler = handler; + } +} + +void WebServer::serveStatic(const char* uri, FS& fs, const char* path, const char* cache_header) { + _addRequestHandler(new StaticRequestHandler(fs, path, uri, cache_header)); +} + +void WebServer::handleClient() { + if (_currentStatus == HC_NONE) { + WiFiClient client = _server.available(); + if (!client) { + return; + } + +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.println("New client"); +#endif + + _currentClient = client; + _currentStatus = HC_WAIT_READ; + _statusChange = millis(); + } + + if (!_currentClient.connected()) { + _currentClient = WiFiClient(); + _currentStatus = HC_NONE; + return; + } + + // Wait for data from client to become available + if (_currentStatus == HC_WAIT_READ) { + if (!_currentClient.available()) { + if (millis() - _statusChange > HTTP_MAX_DATA_WAIT) { + _currentClient = WiFiClient(); + _currentStatus = HC_NONE; + } + yield(); + return; + } + + if (!_parseRequest(_currentClient)) { + _currentClient = WiFiClient(); + _currentStatus = HC_NONE; + return; + } + _currentClient.setTimeout(HTTP_MAX_SEND_WAIT); + _contentLength = CONTENT_LENGTH_NOT_SET; + _handleRequest(); + + if (!_currentClient.connected()) { + _currentClient = WiFiClient(); + _currentStatus = HC_NONE; + return; + } else { + _currentStatus = HC_WAIT_CLOSE; + _statusChange = millis(); + return; + } + } + + if (_currentStatus == HC_WAIT_CLOSE) { + if (millis() - _statusChange > HTTP_MAX_CLOSE_WAIT) { + _currentClient = WiFiClient(); + _currentStatus = HC_NONE; + } else { + yield(); + return; + } + } +} + +void WebServer::close() { + _server.end(); +} + +void WebServer::stop() { + close(); +} + +void WebServer::sendHeader(const String& name, const String& value, bool first) { + String headerLine = name; + headerLine += ": "; + headerLine += value; + headerLine += "\r\n"; + + if (first) { + _responseHeaders = headerLine + _responseHeaders; + } + else { + _responseHeaders += headerLine; + } +} + +void WebServer::setContentLength(size_t contentLength) { + _contentLength = contentLength; +} + +void WebServer::_prepareHeader(String& response, int code, const char* content_type, size_t contentLength) { + response = "HTTP/1."+String(_currentVersion)+" "; + response += String(code); + response += " "; + response += _responseCodeToString(code); + response += "\r\n"; + + if (!content_type) + content_type = "text/html"; + + sendHeader("Content-Type", content_type, true); + if (_contentLength == CONTENT_LENGTH_NOT_SET) { + sendHeader("Content-Length", String(contentLength)); + } else if (_contentLength != CONTENT_LENGTH_UNKNOWN) { + sendHeader("Content-Length", String(_contentLength)); + } else if(_contentLength == CONTENT_LENGTH_UNKNOWN && _currentVersion){ //HTTP/1.1 or above client + //let's do chunked + _chunked = true; + sendHeader("Accept-Ranges","none"); + sendHeader("Transfer-Encoding","chunked"); + } + sendHeader("Connection", "close"); + + response += _responseHeaders; + response += "\r\n"; + _responseHeaders = String(); +} + +void WebServer::send(int code, const char* content_type, const String& content) { + String header; + // Can we asume the following? + //if(code == 200 && content.length() == 0 && _contentLength == CONTENT_LENGTH_NOT_SET) + // _contentLength = CONTENT_LENGTH_UNKNOWN; + _prepareHeader(header, code, content_type, content.length()); + _currentClient.write(header.c_str(), header.length()); + if(content.length()) + sendContent(content); +} + +void WebServer::send_P(int code, PGM_P content_type, PGM_P content) { + size_t contentLength = 0; + + if (content != NULL) { + contentLength = strlen_P(content); + } + + String header; + char type[64]; + memccpy_P((void*)type, (PGM_VOID_P)content_type, 0, sizeof(type)); + _prepareHeader(header, code, (const char* )type, contentLength); + _currentClient.write(header.c_str(), header.length()); + sendContent_P(content); +} + +void WebServer::send_P(int code, PGM_P content_type, PGM_P content, size_t contentLength) { + String header; + char type[64]; + memccpy_P((void*)type, (PGM_VOID_P)content_type, 0, sizeof(type)); + _prepareHeader(header, code, (const char* )type, contentLength); + sendContent(header); + sendContent_P(content, contentLength); +} + +void WebServer::send(int code, char* content_type, const String& content) { + send(code, (const char*)content_type, content); +} + +void WebServer::send(int code, const String& content_type, const String& content) { + send(code, (const char*)content_type.c_str(), content); +} + +void WebServer::sendContent(const String& content) { + const char * footer = "\r\n"; + size_t len = content.length(); + if(_chunked) { + char * chunkSize = (char *)malloc(11); + if(chunkSize){ + sprintf(chunkSize, "%x%s", len, footer); + _currentClient.write(chunkSize, strlen(chunkSize)); + free(chunkSize); + } + } + _currentClient.write(content.c_str(), len); + if(_chunked){ + _currentClient.write(footer, 2); + } +} + +void WebServer::sendContent_P(PGM_P content) { + sendContent_P(content, strlen_P(content)); +} + +void WebServer::sendContent_P(PGM_P content, size_t size) { + const char * footer = "\r\n"; + if(_chunked) { + char * chunkSize = (char *)malloc(11); + if(chunkSize){ + sprintf(chunkSize, "%x%s", size, footer); + _currentClient.write(chunkSize, strlen(chunkSize)); + free(chunkSize); + } + } + _currentClient.write(content, size); + if(_chunked){ + _currentClient.write(footer, 2); + } +} + + +String WebServer::arg(String name) { + for (int i = 0; i < _currentArgCount; ++i) { + if ( _currentArgs[i].key == name ) + return _currentArgs[i].value; + } + return String(); +} + +String WebServer::arg(int i) { + if (i < _currentArgCount) + return _currentArgs[i].value; + return String(); +} + +String WebServer::argName(int i) { + if (i < _currentArgCount) + return _currentArgs[i].key; + return String(); +} + +int WebServer::args() { + return _currentArgCount; +} + +bool WebServer::hasArg(String name) { + for (int i = 0; i < _currentArgCount; ++i) { + if (_currentArgs[i].key == name) + return true; + } + return false; +} + + +String WebServer::header(String name) { + for (int i = 0; i < _headerKeysCount; ++i) { + if (_currentHeaders[i].key.equalsIgnoreCase(name)) + return _currentHeaders[i].value; + } + return String(); +} + +void WebServer::collectHeaders(const char* headerKeys[], const size_t headerKeysCount) { + _headerKeysCount = headerKeysCount + 1; + if (_currentHeaders) + delete[]_currentHeaders; + _currentHeaders = new RequestArgument[_headerKeysCount]; + _currentHeaders[0].key = AUTHORIZATION_HEADER; + for (int i = 1; i < _headerKeysCount; i++){ + _currentHeaders[i].key = headerKeys[i-1]; + } +} + +String WebServer::header(int i) { + if (i < _headerKeysCount) + return _currentHeaders[i].value; + return String(); +} + +String WebServer::headerName(int i) { + if (i < _headerKeysCount) + return _currentHeaders[i].key; + return String(); +} + +int WebServer::headers() { + return _headerKeysCount; +} + +bool WebServer::hasHeader(String name) { + for (int i = 0; i < _headerKeysCount; ++i) { + if ((_currentHeaders[i].key.equalsIgnoreCase(name)) && (_currentHeaders[i].value.length() > 0)) + return true; + } + return false; +} + +String WebServer::hostHeader() { + return _hostHeader; +} + +void WebServer::onFileUpload(THandlerFunction fn) { + _fileUploadHandler = fn; +} + +void WebServer::onNotFound(THandlerFunction fn) { + _notFoundHandler = fn; +} + +void WebServer::_handleRequest() { + bool handled = false; + if (!_currentHandler){ +#ifdef DEBUG_ESP_HTTP_SERVER + DEBUG_OUTPUT.println("request handler not found"); +#endif + } + else { + handled = _currentHandler->handle(*this, _currentMethod, _currentUri); +#ifdef DEBUG_ESP_HTTP_SERVER + if (!handled) { + DEBUG_OUTPUT.println("request handler failed to handle request"); + } +#endif + } + + if (!handled) { + if(_notFoundHandler) { + _notFoundHandler(); + } + else { + send(404, "text/plain", String("Not found: ") + _currentUri); + } + } + + _currentUri = String(); +} + +String WebServer::_responseCodeToString(int code) { + switch (code) { + case 100: return F("Continue"); + case 101: return F("Switching Protocols"); + case 200: return F("OK"); + case 201: return F("Created"); + case 202: return F("Accepted"); + case 203: return F("Non-Authoritative Information"); + case 204: return F("No Content"); + case 205: return F("Reset Content"); + case 206: return F("Partial Content"); + case 300: return F("Multiple Choices"); + case 301: return F("Moved Permanently"); + case 302: return F("Found"); + case 303: return F("See Other"); + case 304: return F("Not Modified"); + case 305: return F("Use Proxy"); + case 307: return F("Temporary Redirect"); + case 400: return F("Bad Request"); + case 401: return F("Unauthorized"); + case 402: return F("Payment Required"); + case 403: return F("Forbidden"); + case 404: return F("Not Found"); + case 405: return F("Method Not Allowed"); + case 406: return F("Not Acceptable"); + case 407: return F("Proxy Authentication Required"); + case 408: return F("Request Time-out"); + case 409: return F("Conflict"); + case 410: return F("Gone"); + case 411: return F("Length Required"); + case 412: return F("Precondition Failed"); + case 413: return F("Request Entity Too Large"); + case 414: return F("Request-URI Too Large"); + case 415: return F("Unsupported Media Type"); + case 416: return F("Requested range not satisfiable"); + case 417: return F("Expectation Failed"); + case 500: return F("Internal Server Error"); + case 501: return F("Not Implemented"); + case 502: return F("Bad Gateway"); + case 503: return F("Service Unavailable"); + case 504: return F("Gateway Time-out"); + case 505: return F("HTTP Version not supported"); + default: return ""; + } +} diff --git a/examples/Advanced/WiFiSetting/WebServer.h b/examples/Advanced/WiFiSetting/WebServer.h new file mode 100644 index 00000000..2f4cc526 --- /dev/null +++ b/examples/Advanced/WiFiSetting/WebServer.h @@ -0,0 +1,186 @@ +/* + WebServer.h - Dead simple web-server. + Supports only one simultaneous client, knows how to handle GET and POST. + + Copyright (c) 2014 Ivan Grokhotkov. All rights reserved. + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA + Modified 8 May 2015 by Hristo Gochkov (proper post and file upload handling) +*/ + + +#ifndef WEBSERVER_H +#define WEBSERVER_H + +#include +#include +#include +#include + +enum HTTPMethod { HTTP_ANY, HTTP_GET, HTTP_POST, HTTP_PUT, HTTP_PATCH, HTTP_DELETE, HTTP_OPTIONS }; +enum HTTPUploadStatus { UPLOAD_FILE_START, UPLOAD_FILE_WRITE, UPLOAD_FILE_END, + UPLOAD_FILE_ABORTED }; +enum HTTPClientStatus { HC_NONE, HC_WAIT_READ, HC_WAIT_CLOSE }; + +#define HTTP_DOWNLOAD_UNIT_SIZE 1460 +#define HTTP_UPLOAD_BUFLEN 2048 +#define HTTP_MAX_DATA_WAIT 1000 //ms to wait for the client to send the request +#define HTTP_MAX_POST_WAIT 1000 //ms to wait for POST data to arrive +#define HTTP_MAX_SEND_WAIT 5000 //ms to wait for data chunk to be ACKed +#define HTTP_MAX_CLOSE_WAIT 2000 //ms to wait for the client to close the connection + +#define CONTENT_LENGTH_UNKNOWN ((size_t) -1) +#define CONTENT_LENGTH_NOT_SET ((size_t) -2) + +class WebServer; + +typedef struct { + HTTPUploadStatus status; + String filename; + String name; + String type; + size_t totalSize; // file size + size_t currentSize; // size of data currently in buf + uint8_t buf[HTTP_UPLOAD_BUFLEN]; +} HTTPUpload; + +#include "detail/RequestHandler.h" + +namespace fs { +class FS; +} + +class WebServer +{ +public: + WebServer(IPAddress addr, int port = 80); + WebServer(int port = 80); + ~WebServer(); + + void begin(); + void handleClient(); + + void close(); + void stop(); + + bool authenticate(const char * username, const char * password); + void requestAuthentication(); + + typedef std::function THandlerFunction; + void on(const String &uri, THandlerFunction handler); + void on(const String &uri, HTTPMethod method, THandlerFunction fn); + void on(const String &uri, HTTPMethod method, THandlerFunction fn, THandlerFunction ufn); + void addHandler(RequestHandler* handler); + void serveStatic(const char* uri, fs::FS& fs, const char* path, const char* cache_header = NULL ); + void onNotFound(THandlerFunction fn); //called when handler is not assigned + void onFileUpload(THandlerFunction fn); //handle file uploads + + String uri() { return _currentUri; } + HTTPMethod method() { return _currentMethod; } + WiFiClient client() { return _currentClient; } + HTTPUpload& upload() { return _currentUpload; } + + String arg(String name); // get request argument value by name + String arg(int i); // get request argument value by number + String argName(int i); // get request argument name by number + int args(); // get arguments count + bool hasArg(String name); // check if argument exists + void collectHeaders(const char* headerKeys[], const size_t headerKeysCount); // set the request headers to collect + String header(String name); // get request header value by name + String header(int i); // get request header value by number + String headerName(int i); // get request header name by number + int headers(); // get header count + bool hasHeader(String name); // check if header exists + + String hostHeader(); // get request host header if available or empty String if not + + // send response to the client + // code - HTTP response code, can be 200 or 404 + // content_type - HTTP content type, like "text/plain" or "image/png" + // content - actual content body + void send(int code, const char* content_type = NULL, const String& content = String("")); + void send(int code, char* content_type, const String& content); + void send(int code, const String& content_type, const String& content); + void send_P(int code, PGM_P content_type, PGM_P content); + void send_P(int code, PGM_P content_type, PGM_P content, size_t contentLength); + + void setContentLength(size_t contentLength); + void sendHeader(const String& name, const String& value, bool first = false); + void sendContent(const String& content); + void sendContent_P(PGM_P content); + void sendContent_P(PGM_P content, size_t size); + + static String urlDecode(const String& text); + +template size_t streamFile(T &file, const String& contentType){ + setContentLength(file.size()); + if (String(file.name()).endsWith(".gz") && + contentType != "application/x-gzip" && + contentType != "application/octet-stream"){ + sendHeader("Content-Encoding", "gzip"); + } + send(200, contentType, ""); + return _currentClient.write(file); +} + +protected: + void _addRequestHandler(RequestHandler* handler); + void _handleRequest(); + bool _parseRequest(WiFiClient& client); + void _parseArguments(String data); + static String _responseCodeToString(int code); + bool _parseForm(WiFiClient& client, String boundary, uint32_t len); + bool _parseFormUploadAborted(); + void _uploadWriteByte(uint8_t b); + uint8_t _uploadReadByte(WiFiClient& client); + void _prepareHeader(String& response, int code, const char* content_type, size_t contentLength); + bool _collectHeader(const char* headerName, const char* headerValue); + + struct RequestArgument { + String key; + String value; + }; + + WiFiServer _server; + + WiFiClient _currentClient; + HTTPMethod _currentMethod; + String _currentUri; + uint8_t _currentVersion; + HTTPClientStatus _currentStatus; + unsigned long _statusChange; + + RequestHandler* _currentHandler; + RequestHandler* _firstHandler; + RequestHandler* _lastHandler; + THandlerFunction _notFoundHandler; + THandlerFunction _fileUploadHandler; + + int _currentArgCount; + RequestArgument* _currentArgs; + HTTPUpload _currentUpload; + + int _headerKeysCount; + RequestArgument* _currentHeaders; + size_t _contentLength; + String _responseHeaders; + + String _hostHeader; + bool _chunked; + +}; + + +#endif //WebServer_H diff --git a/examples/Advanced/WiFiSetting/WiFiSetting.ino b/examples/Advanced/WiFiSetting/WiFiSetting.ino new file mode 100644 index 00000000..2946fc1b --- /dev/null +++ b/examples/Advanced/WiFiSetting/WiFiSetting.ino @@ -0,0 +1,229 @@ +#include +#include +#include +#include +#include "WebServer.h" +#include + +const IPAddress apIP(192, 168, 4, 1); +const char* apSSID = "M5STACK_SETUP"; +boolean settingMode; +String ssidList; +String wifi_ssid; +String wifi_password; + +// DNSServer dnsServer; +WebServer webServer(80); + +// wifi config store +Preferences preferences; + +void setup() { + m5.begin(); + preferences.begin("wifi-config"); + + delay(10); + if (restoreConfig()) { + if (checkConnection()) { + settingMode = false; + startWebServer(); + return; + } + } + settingMode = true; + setupMode(); +} + +void loop() { + if (settingMode) { + } + webServer.handleClient(); +} + +boolean restoreConfig() { + wifi_ssid = preferences.getString("WIFI_SSID"); + wifi_password = preferences.getString("WIFI_PASSWD"); + Serial.print("WIFI-SSID: "); + M5.Lcd.print("WIFI-SSID: "); + Serial.println(wifi_ssid); + M5.Lcd.println(wifi_ssid); + Serial.print("WIFI-PASSWD: "); + M5.Lcd.print("WIFI-PASSWD: "); + Serial.println(wifi_password); + M5.Lcd.println(wifi_password); + WiFi.begin(wifi_ssid.c_str(), wifi_password.c_str()); + + if(wifi_ssid.length() > 0) { + return true; +} else { + return false; + } +} + +boolean checkConnection() { + int count = 0; + Serial.print("Waiting for Wi-Fi connection"); + M5.Lcd.print("Waiting for Wi-Fi connection"); + while ( count < 30 ) { + if (WiFi.status() == WL_CONNECTED) { + Serial.println(); + M5.Lcd.println(); + Serial.println("Connected!"); + M5.Lcd.println("Connected!"); + return (true); + } + delay(500); + Serial.print("."); + M5.Lcd.print("."); + count++; + } + Serial.println("Timed out."); + M5.Lcd.println("Timed out."); + return false; +} + +void startWebServer() { + if (settingMode) { + Serial.print("Starting Web Server at "); + M5.Lcd.print("Starting Web Server at "); + Serial.println(WiFi.softAPIP()); + M5.Lcd.println(WiFi.softAPIP()); + webServer.on("/settings", []() { + String s = "

Wi-Fi Settings

Please enter your password by selecting the SSID.

"; + s += "

Password:
"; + webServer.send(200, "text/html", makePage("Wi-Fi Settings", s)); + }); + webServer.on("/setap", []() { + String ssid = urlDecode(webServer.arg("ssid")); + Serial.print("SSID: "); + M5.Lcd.print("SSID: "); + Serial.println(ssid); + M5.Lcd.println(ssid); + String pass = urlDecode(webServer.arg("pass")); + Serial.print("Password: "); + M5.Lcd.print("Password: "); + Serial.println(pass); + M5.Lcd.println(pass); + Serial.println("Writing SSID to EEPROM..."); + M5.Lcd.println("Writing SSID to EEPROM..."); + + // Store wifi config + Serial.println("Writing Password to nvr..."); + M5.Lcd.println("Writing Password to nvr..."); + preferences.putString("WIFI_SSID", ssid); + preferences.putString("WIFI_PASSWD", pass); + + Serial.println("Write nvr done!"); + M5.Lcd.println("Write nvr done!"); + String s = "

Setup complete.

device will be connected to \""; + s += ssid; + s += "\" after the restart."; + webServer.send(200, "text/html", makePage("Wi-Fi Settings", s)); + delay(3000); + ESP.restart(); + }); + webServer.onNotFound([]() { + String s = "

AP mode

Wi-Fi Settings

"; + webServer.send(200, "text/html", makePage("AP mode", s)); + }); + } + else { + Serial.print("Starting Web Server at "); + M5.Lcd.print("Starting Web Server at "); + Serial.println(WiFi.localIP()); + M5.Lcd.println(WiFi.localIP()); + webServer.on("/", []() { + String s = "

STA mode

Reset Wi-Fi Settings

"; + webServer.send(200, "text/html", makePage("STA mode", s)); + }); + webServer.on("/reset", []() { + // reset the wifi config + preferences.remove("WIFI_SSID"); + preferences.remove("WIFI_PASSWD"); + String s = "

Wi-Fi settings was reset.

Please reset device.

"; + webServer.send(200, "text/html", makePage("Reset Wi-Fi Settings", s)); + delay(3000); + ESP.restart(); + }); + } + webServer.begin(); +} + +void setupMode() { + WiFi.mode(WIFI_MODE_STA); + WiFi.disconnect(); + delay(100); + int n = WiFi.scanNetworks(); + delay(100); + Serial.println(""); + M5.Lcd.println(""); + for (int i = 0; i < n; ++i) { + ssidList += ""; + } + delay(100); + WiFi.softAPConfig(apIP, apIP, IPAddress(255, 255, 255, 0)); + WiFi.softAP(apSSID); + WiFi.mode(WIFI_MODE_AP); + // WiFi.softAPConfig(IPAddress local_ip, IPAddress gateway, IPAddress subnet); + // WiFi.softAP(const char* ssid, const char* passphrase = NULL, int channel = 1, int ssid_hidden = 0); + // dnsServer.start(53, "*", apIP); + startWebServer(); + Serial.print("Starting Access Point at \""); + M5.Lcd.print("Starting Access Point at \""); + Serial.print(apSSID); + M5.Lcd.print(apSSID); + Serial.println("\""); + M5.Lcd.println("\""); +} + +String makePage(String title, String contents) { + String s = ""; + s += ""; + s += ""; + s += title; + s += ""; + s += contents; + s += ""; + return s; +} + +String urlDecode(String input) { + String s = input; + s.replace("%20", " "); + s.replace("+", " "); + s.replace("%21", "!"); + s.replace("%22", "\""); + s.replace("%23", "#"); + s.replace("%24", "$"); + s.replace("%25", "%"); + s.replace("%26", "&"); + s.replace("%27", "\'"); + s.replace("%28", "("); + s.replace("%29", ")"); + s.replace("%30", "*"); + s.replace("%31", "+"); + s.replace("%2C", ","); + s.replace("%2E", "."); + s.replace("%2F", "/"); + s.replace("%2C", ","); + s.replace("%3A", ":"); + s.replace("%3A", ";"); + s.replace("%3C", "<"); + s.replace("%3D", "="); + s.replace("%3E", ">"); + s.replace("%3F", "?"); + s.replace("%40", "@"); + s.replace("%5B", "["); + s.replace("%5C", "\\"); + s.replace("%5D", "]"); + s.replace("%5E", "^"); + s.replace("%5F", "-"); + s.replace("%60", "`"); + return s; +} \ No newline at end of file diff --git a/examples/Advanced/WiFiSetting/detail/RequestHandler.h b/examples/Advanced/WiFiSetting/detail/RequestHandler.h new file mode 100644 index 00000000..c1cc909d --- /dev/null +++ b/examples/Advanced/WiFiSetting/detail/RequestHandler.h @@ -0,0 +1,19 @@ +#ifndef REQUESTHANDLER_H +#define REQUESTHANDLER_H + +class RequestHandler { +public: + virtual ~RequestHandler() { } + virtual bool canHandle(HTTPMethod method, String uri) { (void) method; (void) uri; return false; } + virtual bool canUpload(String uri) { (void) uri; return false; } + virtual bool handle(WebServer& server, HTTPMethod requestMethod, String requestUri) { (void) server; (void) requestMethod; (void) requestUri; return false; } + virtual void upload(WebServer& server, String requestUri, HTTPUpload& upload) { (void) server; (void) requestUri; (void) upload; } + + RequestHandler* next() { return _next; } + void next(RequestHandler* r) { _next = r; } + +private: + RequestHandler* _next = nullptr; +}; + +#endif //REQUESTHANDLER_H diff --git a/examples/Advanced/WiFiSetting/detail/RequestHandlersImpl.h b/examples/Advanced/WiFiSetting/detail/RequestHandlersImpl.h new file mode 100644 index 00000000..04bcd05f --- /dev/null +++ b/examples/Advanced/WiFiSetting/detail/RequestHandlersImpl.h @@ -0,0 +1,153 @@ +#ifndef REQUESTHANDLERSIMPL_H +#define REQUESTHANDLERSIMPL_H + +#include "RequestHandler.h" + +class FunctionRequestHandler : public RequestHandler { +public: + FunctionRequestHandler(WebServer::THandlerFunction fn, WebServer::THandlerFunction ufn, const String &uri, HTTPMethod method) + : _fn(fn) + , _ufn(ufn) + , _uri(uri) + , _method(method) + { + } + + bool canHandle(HTTPMethod requestMethod, String requestUri) override { + if (_method != HTTP_ANY && _method != requestMethod) + return false; + + if (requestUri != _uri) + return false; + + return true; + } + + bool canUpload(String requestUri) override { + if (!_ufn || !canHandle(HTTP_POST, requestUri)) + return false; + + return true; + } + + bool handle(WebServer& server, HTTPMethod requestMethod, String requestUri) override { + (void) server; + if (!canHandle(requestMethod, requestUri)) + return false; + + _fn(); + return true; + } + + void upload(WebServer& server, String requestUri, HTTPUpload& upload) override { + (void) server; + (void) upload; + if (canUpload(requestUri)) + _ufn(); + } + +protected: + WebServer::THandlerFunction _fn; + WebServer::THandlerFunction _ufn; + String _uri; + HTTPMethod _method; +}; + +class StaticRequestHandler : public RequestHandler { +public: + StaticRequestHandler(FS& fs, const char* path, const char* uri, const char* cache_header) + : _fs(fs) + , _uri(uri) + , _path(path) + , _cache_header(cache_header) + { + _isFile = fs.exists(path); + // DEBUGV("StaticRequestHandler: path=%s uri=%s isFile=%d, cache_header=%s\r\n", path, uri, _isFile, cache_header); + _baseUriLength = _uri.length(); + } + + bool canHandle(HTTPMethod requestMethod, String requestUri) override { + if (requestMethod != HTTP_GET) + return false; + + if ((_isFile && requestUri != _uri) || !requestUri.startsWith(_uri)) + return false; + + return true; + } + + bool handle(WebServer& server, HTTPMethod requestMethod, String requestUri) override { + if (!canHandle(requestMethod, requestUri)) + return false; + + // DEBUGV("StaticRequestHandler::handle: request=%s _uri=%s\r\n", requestUri.c_str(), _uri.c_str()); + + String path(_path); + + if (!_isFile) { + // Base URI doesn't point to a file. + // If a directory is requested, look for index file. + if (requestUri.endsWith("/")) requestUri += "index.htm"; + + // Append whatever follows this URI in request to get the file path. + path += requestUri.substring(_baseUriLength); + } + // DEBUGV("StaticRequestHandler::handle: path=%s, isFile=%d\r\n", path.c_str(), _isFile); + + String contentType = getContentType(path); + + // look for gz file, only if the original specified path is not a gz. So part only works to send gzip via content encoding when a non compressed is asked for + // if you point the the path to gzip you will serve the gzip as content type "application/x-gzip", not text or javascript etc... + if (!path.endsWith(".gz") && !_fs.exists(path)) { + String pathWithGz = path + ".gz"; + if(_fs.exists(pathWithGz)) + path += ".gz"; + } + + File f = _fs.open(path, "r"); + if (!f) + return false; + + if (_cache_header.length() != 0) + server.sendHeader("Cache-Control", _cache_header); + + server.streamFile(f, contentType); + return true; + } + + static String getContentType(const String& path) { + if (path.endsWith(".html")) return "text/html"; + else if (path.endsWith(".htm")) return "text/html"; + else if (path.endsWith(".css")) return "text/css"; + else if (path.endsWith(".txt")) return "text/plain"; + else if (path.endsWith(".js")) return "application/javascript"; + else if (path.endsWith(".png")) return "image/png"; + else if (path.endsWith(".gif")) return "image/gif"; + else if (path.endsWith(".jpg")) return "image/jpeg"; + else if (path.endsWith(".ico")) return "image/x-icon"; + else if (path.endsWith(".svg")) return "image/svg+xml"; + else if (path.endsWith(".ttf")) return "application/x-font-ttf"; + else if (path.endsWith(".otf")) return "application/x-font-opentype"; + else if (path.endsWith(".woff")) return "application/font-woff"; + else if (path.endsWith(".woff2")) return "application/font-woff2"; + else if (path.endsWith(".eot")) return "application/vnd.ms-fontobject"; + else if (path.endsWith(".sfnt")) return "application/font-sfnt"; + else if (path.endsWith(".xml")) return "text/xml"; + else if (path.endsWith(".pdf")) return "application/pdf"; + else if (path.endsWith(".zip")) return "application/zip"; + else if(path.endsWith(".gz")) return "application/x-gzip"; + else if (path.endsWith(".appcache")) return "text/cache-manifest"; + return "application/octet-stream"; + } + +protected: + FS _fs; + String _uri; + String _path; + String _cache_header; + bool _isFile; + size_t _baseUriLength; +}; + + +#endif //REQUESTHANDLERSIMPL_H diff --git a/examples/Basics/Display/Display.ino b/examples/Basics/Display/Display.ino index bc0b5425..d7e31bd5 100644 --- a/examples/Basics/Display/Display.ino +++ b/examples/Basics/Display/Display.ino @@ -4,42 +4,42 @@ void setup() { // initialize the M5Stack object - m5.begin(); - m5.lcd.drawBitmap(0, 0, 320, 240, (uint16_t *)gImage_logoM5); + M5.begin(); + M5.lcd.drawBitmap(0, 0, 320, 240, (uint16_t *)gImage_logoM5); delay(500); // Lcd display - m5.Lcd.fillScreen(WHITE); + M5.Lcd.fillScreen(WHITE); delay(500); - m5.Lcd.fillScreen(RED); + M5.Lcd.fillScreen(RED); delay(500); - m5.Lcd.fillScreen(GREEN); + M5.Lcd.fillScreen(GREEN); delay(500); - m5.Lcd.fillScreen(BLUE); + M5.Lcd.fillScreen(BLUE); delay(500); - m5.Lcd.fillScreen(BLACK); + M5.Lcd.fillScreen(BLACK); delay(500); // text print - m5.Lcd.fillScreen(BLACK); - m5.Lcd.setCursor(10, 10); - m5.Lcd.setTextColor(WHITE); - m5.Lcd.setTextSize(1); - m5.Lcd.printf("Display Test!"); + M5.Lcd.fillScreen(BLACK); + M5.Lcd.setCursor(10, 10); + M5.Lcd.setTextColor(WHITE); + M5.Lcd.setTextSize(1); + M5.Lcd.printf("Display Test!"); // draw graphic delay(1000); - m5.Lcd.drawRect(100, 100, 50, 50, BLUE); + M5.Lcd.drawRect(100, 100, 50, 50, BLUE); delay(1000); - m5.Lcd.fillRect(100, 100, 50, 50, BLUE); + M5.Lcd.fillRect(100, 100, 50, 50, BLUE); delay(1000); - m5.Lcd.drawCircle(100, 100, 50, RED); + M5.Lcd.drawCircle(100, 100, 50, RED); delay(1000); - m5.Lcd.fillCircle(100, 100, 50, RED); + M5.Lcd.fillCircle(100, 100, 50, RED); delay(1000); - m5.Lcd.drawTriangle(30, 30, 180, 100, 80, 150, YELLOW); + M5.Lcd.drawTriangle(30, 30, 180, 100, 80, 150, YELLOW); delay(1000); - m5.Lcd.fillTriangle(30, 30, 180, 100, 80, 150, YELLOW); + M5.Lcd.fillTriangle(30, 30, 180, 100, 80, 150, YELLOW); } @@ -47,7 +47,7 @@ void setup() { void loop(){ //rand draw - m5.Lcd.fillTriangle(random(m5.Lcd.width()-1), random(m5.Lcd.height()-1), random(m5.Lcd.width()-1), random(m5.Lcd.height()-1), random(m5.Lcd.width()-1), random(m5.Lcd.height()-1), random(0xfffe)); + M5.Lcd.fillTriangle(random(M5.Lcd.width()-1), random(M5.Lcd.height()-1), random(M5.Lcd.width()-1), random(M5.Lcd.height()-1), random(M5.Lcd.width()-1), random(M5.Lcd.height()-1), random(0xfffe)); - m5.update(); + M5.update(); } diff --git a/examples/Basics/FactoryTest/FactoryTest.ino b/examples/Basics/FactoryTest/FactoryTest.ino index 621dd346..fdff3bd5 100644 --- a/examples/Basics/FactoryTest/FactoryTest.ino +++ b/examples/Basics/FactoryTest/FactoryTest.ino @@ -3,17 +3,17 @@ //TF card test void listDir(fs::FS &fs, const char * dirname, uint8_t levels){ Serial.printf("Listing directory: %s\n", dirname); - m5.Lcd.printf("Listing directory: %s\n", dirname); + M5.Lcd.printf("Listing directory: %s\n", dirname); File root = fs.open(dirname); if(!root){ Serial.println("Failed to open directory"); - m5.Lcd.println("Failed to open directory"); + M5.Lcd.println("Failed to open directory"); return; } if(!root.isDirectory()){ Serial.println("Not a directory"); - m5.Lcd.println("Not a directory"); + M5.Lcd.println("Not a directory"); return; } @@ -21,21 +21,21 @@ void listDir(fs::FS &fs, const char * dirname, uint8_t levels){ while(file){ if(file.isDirectory()){ Serial.print(" DIR : "); - m5.Lcd.print(" DIR : "); + M5.Lcd.print(" DIR : "); Serial.println(file.name()); - m5.Lcd.println(file.name()); + M5.Lcd.println(file.name()); if(levels){ listDir(fs, file.name(), levels -1); } } else { Serial.print(" FILE: "); - m5.Lcd.print(" FILE: "); + M5.Lcd.print(" FILE: "); Serial.print(file.name()); - m5.Lcd.print(file.name()); + M5.Lcd.print(file.name()); Serial.print(" SIZE: "); - m5.Lcd.print(" SIZE: "); + M5.Lcd.print(" SIZE: "); Serial.println(file.size()); - m5.Lcd.println(file.size()); + M5.Lcd.println(file.size()); } file = root.openNextFile(); } @@ -43,40 +43,40 @@ void listDir(fs::FS &fs, const char * dirname, uint8_t levels){ void readFile(fs::FS &fs, const char * path) { Serial.printf("Reading file: %s\n", path); - m5.Lcd.printf("Reading file: %s\n", path); + M5.Lcd.printf("Reading file: %s\n", path); File file = fs.open(path); if(!file){ Serial.println("Failed to open file for reading"); - m5.Lcd.println("Failed to open file for reading"); + M5.Lcd.println("Failed to open file for reading"); return; } Serial.print("Read from file: "); - m5.Lcd.print("Read from file: "); + M5.Lcd.print("Read from file: "); while(file.available()){ int ch = file.read(); Serial.write(ch); - m5.Lcd.write(ch); + M5.Lcd.write(ch); } } void writeFile(fs::FS &fs, const char * path, const char * message){ Serial.printf("Writing file: %s\n", path); - m5.Lcd.printf("Writing file: %s\n", path); + M5.Lcd.printf("Writing file: %s\n", path); File file = fs.open(path, FILE_WRITE); if(!file){ Serial.println("Failed to open file for writing"); - m5.Lcd.println("Failed to open file for writing"); + M5.Lcd.println("Failed to open file for writing"); return; } if(file.print(message)){ Serial.println("File written"); - m5.Lcd.println("File written"); + M5.Lcd.println("File written"); } else { Serial.println("Write failed"); - m5.Lcd.println("Write failed"); + M5.Lcd.println("Write failed"); } } @@ -100,29 +100,29 @@ void wifi_test() { WiFi.disconnect(); delay(100); - m5.lcd.println("scan start"); + M5.lcd.println("scan start"); // WiFi.scanNetworks will return the number of networks found int n = WiFi.scanNetworks(); - m5.lcd.println("scan done"); + M5.lcd.println("scan done"); if (n == 0) { - m5.lcd.println("no networks found"); + M5.lcd.println("no networks found"); } else { - m5.lcd.print(n); - m5.lcd.println(" networks found"); + M5.lcd.print(n); + M5.lcd.println(" networks found"); for (int i = 0; i < n; ++i) { // Print SSID and RSSI for each network found - m5.lcd.print(i + 1); - m5.lcd.print(": "); - m5.lcd.print(WiFi.SSID(i)); - m5.lcd.print(" ("); - m5.lcd.print(WiFi.RSSI(i)); - m5.lcd.print(")"); - m5.lcd.println((WiFi.encryptionType(i) == WIFI_AUTH_OPEN)?" ":"*"); + M5.lcd.print(i + 1); + M5.lcd.print(": "); + M5.lcd.print(WiFi.SSID(i)); + M5.lcd.print(" ("); + M5.lcd.print(WiFi.RSSI(i)); + M5.lcd.print(")"); + M5.lcd.println((WiFi.encryptionType(i) == WIFI_AUTH_OPEN)?" ":"*"); delay(5); } } - m5.lcd.println(""); + M5.lcd.println(""); } bool gpio_test_flg = 0; @@ -153,12 +153,12 @@ void adc_test() { pinMode(35, INPUT); pinMode(36, INPUT); pinMode(34, INPUT); - m5.Lcd.fillScreen(BLACK); + M5.Lcd.fillScreen(BLACK); while(count--) { - m5.Lcd.setCursor(0, 10); - m5.Lcd.setTextColor(WHITE, BLACK); - m5.Lcd.setTextSize(2); - m5.Lcd.printf("ADC1:%d\r\nADC2:%d\r\nADC3:%d\r\n", analogRead(35), analogRead(36), analogRead(34)); + M5.Lcd.setCursor(0, 10); + M5.Lcd.setTextColor(WHITE, BLACK); + M5.Lcd.setTextSize(2); + M5.Lcd.printf("ADC1:%d\r\nADC2:%d\r\nADC3:%d\r\n", analogRead(35), analogRead(36), analogRead(34)); delay(500); } } @@ -411,7 +411,7 @@ void setup() { } // initialize the M5Stack object - m5.begin(); + M5.begin(); // dac test if (gpio_test_flg) @@ -419,26 +419,26 @@ void setup() { adc_test(); } - m5.startupLogo(); + M5.startupLogo(); // Lcd display - m5.lcd.setBrightness(100); - m5.Lcd.fillScreen(BLACK); - m5.Lcd.setCursor(10, 10); - m5.Lcd.setTextColor(WHITE); - m5.Lcd.setTextSize(1); - m5.Lcd.printf("Display Test!"); + M5.lcd.setBrightness(100); + M5.Lcd.fillScreen(BLACK); + M5.Lcd.setCursor(10, 10); + M5.Lcd.setTextColor(WHITE); + M5.Lcd.setTextSize(1); + M5.Lcd.printf("Display Test!"); delay(300); - m5.Lcd.fillScreen(WHITE); + M5.Lcd.fillScreen(WHITE); delay(150); - m5.Lcd.fillScreen(RED); + M5.Lcd.fillScreen(RED); delay(150); - m5.Lcd.fillScreen(GREEN); + M5.Lcd.fillScreen(GREEN); delay(150); - m5.Lcd.fillScreen(BLUE); + M5.Lcd.fillScreen(BLUE); delay(150); - m5.Lcd.fillScreen(BLACK); + M5.Lcd.fillScreen(BLACK); delay(150); // draw graphic @@ -518,42 +518,42 @@ void setup() { //rand draw int i = 250; while(--i) { - m5.Lcd.fillTriangle(random(m5.Lcd.width()-1), random(m5.Lcd.height()-1), random(m5.Lcd.width()-1), random(m5.Lcd.height()-1), random(m5.Lcd.width()-1), random(m5.Lcd.height()-1), random(0xfffe)); + M5.Lcd.fillTriangle(random(M5.Lcd.width()-1), random(M5.Lcd.height()-1), random(M5.Lcd.width()-1), random(M5.Lcd.height()-1), random(M5.Lcd.width()-1), random(M5.Lcd.height()-1), random(0xfffe)); } for(int i=255; i>=0; i--) { - m5.lcd.setBrightness(i); + M5.lcd.setBrightness(i); delay(2); } //wifi test - m5.Lcd.setCursor(0, 10); - m5.Lcd.fillScreen(BLACK); + M5.Lcd.setCursor(0, 10); + M5.Lcd.fillScreen(BLACK); for(int i=0; i<200; i++) { - m5.lcd.setBrightness(i); + M5.lcd.setBrightness(i); delay(2); } - m5.Lcd.println("wifi test:"); + M5.Lcd.println("wifi test:"); wifi_test(); delay(2000); // TF card test - m5.Lcd.fillScreen(BLACK); - m5.Lcd.setCursor(0, 10); - m5.Lcd.printf("TF card test:\r\n"); + M5.Lcd.fillScreen(BLACK); + M5.Lcd.setCursor(0, 10); + M5.Lcd.printf("TF card test:\r\n"); // digitalWrite(TFT_CS, 1); listDir(SD, "/", 0); writeFile(SD, "/hello.txt", "Hello world"); readFile(SD, "/hello.txt"); //Button test - m5.Lcd.println(); - m5.Lcd.println(); - m5.Lcd.print("buttons Test:"); - m5.Lcd.setTextColor(RED); + M5.Lcd.println(); + M5.Lcd.println(); + M5.Lcd.print("buttons Test:"); + M5.Lcd.setTextColor(RED); } // the loop routine runs over and over again forever void loop(){ buttons_test(); - m5.update(); + M5.update(); } diff --git a/examples/Modules/Lora/LoRaDumpRegisters/LoRaDumpRegisters.ino b/examples/Modules/Lora/LoRaDumpRegisters/LoRaDumpRegisters.ino new file mode 100644 index 00000000..73713dec --- /dev/null +++ b/examples/Modules/Lora/LoRaDumpRegisters/LoRaDumpRegisters.ino @@ -0,0 +1,30 @@ +/* + LoRa register dump + + This examples shows how to inspect and output the LoRa radio's + registers on the Serial interface +*/ +#include // include libraries +#include + +void setup() { + M5.begin(); + while (!Serial); + + Serial.println("LoRa Dump Registers"); + + // override the default CS, reset, and IRQ pins (optional) + // LoRa.setPins(7, 6, 1); // set CS, reset, IRQ pin + + if (!LoRa.begin(433E6)) { // initialize ratio at 915 MHz + Serial.println("LoRa init failed. Check your connections."); + while (true); // if failed, do nothing + } + + LoRa.dumpRegisters(Serial); +} + + +void loop() { +} + diff --git a/examples/Modules/Lora/LoRaDuplex/LoRaDuplex.ino b/examples/Modules/Lora/LoRaDuplex/LoRaDuplex.ino new file mode 100644 index 00000000..5cb40895 --- /dev/null +++ b/examples/Modules/Lora/LoRaDuplex/LoRaDuplex.ino @@ -0,0 +1,102 @@ +/* + LoRa Duplex communication + + Sends a message every half second, and polls continually + for new incoming messages. Implements a one-byte addressing scheme, + with 0xFF as the broadcast address. + + Uses readString() from Stream class to read payload. The Stream class' + timeout may affect other functuons, like the radio's callback. For an + + created 28 April 2017 + by Tom Igoe +*/ +#include +#include + +String outgoing; // outgoing message + +byte msgCount = 0; // count of outgoing messages +byte localAddress = 0xBB; // address of this device +byte destination = 0xFF; // destination to send to +long lastSendTime = 0; // last send time +int interval = 2000; // interval between sends + +void setup() { + M5.begin(); + while (!Serial); + + Serial.println("LoRa Duplex"); + + // override the default CS, reset, and IRQ pins (optional) + LoRa.setPins();// set CS, reset, IRQ pin + + if (!LoRa.begin(433E6)) { // initialize ratio at 915 MHz + Serial.println("LoRa init failed. Check your connections."); + while (true); // if failed, do nothing + } + + Serial.println("LoRa init succeeded."); +} + +void loop() { + if (millis() - lastSendTime > interval) { + String message = "HeLoRa World!"; // send a message + sendMessage(message); + Serial.println("Sending " + message); + lastSendTime = millis(); // timestamp the message + interval = random(2000) + 1000; // 2-3 seconds + } + + // parse for a packet, and call onReceive with the result: + onReceive(LoRa.parsePacket()); +} + +void sendMessage(String outgoing) { + LoRa.beginPacket(); // start packet + LoRa.write(destination); // add destination address + LoRa.write(localAddress); // add sender address + LoRa.write(msgCount); // add message ID + LoRa.write(outgoing.length()); // add payload length + LoRa.print(outgoing); // add payload + LoRa.endPacket(); // finish packet and send it + msgCount++; // increment message ID +} + +void onReceive(int packetSize) { + if (packetSize == 0) return; // if there's no packet, return + + // read packet header bytes: + int recipient = LoRa.read(); // recipient address + byte sender = LoRa.read(); // sender address + byte incomingMsgId = LoRa.read(); // incoming msg ID + byte incomingLength = LoRa.read(); // incoming msg length + + String incoming = ""; + + while (LoRa.available()) { + incoming += (char)LoRa.read(); + } + + if (incomingLength != incoming.length()) { // check length for error + Serial.println("error: message length does not match length"); + return; // skip rest of function + } + + // if the recipient isn't this device or broadcast, + if (recipient != localAddress && recipient != 0xFF) { + Serial.println("This message is not for me."); + return; // skip rest of function + } + + // if message is for this device, or broadcast, print details: + Serial.println("Received from: 0x" + String(sender, HEX)); + Serial.println("Sent to: 0x" + String(recipient, HEX)); + Serial.println("Message ID: " + String(incomingMsgId)); + Serial.println("Message length: " + String(incomingLength)); + Serial.println("Message: " + incoming); + Serial.println("RSSI: " + String(LoRa.packetRssi())); + Serial.println("Snr: " + String(LoRa.packetSnr())); + Serial.println(); +} + diff --git a/examples/Modules/Lora/LoRaDuplexCallback/LoRaDuplexCallback.ino b/examples/Modules/Lora/LoRaDuplexCallback/LoRaDuplexCallback.ino new file mode 100644 index 00000000..0a941fdc --- /dev/null +++ b/examples/Modules/Lora/LoRaDuplexCallback/LoRaDuplexCallback.ino @@ -0,0 +1,102 @@ +/* + LoRa Duplex communication wth callback + + Sends a message every half second, and uses callback + for new incoming messages. Implements a one-byte addressing scheme, + with 0xFF as the broadcast address. + + Note: while sending, LoRa radio is not listening for incoming messages. + Note2: when using the callback method, you can't use any of the Stream + functions that rely on the timeout, such as readString, parseInt(), etc. + + created 28 April 2017 + by Tom Igoe +*/ +#include +#include + +String outgoing; // outgoing message +byte msgCount = 0; // count of outgoing messages +byte localAddress = 0xBB; // address of this device +byte destination = 0xFF; // destination to send to +long lastSendTime = 0; // last send time +int interval = 2000; // interval between sends + +void setup() { + M5.begin(); // initialize serial + while (!Serial); + + Serial.println("LoRa Duplex with callback"); + + // override the default CS, reset, and IRQ pins (optional) + LoRa.setPins();// set CS, reset, IRQ pin + + if (!LoRa.begin(433E6)) { // initialize ratio at 915 MHz + Serial.println("LoRa init failed. Check your connections."); + while (true); // if failed, do nothing + } + + LoRa.onReceive(onReceive); + LoRa.receive(); + Serial.println("LoRa init succeeded."); +} + +void loop() { + if (millis() - lastSendTime > interval) { + String message = "HeLoRa World!"; // send a message + sendMessage(message); + Serial.println("Sending " + message); + lastSendTime = millis(); // timestamp the message + interval = random(2000) + 1000; // 2-3 seconds + LoRa.receive(); // go back into receive mode + } +} + +void sendMessage(String outgoing) { + LoRa.beginPacket(); // start packet + LoRa.write(destination); // add destination address + LoRa.write(localAddress); // add sender address + LoRa.write(msgCount); // add message ID + LoRa.write(outgoing.length()); // add payload length + LoRa.print(outgoing); // add payload + LoRa.endPacket(); // finish packet and send it + msgCount++; // increment message ID +} + +void onReceive(int packetSize) { + if (packetSize == 0) return; // if there's no packet, return + + // read packet header bytes: + int recipient = LoRa.read(); // recipient address + byte sender = LoRa.read(); // sender address + byte incomingMsgId = LoRa.read(); // incoming msg ID + byte incomingLength = LoRa.read(); // incoming msg length + + String incoming = ""; // payload of packet + + while (LoRa.available()) { // can't use readString() in callback, so + incoming += (char)LoRa.read(); // add bytes one by one + } + + if (incomingLength != incoming.length()) { // check length for error + Serial.println("error: message length does not match length"); + return; // skip rest of function + } + + // if the recipient isn't this device or broadcast, + if (recipient != localAddress && recipient != 0xFF) { + Serial.println("This message is not for me."); + return; // skip rest of function + } + + // if message is for this device, or broadcast, print details: + Serial.println("Received from: 0x" + String(sender, HEX)); + Serial.println("Sent to: 0x" + String(recipient, HEX)); + Serial.println("Message ID: " + String(incomingMsgId)); + Serial.println("Message length: " + String(incomingLength)); + Serial.println("Message: " + incoming); + Serial.println("RSSI: " + String(LoRa.packetRssi())); + Serial.println("Snr: " + String(LoRa.packetSnr())); + Serial.println(); +} + diff --git a/examples/Modules/Lora/LoRaReceiverCallback/LoRaReceiverCallback.ino b/examples/Modules/Lora/LoRaReceiverCallback/LoRaReceiverCallback.ino new file mode 100644 index 00000000..d6a598ed --- /dev/null +++ b/examples/Modules/Lora/LoRaReceiverCallback/LoRaReceiverCallback.ino @@ -0,0 +1,40 @@ +#include +#include + +void setup() { + Serial.begin(9600); + while (!Serial); + + Serial.println("LoRa Receiver Callback"); + + if (!LoRa.begin(915E6)) { + Serial.println("Starting LoRa failed!"); + while (1); + } + + // register the receive callback + LoRa.onReceive(onReceive); + + // put the radio into receive mode + LoRa.receive(); +} + +void loop() { + // do nothing + LoRa.receive(); +} + +void onReceive(int packetSize) { + // received a packet + Serial.print("Received packet '"); + + // read packet + for (int i = 0; i < packetSize; i++) { + Serial.print((char)LoRa.read()); + } + + // print RSSI of packet + Serial.print("' with RSSI "); + Serial.println(LoRa.packetRssi()); +} + diff --git a/examples/Modules/Lora/LoRaSetSpread/LoRaSetSpread.ino b/examples/Modules/Lora/LoRaSetSpread/LoRaSetSpread.ino new file mode 100644 index 00000000..2c154dd6 --- /dev/null +++ b/examples/Modules/Lora/LoRaSetSpread/LoRaSetSpread.ino @@ -0,0 +1,83 @@ +/* + LoRa Duplex communication with Spreading Factor + + Sends a message every half second, and polls continually + for new incoming messages. Sets the LoRa radio's spreading factor. + + Spreading factor affects how far apart the radio's transmissions + are, across the available bandwidth. Radios with different spreading + factors will not receive each other's transmissions. This is one way you + can filter out radios you want to ignore, without making an addressing scheme. + + Spreading factor affects reliability of transmission at high rates, however, + so avoid a hugh spreading factor when you're sending continually. + + See the Semtech datasheet, http://www.semtech.com/images/datasheet/sx1276.pdf + for more on Spreading Factor. + + created 28 April 2017 + by Tom Igoe +*/ +#include +#include + +byte msgCount = 0; // count of outgoing messages +int interval = 2000; // interval between sends +long lastSendTime = 0; // time of last packet send + +void setup() { + M5.begin(); // initialize serial + while (!Serial); + + Serial.println("LoRa Duplex - Set spreading factor"); + + // override the default CS, reset, and IRQ pins (optional) + LoRa.setPins(); // set CS, reset, IRQ pin + + if (!LoRa.begin(433E6)) { // initialize ratio at 915 MHz + Serial.println("LoRa init failed. Check your connections."); + while (true); // if failed, do nothing + } + + LoRa.setSpreadingFactor(8); // ranges from 6-12,default 7 see API docs + Serial.println("LoRa init succeeded."); +} + +void loop() { + if (millis() - lastSendTime > interval) { + String message = "HeLoRa World! "; // send a message + message += msgCount; + sendMessage(message); + Serial.println("Sending " + message); + lastSendTime = millis(); // timestamp the message + interval = random(2000) + 1000; // 2-3 seconds + msgCount++; + } + + // parse for a packet, and call onReceive with the result: + onReceive(LoRa.parsePacket()); +} + +void sendMessage(String outgoing) { + LoRa.beginPacket(); // start packet + LoRa.print(outgoing); // add payload + LoRa.endPacket(); // finish packet and send it + msgCount++; // increment message ID +} + +void onReceive(int packetSize) { + if (packetSize == 0) return; // if there's no packet, return + + // read packet header bytes: + String incoming = ""; + + while (LoRa.available()) { + incoming += (char)LoRa.read(); + } + + Serial.println("Message: " + incoming); + Serial.println("RSSI: " + String(LoRa.packetRssi())); + Serial.println("Snr: " + String(LoRa.packetSnr())); + Serial.println(); +} + diff --git a/examples/Modules/Lora/LoRaSetSyncWord/LoRaSetSyncWord.ino b/examples/Modules/Lora/LoRaSetSyncWord/LoRaSetSyncWord.ino new file mode 100644 index 00000000..01178424 --- /dev/null +++ b/examples/Modules/Lora/LoRaSetSyncWord/LoRaSetSyncWord.ino @@ -0,0 +1,79 @@ +/* + LoRa Duplex communication with Sync Word + + Sends a message every half second, and polls continually + for new incoming messages. Sets the LoRa radio's Sync Word. + + Spreading factor is basically the radio's network ID. Radios with different + Sync Words will not receive each other's transmissions. This is one way you + can filter out radios you want to ignore, without making an addressing scheme. + + See the Semtech datasheet, http://www.semtech.com/images/datasheet/sx1276.pdf + for more on Sync Word. + + created 28 April 2017 + by Tom Igoe +*/ +#include +#include + +byte msgCount = 0; // count of outgoing messages +int interval = 2000; // interval between sends +long lastSendTime = 0; // time of last packet send + +void setup() { + M5.begin(); // initialize serial + while (!Serial); + + Serial.println("LoRa Duplex - Set sync word"); + + // override the default CS, reset, and IRQ pins (optional) + LoRa.setPins();// set CS, reset, IRQ pin + + if (!LoRa.begin(433E6)) { // initialize ratio at 915 MHz + Serial.println("LoRa init failed. Check your connections."); + while (true); // if failed, do nothing + } + + LoRa.setSyncWord(0xF3); // ranges from 0-0xFF, default 0x34, see API docs + Serial.println("LoRa init succeeded."); +} + +void loop() { + if (millis() - lastSendTime > interval) { + String message = "HeLoRa World! "; // send a message + message += msgCount; + sendMessage(message); + Serial.println("Sending " + message); + lastSendTime = millis(); // timestamp the message + interval = random(2000) + 1000; // 2-3 seconds + msgCount++; + } + + // parse for a packet, and call onReceive with the result: + onReceive(LoRa.parsePacket()); +} + +void sendMessage(String outgoing) { + LoRa.beginPacket(); // start packet + LoRa.print(outgoing); // add payload + LoRa.endPacket(); // finish packet and send it + msgCount++; // increment message ID +} + +void onReceive(int packetSize) { + if (packetSize == 0) return; // if there's no packet, return + + // read packet header bytes: + String incoming = ""; + + while (LoRa.available()) { + incoming += (char)LoRa.read(); + } + + Serial.println("Message: " + incoming); + Serial.println("RSSI: " + String(LoRa.packetRssi())); + Serial.println("Snr: " + String(LoRa.packetSnr())); + Serial.println(); +} + diff --git a/examples/Modules/MPU9250/GetRawData/GetRawData.ino b/examples/Modules/MPU9250/GetRawData/GetRawData.ino deleted file mode 100644 index b4ba11ad..00000000 --- a/examples/Modules/MPU9250/GetRawData/GetRawData.ino +++ /dev/null @@ -1,47 +0,0 @@ -#define MPU9250_INSDE -#include - -MPU9250 mpusensor; - -void setup() { - - M5.begin(); - Serial.println("MPU9250 begin Start!"); - - // Start the MPU9250 - Wire.begin(); - mpusensor.setWire(&Wire); - mpusensor.beginAccel(); - mpusensor.beginMag(); - - // you can set your own offset for mag values - // mpusensor.magXOffset = -50; - // mpusensor.magYOffset = -55; - // mpusensor.magZOffset = -10; - M5.Lcd.setCursor(0, 0); - M5.Lcd.setTextColor(GREEN, BLACK); - M5.Lcd.setTextSize(2); -} - -void loop() { - - // Get the Accelerometer data - mpusensor.accelUpdate(); - M5.Lcd.setCursor(0, 15); - M5.Lcd.println("print accel values"); - M5.Lcd.printf("accelX: %04.2f\r\n", mpusensor.accelX()); - M5.Lcd.printf("accelY: %04.2f\r\n", mpusensor.accelY()); - M5.Lcd.printf("accelZ: %04.2f\r\n", mpusensor.accelZ()); - M5.Lcd.printf("accelSqrt: %4.2f\r\n", mpusensor.accelSqrt()); - - // Get the Magnetometer data - mpusensor.magUpdate(); - M5.Lcd.println(""); - M5.Lcd.println("print mag values"); - M5.Lcd.printf("magX: %03d\r\n", mpusensor.magX()); - M5.Lcd.printf("maxY: %03d\r\n", mpusensor.magY()); - M5.Lcd.printf("magZ: %03d\r\n", mpusensor.magZ()); - M5.Lcd.printf("horizontal dirt: %04.2f\r\n", mpusensor.magHorizDirection()); - M5.Lcd.println("at " + String(millis()) + "ms"); - delay(10); -} diff --git a/examples/Modules/MPU9250/MPU9250BasicAHRS/MPU9250BasicAHRS.ino b/examples/Modules/MPU9250/MPU9250BasicAHRS/MPU9250BasicAHRS.ino new file mode 100644 index 00000000..35f90b7d --- /dev/null +++ b/examples/Modules/MPU9250/MPU9250BasicAHRS/MPU9250BasicAHRS.ino @@ -0,0 +1,420 @@ +/* MPU9250 Basic Example Code + by: Kris Winer + date: April 1, 2014 + license: Beerware - Use this code however you'd like. If you + find it useful you can buy me a beer some time. + Modified by Brent Wilkins July 19, 2016 + + Demonstrate basic MPU-9250 functionality including parameterizing the register + addresses, initializing the sensor, getting properly scaled accelerometer, + gyroscope, and magnetometer data out. Added display functions to allow display + to on breadboard monitor. Addition of 9 DoF sensor fusion using open source + Madgwick and Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini + and the Teensy 3.1. + */ + +#include +#include "utility/MPU9250.h" +#include "utility/quaternionFilters.h" + +#define AHRS true // Set to false for basic data read +#define SerialDebug true // Set to true to get Serial output for debugging +#define LCD + +MPU9250 IMU; + +void setup() +{ + M5.begin(); + Wire.begin(); + +#ifdef LCD + // Start device display with ID of sensor + M5.Lcd.fillScreen(BLACK); + M5.Lcd.setTextColor(WHITE ,BLACK); // Set pixel color; 1 on the monochrome screen + M5.Lcd.setTextSize(2); + M5.Lcd.setCursor(0,0); M5.Lcd.print("MPU9250"); + M5.Lcd.setTextSize(1); + M5.Lcd.setCursor(0, 20); M5.Lcd.print("9-DOF 16-bit"); + M5.Lcd.setCursor(0, 30); M5.Lcd.print("motion sensor"); + M5.Lcd.setCursor(20,40); M5.Lcd.print("60 ug LSB"); + delay(1000); + + // Set up for data display + M5.Lcd.setTextSize(1); // Set text size to normal, 2 is twice normal etc. + M5.Lcd.fillScreen(BLACK); // clears the screen and buffer +#endif // LCD + + // Read the WHO_AM_I register, this is a good test of communication + byte c = IMU.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); + Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); + Serial.print(" I should be "); Serial.println(0x71, HEX); + +#ifdef LCD + M5.Lcd.setCursor(20,0); M5.Lcd.print("MPU9250"); + M5.Lcd.setCursor(0,10); M5.Lcd.print("I AM"); + M5.Lcd.setCursor(0,20); M5.Lcd.print(c, HEX); + M5.Lcd.setCursor(0,30); M5.Lcd.print("I Should Be"); + M5.Lcd.setCursor(0,40); M5.Lcd.print(0x71, HEX); + delay(1000); +#endif // LCD + + if (c == 0x71) // WHO_AM_I should always be 0x68 + { + Serial.println("MPU9250 is online..."); + + // Start by performing self test and reporting values + IMU.MPU9250SelfTest(IMU.SelfTest); + Serial.print("x-axis self test: acceleration trim within : "); + Serial.print(IMU.SelfTest[0],1); Serial.println("% of factory value"); + Serial.print("y-axis self test: acceleration trim within : "); + Serial.print(IMU.SelfTest[1],1); Serial.println("% of factory value"); + Serial.print("z-axis self test: acceleration trim within : "); + Serial.print(IMU.SelfTest[2],1); Serial.println("% of factory value"); + Serial.print("x-axis self test: gyration trim within : "); + Serial.print(IMU.SelfTest[3],1); Serial.println("% of factory value"); + Serial.print("y-axis self test: gyration trim within : "); + Serial.print(IMU.SelfTest[4],1); Serial.println("% of factory value"); + Serial.print("z-axis self test: gyration trim within : "); + Serial.print(IMU.SelfTest[5],1); Serial.println("% of factory value"); + + // Calibrate gyro and accelerometers, load biases in bias registers + IMU.calibrateMPU9250(IMU.gyroBias, IMU.accelBias); + +#ifdef LCD + M5.Lcd.fillScreen(BLACK); + M5.Lcd.setTextSize(1); + M5.Lcd.setCursor(0, 0); M5.Lcd.print("MPU9250 bias"); + M5.Lcd.setCursor(0, 16); M5.Lcd.print(" x y z "); + + M5.Lcd.setCursor(0, 32); M5.Lcd.print((int)(1000*IMU.accelBias[0])); + M5.Lcd.setCursor(32, 32); M5.Lcd.print((int)(1000*IMU.accelBias[1])); + M5.Lcd.setCursor(64, 32); M5.Lcd.print((int)(1000*IMU.accelBias[2])); + M5.Lcd.setCursor(96, 32); M5.Lcd.print("mg"); + + M5.Lcd.setCursor(0, 48); M5.Lcd.print(IMU.gyroBias[0], 1); + M5.Lcd.setCursor(32, 48); M5.Lcd.print(IMU.gyroBias[1], 1); + M5.Lcd.setCursor(64, 48); M5.Lcd.print(IMU.gyroBias[2], 1); + M5.Lcd.setCursor(96, 48); M5.Lcd.print("o/s"); + delay(1000); +#endif // LCD + + IMU.initMPU9250(); + // Initialize device for active mode read of acclerometer, gyroscope, and + // temperature + Serial.println("MPU9250 initialized for active data mode...."); + + // Read the WHO_AM_I register of the magnetometer, this is a good test of + // communication + byte d = IMU.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); + Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); + Serial.print(" I should be "); Serial.println(0x48, HEX); + +#ifdef LCD + M5.Lcd.fillScreen(BLACK); + M5.Lcd.setCursor(20,0); M5.Lcd.print("AK8963"); + M5.Lcd.setCursor(0,10); M5.Lcd.print("I AM"); + M5.Lcd.setCursor(0,20); M5.Lcd.print(d, HEX); + M5.Lcd.setCursor(0,30); M5.Lcd.print("I Should Be"); + M5.Lcd.setCursor(0,40); M5.Lcd.print(0x48, HEX); + delay(1000); +#endif // LCD + + // Get magnetometer calibration from AK8963 ROM + IMU.initAK8963(IMU.magCalibration); + // Initialize device for active mode read of magnetometer + Serial.println("AK8963 initialized for active data mode...."); + if (Serial) + { + // Serial.println("Calibration values: "); + Serial.print("X-Axis sensitivity adjustment value "); + Serial.println(IMU.magCalibration[0], 2); + Serial.print("Y-Axis sensitivity adjustment value "); + Serial.println(IMU.magCalibration[1], 2); + Serial.print("Z-Axis sensitivity adjustment value "); + Serial.println(IMU.magCalibration[2], 2); + } + +#ifdef LCD + M5.Lcd.fillScreen(BLACK); + M5.Lcd.setCursor(20,0); M5.Lcd.print("AK8963"); + M5.Lcd.setCursor(0,10); M5.Lcd.print("ASAX "); M5.Lcd.setCursor(50,10); + M5.Lcd.print(IMU.magCalibration[0], 2); + M5.Lcd.setCursor(0,20); M5.Lcd.print("ASAY "); M5.Lcd.setCursor(50,20); + M5.Lcd.print(IMU.magCalibration[1], 2); + M5.Lcd.setCursor(0,30); M5.Lcd.print("ASAZ "); M5.Lcd.setCursor(50,30); + M5.Lcd.print(IMU.magCalibration[2], 2); + delay(1000); + #endif // LCD + } // if (c == 0x71) + else + { + Serial.print("Could not connect to MPU9250: 0x"); + Serial.println(c, HEX); + while(1) ; // Loop forever if communication doesn't happen + } + + M5.Lcd.setTextSize(1); + M5.Lcd.setTextColor(GREEN ,BLACK); + M5.Lcd.fillScreen(BLACK); +} + +void loop() +{ + // If intPin goes high, all data registers have new data + // On interrupt, check if data ready interrupt + if (IMU.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) + { + IMU.readAccelData(IMU.accelCount); // Read the x/y/z adc values + IMU.getAres(); + + // Now we'll calculate the accleration value into actual g's + // This depends on scale being set + IMU.ax = (float)IMU.accelCount[0]*IMU.aRes; // - accelBias[0]; + IMU.ay = (float)IMU.accelCount[1]*IMU.aRes; // - accelBias[1]; + IMU.az = (float)IMU.accelCount[2]*IMU.aRes; // - accelBias[2]; + + IMU.readGyroData(IMU.gyroCount); // Read the x/y/z adc values + IMU.getGres(); + + // Calculate the gyro value into actual degrees per second + // This depends on scale being set + IMU.gx = (float)IMU.gyroCount[0]*IMU.gRes; + IMU.gy = (float)IMU.gyroCount[1]*IMU.gRes; + IMU.gz = (float)IMU.gyroCount[2]*IMU.gRes; + + IMU.readMagData(IMU.magCount); // Read the x/y/z adc values + IMU.getMres(); + // User environmental x-axis correction in milliGauss, should be + // automatically calculated + IMU.magbias[0] = +470.; + // User environmental x-axis correction in milliGauss TODO axis?? + IMU.magbias[1] = +120.; + // User environmental x-axis correction in milliGauss + IMU.magbias[2] = +125.; + + // Calculate the magnetometer values in milliGauss + // Include factory calibration per data sheet and user environmental + // corrections + // Get actual magnetometer value, this depends on scale being set + IMU.mx = (float)IMU.magCount[0]*IMU.mRes*IMU.magCalibration[0] - + IMU.magbias[0]; + IMU.my = (float)IMU.magCount[1]*IMU.mRes*IMU.magCalibration[1] - + IMU.magbias[1]; + IMU.mz = (float)IMU.magCount[2]*IMU.mRes*IMU.magCalibration[2] - + IMU.magbias[2]; + } // if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) + + // Must be called before updating quaternions! + IMU.updateTime(); + + // Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of + // the magnetometer; the magnetometer z-axis (+ down) is opposite to z-axis + // (+ up) of accelerometer and gyro! We have to make some allowance for this + // orientationmismatch in feeding the output to the quaternion filter. For the + // MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward + // along the x-axis just like in the LSM9DS0 sensor. This rotation can be + // modified to allow any convenient orientation convention. This is ok by + // aircraft orientation standards! Pass gyro rate as rad/s +// MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz); + MahonyQuaternionUpdate(IMU.ax, IMU.ay, IMU.az, IMU.gx*DEG_TO_RAD, + IMU.gy*DEG_TO_RAD, IMU.gz*DEG_TO_RAD, IMU.my, + IMU.mx, IMU.mz, IMU.deltat); + + if (!AHRS) + { + IMU.delt_t = millis() - IMU.count; + if (IMU.delt_t > 500) + { + if(SerialDebug) + { + // Print acceleration values in milligs! + Serial.print("X-acceleration: "); Serial.print(1000*IMU.ax); + Serial.print(" mg "); + Serial.print("Y-acceleration: "); Serial.print(1000*IMU.ay); + Serial.print(" mg "); + Serial.print("Z-acceleration: "); Serial.print(1000*IMU.az); + Serial.println(" mg "); + + // Print gyro values in degree/sec + Serial.print("X-gyro rate: "); Serial.print(IMU.gx, 3); + Serial.print(" degrees/sec "); + Serial.print("Y-gyro rate: "); Serial.print(IMU.gy, 3); + Serial.print(" degrees/sec "); + Serial.print("Z-gyro rate: "); Serial.print(IMU.gz, 3); + Serial.println(" degrees/sec"); + + // Print mag values in degree/sec + Serial.print("X-mag field: "); Serial.print(IMU.mx); + Serial.print(" mG "); + Serial.print("Y-mag field: "); Serial.print(IMU.my); + Serial.print(" mG "); + Serial.print("Z-mag field: "); Serial.print(IMU.mz); + Serial.println(" mG"); + + IMU.tempCount = IMU.readTempData(); // Read the adc values + // Temperature in degrees Centigrade + IMU.temperature = ((float) IMU.tempCount) / 333.87 + 21.0; + // Print temperature in degrees Centigrade + Serial.print("Temperature is "); Serial.print(IMU.temperature, 1); + Serial.println(" degrees C"); + Serial.println(""); + } + +#ifdef LCD + M5.Lcd.fillScreen(BLACK); + M5.Lcd.setTextColor(GREEN ,BLACK); + M5.Lcd.setCursor(0, 0); M5.Lcd.print("MPU9250/AK8963"); + M5.Lcd.setCursor(0, 32); M5.Lcd.print(" x y z "); + + M5.Lcd.setCursor(0, 48); M5.Lcd.print((int)(1000*IMU.ax)); + M5.Lcd.setCursor(32, 48); M5.Lcd.print((int)(1000*IMU.ay)); + M5.Lcd.setCursor(64, 48); M5.Lcd.print((int)(1000*IMU.az)); + M5.Lcd.setCursor(96, 48); M5.Lcd.print("mg"); + + M5.Lcd.setCursor(0, 64); M5.Lcd.print((int)(IMU.gx)); + M5.Lcd.setCursor(32, 64); M5.Lcd.print((int)(IMU.gy)); + M5.Lcd.setCursor(64, 64); M5.Lcd.print((int)(IMU.gz)); + M5.Lcd.setCursor(96, 64); M5.Lcd.print("o/s"); + + M5.Lcd.setCursor(0, 96); M5.Lcd.print((int)(IMU.mx)); + M5.Lcd.setCursor(32, 96); M5.Lcd.print((int)(IMU.my)); + M5.Lcd.setCursor(64, 96); M5.Lcd.print((int)(IMU.mz)); + M5.Lcd.setCursor(96, 96); M5.Lcd.print("mG"); + + M5.Lcd.setCursor(0, 128); M5.Lcd.print("Gyro T "); + M5.Lcd.setCursor(50, 128); M5.Lcd.print(IMU.temperature, 1); + M5.Lcd.print(" C"); +#endif // LCD + + IMU.count = millis(); + // digitalWrite(myLed, !digitalRead(myLed)); // toggle led + } // if (IMU.delt_t > 500) + } // if (!AHRS) + else + { + // Serial print and/or display at 0.5 s rate independent of data rates + IMU.delt_t = millis() - IMU.count; + + // update LCD once per half-second independent of read rate + // if (IMU.delt_t > 500) + if (IMU.delt_t > 100) + { + if(SerialDebug) + { + Serial.print("ax = "); Serial.print((int)1000*IMU.ax); + Serial.print(" ay = "); Serial.print((int)1000*IMU.ay); + Serial.print(" az = "); Serial.print((int)1000*IMU.az); + Serial.println(" mg"); + + Serial.print("gx = "); Serial.print( IMU.gx, 2); + Serial.print(" gy = "); Serial.print( IMU.gy, 2); + Serial.print(" gz = "); Serial.print( IMU.gz, 2); + Serial.println(" deg/s"); + + Serial.print("mx = "); Serial.print( (int)IMU.mx ); + Serial.print(" my = "); Serial.print( (int)IMU.my ); + Serial.print(" mz = "); Serial.print( (int)IMU.mz ); + Serial.println(" mG"); + + Serial.print("q0 = "); Serial.print(*getQ()); + Serial.print(" qx = "); Serial.print(*(getQ() + 1)); + Serial.print(" qy = "); Serial.print(*(getQ() + 2)); + Serial.print(" qz = "); Serial.println(*(getQ() + 3)); + } + + // M5.Lcd.setCursor(0, 0); + // M5.Lcd.printf("aX:%8.2f aY:%8.2f aZ:%8.2f mg \r\n", (int)1000*IMU.ax, (int)1000*IMU.ay, (int)1000*IMU.az); + +// Define output variables from updated quaternion---these are Tait-Bryan +// angles, commonly used in aircraft orientation. In this coordinate system, +// the positive z-axis is down toward Earth. Yaw is the angle between Sensor +// x-axis and Earth magnetic North (or true North if corrected for local +// declination, looking down on the sensor positive yaw is counterclockwise. +// Pitch is angle between sensor x-axis and Earth ground plane, toward the +// Earth is positive, up toward the sky is negative. Roll is angle between +// sensor y-axis and Earth ground plane, y-axis up is positive roll. These +// arise from the definition of the homogeneous rotation matrix constructed +// from quaternions. Tait-Bryan angles as well as Euler angles are +// non-commutative; that is, the get the correct orientation the rotations +// must be applied in the correct order which for this configuration is yaw, +// pitch, and then roll. +// For more see +// http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles +// which has additional links. + IMU.yaw = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() * + *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1) * *(getQ()+1) + - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) * *(getQ()+3)); + IMU.pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() * + *(getQ()+2))); + IMU.roll = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) * + *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1) * *(getQ()+1) + - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) * *(getQ()+3)); + IMU.pitch *= RAD_TO_DEG; + IMU.yaw *= RAD_TO_DEG; + // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is + // 8° 30' E ± 0° 21' (or 8.5°) on 2016-07-19 + // - http://www.ngdc.noaa.gov/geomag-web/#declination + IMU.yaw -= 8.5; + IMU.roll *= RAD_TO_DEG; + + if(Serial) + { + Serial.print("Yaw, Pitch, Roll: "); + Serial.print(IMU.yaw, 2); + Serial.print(", "); + Serial.print(IMU.pitch, 2); + Serial.print(", "); + Serial.println(IMU.roll, 2); + + Serial.print("rate = "); + Serial.print((float)IMU.sumCount/IMU.sum, 2); + Serial.println(" Hz"); + Serial.println(""); + } + +#ifdef LCD + // M5.Lcd.fillScreen(BLACK); + M5.Lcd.setTextFont(2); + + M5.Lcd.setCursor(0, 0); M5.Lcd.print(" x y z "); + M5.Lcd.setCursor(0, 24); + M5.Lcd.printf("% 6d % 6d % 6d mg \r\n", (int)(1000*IMU.ax), (int)(1000*IMU.ay), (int)(1000*IMU.az)); + M5.Lcd.setCursor(0, 44); + M5.Lcd.printf("% 6d % 6d % 6d o/s \r\n", (int)(IMU.gx), (int)(IMU.gy), (int)(IMU.gz)); + M5.Lcd.setCursor(0, 64); + M5.Lcd.printf("% 6d % 6d % 6d mG \r\n", (int)(IMU.mx), (int)(IMU.my), (int)(IMU.mz)); + + M5.Lcd.setCursor(0, 100); + M5.Lcd.printf(" yaw: % 5.2f pitch: % 5.2f roll: % 5.2f \r\n",(IMU.yaw), (IMU.pitch), (IMU.roll)); + + // With these settings the filter is updating at a ~145 Hz rate using the + // Madgwick scheme and >200 Hz using the Mahony scheme even though the + // display refreshes at only 2 Hz. The filter update rate is determined + // mostly by the mathematical steps in the respective algorithms, the + // processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR: + // an ODR of 10 Hz for the magnetometer produce the above rates, maximum + // magnetometer ODR of 100 Hz produces filter update rates of 36 - 145 and + // ~38 Hz for the Madgwick and Mahony schemes, respectively. This is + // presumably because the magnetometer read takes longer than the gyro or + // accelerometer reads. This filter update rate should be fast enough to + // maintain accurate platform orientation for stabilization control of a + // fast-moving robot or quadcopter. Compare to the update rate of 200 Hz + // produced by the on-board Digital Motion Processor of Invensense's MPU6050 + // 6 DoF and MPU9150 9DoF sensors. The 3.3 V 8 MHz Pro Mini is doing pretty + // well! + + // M5.Lcd.setCursor(0, 60); + // M5.Lcd.printf("yaw:%6.2f pitch:%6.2f roll:%6.2f ypr \r\n",(IMU.yaw), (IMU.pitch), (IMU.roll)); + M5.Lcd.setCursor(12, 144); + M5.Lcd.print("rt: "); + M5.Lcd.print((float) IMU.sumCount / IMU.sum, 2); + M5.Lcd.print(" Hz"); +#endif // LCD + + IMU.count = millis(); + IMU.sumCount = 0; + IMU.sum = 0; + } // if (IMU.delt_t > 500) + } // if (AHRS) +} diff --git a/extras/MPU9250.cpp b/extras/MPU9250.cpp new file mode 100644 index 00000000..85e0a26a --- /dev/null +++ b/extras/MPU9250.cpp @@ -0,0 +1,169 @@ +#include "MPU9250.h" + +MPU9250::MPU9250(uint8_t address): + address(address), + magXOffset(0), + magYOffset(0), + magZOffset(0) { +} + +void MPU9250::I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data) { + myWire->beginTransmission(Address); + myWire->write(Register); + myWire->endTransmission(); + + myWire->requestFrom(Address, Nbytes); + uint8_t index=0; + while (myWire->available()) + Data[index++]=myWire->read(); +} + +void MPU9250::I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data) { + myWire->beginTransmission(Address); + myWire->write(Register); + myWire->write(Data); + myWire->endTransmission(); +} + +void MPU9250::setWire(TwoWire* wire) { + myWire = wire; +} + +void MPU9250::beginGyro() { + delay(40); + + I2CwriteByte(address, CONFIG, 0x03); + I2CwriteByte(address, SMPLRT_DIV, 0x04); + + // Set gyroscope full scale range + // Range selects FS_SEL and GFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 + // uint8_t c = I2Cread(address, GYRO_CONFIG); // get current GYRO_CONFIG register value + uint8_t c; + I2Cread(address, GYRO_CONFIG, 1, &c); // get current GYRO_CONFIG register value + + // c = c & ~0xE0; // Clear self-test bits [7:5] + c = c & ~0x03; // Clear Fchoice bits [1:0] + c = c & ~0x18; // Clear GFS bits [4:3] + // c = c | Gscale << 3; // Set full scale range for the gyro + c = c | 0 << 3; // Set full scale range for the gyro + // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG + I2CwriteByte(address, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register + + delay(10); +} + +void MPU9250::gyroUpdate() +{ + uint8_t rawData[6]; // x/y/z gyro register data stored here + I2Cread(address, 0x43, 6, rawData); + gyroBuf[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + gyroBuf[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + gyroBuf[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; +} + +float MPU9250::gyroX() +{ + return (float)gyroBuf[0] * 0.00762939; +} + +float MPU9250::gyroY() +{ + return (float)gyroBuf[1] * 0.00762939; +} + +float MPU9250::gyroZ() +{ + return (float)gyroBuf[2] * 0.00762939; +} + +void MPU9250::beginAccel() { + delay(40); + + I2CwriteByte(address, 27, GYRO_FULL_SCALE_2000_DPS); + I2CwriteByte(address, 28, ACC_FULL_SCALE_16_G); + delay(10); +} + +void MPU9250::beginMag(uint8_t mode) { + delay(10); + + // trun on magnetometor + I2CwriteByte(address, 0x37, 0x02); + delay(10); + + magReadAdjustValues(); + magSetMode(AK8963_MODE_POWERDOWN); + magSetMode(mode); +} + +void MPU9250::magReadAdjustValues() { + magSetMode(AK8963_MODE_POWERDOWN); + magSetMode(AK8963_MODE_FUSEROM); + uint8_t buff[3]; + I2Cread(MAG_ADDRESS, AK8963_RA_ASAX, 3, buff); + magXAdjust = buff[0]; + magYAdjust = buff[1]; + magZAdjust = buff[2]; +} + +void MPU9250::magSetMode(uint8_t mode) { + I2CwriteByte(MAG_ADDRESS, AK8963_RA_CNTL1, mode); + delay(10); +} + +const float Pi = 3.14159; + +float MPU9250::magHorizDirection() { + return atan2((float) magX(), (float) magY()) * 180 / Pi; +} + +void MPU9250::magUpdate() { + I2Cread(MAG_ADDRESS, AK8963_RA_HXL, 7, magBuf); +} + +int16_t MPU9250::magGet(uint8_t highIndex, uint8_t lowIndex) { + return (((int16_t)magBuf[highIndex]) << 8) | magBuf[lowIndex]; +} + +uint16_t adjustMagValue(int16_t value, uint8_t adjust) { + return (value * ((((adjust - 128) * 0.5) / 128) + 1)); +} + +int16_t MPU9250::magX() { + return adjustMagValue(magGet(1, 0), magXAdjust) + magXOffset; +} + +int16_t MPU9250::magY() { + return adjustMagValue(magGet(3, 2), magYAdjust) + magYOffset; +} + +int16_t MPU9250::magZ() { + return adjustMagValue(magGet(5, 4), magZAdjust) + magZOffset; +} + +void MPU9250::accelUpdate() { + I2Cread(address, 0x3B, 14, accelBuf); +} + +float MPU9250::accelGet(uint8_t highIndex, uint8_t lowIndex) { + int16_t v = -(accelBuf[highIndex]<<8 | accelBuf[lowIndex]); + return ((float)v) * 16.0/32768.0; +} + +float MPU9250::accelX() { + return accelGet(0, 1); +} + +float MPU9250::accelY() { + return accelGet(2, 3); +} + +float MPU9250::accelZ() { + return accelGet(4, 5); +} + +float MPU9250::accelSqrt() { + return sqrt(pow(accelGet(0, 1), 2) + + pow(accelGet(2, 3), 2) + + pow(accelGet(4, 5), 2)); +} diff --git a/extras/MPU9250.h b/extras/MPU9250.h new file mode 100644 index 00000000..bb5a943c --- /dev/null +++ b/extras/MPU9250.h @@ -0,0 +1,236 @@ +#ifndef _MPU9250_H_ +#define _MPU9250_H_ +#include +#include +#include + +#define MPU9250_ADDRESS_AD0_LOW 0x68 +#define MPU9250_ADDRESS_AD0_HIGH 0x69 +#define MAG_ADDRESS 0x0C + +#define GYRO_FULL_SCALE_250_DPS 0x00 +#define GYRO_FULL_SCALE_500_DPS 0x08 +#define GYRO_FULL_SCALE_1000_DPS 0x10 +#define GYRO_FULL_SCALE_2000_DPS 0x18 + +#define ACC_FULL_SCALE_2_G 0x00 +#define ACC_FULL_SCALE_4_G 0x08 +#define ACC_FULL_SCALE_8_G 0x10 +#define ACC_FULL_SCALE_16_G 0x18 + +#define AK8963_RA_HXL 0x03 +#define AK8963_RA_CNTL1 0x0A +#define AK8963_RA_ASAX 0x10 + +#define AK8963_MODE_POWERDOWN 0x0 +#define AK8963_MODE_SINGLE 0x1 +#define AK8963_MODE_CONTINUOUS_8HZ 0x2 +#define AK8963_MODE_EXTERNAL 0x4 +#define AK8963_MODE_CONTINUOUS_100HZ 0x6 +#define AK8963_MODE_SELFTEST 0x8 +#define AK8963_MODE_FUSEROM 0xF + +// See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in +// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map +// +//Magnetometer Registers +#define AK8963_ADDRESS 0x0C +#define AK8963_WHO_AM_I 0x00 // should return 0x48 +#define AK8963_INFO 0x01 +#define AK8963_ST1 0x02 // data ready status bit 0 +#define AK8963_XOUT_L 0x03 // data +#define AK8963_XOUT_H 0x04 +#define AK8963_YOUT_L 0x05 +#define AK8963_YOUT_H 0x06 +#define AK8963_ZOUT_L 0x07 +#define AK8963_ZOUT_H 0x08 +#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 +#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 +#define AK8963_ASTC 0x0C // Self test control +#define AK8963_I2CDIS 0x0F // I2C disable +#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value +#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value +#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value + +#define SELF_TEST_X_GYRO 0x00 +#define SELF_TEST_Y_GYRO 0x01 +#define SELF_TEST_Z_GYRO 0x02 + +/*#define X_FINE_GAIN 0x03 // [7:0] fine gain +#define Y_FINE_GAIN 0x04 +#define Z_FINE_GAIN 0x05 +#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer +#define XA_OFFSET_L_TC 0x07 +#define YA_OFFSET_H 0x08 +#define YA_OFFSET_L_TC 0x09 +#define ZA_OFFSET_H 0x0A +#define ZA_OFFSET_L_TC 0x0B */ + +#define SELF_TEST_X_ACCEL 0x0D +#define SELF_TEST_Y_ACCEL 0x0E +#define SELF_TEST_Z_ACCEL 0x0F + +#define SELF_TEST_A 0x10 + +#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope +#define XG_OFFSET_L 0x14 +#define YG_OFFSET_H 0x15 +#define YG_OFFSET_L 0x16 +#define ZG_OFFSET_H 0x17 +#define ZG_OFFSET_L 0x18 +#define SMPLRT_DIV 0x19 +#define CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define ACCEL_CONFIG2 0x1D +#define LP_ACCEL_ODR 0x1E +#define WOM_THR 0x1F + +#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms +#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0] +#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms + +#define FIFO_EN 0x23 +#define I2C_MST_CTRL 0x24 +#define I2C_SLV0_ADDR 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_CTRL 0x27 +#define I2C_SLV1_ADDR 0x28 +#define I2C_SLV1_REG 0x29 +#define I2C_SLV1_CTRL 0x2A +#define I2C_SLV2_ADDR 0x2B +#define I2C_SLV2_REG 0x2C +#define I2C_SLV2_CTRL 0x2D +#define I2C_SLV3_ADDR 0x2E +#define I2C_SLV3_REG 0x2F +#define I2C_SLV3_CTRL 0x30 +#define I2C_SLV4_ADDR 0x31 +#define I2C_SLV4_REG 0x32 +#define I2C_SLV4_DO 0x33 +#define I2C_SLV4_CTRL 0x34 +#define I2C_SLV4_DI 0x35 +#define I2C_MST_STATUS 0x36 +#define INT_PIN_CFG 0x37 +#define INT_ENABLE 0x38 +#define DMP_INT_STATUS 0x39 // Check DMP interrupt +#define INT_STATUS 0x3A +#define ACCEL_XOUT_H 0x3B +#define ACCEL_XOUT_L 0x3C +#define ACCEL_YOUT_H 0x3D +#define ACCEL_YOUT_L 0x3E +#define ACCEL_ZOUT_H 0x3F +#define ACCEL_ZOUT_L 0x40 +#define TEMP_OUT_H 0x41 +#define TEMP_OUT_L 0x42 +#define GYRO_XOUT_H 0x43 +#define GYRO_XOUT_L 0x44 +#define GYRO_YOUT_H 0x45 +#define GYRO_YOUT_L 0x46 +#define GYRO_ZOUT_H 0x47 +#define GYRO_ZOUT_L 0x48 +#define EXT_SENS_DATA_00 0x49 +#define EXT_SENS_DATA_01 0x4A +#define EXT_SENS_DATA_02 0x4B +#define EXT_SENS_DATA_03 0x4C +#define EXT_SENS_DATA_04 0x4D +#define EXT_SENS_DATA_05 0x4E +#define EXT_SENS_DATA_06 0x4F +#define EXT_SENS_DATA_07 0x50 +#define EXT_SENS_DATA_08 0x51 +#define EXT_SENS_DATA_09 0x52 +#define EXT_SENS_DATA_10 0x53 +#define EXT_SENS_DATA_11 0x54 +#define EXT_SENS_DATA_12 0x55 +#define EXT_SENS_DATA_13 0x56 +#define EXT_SENS_DATA_14 0x57 +#define EXT_SENS_DATA_15 0x58 +#define EXT_SENS_DATA_16 0x59 +#define EXT_SENS_DATA_17 0x5A +#define EXT_SENS_DATA_18 0x5B +#define EXT_SENS_DATA_19 0x5C +#define EXT_SENS_DATA_20 0x5D +#define EXT_SENS_DATA_21 0x5E +#define EXT_SENS_DATA_22 0x5F +#define EXT_SENS_DATA_23 0x60 +#define MOT_DETECT_STATUS 0x61 +#define I2C_SLV0_DO 0x63 +#define I2C_SLV1_DO 0x64 +#define I2C_SLV2_DO 0x65 +#define I2C_SLV3_DO 0x66 +#define I2C_MST_DELAY_CTRL 0x67 +#define SIGNAL_PATH_RESET 0x68 +#define MOT_DETECT_CTRL 0x69 +#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP +#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode +#define PWR_MGMT_2 0x6C +#define DMP_BANK 0x6D // Activates a specific bank in the DMP +#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank +#define DMP_REG 0x6F // Register in DMP from which to read or to which to write +#define DMP_REG_1 0x70 +#define DMP_REG_2 0x71 +#define FIFO_COUNTH 0x72 +#define FIFO_COUNTL 0x73 +#define FIFO_R_W 0x74 +#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 +#define XA_OFFSET_H 0x77 +#define XA_OFFSET_L 0x78 +#define YA_OFFSET_H 0x7A +#define YA_OFFSET_L 0x7B +#define ZA_OFFSET_H 0x7D +#define ZA_OFFSET_L 0x7E + +// Using the MSENSR-9250 breakout board, ADO is set to 0 +// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 +#define ADO 0 +#if ADO +#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 +#else +#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 +#define AK8963_ADDRESS 0x0C // Address of magnetometer +#endif + + +class MPU9250 { + public: + int16_t magXOffset, magYOffset, magZOffset; + + // MPU9250(uint8_t address = (uint8_t) MPU9250_ADDRESS_AD0_HIGH): + MPU9250(uint8_t address = (uint8_t) MPU9250_ADDRESS_AD0_LOW); + void setWire(TwoWire *wire); + + void beginGyro(); + void gyroUpdate(); + float gyroX(); + float gyroY(); + float gyroZ(); + + void beginAccel(); + void accelUpdate(); + float accelX(); + float accelY(); + float accelZ(); + float accelSqrt(); + + void beginMag(uint8_t mode = AK8963_MODE_CONTINUOUS_8HZ); + void magUpdate(); + int16_t magX(); + int16_t magY(); + int16_t magZ(); + void magSetMode(uint8_t mode); + float magHorizDirection(); + + private: + TwoWire* myWire; + uint8_t address; + uint8_t accelBuf[14]; + uint8_t magBuf[7]; + uint16_t gyroBuf[3]; + uint8_t magXAdjust, magYAdjust, magZAdjust; + float accelGet(uint8_t highIndex, uint8_t lowIndex); + int16_t magGet(uint8_t highIndex, uint8_t lowIndex); + void magReadAdjustValues(); + void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data); + void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data); +}; + +#endif diff --git a/extras/i2c_MPU9250.h b/extras/i2c_MPU9250.h new file mode 100644 index 00000000..063b0065 --- /dev/null +++ b/extras/i2c_MPU9250.h @@ -0,0 +1,460 @@ +#ifndef i2c_MPU9250_h +#define i2c_MPU9250_h + +#include "i2c.h" +#include "i2c_Sensor.h" + + +/** ###################################################################### + +Driver for the MPU9250-Sensor + + 9D-IMU 3.7mA, 8µA sleep + + GYRO XYZ: [3.9 .. 8000 Hz], ±250, ±500, ±1000, and ±2000°/sec and integrated 16-bit ADCs + 8µA, 3.2mA active + + ACCE XYZ: ±2g, ±4g, ±8g and ±16g and integrated 16-bit ADCs + 8µA, 450µA + + MAGN XYZ: ±4800μT, 14 bit (0.6μT/LSB) or 16 bit (15μT/LSB) + ?µA, 280μA @8Hz + + DMP: Digital Motion Processing + +/// +######################################################################## */ + +class MPU9250 : public i2cSensor +{ + +private: + + /** ######### Register-Map ################################################################# */ + static const uint8_t I2C_ADDRESS =(0x68); + +//MPU9250 Register map + static const uint8_t REG_SELF_TEST_X_GYRO =(0x00); + static const uint8_t REG_SELF_TEST_Y_GYRO =(0x01); + static const uint8_t REG_SELF_TEST_Z_GYRO =(0x02); + static const uint8_t REG_SELF_TEST_X_ACCEL =(0x0D); + static const uint8_t REG_SELF_TEST_Y_ACCEL =(0x0E); + static const uint8_t REG_SELF_TEST_Z_ACCEL =(0x0F); + static const uint8_t REG_XG_OFFSET_H =(0x13); + static const uint8_t REG_XG_OFFSET_L =(0x14); + static const uint8_t REG_YG_OFFSET_H =(0x15); + static const uint8_t REG_YG_OFFSET_L =(0x16); + static const uint8_t REG_ZG_OFFSET_H =(0x17); + static const uint8_t REG_ZG_OFFSET_L =(0x18); + static const uint8_t REG_SMPLRT_DIV =(0x19); + inline const uint8_t HZ_TO_DIV(const uint16_t hz) + { + return ((1000/hz) - 1); + }; + + static const uint8_t REG_CONFIG =(0x1A); + static const uint8_t MSK_FIFO_MODE =(0x40); + static const uint8_t MSK_EXT_SYNC_SET =(0x38); + static const uint8_t MSK_DLPF_CFG =(0x07); + + static const uint8_t REG_GYRO_CONFIG =(0x1B); + static const uint8_t MSK_XGYRO_CTEN =(0x80); + static const uint8_t MSK_YGYRO_CTEN =(0x40); + static const uint8_t MSK_ZGYRO_CTEN =(0x20); + static const uint8_t MSK_GYRO_FS_SEL =(0x18); + static const uint8_t VAL_GYRO_FS_0250 =(0x00); + static const uint8_t VAL_GYRO_FS_0500 =(0x01); + static const uint8_t VAL_GYRO_FS_1000 =(0x02); + static const uint8_t VAL_GYRO_FS_2000 =(0x03); + static const uint8_t MSK_FCHOICE_B =(0x03); + + static const uint8_t REG_ACCEL_CONFIG =(0x1C); + static const uint8_t MSK_AX_ST_EN =(0x80); + static const uint8_t MSK_AY_ST_EN =(0x40); + static const uint8_t MSK_AZ_ST_EN =(0x20); + static const uint8_t MSK_ACCEL_FS_SEL =(0x18); + static const uint8_t VAL_ACCEL_FS_02 =(0x00); + static const uint8_t VAL_ACCEL_FS_04 =(0x01); + static const uint8_t VAL_ACCEL_FS_08 =(0x02); + static const uint8_t VAL_ACCEL_FS_16 =(0x03); + + static const uint8_t REG_ACCEL_CONFIG2 =(0x1D); + static const uint8_t MSK_ACCEL_FCHOICE_B =(0xC0); + static const uint8_t MSK_A_DLPF_CFG =(0x03); + + static const uint8_t REG_LP_ACCEL_ODR =(0x1E); + static const uint8_t MSK_LPOSC_CLKSEL =(0x0F); + + static const uint8_t REG_WOM_THR =(0x1F); + + static const uint8_t REG_FIFO_EN =(0x23); + static const uint8_t MSK_TEMP_FIFO_EN =(0x80); + static const uint8_t MSK_GYRO_XOUT =(0x40); + static const uint8_t MSK_GYRO_YOUT =(0x20); + static const uint8_t MSK_GYRO_ZOUT =(0x10); + static const uint8_t MSK_ACCEL =(0x08); + static const uint8_t MSK_SLV2 =(0x04); + static const uint8_t MSK_SLV1 =(0x02); + static const uint8_t MSK_SLV0 =(0x01); + + static const uint8_t REG_I2C_MST_CTRL =(0x24); + static const uint8_t MSK_MULT_MST_EN =(0x80); + static const uint8_t MSK_WAIT_FOR_ES =(0x40); + static const uint8_t MSK_SLV_3_FIFO_EN =(0x20); + static const uint8_t MSK_I2C_MST_P_NSR =(0x10); + static const uint8_t MSK_I2C_MST_CLK =(0x0F); + + static const uint8_t REG_I2C_SLV0_ADDR =(0x25); + static const uint8_t MSK_I2C_SLV0_RNW =(0x80); + static const uint8_t MSK_I2C_ID_0 =(0x7F); + + static const uint8_t REG_I2C_SLV0_REG =(0x26); + static const uint8_t REG_I2C_SLV0_CTRL =(0x27); + static const uint8_t MSK_I2C_SLV0_EN =(0x80); + static const uint8_t MSK_I2C_SLV0_BYTE_SW =(0x40); + static const uint8_t MSK_I2C_SLV0_REG_DIS =(0x20); + static const uint8_t MSK_I2C_SLV0_GRP =(0x10); + static const uint8_t MSK_I2C_SLV0_LENG =(0x0F); + +// [without SLV0 to SLV4] + + static const uint8_t REG_I2C_MST_STATUS =(0x36); + static const uint8_t MSK_PASS_THROUGH =(0x80); + static const uint8_t MSK_I2C_SLV4_DONE =(0x40); + static const uint8_t MSK_I2C_LOST_ARB =(0x20); + static const uint8_t MSK_I2C_SLV4_NACK =(0x10); + static const uint8_t MSK_I2C_SLV3_NACK =(0x08); + static const uint8_t MSK_I2C_SLV2_NACK =(0x04); + static const uint8_t MSK_I2C_SLV1_NACK =(0x02); + static const uint8_t MSK_I2C_SLV0_NACK =(0x01); + + static const uint8_t REG_INT_PIN_CFG =(0x37); + static const uint8_t MSK_ACTL =(0x80); + static const uint8_t MSK_OPEN =(0x40); + static const uint8_t MSK_LATCH_INT_EN =(0x20); + static const uint8_t MSK_INT_ANYRD_2CLEAR =(0x10); + static const uint8_t MSK_ACTL_FSYNC =(0x08); + static const uint8_t MSK_FSYNC_INT_MODE_EN =(0x04); + static const uint8_t MSK_BYPASS_EN =(0x02); + + static const uint8_t REG_INT_ENABLE =(0x38); + static const uint8_t MSK_WOM_EN =(0x40); + static const uint8_t MSK_FIFO_OFLOW_EN =(0x10); + static const uint8_t MSK_FSYNC_INT_EN =(0x08); + static const uint8_t MSK_RAW_RDY_EN =(0x01); + + static const uint8_t REG_INT_STATUS =(0x3A); + static const uint8_t MSK_WOM_INT =(0x40); + static const uint8_t MSK_FIFO_OFLOW_INT =(0x10); + static const uint8_t MSK_FSYNC_INT =(0x08); + static const uint8_t MSK_RAW_DATA_RDY_INT =(0x01); + + static const uint8_t REG_ACCEL_XOUT_H =(0x3B); + static const uint8_t REG_ACCEL_XOUT_L =(0x3C); + static const uint8_t REG_ACCEL_YOUT_H =(0x3D); + static const uint8_t REG_ACCEL_YOUT_L =(0x3E); + static const uint8_t REG_ACCEL_ZOUT_H =(0x3F); + static const uint8_t REG_ACCEL_ZOUT_L =(0x40); + static const uint8_t REG_TEMP_OUT_H =(0x41); + static const uint8_t REG_TEMP_OUT_L =(0x42); + static const uint8_t REG_GYRO_XOUT_H =(0x43); + static const uint8_t REG_GYRO_XOUT_L =(0x44); + static const uint8_t REG_GYRO_YOUT_H =(0x45); + static const uint8_t REG_GYRO_YOUT_L =(0x46); + static const uint8_t REG_GYRO_ZOUT_H =(0x47); + static const uint8_t REG_GYRO_ZOUT_L =(0x48); + + static const uint8_t REG_EXT_SENS_DATA_00 =(0x49); + static const uint8_t REG_EXT_SENS_DATA_01 =(0x4A); + static const uint8_t REG_EXT_SENS_DATA_02 =(0x4B); + static const uint8_t REG_EXT_SENS_DATA_03 =(0x4C); + static const uint8_t REG_EXT_SENS_DATA_04 =(0x4D); + static const uint8_t REG_EXT_SENS_DATA_05 =(0x4E); + static const uint8_t REG_EXT_SENS_DATA_06 =(0x4F); + static const uint8_t REG_EXT_SENS_DATA_07 =(0x50); + static const uint8_t REG_EXT_SENS_DATA_08 =(0x51); + static const uint8_t REG_EXT_SENS_DATA_09 =(0x52); + static const uint8_t REG_EXT_SENS_DATA_10 =(0x53); + static const uint8_t REG_EXT_SENS_DATA_11 =(0x54); + static const uint8_t REG_EXT_SENS_DATA_12 =(0x55); + static const uint8_t REG_EXT_SENS_DATA_13 =(0x56); + static const uint8_t REG_EXT_SENS_DATA_14 =(0x57); + static const uint8_t REG_EXT_SENS_DATA_15 =(0x58); + static const uint8_t REG_EXT_SENS_DATA_16 =(0x59); + static const uint8_t REG_EXT_SENS_DATA_17 =(0x5A); + static const uint8_t REG_EXT_SENS_DATA_18 =(0x5B); + static const uint8_t REG_EXT_SENS_DATA_19 =(0x5C); + static const uint8_t REG_EXT_SENS_DATA_20 =(0x5D); + static const uint8_t REG_EXT_SENS_DATA_21 =(0x5E); + static const uint8_t REG_EXT_SENS_DATA_22 =(0x5F); + static const uint8_t REG_EXT_SENS_DATA_23 =(0x60); + + static const uint8_t REG_I2C_SLV0_DO =(0x63); + static const uint8_t REG_I2C_SLV1_DO =(0x64); + static const uint8_t REG_I2C_SLV2_DO =(0x65); + static const uint8_t REG_I2C_SLV3_DO =(0x66); + + static const uint8_t REG_I2C_MST_DELAY_CTRL =(0x67); + static const uint8_t MSK_DELAY_ES_SHADOW =(0x80); + static const uint8_t MSK_I2C_SLV4_DLY_EN =(0x10); + static const uint8_t MSK_I2C_SLV3_DLY_EN =(0x08); + static const uint8_t MSK_I2C_SLV2_DLY_EN =(0x04); + static const uint8_t MSK_I2C_SLV1_DLY_EN =(0x02); + static const uint8_t MSK_I2C_SLV0_DLY_EN =(0x01); + + static const uint8_t REG_SIGNAL_PATH_RESET =(0x68); + static const uint8_t MSK_GYRO_RST =(0x04); + static const uint8_t MSK_ACCEL_RST =(0x02); + static const uint8_t MSK_TEMP_RST =(0x01); + + static const uint8_t REG_MOT_DETECT_CTRL =(0x69); + static const uint8_t MSK_ACCEL_INTEL_EN =(0x80); + static const uint8_t MSK_ACCEL_INTEL_MODE =(0x40); + + static const uint8_t REG_USER_CTRL =(0x6A); + static const uint8_t MSK_FIFO_EN =(0x40); + static const uint8_t MSK_I2C_MST_EN =(0x20); + static const uint8_t MSK_I2C_IF_DIS =(0x10); + static const uint8_t MSK_FIFO_RST =(0x04); + static const uint8_t MSK_I2C_MST_RST =(0x02); + static const uint8_t MSK_SIG_COND_RST =(0x01); + + static const uint8_t REG_PWR_MGMT_1 =(0x6B); + static const uint8_t MSK_H_RESET =(0x80); + static const uint8_t MSK_SLEEP =(0x40); + static const uint8_t MSK_CYCLE =(0x20); + static const uint8_t MSK_GYRO_STANDBY_CYCLE =(0x10); + static const uint8_t MSK_PD_PTAT =(0x08); + static const uint8_t MSK_CLKSEL =(0x07); + + static const uint8_t REG_PWR_MGMT_2 =(0x6C); + static const uint8_t MSK_DISABLE_XA =(0x20); + static const uint8_t MSK_DISABLE_YA =(0x10); + static const uint8_t MSK_DISABLE_ZA =(0x08); + static const uint8_t MSK_DISABLE_XG =(0x04); + static const uint8_t MSK_DISABLE_YG =(0x02); + static const uint8_t MSK_DISABLE_ZG =(0x01); + static const uint8_t MSK_DISABLE_XYZA =(0x38); + static const uint8_t MSK_DISABLE_XYZG =(0x07); + + static const uint8_t REG_FIFO_COUNTH =(0x72); + static const uint8_t REG_FIFO_COUNTL =(0x73); + static const uint8_t REG_FIFO_R_W =(0x74); + static const uint8_t REG_WHO_AM_I =(0x75); + static const uint8_t VAL_WHOAMI_6500 =(0x70); + static const uint8_t VAL_WHOAMI_9250 =(0x71); + + static const uint8_t REG_XA_OFFSET_H =(0x77); + static const uint8_t REG_XA_OFFSET_L =(0x78); + static const uint8_t REG_YA_OFFSET_H =(0x7A); + static const uint8_t REG_YA_OFFSET_L =(0x7B); + static const uint8_t REG_ZA_OFFSET_H =(0x7D); + static const uint8_t REG_ZA_OFFSET_L =(0x7E); + +//reset values + static const uint8_t WHOAMI_RESET_VAL =(0x71); + static const uint8_t POWER_MANAGMENT_1_RESET_VAL =(0x01); + static const uint8_t DEFAULT_RESET_VALUE =(0x00); + static const uint8_t WHOAMI_DEFAULT_VAL =(0x68); + + /**< TODO: Switch to separate lib for the AK8963 */ + +//Magnetometer register maps + static const uint8_t REG_MAG_WIA =(0x00); + static const uint8_t REG_MAG_INFO =(0x01); + static const uint8_t REG_MAG_ST1 =(0x02); + static const uint8_t REG_MAG_XOUT_L =(0x03); + static const uint8_t REG_MAG_XOUT_H =(0x04); + static const uint8_t REG_MAG_YOUT_L =(0x05); + static const uint8_t REG_MAG_YOUT_H =(0x06); + static const uint8_t REG_MAG_ZOUT_L =(0x07); + static const uint8_t REG_MAG_ZOUT_H =(0x08); + static const uint8_t REG_MAG_ST2 =(0x09); + static const uint8_t REG_MAG_CNTL =(0x0A); + static const uint8_t REG_MAG_RSV =(0x0B); //reserved mystery meat + static const uint8_t REG_MAG_ASTC =(0x0C); + static const uint8_t REG_MAG_TS1 =(0x0D); + static const uint8_t REG_MAG_TS2 =(0x0E); + static const uint8_t REG_MAG_I2CDIS =(0x0F); + static const uint8_t REG_MAG_ASAX =(0x10); + static const uint8_t REG_MAG_ASAY =(0x11); + static const uint8_t REG_MAG_ASAZ =(0x12); +//Magnetometer register masks + static const uint8_t MPU9250_WIA =(0x48); + static const uint8_t MPU9250_ =(0x00); + + float gSensitivity, aSensitivity; + + /** ######### function definition ################################################################# */ + + +public: + /**< TODO: do i need a constructor? */ + MPU9250(void): gSensitivity(1), aSensitivity(1) + { + }; + + void setEnabled(const uint8_t enable = 1) + { + uint8_t _value; + if (enable) _value = 0; + else _value = 255; + i2c.setRegister(I2C_ADDRESS, REG_PWR_MGMT_1, MSK_SLEEP, _value); + i2c.setRegister(I2C_ADDRESS, REG_PWR_MGMT_2, B00111111, _value); // enable XYZ of Gyr & Acc + }; + + /**< takes 0..3 for 250, 500, 1000, 2000 dps */ + inline void setGSensitivity(const uint8_t gScaleRange = VAL_GYRO_FS_0500) + { + if (gScaleRange<4) i2c.setRegister(I2C_ADDRESS, REG_GYRO_CONFIG, MSK_GYRO_FS_SEL, gScaleRange<<3); + + if (gScaleRange==VAL_GYRO_FS_0250) gSensitivity=131.0; + else if (gScaleRange==VAL_GYRO_FS_0500) gSensitivity=65.5; + else if (gScaleRange==VAL_GYRO_FS_1000) gSensitivity=32.8; + else if (gScaleRange==VAL_GYRO_FS_2000) gSensitivity=16.4; + }; + + /**< takes 0..3 for 2, 4, 8 and 16g */ + inline void setASensitivity(const uint8_t aScaleRange = VAL_ACCEL_FS_04) + { + if (aScaleRange<4) i2c.setRegister(I2C_ADDRESS, REG_ACCEL_CONFIG, MSK_ACCEL_FS_SEL, aScaleRange<<3); + + if (aScaleRange==VAL_ACCEL_FS_02) aSensitivity=16384; + else if (aScaleRange==VAL_ACCEL_FS_04) aSensitivity= 8192; + else if (aScaleRange==VAL_ACCEL_FS_08) aSensitivity= 4096; + else if (aScaleRange==VAL_ACCEL_FS_16) aSensitivity= 2048; + }; + + /**< round numbers are: 1 kHz, 500 Hz, 200, 125, 100, 50, 25, 20, 10 ... */ + inline void setDatarate(const uint16_t hzFreq=100) + { + /**< with clockratedivider for samplerate 1kHz (Configure FChoice) */ + i2c.writeByte(I2C_ADDRESS, REG_SMPLRT_DIV, (uint8_t) HZ_TO_DIV(hzFreq)); + }; + + inline void setBandwidth(const uint16_t hzFreq=100) + { + uint8_t dlpf; + if (hzFreq>184) dlpf = 0; // 460 Hz Acc, FS= 1 kHz // 250 Hz Gyro, 4000 Hz Temp, FS=8kHz + else if (hzFreq>92) dlpf = 1; // 184 Hz Gyro/Acc, 4000 Hz Temp, FS=1kHz + else if (hzFreq>41) dlpf = 2; // 92 Hz Gyro/Acc, 4000 Hz Temp, FS=1kHz + else if (hzFreq>20) dlpf = 3; // 41 Hz Gyro/Acc, 42 Hz Temp, FS=1kHz + else if (hzFreq>10) dlpf = 4; // 20 Hz Gyro/Acc, 20 Hz Temp, FS=1kHz + else if (hzFreq>5) dlpf = 5; // 10 Hz Gyro/Acc, 10 Hz Temp, FS=1kHz + else dlpf = 6; // 5 Hz Gyro/Acc, 5 Hz Temp, FS=1kHz + + /**< Rework, page 15 - FS=1kHz --> Bandbreite einstellen */ + i2c.setRegister(I2C_ADDRESS, REG_CONFIG, MSK_DLPF_CFG, dlpf); // 0x1A DLPF p13 + i2c.setRegister(I2C_ADDRESS, REG_GYRO_CONFIG, MSK_FCHOICE_B, 0); // 0x1B gFChoice + + i2c.setRegister(I2C_ADDRESS, REG_ACCEL_CONFIG2, MSK_A_DLPF_CFG, dlpf); + i2c.setRegister(I2C_ADDRESS, REG_ACCEL_CONFIG2, MSK_ACCEL_FCHOICE_B, 0); // aFChoice + }; + + inline void reset(void) + { + uint8_t rVector; + rVector = MSK_GYRO_RST | MSK_ACCEL_RST | MSK_TEMP_RST; + i2c.setRegister(I2C_ADDRESS, REG_SIGNAL_PATH_RESET, rVector, rVector); // Registers of Sensors + i2c.setRegister(I2C_ADDRESS, REG_USER_CTRL, MSK_SIG_COND_RST, MSK_SIG_COND_RST); // Signal paths of Sensors + i2c.setRegister(I2C_ADDRESS, REG_PWR_MGMT_1, MSK_H_RESET, MSK_H_RESET); // internal Registers + }; + + + inline uint8_t initialize(void) + { + if (i2c.probe(I2C_ADDRESS) == 0) return 0; + + // 0x19 Sample Rate Divider - standard = 0 (+1) + reset(); + delay(20); + + //setEnabled(0); + + setBandwidth(20); + setDatarate(100); + setGSensitivity(VAL_GYRO_FS_0500); + setASensitivity(VAL_ACCEL_FS_04); + + // Clocksource + i2c.setRegister(I2C_ADDRESS, REG_PWR_MGMT_1, MSK_CLKSEL, 1); // should be VAL_CLOCK_PLL_XGYRO, is it? + // PWR MGMT + i2c.setRegister(I2C_ADDRESS, REG_PWR_MGMT_1, MSK_CYCLE | MSK_GYRO_STANDBY_CYCLE | MSK_PD_PTAT, 0); // Normal operation + + // INTs + i2c.setRegister(I2C_ADDRESS, REG_INT_PIN_CFG, MSK_INT_ANYRD_2CLEAR,MSK_INT_ANYRD_2CLEAR); // Clear INT at read + i2c.setRegister(I2C_ADDRESS, REG_INT_ENABLE , MSK_RAW_RDY_EN, MSK_RAW_RDY_EN); // INT: Raw Data Ready to read + + // I2C-Master + i2c.setRegister(I2C_ADDRESS, REG_USER_CTRL , MSK_I2C_MST_EN, 0); // disable I2C-Master + i2c.setRegister(I2C_ADDRESS, REG_INT_PIN_CFG , MSK_BYPASS_EN, MSK_BYPASS_EN); // enable Bypass mode! + + setEnabled(1); + + uint8_t who = i2c.readByte(I2C_ADDRESS, REG_WHO_AM_I); + if (who == VAL_WHOAMI_6500) return 2; + else if (who == VAL_WHOAMI_9250) return 3; + else return 1; + }; + + /**< check for new data, return 1 when Measurement is ready */ + inline uint8_t checkMeasurement(void) + { + if (i2c.readByte(I2C_ADDRESS,REG_INT_STATUS)&MSK_RAW_DATA_RDY_INT) return 1; + else return 0; + }; + + /**< if you started a measurement and want to actively wait for it to finish */ + inline uint8_t awaitMeasurement(void) + { + uint8_t _counter = 0; + while(checkMeasurement()==0) + { + if(++_counter > 250) return 0; //Error out after max of 250ms for a read + delay(1); + } + return 1; // Measurement finished + }; + + /**< TODO: separate RAW and Scaled GET */ + void getMeasurement(float xyz_AccTemGyr[]) + { + uint8_t _data[14]; + i2c.read(I2C_ADDRESS,REG_ACCEL_XOUT_H, _data, 14); + // RAW + xyz_AccTemGyr[0] = int16_t(_data[0]<<8 | _data[1]); // ACC + xyz_AccTemGyr[1] = int16_t(_data[2]<<8 | _data[3]); + xyz_AccTemGyr[2] = int16_t(_data[4]<<8 | _data[5]); + + xyz_AccTemGyr[3] = int16_t(_data[6]<<8 | _data[7]); // TEMP + + xyz_AccTemGyr[4] = int16_t(_data[8]<<8 | _data[9]); // GYR + xyz_AccTemGyr[5] = int16_t(_data[10]<<8| _data[11]); + xyz_AccTemGyr[6] = int16_t(_data[12]<<8| _data[13]); + + // Scale + xyz_AccTemGyr[3] = xyz_AccTemGyr[3] / 333.87 + 21; // TEMP_degC = ((TEMP_OUT – RoomTemp_Offset)/Temp_Sensitivity) + 21degC + + xyz_AccTemGyr[0] /= aSensitivity; // ACC + xyz_AccTemGyr[1] /= aSensitivity; + xyz_AccTemGyr[2] /= aSensitivity; + + xyz_AccTemGyr[4] /= gSensitivity; // GYR + xyz_AccTemGyr[5] /= gSensitivity; + xyz_AccTemGyr[6] /= gSensitivity; + + }; + + /** ######### Digital Motion Processor ################################################################# */ + +}; + +/** ######### Preinstantiate Object ################################################################# */ +/** < it's better when this is done by the user */ +//PRESET preset = PRESET(); + +#endif + + + + diff --git a/library.json b/library.json index 4c58c6c3..63638e60 100644 --- a/library.json +++ b/library.json @@ -10,7 +10,7 @@ "type": "git", "url": "https://github.com/m5stack/m5stack.git" }, - "version": "0.1.1", + "version": "0.1.2", "framework": "arduino", "platforms": "espressif32" } diff --git a/library.properties b/library.properties index 46cc72cb..8b480012 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=M5Stack -version=0.1.1 +version=0.1.2 author=M5Stack maintainer=Zibin Zheng sentence=Driver for M5Stack Core development kit diff --git a/src/M5Stack.cpp b/src/M5Stack.cpp index 9af1fca1..2f9d238b 100644 --- a/src/M5Stack.cpp +++ b/src/M5Stack.cpp @@ -37,14 +37,12 @@ void M5Stack::begin() { setWakeupButton(BUTTON_A_PIN); // MPU9250 -#ifdef MPU9250_INSDE - // Wire.begin(); - // MPU.setWire(&Wire); - // MPU.beginAccel(); - // MPU.beginMag(); -#endif - - Serial.println("OK."); +// #ifdef MPU9250_INSDE +// Wire.begin(); +// IMU.setWire(&Wire); +// #endif + + Serial.println("OK"); } void M5Stack::update() { @@ -102,7 +100,7 @@ void M5Stack::powerOFF() { // ESP32 into deep sleep uint64_t _wakeupPin_mask = 1ULL << _wakeupPin; USE_SERIAL.printf("Enabling EXT1 wakeup on pins GPIO%d\n", _wakeupPin); - esp_deep_sleep_enable_ext1_wakeup(_wakeupPin_mask , ESP_EXT1_WAKEUP_ALL_LOW); + esp_sleep_enable_ext0_wakeup((gpio_num_t)_wakeupPin_mask , 0); while(digitalRead(_wakeupPin) == LOW) { delay(10); @@ -112,4 +110,4 @@ void M5Stack::powerOFF() { USE_SERIAL.println("On power OFF fail!"); } -M5Stack m5stack; +M5Stack M5; diff --git a/src/M5Stack.h b/src/M5Stack.h index ec0ccdcb..9720849f 100644 --- a/src/M5Stack.h +++ b/src/M5Stack.h @@ -76,6 +76,8 @@ #if defined(ESP32) +// #define MPU9250_INSDE + #include #include #include @@ -85,7 +87,7 @@ #include "FS.h" #include "SD.h" -#include "utility/Display.h" // Graphics and font library for ILI9341 driver chip +#include "utility/Display.h" #include "utility/Config.h" #include "utility/Button.h" #include "utility/Speaker.h" @@ -93,12 +95,12 @@ #include "utility/music_8bit.h" #ifdef MPU9250_INSDE #include "utility/MPU9250.h" +#include "utility/quaternionFilters.h" #endif extern "C" { #include "freertos/FreeRTOS.h" #include "freertos/task.h" -#include "esp_deep_sleep.h" #include "esp32-hal-dac.h" } @@ -130,16 +132,15 @@ class M5Stack { // MPU9250 // #ifdef MPU9250_INSDE -// MPU9250 MPU = MPU9250(); +// MPU9250 IMU = MPU9250(); // #endif private: uint8_t _wakeupPin; }; -extern M5Stack m5stack; -#define m5 m5stack -#define M5 m5stack +extern M5Stack M5; +#define m5 M5 #define lcd Lcd #else diff --git a/src/utility/Display.cpp b/src/utility/Display.cpp index 547f560a..7a250e5a 100644 --- a/src/utility/Display.cpp +++ b/src/utility/Display.cpp @@ -3680,7 +3680,7 @@ void ILI9341::writeHzkGbk(const uint8_t* c) issue on GitHub: -/*************************************************** + *************************************************** The Adafruit_ILI9341 library has been used as a starting point for this library. @@ -3699,9 +3699,9 @@ void ILI9341::writeHzkGbk(const uint8_t* c) Written by Limor Fried/Ladyada for Adafruit Industries. MIT license, all text above must be included in any redistribution - ****************************************************/ + **************************************************** -/**************************************************** + **************************************************** Some member funtions have been imported from the Adafruit_GFX library. The license associated with these is reproduced below. @@ -3739,9 +3739,9 @@ void ILI9341::writeHzkGbk(const uint8_t* c) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - ****************************************************/ + **************************************************** -/**************************************************** + **************************************************** In compliance with the licence.txt file for the Adafruit_GFX library the following is included. diff --git a/src/utility/MPU9250.cpp b/src/utility/MPU9250.cpp index 3e0ae0d3..c57fd578 100644 --- a/src/utility/MPU9250.cpp +++ b/src/utility/MPU9250.cpp @@ -1,122 +1,474 @@ #include "MPU9250.h" -MPU9250::MPU9250(uint8_t address): - address(address), - magXOffset(0), - magYOffset(0), - magZOffset(0) { -} +//============================================================================== +//====== Set of useful function to access acceleration. gyroscope, magnetometer, +//====== and temperature data +//============================================================================== -void MPU9250::I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data) { - myWire->beginTransmission(Address); - myWire->write(Register); - myWire->endTransmission(); +void MPU9250::getMres() { + switch (Mscale) + { + // Possible magnetometer scales (and their register bit settings) are: + // 14 bit resolution (0) and 16 bit resolution (1) + case MFS_14BITS: + mRes = 10.*4912./8190.; // Proper scale to return milliGauss + break; + case MFS_16BITS: + mRes = 10.*4912./32760.0; // Proper scale to return milliGauss + break; + } +} - myWire->requestFrom(Address, Nbytes); - uint8_t index=0; - while (myWire->available()) - Data[index++]=myWire->read(); +void MPU9250::getGres() { + switch (Gscale) + { + // Possible gyro scales (and their register bit settings) are: + // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + case GFS_250DPS: + gRes = 250.0/32768.0; + break; + case GFS_500DPS: + gRes = 500.0/32768.0; + break; + case GFS_1000DPS: + gRes = 1000.0/32768.0; + break; + case GFS_2000DPS: + gRes = 2000.0/32768.0; + break; + } } -void MPU9250::I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data) { - myWire->beginTransmission(Address); - myWire->write(Register); - myWire->write(Data); - myWire->endTransmission(); +void MPU9250::getAres() { + switch (Ascale) + { + // Possible accelerometer scales (and their register bit settings) are: + // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11). + // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value: + case AFS_2G: + aRes = 2.0/32768.0; + break; + case AFS_4G: + aRes = 4.0/32768.0; + break; + case AFS_8G: + aRes = 8.0/32768.0; + break; + case AFS_16G: + aRes = 16.0/32768.0; + break; + } } -void MPU9250::setWire(TwoWire* wire) { - myWire = wire; + +void MPU9250::readAccelData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z accel register data stored here + readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array + destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; } -void MPU9250::beginAccel() { - delay(40); - I2CwriteByte(address, 27, GYRO_FULL_SCALE_2000_DPS); - I2CwriteByte(address, 28, ACC_FULL_SCALE_16_G); - delay(10); +void MPU9250::readGyroData(int16_t * destination) +{ + uint8_t rawData[6]; // x/y/z gyro register data stored here + readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array + destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value + destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ; + destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ; } -void MPU9250::magReadAdjustValues() { - magSetMode(AK8963_MODE_POWERDOWN); - magSetMode(AK8963_MODE_FUSEROM); - uint8_t buff[3]; - I2Cread(MAG_ADDRESS, AK8963_RA_ASAX, 3, buff); - magXAdjust = buff[0]; - magYAdjust = buff[1]; - magZAdjust = buff[2]; +void MPU9250::readMagData(int16_t * destination) +{ + // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of + // data acquisition + uint8_t rawData[7]; + // Wait for magnetometer data ready bit to be set + if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) + { + // Read the six raw data and ST2 registers sequentially into data array + readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); + uint8_t c = rawData[6]; // End data read by reading ST2 register + // Check if magnetic sensor overflow set, if not then report data + if(!(c & 0x08)) + { + // Turn the MSB and LSB into a signed 16-bit value + destination[0] = ((int16_t)rawData[1] << 8) | rawData[0]; + // Data stored as little Endian + destination[1] = ((int16_t)rawData[3] << 8) | rawData[2]; + destination[2] = ((int16_t)rawData[5] << 8) | rawData[4]; + } + } } -void MPU9250::beginMag(uint8_t mode) { - delay(10); +int16_t MPU9250::readTempData() +{ + uint8_t rawData[2]; // x/y/z gyro register data stored here + readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array + return ((int16_t)rawData[0] << 8) | rawData[1]; // Turn the MSB and LSB into a 16-bit value +} - // trun on magnetometor - I2CwriteByte(address, 0x37, 0x02); - delay(10); +// Calculate the time the last update took for use in the quaternion filters +void MPU9250::updateTime() +{ + Now = micros(); + + // Set integration time by time elapsed since last filter update + deltat = ((Now - lastUpdate) / 1000000.0f); + lastUpdate = Now; - magReadAdjustValues(); - magSetMode(AK8963_MODE_POWERDOWN); - magSetMode(mode); + sum += deltat; // sum for averaging filter update rate + sumCount++; } -void MPU9250::magSetMode(uint8_t mode) { - I2CwriteByte(MAG_ADDRESS, AK8963_RA_CNTL1, mode); +void MPU9250::initAK8963(float * destination) +{ + // First extract the factory calibration for each magnetometer axis + uint8_t rawData[3]; // x/y/z gyro calibration data stored here + writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer + delay(10); + writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode + delay(10); + readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values + destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc. + destination[1] = (float)(rawData[1] - 128)/256. + 1.; + destination[2] = (float)(rawData[2] - 128)/256. + 1.; + writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer + delay(10); + // Configure the magnetometer for continuous read and highest resolution + // set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register, + // and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates + writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR delay(10); } -const float Pi = 3.14159; +void MPU9250::initMPU9250() +{ + // wake up device + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors + delay(100); // Wait for all registers to reset -float MPU9250::magHorizDirection() { - return atan2((float) magX(), (float) magY()) * 180 / Pi; -} + // get stable time source + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else + delay(200); + + // Configure Gyro and Thermometer + // Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively; + // minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot + // be higher than 1 / 0.0059 = 170 Hz + // DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both + // With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz + writeByte(MPU9250_ADDRESS, CONFIG, 0x03); -void MPU9250::magUpdate() { - I2Cread(MAG_ADDRESS, AK8963_RA_HXL, 7, magBuf); -} + // Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV) + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate + // determined inset in CONFIG above + + // Set gyroscope full scale range + // Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3 + uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG); // get current GYRO_CONFIG register value + // c = c & ~0xE0; // Clear self-test bits [7:5] + c = c & ~0x02; // Clear Fchoice bits [1:0] + c = c & ~0x18; // Clear AFS bits [4:3] + c = c | Gscale << 3; // Set full scale range for the gyro + // c =| 0x00; // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c ); // Write new GYRO_CONFIG value to register + + // Set accelerometer full-scale range configuration + c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG); // get current ACCEL_CONFIG register value + // c = c & ~0xE0; // Clear self-test bits [7:5] + c = c & ~0x18; // Clear AFS bits [4:3] + c = c | Ascale << 3; // Set full scale range for the accelerometer + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c); // Write new ACCEL_CONFIG register value -int16_t MPU9250::magGet(uint8_t highIndex, uint8_t lowIndex) { - return (((int16_t)magBuf[highIndex]) << 8) | magBuf[lowIndex]; -} + // Set accelerometer sample rate configuration + // It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for + // accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz + c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2); // get current ACCEL_CONFIG2 register value + c = c & ~0x0F; // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0]) + c = c | 0x03; // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c); // Write new ACCEL_CONFIG2 register value + // The accelerometer, gyro, and thermometer are set to 1 kHz sample rates, + // but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting -uint16_t adjustMagValue(int16_t value, uint8_t adjust) { - return (value * ((((adjust - 128) * 0.5) / 128) + 1)); + // Configure Interrupts and Bypass Enable + // Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared, + // clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips + // can join the I2C bus and all can be controlled by the Arduino as master + writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22); + writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt + delay(100); } -int16_t MPU9250::magX() { - return adjustMagValue(magGet(1, 0), magXAdjust) + magXOffset; -} -int16_t MPU9250::magY() { - return adjustMagValue(magGet(3, 2), magYAdjust) + magYOffset; -} +// Function which accumulates gyro and accelerometer data after device +// initialization. It calculates the average of the at-rest readings and then +// loads the resulting offsets into accelerometer and gyro bias registers. +void MPU9250::calibrateMPU9250(float * gyroBias, float * accelBias) +{ + uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data + uint16_t ii, packet_count, fifo_count; + int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0}; + + // reset device + // Write a one to bit 7 reset bit; toggle reset device + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); + delay(100); + + // get stable time source; Auto select clock source to be PLL gyroscope + // reference if ready else use the internal oscillator, bits 2:0 = 001 + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); + writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00); + delay(200); -int16_t MPU9250::magZ() { - return adjustMagValue(magGet(5, 4), magZAdjust) + magZOffset; -} + // Configure device for bias calculation + writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO + writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source + writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP + delay(15); + +// Configure MPU6050 gyro and accelerometer for bias calculation + writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity + writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity + + uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec + uint16_t accelsensitivity = 16384; // = 16384 LSB/g -void MPU9250::accelUpdate() { - I2Cread(address, 0x3B, 14, accelBuf); -} + // Configure FIFO to capture accelerometer and gyro data for bias calculation + writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150) + delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes + +// At end of sample accumulation, turn off FIFO sensor read + writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO + readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count + fifo_count = ((uint16_t)data[0] << 8) | data[1]; + packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging + + for (ii = 0; ii < packet_count; ii++) + { + int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0}; + readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging + accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ); // Form signed 16-bit integer for each sample in FIFO + accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ); + accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ); + gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ); + gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ); + gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]); + + accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases + accel_bias[1] += (int32_t) accel_temp[1]; + accel_bias[2] += (int32_t) accel_temp[2]; + gyro_bias[0] += (int32_t) gyro_temp[0]; + gyro_bias[1] += (int32_t) gyro_temp[1]; + gyro_bias[2] += (int32_t) gyro_temp[2]; + } + accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases + accel_bias[1] /= (int32_t) packet_count; + accel_bias[2] /= (int32_t) packet_count; + gyro_bias[0] /= (int32_t) packet_count; + gyro_bias[1] /= (int32_t) packet_count; + gyro_bias[2] /= (int32_t) packet_count; + + if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation + else {accel_bias[2] += (int32_t) accelsensitivity;} + +// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup + data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format + data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases + data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF; + data[3] = (-gyro_bias[1]/4) & 0xFF; + data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF; + data[5] = (-gyro_bias[2]/4) & 0xFF; + +// Push gyro biases to hardware registers + writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]); + writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]); + writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]); + writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]); + writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]); + writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]); + +// Output scaled gyro biases for display in the main program + gyroBias[0] = (float) gyro_bias[0]/(float) gyrosensitivity; + gyroBias[1] = (float) gyro_bias[1]/(float) gyrosensitivity; + gyroBias[2] = (float) gyro_bias[2]/(float) gyrosensitivity; -float MPU9250::accelGet(uint8_t highIndex, uint8_t lowIndex) { - int16_t v = -(accelBuf[highIndex]<<8 | accelBuf[lowIndex]); - return ((float)v) * 16.0/32768.0; +// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain +// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold +// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature +// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that +// the accelerometer biases calculated above must be divided by 8. + + int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases + readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values + accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]); + accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]); + accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]); + + uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers + uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis + + for(ii = 0; ii < 3; ii++) { + if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit + } + + // Construct total accelerometer bias, including calculated average accelerometer bias from above + accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale) + accel_bias_reg[1] -= (accel_bias[1]/8); + accel_bias_reg[2] -= (accel_bias[2]/8); + + data[0] = (accel_bias_reg[0] >> 8) & 0xFF; + data[1] = (accel_bias_reg[0]) & 0xFF; + data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[2] = (accel_bias_reg[1] >> 8) & 0xFF; + data[3] = (accel_bias_reg[1]) & 0xFF; + data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers + data[4] = (accel_bias_reg[2] >> 8) & 0xFF; + data[5] = (accel_bias_reg[2]) & 0xFF; + data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers + +// Apparently this is not working for the acceleration biases in the MPU-9250 +// Are we handling the temperature correction bit properly? +// Push accelerometer biases to hardware registers + writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]); + writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]); + writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]); + writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]); + writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]); + writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]); + +// Output scaled accelerometer biases for display in the main program + accelBias[0] = (float)accel_bias[0]/(float)accelsensitivity; + accelBias[1] = (float)accel_bias[1]/(float)accelsensitivity; + accelBias[2] = (float)accel_bias[2]/(float)accelsensitivity; } -float MPU9250::accelX() { - return accelGet(0, 1); + +// Accelerometer and gyroscope self test; check calibration wrt factory settings +void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass +{ + uint8_t rawData[6] = {0, 0, 0, 0, 0, 0}; + uint8_t selfTest[6]; + int16_t gAvg[3], aAvg[3], aSTAvg[3], gSTAvg[3]; + float factoryTrim[6]; + uint8_t FS = 0; + + writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set gyro sample rate to 1 kHz + writeByte(MPU9250_ADDRESS, CONFIG, 0x02); // Set gyro sample rate to 1 kHz and DLPF to 92 Hz + writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 1< +/* + Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library. + Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or + a 3.3 V Teensy 3.1. We have disabled the internal pull-ups used by the Wire + library in the Wire.h/twi.c utility file. We are also using the 400 kHz fast + I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file. + */ +#ifndef _MPU9250_H_ +#define _MPU9250_H_ + +#include #include -#include - -#define MPU9250_ADDRESS_AD0_LOW 0x68 -#define MPU9250_ADDRESS_AD0_HIGH 0x69 -#define MAG_ADDRESS 0x0C - -#define GYRO_FULL_SCALE_250_DPS 0x00 -#define GYRO_FULL_SCALE_500_DPS 0x08 -#define GYRO_FULL_SCALE_1000_DPS 0x10 -#define GYRO_FULL_SCALE_2000_DPS 0x18 - -#define ACC_FULL_SCALE_2_G 0x00 -#define ACC_FULL_SCALE_4_G 0x08 -#define ACC_FULL_SCALE_8_G 0x10 -#define ACC_FULL_SCALE_16_G 0x18 - -#define AK8963_RA_HXL 0x03 -#define AK8963_RA_CNTL1 0x0A -#define AK8963_RA_ASAX 0x10 - -#define AK8963_MODE_POWERDOWN 0x0 -#define AK8963_MODE_SINGLE 0x1 -#define AK8963_MODE_CONTINUOUS_8HZ 0x2 -#define AK8963_MODE_EXTERNAL 0x4 -#define AK8963_MODE_CONTINUOUS_100HZ 0x6 -#define AK8963_MODE_SELFTEST 0x8 -#define AK8963_MODE_FUSEROM 0xF - -class MPU9250 { + +// See also MPU-9250 Register Map and Descriptions, Revision 4.0, +// RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in above +// document; the MPU9250 and MPU9150 are virtually identical but the latter has +// a different register map + +//Magnetometer Registers +#define AK8963_ADDRESS 0x0C +#define WHO_AM_I_AK8963 0x00 // should return 0x48 +#define INFO 0x01 +#define AK8963_ST1 0x02 // data ready status bit 0 +#define AK8963_XOUT_L 0x03 // data +#define AK8963_XOUT_H 0x04 +#define AK8963_YOUT_L 0x05 +#define AK8963_YOUT_H 0x06 +#define AK8963_ZOUT_L 0x07 +#define AK8963_ZOUT_H 0x08 +#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2 +#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0 +#define AK8963_ASTC 0x0C // Self test control +#define AK8963_I2CDIS 0x0F // I2C disable +#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value +#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value +#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value + +#define SELF_TEST_X_GYRO 0x00 +#define SELF_TEST_Y_GYRO 0x01 +#define SELF_TEST_Z_GYRO 0x02 + +/*#define X_FINE_GAIN 0x03 // [7:0] fine gain +#define Y_FINE_GAIN 0x04 +#define Z_FINE_GAIN 0x05 +#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer +#define XA_OFFSET_L_TC 0x07 +#define YA_OFFSET_H 0x08 +#define YA_OFFSET_L_TC 0x09 +#define ZA_OFFSET_H 0x0A +#define ZA_OFFSET_L_TC 0x0B */ + +#define SELF_TEST_X_ACCEL 0x0D +#define SELF_TEST_Y_ACCEL 0x0E +#define SELF_TEST_Z_ACCEL 0x0F + +#define SELF_TEST_A 0x10 + +#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope +#define XG_OFFSET_L 0x14 +#define YG_OFFSET_H 0x15 +#define YG_OFFSET_L 0x16 +#define ZG_OFFSET_H 0x17 +#define ZG_OFFSET_L 0x18 +#define SMPLRT_DIV 0x19 +#define CONFIG 0x1A +#define GYRO_CONFIG 0x1B +#define ACCEL_CONFIG 0x1C +#define ACCEL_CONFIG2 0x1D +#define LP_ACCEL_ODR 0x1E +#define WOM_THR 0x1F + +// Duration counter threshold for motion interrupt generation, 1 kHz rate, +// LSB = 1 ms +#define MOT_DUR 0x20 +// Zero-motion detection threshold bits [7:0] +#define ZMOT_THR 0x21 +// Duration counter threshold for zero motion interrupt generation, 16 Hz rate, +// LSB = 64 ms +#define ZRMOT_DUR 0x22 + +#define FIFO_EN 0x23 +#define I2C_MST_CTRL 0x24 +#define I2C_SLV0_ADDR 0x25 +#define I2C_SLV0_REG 0x26 +#define I2C_SLV0_CTRL 0x27 +#define I2C_SLV1_ADDR 0x28 +#define I2C_SLV1_REG 0x29 +#define I2C_SLV1_CTRL 0x2A +#define I2C_SLV2_ADDR 0x2B +#define I2C_SLV2_REG 0x2C +#define I2C_SLV2_CTRL 0x2D +#define I2C_SLV3_ADDR 0x2E +#define I2C_SLV3_REG 0x2F +#define I2C_SLV3_CTRL 0x30 +#define I2C_SLV4_ADDR 0x31 +#define I2C_SLV4_REG 0x32 +#define I2C_SLV4_DO 0x33 +#define I2C_SLV4_CTRL 0x34 +#define I2C_SLV4_DI 0x35 +#define I2C_MST_STATUS 0x36 +#define INT_PIN_CFG 0x37 +#define INT_ENABLE 0x38 +#define DMP_INT_STATUS 0x39 // Check DMP interrupt +#define INT_STATUS 0x3A +#define ACCEL_XOUT_H 0x3B +#define ACCEL_XOUT_L 0x3C +#define ACCEL_YOUT_H 0x3D +#define ACCEL_YOUT_L 0x3E +#define ACCEL_ZOUT_H 0x3F +#define ACCEL_ZOUT_L 0x40 +#define TEMP_OUT_H 0x41 +#define TEMP_OUT_L 0x42 +#define GYRO_XOUT_H 0x43 +#define GYRO_XOUT_L 0x44 +#define GYRO_YOUT_H 0x45 +#define GYRO_YOUT_L 0x46 +#define GYRO_ZOUT_H 0x47 +#define GYRO_ZOUT_L 0x48 +#define EXT_SENS_DATA_00 0x49 +#define EXT_SENS_DATA_01 0x4A +#define EXT_SENS_DATA_02 0x4B +#define EXT_SENS_DATA_03 0x4C +#define EXT_SENS_DATA_04 0x4D +#define EXT_SENS_DATA_05 0x4E +#define EXT_SENS_DATA_06 0x4F +#define EXT_SENS_DATA_07 0x50 +#define EXT_SENS_DATA_08 0x51 +#define EXT_SENS_DATA_09 0x52 +#define EXT_SENS_DATA_10 0x53 +#define EXT_SENS_DATA_11 0x54 +#define EXT_SENS_DATA_12 0x55 +#define EXT_SENS_DATA_13 0x56 +#define EXT_SENS_DATA_14 0x57 +#define EXT_SENS_DATA_15 0x58 +#define EXT_SENS_DATA_16 0x59 +#define EXT_SENS_DATA_17 0x5A +#define EXT_SENS_DATA_18 0x5B +#define EXT_SENS_DATA_19 0x5C +#define EXT_SENS_DATA_20 0x5D +#define EXT_SENS_DATA_21 0x5E +#define EXT_SENS_DATA_22 0x5F +#define EXT_SENS_DATA_23 0x60 +#define MOT_DETECT_STATUS 0x61 +#define I2C_SLV0_DO 0x63 +#define I2C_SLV1_DO 0x64 +#define I2C_SLV2_DO 0x65 +#define I2C_SLV3_DO 0x66 +#define I2C_MST_DELAY_CTRL 0x67 +#define SIGNAL_PATH_RESET 0x68 +#define MOT_DETECT_CTRL 0x69 +#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP +#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode +#define PWR_MGMT_2 0x6C +#define DMP_BANK 0x6D // Activates a specific bank in the DMP +#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank +#define DMP_REG 0x6F // Register in DMP from which to read or to which to write +#define DMP_REG_1 0x70 +#define DMP_REG_2 0x71 +#define FIFO_COUNTH 0x72 +#define FIFO_COUNTL 0x73 +#define FIFO_R_W 0x74 +#define WHO_AM_I_MPU9250 0x75 // Should return 0x71 +#define XA_OFFSET_H 0x77 +#define XA_OFFSET_L 0x78 +#define YA_OFFSET_H 0x7A +#define YA_OFFSET_L 0x7B +#define ZA_OFFSET_H 0x7D +#define ZA_OFFSET_L 0x7E + +// Using the MPU-9250 breakout board, ADO is set to 0 +// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1 +#define ADO 0 +#if ADO +#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1 +#else +#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0 +#define AK8963_ADDRESS 0x0C // Address of magnetometer +#endif // AD0 + +class MPU9250 +{ + protected: + // Set initial input parameters + enum Ascale { + AFS_2G = 0, + AFS_4G, + AFS_8G, + AFS_16G + }; + + enum Gscale { + GFS_250DPS = 0, + GFS_500DPS, + GFS_1000DPS, + GFS_2000DPS + }; + + enum Mscale { + MFS_14BITS = 0, // 0.6 mG per LSB + MFS_16BITS // 0.15 mG per LSB + }; + + // Specify sensor full scale + uint8_t Gscale = GFS_250DPS; + uint8_t Ascale = AFS_2G; + // Choose either 14-bit or 16-bit magnetometer resolution + uint8_t Mscale = MFS_16BITS; + // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read + uint8_t Mmode = 0x02; + public: - int16_t magXOffset, magYOffset, magZOffset; - - // MPU9250(uint8_t address = (uint8_t) MPU9250_ADDRESS_AD0_HIGH): - MPU9250(uint8_t address = (uint8_t) MPU9250_ADDRESS_AD0_LOW); - void setWire(TwoWire *wire); - - void beginAccel(); - void accelUpdate(); - float accelX(); - float accelY(); - float accelZ(); - float accelSqrt(); - - void beginMag(uint8_t mode = AK8963_MODE_CONTINUOUS_8HZ); - void magUpdate(); - int16_t magX(); - int16_t magY(); - int16_t magZ(); - void magSetMode(uint8_t mode); - float magHorizDirection(); - - private: - TwoWire* myWire; - uint8_t address; - uint8_t accelBuf[14]; - uint8_t magBuf[7]; - uint8_t magXAdjust, magYAdjust, magZAdjust; - float accelGet(uint8_t highIndex, uint8_t lowIndex); - int16_t magGet(uint8_t highIndex, uint8_t lowIndex); - void magReadAdjustValues(); - void I2Cread(uint8_t Address, uint8_t Register, uint8_t Nbytes, uint8_t* Data); - void I2CwriteByte(uint8_t Address, uint8_t Register, uint8_t Data); -}; - -#endif + float pitch, yaw, roll; + float temperature; // Stores the real internal chip temperature in Celsius + int16_t tempCount; // Temperature raw count output + uint32_t delt_t = 0; // Used to control display output rate + + uint32_t count = 0, sumCount = 0; // used to control display output rate + float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes + uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval + uint32_t Now = 0; // used to calculate integration interval + + int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output + int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output + // Scale resolutions per LSB for the sensors + float aRes, gRes, mRes; + // Variables to hold latest sensor data values + float ax, ay, az, gx, gy, gz, mx, my, mz; + // Factory mag calibration and mag bias + float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; + // Bias corrections for gyro and accelerometer + float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; + float SelfTest[6]; + // Stores the 16-bit signed accelerometer sensor output + int16_t accelCount[3]; + + public: + void getMres(); + void getGres(); + void getAres(); + void readAccelData(int16_t *); + void readGyroData(int16_t *); + void readMagData(int16_t *); + int16_t readTempData(); + void updateTime(); + void initAK8963(float *); + void initMPU9250(); + void calibrateMPU9250(float * gyroBias, float * accelBias); + void MPU9250SelfTest(float * destination); + void writeByte(uint8_t, uint8_t, uint8_t); + uint8_t readByte(uint8_t, uint8_t); + void readBytes(uint8_t, uint8_t, uint8_t, uint8_t *); +}; // class MPU9250 + +#endif // _MPU9250_H_ diff --git a/src/utility/quaternionFilters.cpp b/src/utility/quaternionFilters.cpp new file mode 100644 index 00000000..87094fec --- /dev/null +++ b/src/utility/quaternionFilters.cpp @@ -0,0 +1,234 @@ + +// Implementation of Sebastian Madgwick's "...efficient orientation filter +// for... inertial/magnetic sensor arrays" +// (see http://www.x-io.co.uk/category/open-source/ for examples & more details) +// which fuses acceleration, rotation rate, and magnetic moments to produce a +// quaternion-based estimate of absolute device orientation -- which can be +// converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc. +// The performance of the orientation filter is at least as good as conventional +// Kalman-based filtering algorithms but is much less computationally +// intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz! + +#include "quaternionFilters.h" + +// These are the free parameters in the Mahony filter and fusion scheme, Kp +// for proportional feedback, Ki for integral +#define Kp 2.0f * 5.0f +#define Ki 0.0f + +static float GyroMeasError = PI * (40.0f / 180.0f); +// gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s) +static float GyroMeasDrift = PI * (0.0f / 180.0f); +// There is a tradeoff in the beta parameter between accuracy and response +// speed. In the original Madgwick study, beta of 0.041 (corresponding to +// GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy. +// However, with this value, the LSM9SD0 response time is about 10 seconds +// to a stable initial quaternion. Subsequent changes also require a +// longish lag time to a stable output, not fast enough for a quadcopter or +// robot car! By increasing beta (GyroMeasError) by about a factor of +// fifteen, the response time constant is reduced to ~2 sec. I haven't +// noticed any reduction in solution accuracy. This is essentially the I +// coefficient in a PID control sense; the bigger the feedback coefficient, +// the faster the solution converges, usually at the expense of accuracy. +// In any case, this is the free parameter in the Madgwick filtering and +// fusion scheme. +static float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // Compute beta +// Compute zeta, the other free parameter in the Madgwick scheme usually +// set to a small or zero value +static float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; + +// Vector to hold integral error for Mahony method +static float eInt[3] = {0.0f, 0.0f, 0.0f}; +// Vector to hold quaternion +static float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; + +void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat) +{ + // short name local variable for readability + float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; + float norm; + float hx, hy, _2bx, _2bz; + float s1, s2, s3, s4; + float qDot1, qDot2, qDot3, qDot4; + + // Auxiliary variables to avoid repeated arithmetic + float _2q1mx; + float _2q1my; + float _2q1mz; + float _2q2mx; + float _4bx; + float _4bz; + float _2q1 = 2.0f * q1; + float _2q2 = 2.0f * q2; + float _2q3 = 2.0f * q3; + float _2q4 = 2.0f * q4; + float _2q1q3 = 2.0f * q1 * q3; + float _2q3q4 = 2.0f * q3 * q4; + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q1q4 = q1 * q4; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q2q4 = q2 * q4; + float q3q3 = q3 * q3; + float q3q4 = q3 * q4; + float q4q4 = q4 * q4; + + // Normalise accelerometer measurement + norm = sqrt(ax * ax + ay * ay + az * az); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f/norm; + ax *= norm; + ay *= norm; + az *= norm; + + // Normalise magnetometer measurement + norm = sqrt(mx * mx + my * my + mz * mz); + if (norm == 0.0f) return; // handle NaN + norm = 1.0f/norm; + mx *= norm; + my *= norm; + mz *= norm; + + // Reference direction of Earth's magnetic field + _2q1mx = 2.0f * q1 * mx; + _2q1my = 2.0f * q1 * my; + _2q1mz = 2.0f * q1 * mz; + _2q2mx = 2.0f * q2 * mx; + hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4; + hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4; + _2bx = sqrt(hx * hx + hy * hy); + _2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + // Gradient decent algorithm corrective step + s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz); + norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude + norm = 1.0f/norm; + s1 *= norm; + s2 *= norm; + s3 *= norm; + s4 *= norm; + + // Compute rate of change of quaternion + qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1; + qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2; + qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3; + qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4; + + // Integrate to yield quaternion + q1 += qDot1 * deltat; + q2 += qDot2 * deltat; + q3 += qDot3 * deltat; + q4 += qDot4 * deltat; + norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion + norm = 1.0f/norm; + q[0] = q1 * norm; + q[1] = q2 * norm; + q[2] = q3 * norm; + q[3] = q4 * norm; +} + + + +// Similar to Madgwick scheme but uses proportional and integral filtering on +// the error between estimated reference vectors and measured ones. +void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat) +{ + // short name local variable for readability + float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; + float norm; + float hx, hy, bx, bz; + float vx, vy, vz, wx, wy, wz; + float ex, ey, ez; + float pa, pb, pc; + + // Auxiliary variables to avoid repeated arithmetic + float q1q1 = q1 * q1; + float q1q2 = q1 * q2; + float q1q3 = q1 * q3; + float q1q4 = q1 * q4; + float q2q2 = q2 * q2; + float q2q3 = q2 * q3; + float q2q4 = q2 * q4; + float q3q3 = q3 * q3; + float q3q4 = q3 * q4; + float q4q4 = q4 * q4; + + // Normalise accelerometer measurement + norm = sqrt(ax * ax + ay * ay + az * az); + if (norm == 0.0f) return; // Handle NaN + norm = 1.0f / norm; // Use reciprocal for division + ax *= norm; + ay *= norm; + az *= norm; + + // Normalise magnetometer measurement + norm = sqrt(mx * mx + my * my + mz * mz); + if (norm == 0.0f) return; // Handle NaN + norm = 1.0f / norm; // Use reciprocal for division + mx *= norm; + my *= norm; + mz *= norm; + + // Reference direction of Earth's magnetic field + hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3); + hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2); + bx = sqrt((hx * hx) + (hy * hy)); + bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3); + + // Estimated direction of gravity and magnetic field + vx = 2.0f * (q2q4 - q1q3); + vy = 2.0f * (q1q2 + q3q4); + vz = q1q1 - q2q2 - q3q3 + q4q4; + wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3); + wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4); + wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3); + + // Error is cross product between estimated direction and measured direction of gravity + ex = (ay * vz - az * vy) + (my * wz - mz * wy); + ey = (az * vx - ax * vz) + (mz * wx - mx * wz); + ez = (ax * vy - ay * vx) + (mx * wy - my * wx); + if (Ki > 0.0f) + { + eInt[0] += ex; // accumulate integral error + eInt[1] += ey; + eInt[2] += ez; + } + else + { + eInt[0] = 0.0f; // prevent integral wind up + eInt[1] = 0.0f; + eInt[2] = 0.0f; + } + + // Apply feedback terms + gx = gx + Kp * ex + Ki * eInt[0]; + gy = gy + Kp * ey + Ki * eInt[1]; + gz = gz + Kp * ez + Ki * eInt[2]; + + // Integrate rate of change of quaternion + pa = q2; + pb = q3; + pc = q4; + q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat); + q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat); + q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat); + q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat); + + // Normalise quaternion + norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); + norm = 1.0f / norm; + q[0] = q1 * norm; + q[1] = q2 * norm; + q[2] = q3 * norm; + q[3] = q4 * norm; +} + +const float * getQ () { return q; } diff --git a/src/utility/quaternionFilters.h b/src/utility/quaternionFilters.h new file mode 100644 index 00000000..2b18a0f2 --- /dev/null +++ b/src/utility/quaternionFilters.h @@ -0,0 +1,14 @@ +#ifndef _QUATERNIONFILTERS_H_ +#define _QUATERNIONFILTERS_H_ + +#include + +void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, + float gz, float mx, float my, float mz, + float deltat); +void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, + float gz, float mx, float my, float mz, + float deltat); +const float * getQ(); + +#endif // _QUATERNIONFILTERS_H_