972 lines
26 KiB
C++
972 lines
26 KiB
C++
|
/*************************************************************************
|
||
|
* Arduino library for ESP32 based Freematics ONE+ and Freematics Esprit
|
||
|
* Distributed under BSD license
|
||
|
* Visit https://freematics.com for more information
|
||
|
* (C)2017-2019 Developed by Stanley Huang <stanley@freematics.com.au>
|
||
|
*************************************************************************/
|
||
|
|
||
|
#include <Arduino.h>
|
||
|
#include <SPI.h>
|
||
|
#include <stdio.h>
|
||
|
#include <string.h>
|
||
|
#include <stdlib.h>
|
||
|
|
||
|
#include "freertos/FreeRTOS.h"
|
||
|
#include "freertos/task.h"
|
||
|
#include "esp_system.h"
|
||
|
#include "esp_pm.h"
|
||
|
#include "esp_task_wdt.h"
|
||
|
#include "nvs_flash.h"
|
||
|
#include "driver/uart.h"
|
||
|
#include "esp_log.h"
|
||
|
#include "soc/uart_struct.h"
|
||
|
#include "soc/rtc_cntl_reg.h"
|
||
|
|
||
|
#include "FreematicsPlus.h"
|
||
|
#include "FreematicsGPS.h"
|
||
|
|
||
|
#ifdef ARDUINO_ESP32C3_DEV
|
||
|
#include "driver/temp_sensor.h"
|
||
|
#else
|
||
|
#include "soc/sens_reg.h"
|
||
|
#endif
|
||
|
|
||
|
#define VERBOSE_LINK 0
|
||
|
#define VERBOSE_XBEE 0
|
||
|
|
||
|
static TinyGPS gps;
|
||
|
static bool gpsHasDecodedData = false;
|
||
|
static uart_port_t gpsUARTNum = GPS_UART_NUM;
|
||
|
static int pinGPSRx = PIN_GPS_UART_RXD;
|
||
|
static int pinGPSTx = PIN_GPS_UART_TXD;
|
||
|
static Task taskGPS;
|
||
|
static GPS_DATA gpsData = {0};
|
||
|
// u-blox M10 commands for setting GGA to 5Hz, RMC to 1Hz and disabling NAV_PVT
|
||
|
static const uint8_t gpsSettings[] = {0xB5, 0x62, 0x06, 0x8A, 0x13, 0x00, 0x01, 0x01, 0x00, 0x00, 0xBB, 0x00, 0x91, 0x20, 0x02, 0xAC, 0x00, 0x91, 0x20, 0x0A, 0x07, 0x00, 0x91, 0x20, 0x00, 0x32, 0x74};
|
||
|
|
||
|
#ifndef ARDUINO_ESP32C3_DEV
|
||
|
|
||
|
static uint32_t inline getCycleCount()
|
||
|
{
|
||
|
uint32_t ccount;
|
||
|
__asm__ __volatile__("esync; rsr %0,ccount":"=a" (ccount));
|
||
|
return ccount;
|
||
|
}
|
||
|
|
||
|
static uint8_t inline readRxPin()
|
||
|
{
|
||
|
#if PIN_GPS_UART_RXD < 32
|
||
|
return (uint8_t)(GPIO.in >> PIN_GPS_UART_RXD) << 7;
|
||
|
#else
|
||
|
return (uint8_t)(GPIO.in1.val >> (PIN_GPS_UART_RXD - 32)) << 7;
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
static void inline setTxPinHigh()
|
||
|
{
|
||
|
#if PIN_GPS_UART_TXD < 32
|
||
|
GPIO.out_w1ts = ((uint32_t)1 << PIN_GPS_UART_TXD);
|
||
|
#else
|
||
|
GPIO.out1_w1ts.val = ((uint32_t)1 << (PIN_GPS_UART_TXD - 32));
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
static void inline setTxPinLow()
|
||
|
{
|
||
|
#if PIN_GPS_UART_TXD < 32
|
||
|
GPIO.out_w1tc = ((uint32_t)1 << PIN_GPS_UART_TXD);
|
||
|
#else
|
||
|
GPIO.out1_w1tc.val = ((uint32_t)1 << (PIN_GPS_UART_TXD - 32));
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
static void softSerialTx(uint32_t baudrate, uint8_t c)
|
||
|
{
|
||
|
uint32_t start = getCycleCount();
|
||
|
uint32_t duration;
|
||
|
// start bit
|
||
|
setTxPinLow();
|
||
|
for (uint32_t i = 1; i <= 8; i++, c >>= 1) {
|
||
|
duration = i * F_CPU / baudrate;
|
||
|
while (getCycleCount() - start < duration);
|
||
|
if (c & 0x1)
|
||
|
setTxPinHigh();
|
||
|
else
|
||
|
setTxPinLow();
|
||
|
}
|
||
|
duration = (uint32_t)9 * F_CPU / baudrate;
|
||
|
while (getCycleCount() - start < duration);
|
||
|
setTxPinHigh();
|
||
|
duration = (uint32_t)10 * F_CPU / baudrate;
|
||
|
while (getCycleCount() - start < duration);
|
||
|
}
|
||
|
|
||
|
static void gps_soft_decode_task(void* inst)
|
||
|
{
|
||
|
// start receiving and decoding
|
||
|
for (;;) {
|
||
|
uint8_t c = 0;
|
||
|
do {
|
||
|
taskYIELD();
|
||
|
} while (readRxPin());
|
||
|
uint32_t start = getCycleCount();
|
||
|
uint32_t duration;
|
||
|
for (uint32_t i = 1; i <= 7; i++) {
|
||
|
taskYIELD();
|
||
|
duration = i * F_CPU / GPS_SOFT_BAUDRATE + F_CPU / GPS_SOFT_BAUDRATE / 3;
|
||
|
while (getCycleCount() - start < duration);
|
||
|
c = (c | readRxPin()) >> 1;
|
||
|
}
|
||
|
if (gps.encode(c)) {
|
||
|
gpsHasDecodedData = true;
|
||
|
}
|
||
|
duration = (uint32_t)9 * F_CPU / GPS_SOFT_BAUDRATE + F_CPU / GPS_SOFT_BAUDRATE / 2;
|
||
|
do {
|
||
|
taskYIELD();
|
||
|
} while (getCycleCount() - start < duration);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
#endif
|
||
|
|
||
|
static void gps_decode_task(void* inst)
|
||
|
{
|
||
|
for (;;) {
|
||
|
uint8_t c = 0;
|
||
|
int len = uart_read_bytes(gpsUARTNum, &c, 1, 60000 / portTICK_RATE_MS);
|
||
|
if (len != 1) continue;
|
||
|
//Serial.print((char)c);
|
||
|
if (gps.encode(c)) {
|
||
|
gpsHasDecodedData = true;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
|
||
|
// get chip temperature sensor
|
||
|
int readChipTemperature()
|
||
|
{
|
||
|
#ifdef ARDUINO_ESP32C3_DEV
|
||
|
static bool inited = false;
|
||
|
float tsens_out = 0;
|
||
|
if (!inited) {
|
||
|
temp_sensor_config_t temp_sensor = TSENS_CONFIG_DEFAULT();
|
||
|
temp_sensor_get_config(&temp_sensor);
|
||
|
temp_sensor.dac_offset = TSENS_DAC_DEFAULT;
|
||
|
temp_sensor_set_config(temp_sensor);
|
||
|
temp_sensor_start();
|
||
|
inited = true;
|
||
|
}
|
||
|
temp_sensor_read_celsius(&tsens_out);
|
||
|
return tsens_out;
|
||
|
#else
|
||
|
SET_PERI_REG_BITS(SENS_SAR_MEAS_WAIT2_REG, SENS_FORCE_XPD_SAR, 3, SENS_FORCE_XPD_SAR_S);
|
||
|
SET_PERI_REG_BITS(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_CLK_DIV, 10, SENS_TSENS_CLK_DIV_S);
|
||
|
CLEAR_PERI_REG_MASK(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_POWER_UP);
|
||
|
CLEAR_PERI_REG_MASK(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_DUMP_OUT);
|
||
|
SET_PERI_REG_MASK(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_POWER_UP_FORCE);
|
||
|
SET_PERI_REG_MASK(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_POWER_UP);
|
||
|
ets_delay_us(100);
|
||
|
SET_PERI_REG_MASK(SENS_SAR_TSENS_CTRL_REG, SENS_TSENS_DUMP_OUT);
|
||
|
ets_delay_us(5);
|
||
|
int res = GET_PERI_REG_BITS2(SENS_SAR_SLAVE_ADDR3_REG, SENS_TSENS_OUT, SENS_TSENS_OUT_S);
|
||
|
return (res - 32) * 5 / 9;
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
int readChipHallSensor()
|
||
|
{
|
||
|
return 0; // FIXME
|
||
|
}
|
||
|
|
||
|
bool Task::create(void (*task)(void*), const char* name, int priority, int stacksize)
|
||
|
{
|
||
|
if (xHandle) return false;
|
||
|
/* Create the task, storing the handle. */
|
||
|
BaseType_t xReturned = xTaskCreate(task, name, stacksize, (void*)this, priority, &xHandle);
|
||
|
return xReturned == pdPASS;
|
||
|
}
|
||
|
|
||
|
void Task::destroy()
|
||
|
{
|
||
|
if (xHandle) {
|
||
|
TaskHandle_t x = xHandle;
|
||
|
xHandle = 0;
|
||
|
vTaskDelete(x);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void Task::sleep(uint32_t ms)
|
||
|
{
|
||
|
vTaskDelay(ms / portTICK_PERIOD_MS);
|
||
|
}
|
||
|
|
||
|
bool Task::running()
|
||
|
{
|
||
|
return xHandle != 0;
|
||
|
}
|
||
|
|
||
|
void Task::suspend()
|
||
|
{
|
||
|
if (xHandle) vTaskSuspend(xHandle);
|
||
|
}
|
||
|
|
||
|
void Task::resume()
|
||
|
{
|
||
|
if (xHandle) vTaskResume(xHandle);
|
||
|
}
|
||
|
|
||
|
Mutex::Mutex()
|
||
|
{
|
||
|
xSemaphore = xSemaphoreCreateMutex();
|
||
|
xSemaphoreGive(xSemaphore);
|
||
|
}
|
||
|
|
||
|
void Mutex::lock()
|
||
|
{
|
||
|
xSemaphoreTake(xSemaphore, portMAX_DELAY);
|
||
|
}
|
||
|
|
||
|
void Mutex::unlock()
|
||
|
{
|
||
|
xSemaphoreGive(xSemaphore);
|
||
|
}
|
||
|
|
||
|
bool CLink_UART::begin(unsigned int baudrate, int rxPin, int txPin)
|
||
|
{
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.println("[UART BEGIN]");
|
||
|
#endif
|
||
|
uart_config_t uart_config = {
|
||
|
.baud_rate = (int)baudrate,
|
||
|
.data_bits = UART_DATA_8_BITS,
|
||
|
.parity = UART_PARITY_DISABLE,
|
||
|
.stop_bits = UART_STOP_BITS_1,
|
||
|
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||
|
.rx_flow_ctrl_thresh = 122,
|
||
|
};
|
||
|
//Configure UART parameters
|
||
|
uart_param_config(LINK_UART_NUM, &uart_config);
|
||
|
//Set UART pins
|
||
|
uart_set_pin(LINK_UART_NUM, txPin, rxPin, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
||
|
//Install UART driver
|
||
|
if (uart_driver_install(LINK_UART_NUM, LINK_UART_BUF_SIZE, 0, 0, NULL, 0) != ESP_OK)
|
||
|
return false;
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
void CLink_UART::end()
|
||
|
{
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.println("[UART END]");
|
||
|
#endif
|
||
|
uart_driver_delete(LINK_UART_NUM);
|
||
|
}
|
||
|
|
||
|
int CLink_UART::receive(char* buffer, int bufsize, unsigned int timeout)
|
||
|
{
|
||
|
unsigned char n = 0;
|
||
|
unsigned long startTime = millis();
|
||
|
unsigned long elapsed;
|
||
|
for (;;) {
|
||
|
elapsed = millis() - startTime;
|
||
|
if (elapsed > timeout) break;
|
||
|
if (n >= bufsize - 1) break;
|
||
|
int len = uart_read_bytes(LINK_UART_NUM, (uint8_t*)buffer + n, bufsize - n - 1, 1);
|
||
|
if (len < 0) break;
|
||
|
if (len == 0) continue;
|
||
|
buffer[n + len] = 0;
|
||
|
if (strstr(buffer + n, "\r>")) {
|
||
|
n = n + len - 1;
|
||
|
buffer[n] = 0;
|
||
|
break;
|
||
|
}
|
||
|
n += len;
|
||
|
if (strstr(buffer, "...")) {
|
||
|
buffer[0] = 0;
|
||
|
n = 0;
|
||
|
timeout += OBD_TIMEOUT_LONG;
|
||
|
}
|
||
|
}
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.print("[UART RECV]");
|
||
|
Serial.println(buffer);
|
||
|
#endif
|
||
|
return n;
|
||
|
}
|
||
|
|
||
|
bool CLink_UART::send(const char* str)
|
||
|
{
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.print("[UART SEND]");
|
||
|
Serial.println(str);
|
||
|
#endif
|
||
|
int len = strlen(str);
|
||
|
return uart_write_bytes(LINK_UART_NUM, str, len) == len;
|
||
|
}
|
||
|
|
||
|
int CLink_UART::sendCommand(const char* cmd, char* buf, int bufsize, unsigned int timeout)
|
||
|
{
|
||
|
send(cmd);
|
||
|
return receive(buf, bufsize, timeout);
|
||
|
}
|
||
|
|
||
|
int CLink_UART::read()
|
||
|
{
|
||
|
uint8_t c;
|
||
|
if (uart_read_bytes(LINK_UART_NUM, &c, 1, 1) == 1)
|
||
|
return c;
|
||
|
else
|
||
|
return -1;
|
||
|
}
|
||
|
|
||
|
bool CLink_UART::changeBaudRate(unsigned int baudrate)
|
||
|
{
|
||
|
char buf[32];
|
||
|
sprintf(buf, "ATBR1 %X\r", baudrate);
|
||
|
sendCommand(buf, buf, sizeof(buf), 1000);
|
||
|
delay(50);
|
||
|
end();
|
||
|
return begin(baudrate);
|
||
|
}
|
||
|
|
||
|
bool CLink_SPI::begin(unsigned int freq, int rxPin, int txPin)
|
||
|
{
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.println("[SPI BEGIN]");
|
||
|
#endif
|
||
|
pinMode(PIN_LINK_SPI_READY, INPUT);
|
||
|
pinMode(PIN_LINK_SPI_CS, OUTPUT);
|
||
|
digitalWrite(PIN_LINK_SPI_CS, HIGH);
|
||
|
delay(50);
|
||
|
for (uint32_t t = millis(); millis() - t < 50; ) {
|
||
|
if (digitalRead(PIN_LINK_SPI_READY) == LOW) return false;
|
||
|
}
|
||
|
SPI.begin();
|
||
|
SPI.setFrequency(freq);
|
||
|
esp_sleep_pd_config(ESP_PD_DOMAIN_RTC_PERIPH, ESP_PD_OPTION_ON);
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
void CLink_SPI::end()
|
||
|
{
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.println("[SPI END]");
|
||
|
#endif
|
||
|
SPI.end();
|
||
|
}
|
||
|
|
||
|
int CLink_SPI::receive(char* buffer, int bufsize, unsigned int timeout)
|
||
|
{
|
||
|
int n = 0;
|
||
|
bool eos = false;
|
||
|
bool matched = false;
|
||
|
portMUX_TYPE m = portMUX_INITIALIZER_UNLOCKED;
|
||
|
uint32_t t = millis();
|
||
|
do {
|
||
|
while (digitalRead(PIN_LINK_SPI_READY) == HIGH) {
|
||
|
if (millis() - t > 3000) return -1;
|
||
|
delay(1);
|
||
|
}
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.println("[SPI RECV]");
|
||
|
#endif
|
||
|
portENTER_CRITICAL(&m);
|
||
|
digitalWrite(PIN_LINK_SPI_CS, LOW);
|
||
|
while (digitalRead(PIN_LINK_SPI_READY) == LOW && millis() - t < timeout) {
|
||
|
char c = SPI.transfer(' ');
|
||
|
if (c == 0 && c == 0xff) continue;
|
||
|
if (!eos) eos = (c == 0x9);
|
||
|
if (eos) continue;
|
||
|
if (!matched) {
|
||
|
// match header
|
||
|
if (n == 0 && c != header[0]) continue;
|
||
|
if (n == bufsize - 1) continue;
|
||
|
buffer[n++] = c;
|
||
|
if (n == sizeof(header)) {
|
||
|
matched = memcmp(buffer, header, sizeof(header)) == 0;
|
||
|
if (matched) {
|
||
|
n = 0;
|
||
|
} else {
|
||
|
memmove(buffer, buffer + 1, --n);
|
||
|
}
|
||
|
}
|
||
|
continue;
|
||
|
}
|
||
|
if (n > 3 && c == '.' && buffer[n - 1] == '.' && buffer[n - 2] == '.') {
|
||
|
// SEARCHING...
|
||
|
n = 0;
|
||
|
timeout += OBD_TIMEOUT_LONG;
|
||
|
} else {
|
||
|
if (n == bufsize - 1) {
|
||
|
int bytesDumped = dumpLine(buffer, n);
|
||
|
n -= bytesDumped;
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.println("[SPI BUFFER FULL]");
|
||
|
#endif
|
||
|
}
|
||
|
buffer[n++] = c;
|
||
|
}
|
||
|
}
|
||
|
digitalWrite(PIN_LINK_SPI_CS, HIGH);
|
||
|
portEXIT_CRITICAL(&m);
|
||
|
} while (!eos && millis() - t < timeout);
|
||
|
#if VERBOSE_LINK
|
||
|
if (!eos) {
|
||
|
// timed out
|
||
|
Serial.println("[SPI RECV TIMEOUT]");
|
||
|
}
|
||
|
#endif
|
||
|
buffer[n] = 0;
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.print("[SPI RECV]");
|
||
|
Serial.println(buffer);
|
||
|
#endif
|
||
|
// wait for READY pin to restore high level so SPI bus is released
|
||
|
if (eos) while (digitalRead(PIN_LINK_SPI_READY) == LOW) delay(1);
|
||
|
return n;
|
||
|
}
|
||
|
|
||
|
bool CLink_SPI::send(const char* str)
|
||
|
{
|
||
|
if (digitalRead(PIN_LINK_SPI_READY) == LOW) {
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.println("[SPI NOT READY]");
|
||
|
#endif
|
||
|
return false;
|
||
|
}
|
||
|
portMUX_TYPE m = portMUX_INITIALIZER_UNLOCKED;
|
||
|
#if VERBOSE_LINK
|
||
|
Serial.print("[SPI SEND]");
|
||
|
Serial.println(str);
|
||
|
#endif
|
||
|
int len = strlen(str);
|
||
|
uint8_t tail = 0x1B;
|
||
|
portENTER_CRITICAL(&m);
|
||
|
digitalWrite(PIN_LINK_SPI_CS, LOW);
|
||
|
delay(1);
|
||
|
SPI.writeBytes((uint8_t*)header, sizeof(header));
|
||
|
SPI.writeBytes((uint8_t*)str, len);
|
||
|
SPI.writeBytes((uint8_t*)&tail, 1);
|
||
|
delay(1);
|
||
|
digitalWrite(PIN_LINK_SPI_CS, HIGH);
|
||
|
portEXIT_CRITICAL(&m);
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
int CLink_SPI::sendCommand(const char* cmd, char* buf, int bufsize, unsigned int timeout)
|
||
|
{
|
||
|
uint32_t t = millis();
|
||
|
int n = 0;
|
||
|
for (byte i = 0; i < 30 && millis() - t < timeout; i++) {
|
||
|
if (!send(cmd)) {
|
||
|
delay(50);
|
||
|
continue;
|
||
|
}
|
||
|
n = receive(buf, bufsize, timeout);
|
||
|
if (n == -1) {
|
||
|
Serial.print('_');
|
||
|
n = 0;
|
||
|
continue;
|
||
|
}
|
||
|
if (n == 0 || (buf[1] != 'O' && !memcmp(buf + 5, "NO DATA", 7))) {
|
||
|
// data not ready
|
||
|
delay(50);
|
||
|
} else {
|
||
|
break;
|
||
|
}
|
||
|
}
|
||
|
return n;
|
||
|
}
|
||
|
|
||
|
void FreematicsESP32::gpsEnd(bool powerOff)
|
||
|
{
|
||
|
if (m_flags & FLAG_GNSS_USE_LINK) {
|
||
|
if (powerOff) {
|
||
|
char buf[16];
|
||
|
link->sendCommand("ATGPSOFF\r", buf, sizeof(buf), 0);
|
||
|
}
|
||
|
} else {
|
||
|
taskGPS.destroy();
|
||
|
if (m_flags & FLAG_GNSS_SOFT_SERIAL) {
|
||
|
#ifndef ARDUINO_ESP32C3_DEV
|
||
|
setTxPinLow();
|
||
|
#endif
|
||
|
} else {
|
||
|
uart_driver_delete(gpsUARTNum);
|
||
|
}
|
||
|
if (powerOff && m_pinGPSPower) digitalWrite(m_pinGPSPower, LOW);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
bool FreematicsESP32::gpsBeginExt(int baudrate)
|
||
|
{
|
||
|
if (devType > 0 && devType <= 13) {
|
||
|
#ifdef ARDUINO_ESP32C3_DEV
|
||
|
pinGPSRx = 18;
|
||
|
pinGPSTx = 19;
|
||
|
#else
|
||
|
pinGPSRx = 32;
|
||
|
pinGPSTx = 33;
|
||
|
#endif
|
||
|
m_pinGPSPower = PIN_GPS_POWER2;
|
||
|
} else {
|
||
|
pinGPSRx = PIN_GPS_UART_RXD;
|
||
|
pinGPSTx = PIN_GPS_UART_TXD;
|
||
|
}
|
||
|
// switch on GNSS power
|
||
|
if (m_pinGPSPower) {
|
||
|
pinMode(m_pinGPSPower, OUTPUT);
|
||
|
digitalWrite(m_pinGPSPower, HIGH);
|
||
|
}
|
||
|
if (!(m_flags & FLAG_GNSS_SOFT_SERIAL)) {
|
||
|
uart_config_t uart_config = {
|
||
|
.baud_rate = baudrate,
|
||
|
.data_bits = UART_DATA_8_BITS,
|
||
|
.parity = UART_PARITY_DISABLE,
|
||
|
.stop_bits = UART_STOP_BITS_1,
|
||
|
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||
|
.rx_flow_ctrl_thresh = 122,
|
||
|
};
|
||
|
|
||
|
// configure UART parameters
|
||
|
uart_param_config(gpsUARTNum, &uart_config);
|
||
|
// set UART pins
|
||
|
uart_set_pin(gpsUARTNum, pinGPSTx, pinGPSRx, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
||
|
// install UART driver
|
||
|
uart_driver_install(gpsUARTNum, UART_BUF_SIZE, 0, 0, NULL, 0);
|
||
|
// apply GNSS settings
|
||
|
delay(300);
|
||
|
gpsSendCommand(gpsSettings, sizeof(gpsSettings));
|
||
|
// start decoding task
|
||
|
taskGPS.create(gps_decode_task, "GPS", 1);
|
||
|
} else {
|
||
|
#ifndef ARDUINO_ESP32C3_DEV
|
||
|
pinMode(PIN_GPS_UART_RXD, INPUT);
|
||
|
pinMode(PIN_GPS_UART_TXD, OUTPUT);
|
||
|
setTxPinHigh();
|
||
|
delay(300);
|
||
|
#ifndef ARDUINO_ESP32C3_DEV
|
||
|
if (m_flags & FLAG_GNSS_SOFT_SERIAL) {
|
||
|
// set GNSS baudrate to 38400bps for M10
|
||
|
const uint8_t packet[] = {0x0, 0x0, 0xB5, 0x62, 0x06, 0x8A, 0x0C, 0x00, 0x01, 0x01, 0x00, 0x00, 0x01, 0x00, 0x52, 0x40, 0x00, 0x96, 0x00, 0x00, 0xC7, 0x2B};
|
||
|
// set GNSS baudrate to 38400bps for M8
|
||
|
//const uint8_t packet[] = {0x0, 0x0, 0xB5, 0x62, 0x06, 0x0, 0x14, 0x0, 0x01, 0x0, 0x0, 0x0, 0xD0, 0x08, 0x0, 0x0, 0x0, 0x96, 0x0, 0x0, 0x7, 0x0, 0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x93, 0x90};
|
||
|
for (int i = 0; i < sizeof(packet); i++) softSerialTx(baudrate, packet[i]);
|
||
|
}
|
||
|
#endif
|
||
|
// start GPS decoding task (soft serial)
|
||
|
taskGPS.create(gps_soft_decode_task, "GPS", 1);
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
// test run for a while to see if there is data decoded
|
||
|
uint16_t s1 = 0, s2 = 0;
|
||
|
gps.stats(&s1, 0);
|
||
|
for (int i = 0; i < 11; i++) {
|
||
|
delay(100);
|
||
|
gps.stats(&s2, 0);
|
||
|
if (s1 != s2) {
|
||
|
#ifndef ARDUINO_ESP32C3_DEV
|
||
|
if (m_flags & FLAG_GNSS_SOFT_SERIAL) {
|
||
|
// apply GNSS settings
|
||
|
for (int i = 0; i < sizeof(gpsSettings); i++) softSerialTx(GPS_SOFT_BAUDRATE, gpsSettings[i]);
|
||
|
}
|
||
|
#endif
|
||
|
return true;
|
||
|
}
|
||
|
}
|
||
|
// turn off GNSS power if no data in
|
||
|
gpsEnd();
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
bool FreematicsESP32::gpsBegin()
|
||
|
{
|
||
|
if (!link) return false;
|
||
|
m_flags |= FLAG_GNSS_USE_LINK;
|
||
|
|
||
|
char buf[256];
|
||
|
link->sendCommand("ATGPSON\r", buf, sizeof(buf), 100);
|
||
|
delay(300);
|
||
|
// apply GNSS settings
|
||
|
int n = sprintf(buf, "ATGDS");
|
||
|
for (int i = 0; i < sizeof(gpsSettings); i++) n += sprintf(buf + n, "%02X ", gpsSettings[i]);
|
||
|
buf[n - 1] = '\r';
|
||
|
link->sendCommand(buf, buf, sizeof(buf), 100);
|
||
|
|
||
|
uint32_t t = millis();
|
||
|
bool success = false;
|
||
|
do {
|
||
|
memset(buf, 0, sizeof(buf));
|
||
|
if (gpsGetNMEA(buf, sizeof(buf)) > 0 && strstr(buf, ("$GNGGA"))) {
|
||
|
success = true;
|
||
|
break;
|
||
|
}
|
||
|
} while (millis() - t < 1000);
|
||
|
if (success) {
|
||
|
m_pinGPSPower = 0;
|
||
|
return true;
|
||
|
}
|
||
|
gpsEnd();
|
||
|
m_flags &= ~FLAG_GNSS_USE_LINK;
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
bool FreematicsESP32::gpsGetData(GPS_DATA** pgd)
|
||
|
{
|
||
|
if (pgd) *pgd = &gpsData;
|
||
|
if (m_flags & FLAG_GNSS_USE_LINK) {
|
||
|
char buf[160];
|
||
|
if (!link || link->sendCommand("ATGPS\r", buf, sizeof(buf), 100) == 0) {
|
||
|
return false;
|
||
|
}
|
||
|
char *s = strstr(buf, "$GNIFO,");
|
||
|
if (!s) return false;
|
||
|
s += 7;
|
||
|
float lat = 0;
|
||
|
float lng = 0;
|
||
|
float alt = 0;
|
||
|
bool good = false;
|
||
|
do {
|
||
|
uint32_t date = atoi(s);
|
||
|
if (!(s = strchr(s, ','))) break;
|
||
|
uint32_t time = atoi(++s);
|
||
|
if (!(s = strchr(s, ','))) break;
|
||
|
gpsData.date = date;
|
||
|
gpsData.time = time;
|
||
|
lat = (float)atoi(++s) / 1000000;
|
||
|
if (!(s = strchr(s, ','))) break;
|
||
|
lng = (float)atoi(++s) / 1000000;
|
||
|
if (!(s = strchr(s, ','))) break;
|
||
|
alt = (float)atoi(++s) / 100;
|
||
|
good = true;
|
||
|
if (!(s = strchr(s, ','))) break;
|
||
|
gpsData.speed = (float)atoi(++s) / 100;
|
||
|
if (!(s = strchr(s, ','))) break;
|
||
|
gpsData.heading = atoi(++s) / 100;
|
||
|
if (!(s = strchr(s, ','))) break;
|
||
|
gpsData.sat = atoi(++s);
|
||
|
if (!(s = strchr(s, ','))) break;
|
||
|
gpsData.hdop = atoi(++s);
|
||
|
} while(0);
|
||
|
if (good && (gpsData.lat || gpsData.lng)) {
|
||
|
// filter out invalid coordinates
|
||
|
good = (abs(lat * 1000000 - gpsData.lat * 1000000) < 100000 && abs(lng * 1000000 - gpsData.lng * 1000000) < 100000);
|
||
|
}
|
||
|
if (!good) return false;
|
||
|
gpsData.lat = lat;
|
||
|
gpsData.lng = lng;
|
||
|
gpsData.alt = alt;
|
||
|
gpsData.ts = millis();
|
||
|
return true;
|
||
|
} else {
|
||
|
gps.stats(&gpsData.sentences, &gpsData.errors);
|
||
|
if (!gpsHasDecodedData) return false;
|
||
|
long lat, lng;
|
||
|
bool good = true;
|
||
|
gps.get_position(&lat, &lng, 0);
|
||
|
if (gpsData.lat || gpsData.lng) {
|
||
|
// filter out invalid coordinates
|
||
|
good = (abs(lat - gpsData.lat * 1000000) < 100000 && abs(lng - gpsData.lng * 1000000) < 100000);
|
||
|
}
|
||
|
if (!good) return false;
|
||
|
gpsData.ts = millis();
|
||
|
gpsData.lat = (float)lat / 1000000;
|
||
|
gpsData.lng = (float)lng / 1000000;
|
||
|
gps.get_datetime((unsigned long*)&gpsData.date, (unsigned long*)&gpsData.time, 0);
|
||
|
long alt = gps.altitude();
|
||
|
if (alt != TinyGPS::GPS_INVALID_ALTITUDE) gpsData.alt = (float)alt / 100;
|
||
|
unsigned long knot = gps.speed();
|
||
|
if (knot != TinyGPS::GPS_INVALID_SPEED) gpsData.speed = (float)knot / 100;
|
||
|
unsigned long course = gps.course();
|
||
|
if (course < 36000) gpsData.heading = course / 100;
|
||
|
unsigned short sat = gps.satellites();
|
||
|
if (sat != TinyGPS::GPS_INVALID_SATELLITES) gpsData.sat = sat;
|
||
|
unsigned long hdop = gps.hdop();
|
||
|
gpsData.hdop = hdop > 2550 ? 255 : hdop / 10;
|
||
|
gpsHasDecodedData = false;
|
||
|
return true;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
int FreematicsESP32::gpsGetNMEA(char* buffer, int bufsize)
|
||
|
{
|
||
|
if (m_flags & FLAG_GNSS_USE_LINK) {
|
||
|
return link->sendCommand("ATGRR\r", buffer, bufsize, 200);
|
||
|
} else {
|
||
|
return 0;
|
||
|
}
|
||
|
}
|
||
|
|
||
|
void FreematicsESP32::gpsSendCommand(const uint8_t* cmd, int len)
|
||
|
{
|
||
|
if (m_flags & FLAG_GNSS_SOFT_SERIAL) {
|
||
|
#ifndef ARDUINO_ESP32C3_DEV
|
||
|
for (int n = 0; n < len; n++) {
|
||
|
softSerialTx(GPS_SOFT_BAUDRATE, cmd[n]);
|
||
|
}
|
||
|
#endif
|
||
|
} else {
|
||
|
uart_write_bytes(gpsUARTNum, cmd, len);
|
||
|
}
|
||
|
}
|
||
|
|
||
|
bool FreematicsESP32::xbBegin(unsigned long baudrate, int pinRx, int pinTx)
|
||
|
{
|
||
|
uart_config_t uart_config = {
|
||
|
.baud_rate = (int)baudrate,
|
||
|
.data_bits = UART_DATA_8_BITS,
|
||
|
.parity = UART_PARITY_DISABLE,
|
||
|
.stop_bits = UART_STOP_BITS_1,
|
||
|
.flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
|
||
|
.rx_flow_ctrl_thresh = 122,
|
||
|
};
|
||
|
|
||
|
#if VERBOSE_XBEE
|
||
|
Serial.print("Bee Rx:");
|
||
|
Serial.print(pinRx);
|
||
|
Serial.print(" Tx:");
|
||
|
Serial.println(pinTx);
|
||
|
#endif
|
||
|
//Configure UART parameters
|
||
|
uart_param_config(BEE_UART_NUM, &uart_config);
|
||
|
//Set UART pins
|
||
|
uart_set_pin(BEE_UART_NUM, pinTx, pinRx, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
|
||
|
//Install UART driver
|
||
|
uart_driver_install(BEE_UART_NUM, UART_BUF_SIZE, 0, 0, NULL, 0);
|
||
|
|
||
|
#ifdef PIN_BEE_PWR
|
||
|
pinMode(PIN_BEE_PWR, OUTPUT);
|
||
|
digitalWrite(PIN_BEE_PWR, LOW);
|
||
|
xbTogglePower();
|
||
|
#endif
|
||
|
return true;
|
||
|
}
|
||
|
|
||
|
void FreematicsESP32::xbEnd()
|
||
|
{
|
||
|
uart_driver_delete(BEE_UART_NUM);
|
||
|
}
|
||
|
|
||
|
void FreematicsESP32::xbWrite(const char* cmd)
|
||
|
{
|
||
|
uart_write_bytes(BEE_UART_NUM, cmd, strlen(cmd));
|
||
|
#if VERBOSE_XBEE
|
||
|
Serial.print("=== SENT@");
|
||
|
Serial.print(millis());
|
||
|
Serial.println(" ===");
|
||
|
Serial.println(cmd);
|
||
|
Serial.println("==================");
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
void FreematicsESP32::xbWrite(const char* data, int len)
|
||
|
{
|
||
|
uart_write_bytes(BEE_UART_NUM, data, len);
|
||
|
}
|
||
|
|
||
|
int FreematicsESP32::xbRead(char* buffer, int bufsize, unsigned int timeout)
|
||
|
{
|
||
|
return uart_read_bytes(BEE_UART_NUM, buffer, bufsize, timeout / portTICK_RATE_MS);
|
||
|
}
|
||
|
|
||
|
int FreematicsESP32::xbReceive(char* buffer, int bufsize, unsigned int timeout, const char** expected, byte expectedCount)
|
||
|
{
|
||
|
int bytesRecv = 0;
|
||
|
uint32_t t = millis();
|
||
|
do {
|
||
|
if (bytesRecv >= bufsize - 16) {
|
||
|
bytesRecv -= dumpLine(buffer, bytesRecv);
|
||
|
}
|
||
|
int n = xbRead(buffer + bytesRecv, bufsize - bytesRecv - 1, 50);
|
||
|
if (n > 0) {
|
||
|
#if VERBOSE_XBEE
|
||
|
Serial.print("=== RECV@");
|
||
|
Serial.print(millis());
|
||
|
Serial.println(" ===");
|
||
|
buffer[bytesRecv + n] = 0;
|
||
|
Serial.print(buffer + bytesRecv);
|
||
|
Serial.println("==================");
|
||
|
#endif
|
||
|
bytesRecv += n;
|
||
|
buffer[bytesRecv] = 0;
|
||
|
for (byte i = 0; i < expectedCount; i++) {
|
||
|
// match expected string(s)
|
||
|
if (expected[i] && strstr(buffer, expected[i])) return i + 1;
|
||
|
}
|
||
|
} else if (n == -1) {
|
||
|
// an erroneous reading
|
||
|
#if VERBOSE_XBEE
|
||
|
Serial.print("RECV ERROR");
|
||
|
#endif
|
||
|
break;
|
||
|
}
|
||
|
} while (millis() - t < timeout);
|
||
|
buffer[bytesRecv] = 0;
|
||
|
return bytesRecv == 0 ? 0 : -1;
|
||
|
}
|
||
|
|
||
|
void FreematicsESP32::xbPurge()
|
||
|
{
|
||
|
uart_flush(BEE_UART_NUM);
|
||
|
}
|
||
|
|
||
|
void FreematicsESP32::xbTogglePower(unsigned int duration)
|
||
|
{
|
||
|
#ifdef PIN_BEE_PWR
|
||
|
digitalWrite(PIN_BEE_PWR, HIGH);
|
||
|
#if VERBOSE_XBEE
|
||
|
Serial.print("Pin ");
|
||
|
Serial.print(PIN_BEE_PWR);
|
||
|
Serial.println(" pull up");
|
||
|
#endif
|
||
|
delay(duration);
|
||
|
digitalWrite(PIN_BEE_PWR, LOW);
|
||
|
#if VERBOSE_XBEE
|
||
|
Serial.print("Pin ");
|
||
|
Serial.print(PIN_BEE_PWR);
|
||
|
Serial.println(" pull down");
|
||
|
#endif
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
void FreematicsESP32::buzzer(int freq)
|
||
|
{
|
||
|
#ifdef PIN_BUZZER
|
||
|
if (freq) {
|
||
|
ledcWriteTone(0, freq);
|
||
|
ledcWrite(0, 255);
|
||
|
} else {
|
||
|
ledcWrite(0, 0);
|
||
|
}
|
||
|
#endif
|
||
|
}
|
||
|
|
||
|
byte FreematicsESP32::getDeviceType()
|
||
|
{
|
||
|
if (!link) return 0;
|
||
|
char buf[32];
|
||
|
if (link && link->sendCommand("ATI\r", buf, sizeof(buf), 1000)) {
|
||
|
char *p = strstr(buf, "OBD");
|
||
|
if (p && (p = strchr(p, ' '))) {
|
||
|
p += 2;
|
||
|
if (isdigit(*p) && *(p + 1) == '.' && isdigit(*(p + 2))) {
|
||
|
devType = (*p - '0') * 10 + (*(p + 2) - '0');
|
||
|
return devType;
|
||
|
}
|
||
|
}
|
||
|
}
|
||
|
return 0;
|
||
|
}
|
||
|
|
||
|
bool FreematicsESP32::reactivateLink()
|
||
|
{
|
||
|
if (!link) return false;
|
||
|
for (int n = 0; n < 30; n++) {
|
||
|
char buf[32];
|
||
|
if (link->sendCommand("ATI\r", buf, sizeof(buf), 1000)) return true;
|
||
|
}
|
||
|
return false;
|
||
|
}
|
||
|
|
||
|
void FreematicsESP32::resetLink()
|
||
|
{
|
||
|
#ifdef PIN_LINK_RESET
|
||
|
if (devType >= 14) {
|
||
|
digitalWrite(PIN_LINK_RESET, LOW);
|
||
|
delay(50);
|
||
|
digitalWrite(PIN_LINK_RESET, HIGH);
|
||
|
delay(1000);
|
||
|
return;
|
||
|
}
|
||
|
#endif
|
||
|
char buf[16];
|
||
|
if (link) link->sendCommand("ATR\r", buf, sizeof(buf), 100);
|
||
|
}
|
||
|
|
||
|
bool FreematicsESP32::begin(bool useCoProc, bool useCellular)
|
||
|
{
|
||
|
// set wifi max power
|
||
|
esp_wifi_set_max_tx_power(80);
|
||
|
|
||
|
// set watchdog timeout to 600 seconds
|
||
|
esp_task_wdt_init(600, 0);
|
||
|
|
||
|
m_flags = 0;
|
||
|
m_pinGPSPower = PIN_GPS_POWER;
|
||
|
|
||
|
#if PIN_BUZZER
|
||
|
// set up buzzer
|
||
|
ledcSetup(0, 2000, 8);
|
||
|
ledcAttachPin(PIN_BUZZER, 0);
|
||
|
#endif
|
||
|
|
||
|
if (useCoProc) do {
|
||
|
if (link) return false;
|
||
|
#ifdef PIN_LINK_RESET
|
||
|
pinMode(PIN_LINK_RESET, OUTPUT);
|
||
|
digitalWrite(PIN_LINK_RESET, HIGH);
|
||
|
#endif
|
||
|
CLink_UART *linkUART = new CLink_UART;
|
||
|
#if 0
|
||
|
linkUART->begin(115200);
|
||
|
char buf[16];
|
||
|
// lift baudrate to 512Kbps
|
||
|
linkUART->sendCommand("ATBR1 7D000\r", buf, sizeof(buf), 50);
|
||
|
linkUART->end();
|
||
|
delay(50);
|
||
|
#endif
|
||
|
if (linkUART->begin(115200)) {
|
||
|
link = linkUART;
|
||
|
for (byte n = 0; n < 3 && !getDeviceType(); n++);
|
||
|
if (devType) {
|
||
|
m_flags |= FLAG_USE_UART_LINK;
|
||
|
break;
|
||
|
}
|
||
|
link = 0;
|
||
|
linkUART->end();
|
||
|
}
|
||
|
delete linkUART;
|
||
|
linkUART = 0;
|
||
|
#if 0
|
||
|
CLink_SPI *linkSPI = new CLink_SPI;
|
||
|
if (linkSPI->begin()) {
|
||
|
link = linkSPI;
|
||
|
for (byte n = 0; n < 10 && !getDeviceType(); n++);
|
||
|
if (devType) {
|
||
|
m_pinGPSPower = PIN_GPS_POWER2;
|
||
|
break;
|
||
|
}
|
||
|
link = 0;
|
||
|
linkSPI->end();
|
||
|
}
|
||
|
delete linkSPI;
|
||
|
linkSPI = 0;
|
||
|
#endif
|
||
|
useCoProc = false;
|
||
|
} while(0);
|
||
|
|
||
|
if (useCellular) {
|
||
|
int pinRx = PIN_BEE_UART_RXD;
|
||
|
int pinTx = PIN_BEE_UART_TXD;
|
||
|
if (devType == 13) {
|
||
|
pinRx = 32;
|
||
|
pinTx = 33;
|
||
|
} else if ((devType == 11 && !(m_flags & FLAG_USE_UART_LINK)) || devType == 12 || devType == 0) {
|
||
|
#ifndef ARDUINO_ESP32C3_DEV
|
||
|
//pinRx = 16;
|
||
|
//pinTx = 17;
|
||
|
#endif
|
||
|
}
|
||
|
pinMode(PIN_BEE_PWR, OUTPUT);
|
||
|
digitalWrite(PIN_BEE_PWR, LOW);
|
||
|
xbBegin(XBEE_BAUDRATE, pinRx, pinTx);
|
||
|
m_flags |= FLAG_USE_CELL;
|
||
|
if (useCoProc) {
|
||
|
m_flags |= FLAG_GNSS_SOFT_SERIAL;
|
||
|
}
|
||
|
}
|
||
|
gpsUARTNum = useCoProc ? GPS_UART_NUM : LINK_UART_NUM;
|
||
|
return devType != 0;
|
||
|
}
|