mirror of
https://github.com/ddv2005/AirsoftTracker.git
synced 2026-06-28 05:56:46 +00:00
Initial commit
This commit is contained in:
unknown
@@ -0,0 +1,105 @@
|
||||
/*!
|
||||
* @file Adafruit_AHRS_FusionInterface.h
|
||||
*
|
||||
* @section license License
|
||||
*
|
||||
* The MIT License (MIT)
|
||||
*
|
||||
* Copyright (c) 2020 Ha Thach (tinyusb.org) for Adafruit Industries
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#ifndef ADAFRUIT_AHRS_FUSIONINTERFACE_H_
|
||||
#define ADAFRUIT_AHRS_FUSIONINTERFACE_H_
|
||||
|
||||
/*!
|
||||
* @brief The common interface for the fusion algorithms.
|
||||
*/
|
||||
class Adafruit_AHRS_FusionInterface {
|
||||
public:
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
* @brief Initializes the sensor fusion filter.
|
||||
*
|
||||
* @param sampleFrequency The sensor sample rate in herz(samples per second).
|
||||
*/
|
||||
/**************************************************************************/
|
||||
virtual void begin(float sampleFrequency) = 0;
|
||||
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
* @brief Updates the filter with new gyroscope, accelerometer, and
|
||||
* magnetometer data.
|
||||
*
|
||||
* @param gx The gyroscope x axis. In DPS.
|
||||
* @param gy The gyroscope y axis. In DPS.
|
||||
* @param gz The gyroscope z axis. In DPS.
|
||||
* @param ax The accelerometer x axis. In g.
|
||||
* @param ay The accelerometer y axis. In g.
|
||||
* @param az The accelerometer z axis. In g.
|
||||
* @param mx The magnetometer x axis. In uT.
|
||||
* @param my The magnetometer y axis. In uT.
|
||||
* @param mz The magnetometer z axis. In uT.
|
||||
*/
|
||||
/**************************************************************************/
|
||||
virtual void update(float gx, float gy, float gz, float ax, float ay,
|
||||
float az, float mx, float my, float mz, float dt) = 0;
|
||||
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
* @brief Gets the current roll of the sensors.
|
||||
*
|
||||
* @return The current sensor roll.
|
||||
*/
|
||||
/**************************************************************************/
|
||||
virtual float getRoll() = 0;
|
||||
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
* @brief Gets the current pitch of the sensors.
|
||||
*
|
||||
* @return The current sensor pitch.
|
||||
*/
|
||||
/**************************************************************************/
|
||||
virtual float getPitch() = 0;
|
||||
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
* @brief Gets the current yaw of the sensors.
|
||||
*
|
||||
* @return The current sensor yaw.
|
||||
*/
|
||||
/**************************************************************************/
|
||||
virtual float getYaw() = 0;
|
||||
virtual void getQuaternion(float *w, float *x, float *y, float *z) = 0;
|
||||
virtual void setQuaternion(float w, float x, float y, float z) = 0;
|
||||
|
||||
/**************************************************************************/
|
||||
/*!
|
||||
* @brief Gets the current gravity vector of the sensor.
|
||||
*
|
||||
* @param x A float pointer to write the gravity vector x component to. In g.
|
||||
* @param y A float pointer to write the gravity vector y component to. In g.
|
||||
* @param z A float pointer to write the gravity vector z component to. In g.
|
||||
*/
|
||||
virtual void getGravityVector(float *x, float *y, float *z) = 0;
|
||||
};
|
||||
|
||||
#endif /* ADAFRUIT_AHRS_FUSIONINTERFACE_H_ */
|
||||
@@ -0,0 +1,296 @@
|
||||
//=============================================================================================
|
||||
// Madgwick.c
|
||||
//=============================================================================================
|
||||
//
|
||||
// Implementation of Madgwick's IMU and AHRS algorithms.
|
||||
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
|
||||
//
|
||||
// From the x-io website "Open-source resources available on this website are
|
||||
// provided under the GNU General Public Licence unless an alternative licence
|
||||
// is provided in source."
|
||||
//
|
||||
// Date Author Notes
|
||||
// 29/09/2011 SOH Madgwick Initial release
|
||||
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||||
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
|
||||
//
|
||||
//=============================================================================================
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Header files
|
||||
|
||||
#include "Adafruit_AHRS_Madgwick.h"
|
||||
#include <math.h>
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Definitions
|
||||
|
||||
#define sampleFreqDef 100.0f // sample frequency in Hz
|
||||
#define betaDef 0.5f // 2 * proportional gain
|
||||
|
||||
//============================================================================================
|
||||
// Functions
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// AHRS algorithm update
|
||||
|
||||
Adafruit_Madgwick::Adafruit_Madgwick() : Adafruit_Madgwick(betaDef) {}
|
||||
|
||||
Adafruit_Madgwick::Adafruit_Madgwick(float gain) {
|
||||
beta = gain;
|
||||
q0 = 1.0f;
|
||||
q1 = 0.0f;
|
||||
q2 = 0.0f;
|
||||
q3 = 0.0f;
|
||||
invSampleFreq = 1.0f / sampleFreqDef;
|
||||
anglesComputed = false;
|
||||
}
|
||||
|
||||
void Adafruit_Madgwick::update(float gx, float gy, float gz, float ax, float ay,
|
||||
float az, float mx, float my, float mz,
|
||||
float dt) {
|
||||
float recipNorm;
|
||||
float s0, s1, s2, s3;
|
||||
float qDot1, qDot2, qDot3, qDot4;
|
||||
float hx, hy;
|
||||
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
|
||||
_2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
|
||||
q2q2, q2q3, q3q3;
|
||||
|
||||
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in
|
||||
// magnetometer normalisation)
|
||||
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
|
||||
updateIMU(gx, gy, gz, ax, ay, az, dt);
|
||||
return;
|
||||
}
|
||||
|
||||
// Convert gyroscope degrees/sec to radians/sec
|
||||
//gx *= 0.0174533f;
|
||||
//gy *= 0.0174533f;
|
||||
//gz *= 0.0174533f;
|
||||
|
||||
// Rate of change of quaternion from gyroscope
|
||||
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
|
||||
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
|
||||
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
|
||||
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid (avoids NaN in
|
||||
// accelerometer normalisation)
|
||||
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
|
||||
|
||||
// Normalise accelerometer measurement
|
||||
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Normalise magnetometer measurement
|
||||
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
|
||||
mx *= recipNorm;
|
||||
my *= recipNorm;
|
||||
mz *= recipNorm;
|
||||
|
||||
// Auxiliary variables to avoid repeated arithmetic
|
||||
_2q0mx = 2.0f * q0 * mx;
|
||||
_2q0my = 2.0f * q0 * my;
|
||||
_2q0mz = 2.0f * q0 * mz;
|
||||
_2q1mx = 2.0f * q1 * mx;
|
||||
_2q0 = 2.0f * q0;
|
||||
_2q1 = 2.0f * q1;
|
||||
_2q2 = 2.0f * q2;
|
||||
_2q3 = 2.0f * q3;
|
||||
_2q0q2 = 2.0f * q0 * q2;
|
||||
_2q2q3 = 2.0f * q2 * q3;
|
||||
q0q0 = q0 * q0;
|
||||
q0q1 = q0 * q1;
|
||||
q0q2 = q0 * q2;
|
||||
q0q3 = q0 * q3;
|
||||
q1q1 = q1 * q1;
|
||||
q1q2 = q1 * q2;
|
||||
q1q3 = q1 * q3;
|
||||
q2q2 = q2 * q2;
|
||||
q2q3 = q2 * q3;
|
||||
q3q3 = q3 * q3;
|
||||
|
||||
// Reference direction of Earth's magnetic field
|
||||
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 +
|
||||
_2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
|
||||
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 +
|
||||
my * q2q2 + _2q2 * mz * q3 - my * q3q3;
|
||||
_2bx = sqrtf(hx * hx + hy * hy);
|
||||
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 +
|
||||
_2q2 * my * q3 - mz * q2q2 + mz * q3q3;
|
||||
_4bx = 2.0f * _2bx;
|
||||
_4bz = 2.0f * _2bz;
|
||||
|
||||
// Gradient decent algorithm corrective step
|
||||
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
|
||||
_2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
|
||||
_2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
|
||||
(-_2bx * q3 + _2bz * q1) *
|
||||
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
|
||||
_2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
|
||||
_2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
|
||||
4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
|
||||
_2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
|
||||
(_2bx * q2 + _2bz * q0) *
|
||||
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
|
||||
(_2bx * q3 - _4bz * q1) *
|
||||
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
|
||||
_2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
|
||||
4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
|
||||
(-_4bx * q2 - _2bz * q0) *
|
||||
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
|
||||
(_2bx * q1 + _2bz * q3) *
|
||||
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
|
||||
(_2bx * q0 - _4bz * q2) *
|
||||
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
|
||||
_2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
|
||||
(-_4bx * q3 + _2bz * q1) *
|
||||
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
|
||||
(-_2bx * q0 + _2bz * q2) *
|
||||
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
|
||||
_2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
|
||||
s3 * s3); // normalise step magnitude
|
||||
s0 *= recipNorm;
|
||||
s1 *= recipNorm;
|
||||
s2 *= recipNorm;
|
||||
s3 *= recipNorm;
|
||||
|
||||
// Apply feedback step
|
||||
qDot1 -= beta * s0;
|
||||
qDot2 -= beta * s1;
|
||||
qDot3 -= beta * s2;
|
||||
qDot4 -= beta * s3;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion to yield quaternion
|
||||
q0 += qDot1 * dt;
|
||||
q1 += qDot2 * dt;
|
||||
q2 += qDot3 * dt;
|
||||
q3 += qDot4 * dt;
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
anglesComputed = 0;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// IMU algorithm update
|
||||
|
||||
void Adafruit_Madgwick::updateIMU(float gx, float gy, float gz, float ax,
|
||||
float ay, float az, float dt) {
|
||||
float recipNorm;
|
||||
float s0, s1, s2, s3;
|
||||
float qDot1, qDot2, qDot3, qDot4;
|
||||
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
|
||||
q3q3;
|
||||
|
||||
// Convert gyroscope degrees/sec to radians/sec
|
||||
//gx *= 0.0174533f;
|
||||
//gy *= 0.0174533f;
|
||||
//gz *= 0.0174533f;
|
||||
|
||||
// Rate of change of quaternion from gyroscope
|
||||
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
|
||||
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
|
||||
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
|
||||
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid (avoids NaN in
|
||||
// accelerometer normalisation)
|
||||
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
|
||||
|
||||
// Normalise accelerometer measurement
|
||||
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Auxiliary variables to avoid repeated arithmetic
|
||||
_2q0 = 2.0f * q0;
|
||||
_2q1 = 2.0f * q1;
|
||||
_2q2 = 2.0f * q2;
|
||||
_2q3 = 2.0f * q3;
|
||||
_4q0 = 4.0f * q0;
|
||||
_4q1 = 4.0f * q1;
|
||||
_4q2 = 4.0f * q2;
|
||||
_8q1 = 8.0f * q1;
|
||||
_8q2 = 8.0f * q2;
|
||||
q0q0 = q0 * q0;
|
||||
q1q1 = q1 * q1;
|
||||
q2q2 = q2 * q2;
|
||||
q3q3 = q3 * q3;
|
||||
|
||||
// Gradient decent algorithm corrective step
|
||||
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
|
||||
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 +
|
||||
_8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
|
||||
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 +
|
||||
_8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
|
||||
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
|
||||
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 +
|
||||
s3 * s3); // normalise step magnitude
|
||||
s0 *= recipNorm;
|
||||
s1 *= recipNorm;
|
||||
s2 *= recipNorm;
|
||||
s3 *= recipNorm;
|
||||
|
||||
// Apply feedback step
|
||||
qDot1 -= beta * s0;
|
||||
qDot2 -= beta * s1;
|
||||
qDot3 -= beta * s2;
|
||||
qDot4 -= beta * s3;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion to yield quaternion
|
||||
q0 += qDot1 * dt;
|
||||
q1 += qDot2 * dt;
|
||||
q2 += qDot3 * dt;
|
||||
q3 += qDot4 * dt;
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
anglesComputed = 0;
|
||||
}
|
||||
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Fast inverse square-root
|
||||
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
|
||||
|
||||
float Adafruit_Madgwick::invSqrt(float x) {
|
||||
float halfx = 0.5f * x;
|
||||
float y = x;
|
||||
long i = *(long *)&y;
|
||||
i = 0x5f3759df - (i >> 1);
|
||||
y = *(float *)&i;
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
return y;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
|
||||
void Adafruit_Madgwick::computeAngles() {
|
||||
roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
|
||||
pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
|
||||
yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
|
||||
grav[0] = 2.0f * (q1 * q3 - q0 * q2);
|
||||
grav[1] = 2.0f * (q0 * q1 + q2 * q3);
|
||||
grav[2] = 2.0f * (q1 * q0 - 0.5f + q3 * q3);
|
||||
anglesComputed = 1;
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
//=============================================================================================
|
||||
// Madgwick.h
|
||||
//=============================================================================================
|
||||
//
|
||||
// Implementation of Madgwick's IMU and AHRS algorithms.
|
||||
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
|
||||
//
|
||||
// From the x-io website "Open-source resources available on this website are
|
||||
// provided under the GNU General Public Licence unless an alternative licence
|
||||
// is provided in source."
|
||||
//
|
||||
// Date Author Notes
|
||||
// 29/09/2011 SOH Madgwick Initial release
|
||||
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||||
//
|
||||
//=============================================================================================
|
||||
#ifndef __Adafruit_Madgwick_h__
|
||||
#define __Adafruit_Madgwick_h__
|
||||
|
||||
#include "Adafruit_AHRS_FusionInterface.h"
|
||||
#include <math.h>
|
||||
|
||||
//--------------------------------------------------------------------------------------------
|
||||
// Variable declaration
|
||||
class Adafruit_Madgwick : public Adafruit_AHRS_FusionInterface {
|
||||
private:
|
||||
static float invSqrt(float x);
|
||||
float beta; // algorithm gain
|
||||
float q0;
|
||||
float q1;
|
||||
float q2;
|
||||
float q3; // quaternion of sensor frame relative to auxiliary frame
|
||||
float invSampleFreq;
|
||||
float roll, pitch, yaw;
|
||||
float grav[3];
|
||||
bool anglesComputed = false;
|
||||
void computeAngles();
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Function declarations
|
||||
public:
|
||||
Adafruit_Madgwick();
|
||||
Adafruit_Madgwick(float gain);
|
||||
void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
|
||||
void update(float gx, float gy, float gz, float ax, float ay, float az,
|
||||
float mx, float my, float mz, float dt);
|
||||
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az,
|
||||
float dt);
|
||||
// float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 *
|
||||
// q0 + 2.0f * q3 * q3 - 1.0f);}; float getRoll(){return -1.0f * asinf(2.0f *
|
||||
// q1 * q3 + 2.0f * q0 * q2);}; float getYaw(){return atan2f(2.0f * q1 * q2
|
||||
// - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
|
||||
float getBeta() { return beta; }
|
||||
void setBeta(float beta) { this->beta = beta; }
|
||||
float getRoll() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return roll * 57.29578f;
|
||||
}
|
||||
float getPitch() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return pitch * 57.29578f;
|
||||
}
|
||||
float getYaw() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return yaw * 57.29578f + 180.0f;
|
||||
}
|
||||
float getRollRadians() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return roll;
|
||||
}
|
||||
float getPitchRadians() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return pitch;
|
||||
}
|
||||
float getYawRadians() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return yaw;
|
||||
}
|
||||
void getQuaternion(float *w, float *x, float *y, float *z) {
|
||||
*w = q0;
|
||||
*x = q1;
|
||||
*y = q2;
|
||||
*z = q3;
|
||||
}
|
||||
void setQuaternion(float w, float x, float y, float z) {
|
||||
q0 = w;
|
||||
q1 = x;
|
||||
q2 = y;
|
||||
q3 = z;
|
||||
}
|
||||
void getGravityVector(float *x, float *y, float *z) {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
*x = grav[0];
|
||||
*y = grav[1];
|
||||
*z = grav[2];
|
||||
}
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,277 @@
|
||||
//=============================================================================================
|
||||
// Adafruit_Mahony.c
|
||||
//=============================================================================================
|
||||
//
|
||||
// Madgwick's implementation of Mayhony's AHRS algorithm.
|
||||
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
|
||||
//
|
||||
// From the x-io website "Open-source resources available on this website are
|
||||
// provided under the GNU General Public Licence unless an alternative licence
|
||||
// is provided in source."
|
||||
//
|
||||
// Date Author Notes
|
||||
// 29/09/2011 SOH Madgwick Initial release
|
||||
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||||
//
|
||||
// Algorithm paper:
|
||||
// http://ieeexplore.ieee.org/xpl/login.jsp?tp=&arnumber=4608934&url=http%3A%2F%2Fieeexplore.ieee.org%2Fstamp%2Fstamp.jsp%3Ftp%3D%26arnumber%3D4608934
|
||||
//
|
||||
//=============================================================================================
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Header files
|
||||
|
||||
#include "Adafruit_AHRS_Mahony.h"
|
||||
#include <math.h>
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Definitions
|
||||
|
||||
#define DEFAULT_SAMPLE_FREQ 100.0f // sample frequency in Hz
|
||||
#define twoKpDef (2.0f * 12.5f) // 2 * proportional gain
|
||||
#define twoKiDef (2.0f * 0.0f) // 2 * integral gain
|
||||
|
||||
//============================================================================================
|
||||
// Functions
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// AHRS algorithm update
|
||||
|
||||
Adafruit_Mahony::Adafruit_Mahony() : Adafruit_Mahony(twoKpDef, twoKiDef) {}
|
||||
|
||||
Adafruit_Mahony::Adafruit_Mahony(float prop_gain, float int_gain) {
|
||||
twoKp = prop_gain; // 2 * proportional gain (Kp)
|
||||
twoKi = int_gain; // 2 * integral gain (Ki)
|
||||
q0 = 1.0f;
|
||||
q1 = 0.0f;
|
||||
q2 = 0.0f;
|
||||
q3 = 0.0f;
|
||||
integralFBx = 0.0f;
|
||||
integralFBy = 0.0f;
|
||||
integralFBz = 0.0f;
|
||||
anglesComputed = false;
|
||||
invSampleFreq = 1.0f / DEFAULT_SAMPLE_FREQ;
|
||||
}
|
||||
|
||||
void Adafruit_Mahony::update(float gx, float gy, float gz, float ax, float ay,
|
||||
float az, float mx, float my, float mz, float dt) {
|
||||
float recipNorm;
|
||||
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
|
||||
float hx, hy, bx, bz;
|
||||
float halfvx, halfvy, halfvz, halfwx, halfwy, halfwz;
|
||||
float halfex, halfey, halfez;
|
||||
float qa, qb, qc;
|
||||
|
||||
// Use IMU algorithm if magnetometer measurement invalid
|
||||
// (avoids NaN in magnetometer normalisation)
|
||||
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
|
||||
updateIMU(gx, gy, gz, ax, ay, az, dt);
|
||||
return;
|
||||
}
|
||||
|
||||
// Convert gyroscope degrees/sec to radians/sec
|
||||
//gx *= 0.0174533f;
|
||||
//gy *= 0.0174533f;
|
||||
//gz *= 0.0174533f;
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid
|
||||
// (avoids NaN in accelerometer normalisation)
|
||||
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
|
||||
|
||||
// Normalise accelerometer measurement
|
||||
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Normalise magnetometer measurement
|
||||
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
|
||||
mx *= recipNorm;
|
||||
my *= recipNorm;
|
||||
mz *= recipNorm;
|
||||
|
||||
// Auxiliary variables to avoid repeated arithmetic
|
||||
q0q0 = q0 * q0;
|
||||
q0q1 = q0 * q1;
|
||||
q0q2 = q0 * q2;
|
||||
q0q3 = q0 * q3;
|
||||
q1q1 = q1 * q1;
|
||||
q1q2 = q1 * q2;
|
||||
q1q3 = q1 * q3;
|
||||
q2q2 = q2 * q2;
|
||||
q2q3 = q2 * q3;
|
||||
q3q3 = q3 * q3;
|
||||
|
||||
// Reference direction of Earth's magnetic field
|
||||
hx = 2.0f *
|
||||
(mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
|
||||
hy = 2.0f *
|
||||
(mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
|
||||
bx = sqrtf(hx * hx + hy * hy);
|
||||
bz = 2.0f *
|
||||
(mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
|
||||
|
||||
// Estimated direction of gravity and magnetic field
|
||||
halfvx = q1q3 - q0q2;
|
||||
halfvy = q0q1 + q2q3;
|
||||
halfvz = q0q0 - 0.5f + q3q3;
|
||||
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
|
||||
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
|
||||
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
|
||||
|
||||
// Error is sum of cross product between estimated direction
|
||||
// and measured direction of field vectors
|
||||
halfex = (ay * halfvz - az * halfvy) + (my * halfwz - mz * halfwy);
|
||||
halfey = (az * halfvx - ax * halfvz) + (mz * halfwx - mx * halfwz);
|
||||
halfez = (ax * halfvy - ay * halfvx) + (mx * halfwy - my * halfwx);
|
||||
|
||||
// Compute and apply integral feedback if enabled
|
||||
if (twoKi > 0.0f) {
|
||||
// integral error scaled by Ki
|
||||
integralFBx += twoKi * halfex * dt;
|
||||
integralFBy += twoKi * halfey * dt;
|
||||
integralFBz += twoKi * halfez * dt;
|
||||
gx += integralFBx; // apply integral feedback
|
||||
gy += integralFBy;
|
||||
gz += integralFBz;
|
||||
} else {
|
||||
integralFBx = 0.0f; // prevent integral windup
|
||||
integralFBy = 0.0f;
|
||||
integralFBz = 0.0f;
|
||||
}
|
||||
|
||||
// Apply proportional feedback
|
||||
gx += twoKp * halfex;
|
||||
gy += twoKp * halfey;
|
||||
gz += twoKp * halfez;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion
|
||||
gx *= (0.5f * dt); // pre-multiply common factors
|
||||
gy *= (0.5f * dt);
|
||||
gz *= (0.5f * dt);
|
||||
qa = q0;
|
||||
qb = q1;
|
||||
qc = q2;
|
||||
q0 += (-qb * gx - qc * gy - q3 * gz);
|
||||
q1 += (qa * gx + qc * gz - q3 * gy);
|
||||
q2 += (qa * gy - qb * gz + q3 * gx);
|
||||
q3 += (qa * gz + qb * gy - qc * gx);
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
anglesComputed = 0;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// IMU algorithm update
|
||||
|
||||
void Adafruit_Mahony::updateIMU(float gx, float gy, float gz, float ax,
|
||||
float ay, float az, float dt) {
|
||||
float recipNorm;
|
||||
float halfvx, halfvy, halfvz;
|
||||
float halfex, halfey, halfez;
|
||||
float qa, qb, qc;
|
||||
|
||||
// Convert gyroscope degrees/sec to radians/sec
|
||||
//gx *= 0.0174533f;
|
||||
//gy *= 0.0174533f;
|
||||
//gz *= 0.0174533f;
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid
|
||||
// (avoids NaN in accelerometer normalisation)
|
||||
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
|
||||
|
||||
// Normalise accelerometer measurement
|
||||
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Estimated direction of gravity
|
||||
halfvx = q1 * q3 - q0 * q2;
|
||||
halfvy = q0 * q1 + q2 * q3;
|
||||
halfvz = q0 * q0 - 0.5f + q3 * q3;
|
||||
|
||||
// Error is sum of cross product between estimated
|
||||
// and measured direction of gravity
|
||||
halfex = (ay * halfvz - az * halfvy);
|
||||
halfey = (az * halfvx - ax * halfvz);
|
||||
halfez = (ax * halfvy - ay * halfvx);
|
||||
|
||||
// Compute and apply integral feedback if enabled
|
||||
if (twoKi > 0.0f) {
|
||||
// integral error scaled by Ki
|
||||
integralFBx += twoKi * halfex * dt;
|
||||
integralFBy += twoKi * halfey * dt;
|
||||
integralFBz += twoKi * halfez * dt;
|
||||
gx += integralFBx; // apply integral feedback
|
||||
gy += integralFBy;
|
||||
gz += integralFBz;
|
||||
} else {
|
||||
integralFBx = 0.0f; // prevent integral windup
|
||||
integralFBy = 0.0f;
|
||||
integralFBz = 0.0f;
|
||||
}
|
||||
|
||||
// Apply proportional feedback
|
||||
gx += twoKp * halfex;
|
||||
gy += twoKp * halfey;
|
||||
gz += twoKp * halfez;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion
|
||||
gx *= (0.5f * dt); // pre-multiply common factors
|
||||
gy *= (0.5f * dt);
|
||||
gz *= (0.5f * dt);
|
||||
qa = q0;
|
||||
qb = q1;
|
||||
qc = q2;
|
||||
q0 += (-qb * gx - qc * gy - q3 * gz);
|
||||
q1 += (qa * gx + qc * gz - q3 * gy);
|
||||
q2 += (qa * gy - qb * gz + q3 * gx);
|
||||
q3 += (qa * gz + qb * gy - qc * gx);
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
anglesComputed = 0;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Fast inverse square-root
|
||||
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
|
||||
|
||||
float Adafruit_Mahony::invSqrt(float x) {
|
||||
float halfx = 0.5f * x;
|
||||
float y = x;
|
||||
long i = *(long *)&y;
|
||||
i = 0x5f3759df - (i >> 1);
|
||||
y = *(float *)&i;
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
return y;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
|
||||
void Adafruit_Mahony::computeAngles() {
|
||||
roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2);
|
||||
pitch = asinf(-2.0f * (q1 * q3 - q0 * q2));
|
||||
yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3);
|
||||
grav[0] = 2.0f * (q1 * q3 - q0 * q2);
|
||||
grav[1] = 2.0f * (q0 * q1 + q2 * q3);
|
||||
grav[2] = 2.0f * (q1 * q0 - 0.5f + q3 * q3);
|
||||
anglesComputed = 1;
|
||||
}
|
||||
|
||||
//============================================================================================
|
||||
// END OF CODE
|
||||
//============================================================================================
|
||||
@@ -0,0 +1,103 @@
|
||||
//=============================================================================================
|
||||
// Adafruit_AHRS_Mahony.h
|
||||
//=============================================================================================
|
||||
//
|
||||
// Madgwick's implementation of Mayhony's AHRS algorithm.
|
||||
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
|
||||
//
|
||||
// Date Author Notes
|
||||
// 29/09/2011 SOH Madgwick Initial release
|
||||
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||||
//
|
||||
//=============================================================================================
|
||||
#ifndef __Adafruit_Mahony_h__
|
||||
#define __Adafruit_Mahony_h__
|
||||
|
||||
#include "Adafruit_AHRS_FusionInterface.h"
|
||||
#include <math.h>
|
||||
|
||||
//--------------------------------------------------------------------------------------------
|
||||
// Variable declaration
|
||||
|
||||
class Adafruit_Mahony : public Adafruit_AHRS_FusionInterface {
|
||||
private:
|
||||
float twoKp; // 2 * proportional gain (Kp)
|
||||
float twoKi; // 2 * integral gain (Ki)
|
||||
float q0, q1, q2,
|
||||
q3; // quaternion of sensor frame relative to auxiliary frame
|
||||
float integralFBx, integralFBy,
|
||||
integralFBz; // integral error terms scaled by Ki
|
||||
float invSampleFreq;
|
||||
float roll, pitch, yaw;
|
||||
float grav[3];
|
||||
bool anglesComputed = false;
|
||||
static float invSqrt(float x);
|
||||
void computeAngles();
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Function declarations
|
||||
|
||||
public:
|
||||
Adafruit_Mahony();
|
||||
Adafruit_Mahony(float prop_gain, float int_gain);
|
||||
void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
|
||||
void update(float gx, float gy, float gz, float ax, float ay, float az,
|
||||
float mx, float my, float mz, float dt);
|
||||
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az,
|
||||
float dt);
|
||||
float getKp() { return twoKp / 2.0f; }
|
||||
void setKp(float Kp) { twoKp = 2.0f * Kp; }
|
||||
float getKi() { return twoKi / 2.0f; }
|
||||
void setKi(float Ki) { twoKi = 2.0f * Ki; }
|
||||
float getRoll() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return roll * 57.29578f;
|
||||
}
|
||||
float getPitch() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return pitch * 57.29578f;
|
||||
}
|
||||
float getYaw() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return yaw * 57.29578f + 180.0f;
|
||||
}
|
||||
float getRollRadians() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return roll;
|
||||
}
|
||||
float getPitchRadians() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return pitch;
|
||||
}
|
||||
float getYawRadians() {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
return yaw;
|
||||
}
|
||||
void getQuaternion(float *w, float *x, float *y, float *z) {
|
||||
*w = q0;
|
||||
*x = q1;
|
||||
*y = q2;
|
||||
*z = q3;
|
||||
}
|
||||
void setQuaternion(float w, float x, float y, float z) {
|
||||
q0 = w;
|
||||
q1 = x;
|
||||
q2 = y;
|
||||
q3 = z;
|
||||
}
|
||||
void getGravityVector(float *x, float *y, float *z) {
|
||||
if (!anglesComputed)
|
||||
computeAngles();
|
||||
*x = grav[0];
|
||||
*y = grav[1];
|
||||
*z = grav[2];
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,251 @@
|
||||
//=============================================================================================
|
||||
// MadgwickAHRS.c
|
||||
//=============================================================================================
|
||||
//
|
||||
// Implementation of Madgwick's IMU and AHRS algorithms.
|
||||
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
|
||||
//
|
||||
// From the x-io website "Open-source resources available on this website are
|
||||
// provided under the GNU General Public Licence unless an alternative licence
|
||||
// is provided in source."
|
||||
//
|
||||
// Date Author Notes
|
||||
// 29/09/2011 SOH Madgwick Initial release
|
||||
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||||
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
|
||||
//
|
||||
//=============================================================================================
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Header files
|
||||
|
||||
#include "MadgwickAHRS.h"
|
||||
#include <math.h>
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Definitions
|
||||
|
||||
#define sampleFreqDef 100.0f // sample frequency in Hz
|
||||
#define betaDef 0.5f // 2 * proportional gain
|
||||
|
||||
|
||||
//============================================================================================
|
||||
// Functions
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// AHRS algorithm update
|
||||
|
||||
Madgwick::Madgwick() {
|
||||
beta = betaDef;
|
||||
q0 = 1.0f;
|
||||
q1 = 0.0f;
|
||||
q2 = 0.0f;
|
||||
q3 = 0.0f;
|
||||
invSampleFreq = 1.0f / sampleFreqDef;
|
||||
anglesComputed = 0;
|
||||
}
|
||||
|
||||
void Madgwick::update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat) {
|
||||
float recipNorm;
|
||||
float s0, s1, s2, s3;
|
||||
float qDot1, qDot2, qDot3, qDot4;
|
||||
float hx, hy;
|
||||
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
|
||||
|
||||
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
|
||||
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
|
||||
updateIMU(gx, gy, gz, ax, ay, az);
|
||||
return;
|
||||
}
|
||||
|
||||
// Convert gyroscope degrees/sec to radians/sec
|
||||
//gx *= 0.0174533f;
|
||||
//gy *= 0.0174533f;
|
||||
//gz *= 0.0174533f;
|
||||
|
||||
// Rate of change of quaternion from gyroscope
|
||||
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
|
||||
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
|
||||
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
|
||||
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
|
||||
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
|
||||
|
||||
// Normalise accelerometer measurement
|
||||
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Normalise magnetometer measurement
|
||||
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
|
||||
mx *= recipNorm;
|
||||
my *= recipNorm;
|
||||
mz *= recipNorm;
|
||||
|
||||
// Auxiliary variables to avoid repeated arithmetic
|
||||
_2q0mx = 2.0f * q0 * mx;
|
||||
_2q0my = 2.0f * q0 * my;
|
||||
_2q0mz = 2.0f * q0 * mz;
|
||||
_2q1mx = 2.0f * q1 * mx;
|
||||
_2q0 = 2.0f * q0;
|
||||
_2q1 = 2.0f * q1;
|
||||
_2q2 = 2.0f * q2;
|
||||
_2q3 = 2.0f * q3;
|
||||
_2q0q2 = 2.0f * q0 * q2;
|
||||
_2q2q3 = 2.0f * q2 * q3;
|
||||
q0q0 = q0 * q0;
|
||||
q0q1 = q0 * q1;
|
||||
q0q2 = q0 * q2;
|
||||
q0q3 = q0 * q3;
|
||||
q1q1 = q1 * q1;
|
||||
q1q2 = q1 * q2;
|
||||
q1q3 = q1 * q3;
|
||||
q2q2 = q2 * q2;
|
||||
q2q3 = q2 * q3;
|
||||
q3q3 = q3 * q3;
|
||||
|
||||
// Reference direction of Earth's magnetic field
|
||||
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
|
||||
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
|
||||
_2bx = sqrtf(hx * hx + hy * hy);
|
||||
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
|
||||
_4bx = 2.0f * _2bx;
|
||||
_4bz = 2.0f * _2bz;
|
||||
|
||||
// Gradient decent algorithm corrective step
|
||||
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
|
||||
s0 *= recipNorm;
|
||||
s1 *= recipNorm;
|
||||
s2 *= recipNorm;
|
||||
s3 *= recipNorm;
|
||||
|
||||
// Apply feedback step
|
||||
qDot1 -= beta * s0;
|
||||
qDot2 -= beta * s1;
|
||||
qDot3 -= beta * s2;
|
||||
qDot4 -= beta * s3;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion to yield quaternion
|
||||
q0 += qDot1 * deltat;
|
||||
q1 += qDot2 * deltat;
|
||||
q2 += qDot3 * deltat;
|
||||
q3 += qDot4 * deltat;
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
anglesComputed = 0;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// IMU algorithm update
|
||||
|
||||
void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
|
||||
float recipNorm;
|
||||
float s0, s1, s2, s3;
|
||||
float qDot1, qDot2, qDot3, qDot4;
|
||||
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
|
||||
|
||||
// Convert gyroscope degrees/sec to radians/sec
|
||||
//gx *= 0.0174533f;
|
||||
//gy *= 0.0174533f;
|
||||
//gz *= 0.0174533f;
|
||||
|
||||
// Rate of change of quaternion from gyroscope
|
||||
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
|
||||
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
|
||||
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
|
||||
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);
|
||||
|
||||
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
|
||||
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
|
||||
|
||||
// Normalise accelerometer measurement
|
||||
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
|
||||
ax *= recipNorm;
|
||||
ay *= recipNorm;
|
||||
az *= recipNorm;
|
||||
|
||||
// Auxiliary variables to avoid repeated arithmetic
|
||||
_2q0 = 2.0f * q0;
|
||||
_2q1 = 2.0f * q1;
|
||||
_2q2 = 2.0f * q2;
|
||||
_2q3 = 2.0f * q3;
|
||||
_4q0 = 4.0f * q0;
|
||||
_4q1 = 4.0f * q1;
|
||||
_4q2 = 4.0f * q2;
|
||||
_8q1 = 8.0f * q1;
|
||||
_8q2 = 8.0f * q2;
|
||||
q0q0 = q0 * q0;
|
||||
q1q1 = q1 * q1;
|
||||
q2q2 = q2 * q2;
|
||||
q3q3 = q3 * q3;
|
||||
|
||||
// Gradient decent algorithm corrective step
|
||||
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
|
||||
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
|
||||
s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
|
||||
s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
|
||||
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
|
||||
s0 *= recipNorm;
|
||||
s1 *= recipNorm;
|
||||
s2 *= recipNorm;
|
||||
s3 *= recipNorm;
|
||||
|
||||
// Apply feedback step
|
||||
qDot1 -= beta * s0;
|
||||
qDot2 -= beta * s1;
|
||||
qDot3 -= beta * s2;
|
||||
qDot4 -= beta * s3;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion to yield quaternion
|
||||
q0 += qDot1 * invSampleFreq;
|
||||
q1 += qDot2 * invSampleFreq;
|
||||
q2 += qDot3 * invSampleFreq;
|
||||
q3 += qDot4 * invSampleFreq;
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
|
||||
q0 *= recipNorm;
|
||||
q1 *= recipNorm;
|
||||
q2 *= recipNorm;
|
||||
q3 *= recipNorm;
|
||||
anglesComputed = 0;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Fast inverse square-root
|
||||
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
|
||||
|
||||
float Madgwick::invSqrt(float x) {
|
||||
float halfx = 0.5f * x;
|
||||
float y = x;
|
||||
long i = *(long*)&y;
|
||||
i = 0x5f3759df - (i>>1);
|
||||
y = *(float*)&i;
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
return y;
|
||||
}
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
|
||||
void Madgwick::computeAngles()
|
||||
{
|
||||
roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
|
||||
pitch = asinf(-2.0f * (q1*q3 - q0*q2));
|
||||
yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
|
||||
anglesComputed = 1;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,74 @@
|
||||
//=============================================================================================
|
||||
// MadgwickAHRS.h
|
||||
//=============================================================================================
|
||||
//
|
||||
// Implementation of Madgwick's IMU and AHRS algorithms.
|
||||
// See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/
|
||||
//
|
||||
// From the x-io website "Open-source resources available on this website are
|
||||
// provided under the GNU General Public Licence unless an alternative licence
|
||||
// is provided in source."
|
||||
//
|
||||
// Date Author Notes
|
||||
// 29/09/2011 SOH Madgwick Initial release
|
||||
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||||
//
|
||||
//=============================================================================================
|
||||
#ifndef MadgwickAHRS_h
|
||||
#define MadgwickAHRS_h
|
||||
#include <math.h>
|
||||
|
||||
//--------------------------------------------------------------------------------------------
|
||||
// Variable declaration
|
||||
class Madgwick{
|
||||
private:
|
||||
static float invSqrt(float x);
|
||||
float beta; // algorithm gain
|
||||
float q0;
|
||||
float q1;
|
||||
float q2;
|
||||
float q3; // quaternion of sensor frame relative to auxiliary frame
|
||||
float invSampleFreq;
|
||||
float roll;
|
||||
float pitch;
|
||||
float yaw;
|
||||
char anglesComputed;
|
||||
void computeAngles();
|
||||
|
||||
//-------------------------------------------------------------------------------------------
|
||||
// Function declarations
|
||||
public:
|
||||
Madgwick(void);
|
||||
void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; }
|
||||
void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float deltat);
|
||||
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
|
||||
//float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);};
|
||||
//float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);};
|
||||
//float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);};
|
||||
float getRoll() {
|
||||
if (!anglesComputed) computeAngles();
|
||||
return roll * 57.29578f;
|
||||
}
|
||||
float getPitch() {
|
||||
if (!anglesComputed) computeAngles();
|
||||
return pitch * 57.29578f;
|
||||
}
|
||||
float getYaw() {
|
||||
if (!anglesComputed) computeAngles();
|
||||
return yaw * 57.29578f + 180.0f;
|
||||
}
|
||||
float getRollRadians() {
|
||||
if (!anglesComputed) computeAngles();
|
||||
return roll;
|
||||
}
|
||||
float getPitchRadians() {
|
||||
if (!anglesComputed) computeAngles();
|
||||
return pitch;
|
||||
}
|
||||
float getYawRadians() {
|
||||
if (!anglesComputed) computeAngles();
|
||||
return yaw;
|
||||
}
|
||||
};
|
||||
#endif
|
||||
|
||||
@@ -0,0 +1,101 @@
|
||||
/*
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program 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 General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Adam M Rivera
|
||||
* With direction from: Andrew Tridgell, Jason Short, Justin Beech
|
||||
*
|
||||
* Adapted from: http://www.societyofrobots.com/robotforum/index.php?topic=11855.0
|
||||
* Scott Ferguson
|
||||
* scottfromscott@gmail.com
|
||||
*
|
||||
*/
|
||||
#include "AP_Declination.h"
|
||||
|
||||
#include <cmath>
|
||||
#include <inttypes.h>
|
||||
|
||||
/*
|
||||
calculate magnetic field intensity and orientation
|
||||
*/
|
||||
bool AP_Declination::get_mag_field_ef(float latitude_deg, float longitude_deg, float &declination_deg)
|
||||
{
|
||||
bool valid_input_data = true;
|
||||
|
||||
/* round down to nearest sampling resolution */
|
||||
int32_t min_lat = static_cast<int32_t>(static_cast<int32_t>(latitude_deg / SAMPLING_RES) * SAMPLING_RES);
|
||||
int32_t min_lon = static_cast<int32_t>(static_cast<int32_t>(longitude_deg / SAMPLING_RES) * SAMPLING_RES);
|
||||
|
||||
/* for the rare case of hitting the bounds exactly
|
||||
* the rounding logic wouldn't fit, so enforce it.
|
||||
*/
|
||||
|
||||
/* limit to table bounds - required for maxima even when table spans full globe range */
|
||||
if (latitude_deg <= SAMPLING_MIN_LAT) {
|
||||
min_lat = static_cast<int32_t>(SAMPLING_MIN_LAT);
|
||||
valid_input_data = false;
|
||||
}
|
||||
|
||||
if (latitude_deg >= SAMPLING_MAX_LAT) {
|
||||
min_lat = static_cast<int32_t>(static_cast<int32_t>(latitude_deg / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES);
|
||||
valid_input_data = false;
|
||||
}
|
||||
|
||||
if (longitude_deg <= SAMPLING_MIN_LON) {
|
||||
min_lon = static_cast<int32_t>(SAMPLING_MIN_LON);
|
||||
valid_input_data = false;
|
||||
}
|
||||
|
||||
if (longitude_deg >= SAMPLING_MAX_LON) {
|
||||
min_lon = static_cast<int32_t>(static_cast<int32_t>(longitude_deg / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES);
|
||||
valid_input_data = false;
|
||||
}
|
||||
if(valid_input_data)
|
||||
{
|
||||
/* find index of nearest low sampling point */
|
||||
uint32_t min_lat_index = static_cast<uint32_t>((-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES);
|
||||
uint32_t min_lon_index = static_cast<uint32_t>((-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES);
|
||||
|
||||
/* calculate declination */
|
||||
|
||||
float data_sw = declination_table[min_lat_index][min_lon_index];
|
||||
float data_se = declination_table[min_lat_index][min_lon_index + 1];;
|
||||
float data_ne = declination_table[min_lat_index + 1][min_lon_index + 1];
|
||||
float data_nw = declination_table[min_lat_index + 1][min_lon_index];
|
||||
|
||||
/* perform bilinear interpolation on the four grid corners */
|
||||
|
||||
float data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw;
|
||||
float data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw;
|
||||
|
||||
declination_deg = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min;
|
||||
}
|
||||
return valid_input_data;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
calculate magnetic field intensity and orientation
|
||||
*/
|
||||
float AP_Declination::get_declination(float latitude_deg, float longitude_deg)
|
||||
{
|
||||
float declination_deg=0;
|
||||
|
||||
if(get_mag_field_ef(latitude_deg, longitude_deg, declination_deg))
|
||||
return declination_deg;
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,32 @@
|
||||
#pragma once
|
||||
|
||||
/*
|
||||
magnetic data derived from WMM
|
||||
*/
|
||||
class AP_Declination
|
||||
{
|
||||
public:
|
||||
/*
|
||||
* Calculates the magnetic intensity, declination and inclination at a given WGS-84 latitude and longitude.
|
||||
* Assumes a WGS-84 height of zero
|
||||
* latitude and longitude have units of degrees
|
||||
* declination and inclination are returned in degrees
|
||||
* intensity is returned in Gauss
|
||||
* Boolean returns false if latitude and longitude are outside the valid input range of +-60 latitude and +-180 longitude
|
||||
*/
|
||||
static bool get_mag_field_ef(float latitude_deg, float longitude_deg, float &declination_deg);
|
||||
|
||||
/*
|
||||
get declination in degrees for a given latitude_deg and longitude_deg
|
||||
*/
|
||||
static float get_declination(float latitude_deg, float longitude_deg);
|
||||
|
||||
private:
|
||||
static const float SAMPLING_RES;
|
||||
static const float SAMPLING_MIN_LAT;
|
||||
static const float SAMPLING_MAX_LAT;
|
||||
static const float SAMPLING_MIN_LON;
|
||||
static const float SAMPLING_MAX_LON;
|
||||
|
||||
static const float declination_table[37][73];
|
||||
};
|
||||
@@ -0,0 +1,192 @@
|
||||
#!/usr/bin/env python3
|
||||
'''
|
||||
generate field tables from IGRF12. Note that this requires python3
|
||||
'''
|
||||
|
||||
import igrf12 as igrf
|
||||
import numpy as np
|
||||
import datetime
|
||||
from pathlib import Path
|
||||
from pymavlink.rotmat import Vector3, Matrix3
|
||||
import math
|
||||
|
||||
import argparse
|
||||
parser = argparse.ArgumentParser(description='generate mag tables')
|
||||
parser.add_argument('--sampling-res', type=int, default=5, help='sampling resolution, degrees')
|
||||
parser.add_argument('--check-error', action='store_true', help='check max error')
|
||||
parser.add_argument('--filename', type=str, default='tables.cpp', help='tables file')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
if not Path("AP_Declination.h").is_file():
|
||||
raise OSError("Please run this tool from the AP_Declination directory")
|
||||
|
||||
|
||||
def write_table(f,name, table):
|
||||
'''write one table'''
|
||||
f.write("const float AP_Declination::%s[%u][%u] = {\n" %
|
||||
(name, NUM_LAT, NUM_LON))
|
||||
for i in range(NUM_LAT):
|
||||
f.write(" {")
|
||||
for j in range(NUM_LON):
|
||||
f.write("%.5ff" % table[i][j])
|
||||
if j != NUM_LON-1:
|
||||
f.write(",")
|
||||
f.write("}")
|
||||
if i != NUM_LAT-1:
|
||||
f.write(",")
|
||||
f.write("\n")
|
||||
f.write("};\n\n")
|
||||
|
||||
date = datetime.datetime.now()
|
||||
|
||||
SAMPLING_RES = args.sampling_res
|
||||
SAMPLING_MIN_LAT = -90
|
||||
SAMPLING_MAX_LAT = 90
|
||||
SAMPLING_MIN_LON = -180
|
||||
SAMPLING_MAX_LON = 180
|
||||
|
||||
lats = np.arange(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+SAMPLING_RES, SAMPLING_RES)
|
||||
lons = np.arange(SAMPLING_MIN_LON, SAMPLING_MAX_LON+SAMPLING_RES, SAMPLING_RES)
|
||||
|
||||
NUM_LAT = lats.size
|
||||
NUM_LON = lons.size
|
||||
|
||||
intensity_table = np.empty((NUM_LAT, NUM_LON))
|
||||
inclination_table = np.empty((NUM_LAT, NUM_LON))
|
||||
declination_table = np.empty((NUM_LAT, NUM_LON))
|
||||
|
||||
max_error = 0
|
||||
max_error_pos = None
|
||||
max_error_field = None
|
||||
|
||||
def get_igrf(lat, lon):
|
||||
'''return field as [declination_deg, inclination_deg, intensity_gauss]'''
|
||||
mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1)
|
||||
intensity = float(mag.total/1e5)
|
||||
inclination = float(mag.incl)
|
||||
declination = float(mag.decl)
|
||||
return [declination, inclination, intensity]
|
||||
|
||||
def interpolate_table(table, latitude_deg, longitude_deg):
|
||||
'''interpolate inside a table for a given lat/lon in degrees'''
|
||||
# round down to nearest sampling resolution
|
||||
min_lat = int(math.floor(latitude_deg / SAMPLING_RES) * SAMPLING_RES)
|
||||
min_lon = int(math.floor(longitude_deg / SAMPLING_RES) * SAMPLING_RES)
|
||||
|
||||
# find index of nearest low sampling point
|
||||
min_lat_index = int(math.floor(-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES)
|
||||
min_lon_index = int(math.floor(-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES)
|
||||
|
||||
# calculate intensity
|
||||
data_sw = table[min_lat_index][min_lon_index]
|
||||
data_se = table[min_lat_index][min_lon_index + 1]
|
||||
data_ne = table[min_lat_index + 1][min_lon_index + 1]
|
||||
data_nw = table[min_lat_index + 1][min_lon_index]
|
||||
|
||||
# perform bilinear interpolation on the four grid corners
|
||||
data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw
|
||||
data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw
|
||||
|
||||
value = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min
|
||||
return value
|
||||
|
||||
|
||||
'''
|
||||
calculate magnetic field intensity and orientation, interpolating in tables
|
||||
|
||||
returns array [declination_deg, inclination_deg, intensity] or None
|
||||
'''
|
||||
def interpolate_field(latitude_deg, longitude_deg):
|
||||
# limit to table bounds
|
||||
if latitude_deg < SAMPLING_MIN_LAT:
|
||||
return None
|
||||
if latitude_deg >= SAMPLING_MAX_LAT:
|
||||
return None
|
||||
if longitude_deg < SAMPLING_MIN_LON:
|
||||
return None
|
||||
if longitude_deg >= SAMPLING_MAX_LON:
|
||||
return None
|
||||
|
||||
intensity_gauss = interpolate_table(intensity_table, latitude_deg, longitude_deg)
|
||||
declination_deg = interpolate_table(declination_table, latitude_deg, longitude_deg)
|
||||
inclination_deg = interpolate_table(inclination_table, latitude_deg, longitude_deg)
|
||||
|
||||
return [declination_deg, inclination_deg, intensity_gauss]
|
||||
|
||||
def field_to_Vector3(mag):
|
||||
'''return mGauss field from dec, inc and intensity'''
|
||||
R = Matrix3()
|
||||
mag_ef = Vector3(mag[2]*1000.0, 0.0, 0.0)
|
||||
R.from_euler(0.0, -math.radians(mag[1]), math.radians(mag[0]))
|
||||
return R * mag_ef
|
||||
|
||||
def test_error(lat, lon):
|
||||
'''check for error from lat,lon'''
|
||||
global max_error, max_error_pos, max_error_field
|
||||
mag1 = get_igrf(lat, lon)
|
||||
mag2 = interpolate_field(lat, lon)
|
||||
ef1 = field_to_Vector3(mag1)
|
||||
ef2 = field_to_Vector3(mag2)
|
||||
err = (ef1 - ef2).length()
|
||||
if err > max_error or err > 100:
|
||||
print(lat, lon, err, ef1, ef2)
|
||||
max_error = err
|
||||
max_error_pos = (lat, lon)
|
||||
max_error_field = ef1 - ef2
|
||||
|
||||
def test_max_error(lat, lon):
|
||||
'''check for maximum error from lat,lon over SAMPLING_RES range'''
|
||||
steps = 3
|
||||
delta = SAMPLING_RES/steps
|
||||
for i in range(steps):
|
||||
for j in range(steps):
|
||||
lat2 = lat + i * delta
|
||||
lon2 = lon + j * delta
|
||||
if lat2 >= SAMPLING_MAX_LAT or lon2 >= SAMPLING_MAX_LON:
|
||||
continue
|
||||
if lat2 <= SAMPLING_MIN_LAT or lon2 <= SAMPLING_MIN_LON:
|
||||
continue
|
||||
test_error(lat2, lon2)
|
||||
|
||||
for i,lat in enumerate(lats):
|
||||
for j,lon in enumerate(lons):
|
||||
mag = get_igrf(lat, lon)
|
||||
declination_table[i][j] = mag[0]
|
||||
inclination_table[i][j] = mag[1]
|
||||
intensity_table[i][j] = mag[2]
|
||||
|
||||
with open(args.filename, 'w') as f:
|
||||
f.write('''// this is an auto-generated file from the IGRF tables. Do not edit
|
||||
// To re-generate run generate/generate.py
|
||||
|
||||
#include "AP_Declination.h"
|
||||
|
||||
''')
|
||||
|
||||
f.write('''const float AP_Declination::SAMPLING_RES = %u;
|
||||
const float AP_Declination::SAMPLING_MIN_LAT = %u;
|
||||
const float AP_Declination::SAMPLING_MAX_LAT = %u;
|
||||
const float AP_Declination::SAMPLING_MIN_LON = %u;
|
||||
const float AP_Declination::SAMPLING_MAX_LON = %u;
|
||||
|
||||
''' % (SAMPLING_RES,
|
||||
SAMPLING_MIN_LAT,
|
||||
SAMPLING_MAX_LAT,
|
||||
SAMPLING_MIN_LON,
|
||||
SAMPLING_MAX_LON))
|
||||
|
||||
|
||||
write_table(f,'declination_table', declination_table)
|
||||
# write_table(f,'inclination_table', inclination_table)
|
||||
# write_table(f,'intensity_table', intensity_table)
|
||||
|
||||
if args.check_error:
|
||||
print("Checking for maximum error")
|
||||
for lat in range(-60,60,1):
|
||||
for lon in range(-180,180,1):
|
||||
test_max_error(lat, lon)
|
||||
print("Generated with max error %.2f %s at (%.2f,%.2f)" % (
|
||||
max_error, max_error_field, max_error_pos[0], max_error_pos[1]))
|
||||
|
||||
print("Table generated in %s" % args.filename)
|
||||
@@ -0,0 +1,192 @@
|
||||
#!/usr/bin/env python3
|
||||
'''
|
||||
generate field tables from IGRF12. Note that this requires python3
|
||||
'''
|
||||
|
||||
import igrf
|
||||
import numpy as np
|
||||
import datetime
|
||||
from pathlib import Path
|
||||
from pymavlink.rotmat import Vector3, Matrix3
|
||||
import math
|
||||
|
||||
import argparse
|
||||
parser = argparse.ArgumentParser(description='generate mag tables')
|
||||
parser.add_argument('--sampling-res', type=int, default=5, help='sampling resolution, degrees')
|
||||
parser.add_argument('--check-error', action='store_true', help='check max error')
|
||||
parser.add_argument('--filename', type=str, default='tables.cpp', help='tables file')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
if not Path("AP_Declination.h").is_file():
|
||||
raise OSError("Please run this tool from the AP_Declination directory")
|
||||
|
||||
|
||||
def write_table(f,name, table):
|
||||
'''write one table'''
|
||||
f.write("const float AP_Declination::%s[%u][%u] = {\n" %
|
||||
(name, NUM_LAT, NUM_LON))
|
||||
for i in range(NUM_LAT):
|
||||
f.write(" {")
|
||||
for j in range(NUM_LON):
|
||||
f.write("%.5ff" % table[i][j])
|
||||
if j != NUM_LON-1:
|
||||
f.write(",")
|
||||
f.write("}")
|
||||
if i != NUM_LAT-1:
|
||||
f.write(",")
|
||||
f.write("\n")
|
||||
f.write("};\n\n")
|
||||
|
||||
date = datetime.datetime.now()
|
||||
|
||||
SAMPLING_RES = args.sampling_res
|
||||
SAMPLING_MIN_LAT = -90
|
||||
SAMPLING_MAX_LAT = 90
|
||||
SAMPLING_MIN_LON = -180
|
||||
SAMPLING_MAX_LON = 180
|
||||
|
||||
lats = np.arange(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+SAMPLING_RES, SAMPLING_RES)
|
||||
lons = np.arange(SAMPLING_MIN_LON, SAMPLING_MAX_LON+SAMPLING_RES, SAMPLING_RES)
|
||||
|
||||
NUM_LAT = lats.size
|
||||
NUM_LON = lons.size
|
||||
|
||||
intensity_table = np.empty((NUM_LAT, NUM_LON))
|
||||
inclination_table = np.empty((NUM_LAT, NUM_LON))
|
||||
declination_table = np.empty((NUM_LAT, NUM_LON))
|
||||
|
||||
max_error = 0
|
||||
max_error_pos = None
|
||||
max_error_field = None
|
||||
|
||||
def get_igrf(lat, lon):
|
||||
'''return field as [declination_deg, inclination_deg, intensity_gauss]'''
|
||||
mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1)
|
||||
intensity = float(mag.total/1e5)
|
||||
inclination = float(mag.incl)
|
||||
declination = float(mag.decl)
|
||||
return [declination, inclination, intensity]
|
||||
|
||||
def interpolate_table(table, latitude_deg, longitude_deg):
|
||||
'''interpolate inside a table for a given lat/lon in degrees'''
|
||||
# round down to nearest sampling resolution
|
||||
min_lat = int(math.floor(latitude_deg / SAMPLING_RES) * SAMPLING_RES)
|
||||
min_lon = int(math.floor(longitude_deg / SAMPLING_RES) * SAMPLING_RES)
|
||||
|
||||
# find index of nearest low sampling point
|
||||
min_lat_index = int(math.floor(-(SAMPLING_MIN_LAT) + min_lat) / SAMPLING_RES)
|
||||
min_lon_index = int(math.floor(-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES)
|
||||
|
||||
# calculate intensity
|
||||
data_sw = table[min_lat_index][min_lon_index]
|
||||
data_se = table[min_lat_index][min_lon_index + 1]
|
||||
data_ne = table[min_lat_index + 1][min_lon_index + 1]
|
||||
data_nw = table[min_lat_index + 1][min_lon_index]
|
||||
|
||||
# perform bilinear interpolation on the four grid corners
|
||||
data_min = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_se - data_sw) + data_sw
|
||||
data_max = ((longitude_deg - min_lon) / SAMPLING_RES) * (data_ne - data_nw) + data_nw
|
||||
|
||||
value = ((latitude_deg - min_lat) / SAMPLING_RES) * (data_max - data_min) + data_min
|
||||
return value
|
||||
|
||||
|
||||
'''
|
||||
calculate magnetic field intensity and orientation, interpolating in tables
|
||||
|
||||
returns array [declination_deg, inclination_deg, intensity] or None
|
||||
'''
|
||||
def interpolate_field(latitude_deg, longitude_deg):
|
||||
# limit to table bounds
|
||||
if latitude_deg < SAMPLING_MIN_LAT:
|
||||
return None
|
||||
if latitude_deg >= SAMPLING_MAX_LAT:
|
||||
return None
|
||||
if longitude_deg < SAMPLING_MIN_LON:
|
||||
return None
|
||||
if longitude_deg >= SAMPLING_MAX_LON:
|
||||
return None
|
||||
|
||||
intensity_gauss = interpolate_table(intensity_table, latitude_deg, longitude_deg)
|
||||
declination_deg = interpolate_table(declination_table, latitude_deg, longitude_deg)
|
||||
inclination_deg = interpolate_table(inclination_table, latitude_deg, longitude_deg)
|
||||
|
||||
return [declination_deg, inclination_deg, intensity_gauss]
|
||||
|
||||
def field_to_Vector3(mag):
|
||||
'''return mGauss field from dec, inc and intensity'''
|
||||
R = Matrix3()
|
||||
mag_ef = Vector3(mag[2]*1000.0, 0.0, 0.0)
|
||||
R.from_euler(0.0, -math.radians(mag[1]), math.radians(mag[0]))
|
||||
return R * mag_ef
|
||||
|
||||
def test_error(lat, lon):
|
||||
'''check for error from lat,lon'''
|
||||
global max_error, max_error_pos, max_error_field
|
||||
mag1 = get_igrf(lat, lon)
|
||||
mag2 = interpolate_field(lat, lon)
|
||||
ef1 = field_to_Vector3(mag1)
|
||||
ef2 = field_to_Vector3(mag2)
|
||||
err = (ef1 - ef2).length()
|
||||
if err > max_error or err > 100:
|
||||
print(lat, lon, err, ef1, ef2)
|
||||
max_error = err
|
||||
max_error_pos = (lat, lon)
|
||||
max_error_field = ef1 - ef2
|
||||
|
||||
def test_max_error(lat, lon):
|
||||
'''check for maximum error from lat,lon over SAMPLING_RES range'''
|
||||
steps = 3
|
||||
delta = SAMPLING_RES/steps
|
||||
for i in range(steps):
|
||||
for j in range(steps):
|
||||
lat2 = lat + i * delta
|
||||
lon2 = lon + j * delta
|
||||
if lat2 >= SAMPLING_MAX_LAT or lon2 >= SAMPLING_MAX_LON:
|
||||
continue
|
||||
if lat2 <= SAMPLING_MIN_LAT or lon2 <= SAMPLING_MIN_LON:
|
||||
continue
|
||||
test_error(lat2, lon2)
|
||||
|
||||
for i,lat in enumerate(lats):
|
||||
for j,lon in enumerate(lons):
|
||||
mag = get_igrf(lat, lon)
|
||||
declination_table[i][j] = mag[0]
|
||||
inclination_table[i][j] = mag[1]
|
||||
intensity_table[i][j] = mag[2]
|
||||
|
||||
with open(args.filename, 'w') as f:
|
||||
f.write('''// this is an auto-generated file from the IGRF tables. Do not edit
|
||||
// To re-generate run generate/generate.py
|
||||
|
||||
#include "AP_Declination.h"
|
||||
|
||||
''')
|
||||
|
||||
f.write('''const float AP_Declination::SAMPLING_RES = %u;
|
||||
const float AP_Declination::SAMPLING_MIN_LAT = %u;
|
||||
const float AP_Declination::SAMPLING_MAX_LAT = %u;
|
||||
const float AP_Declination::SAMPLING_MIN_LON = %u;
|
||||
const float AP_Declination::SAMPLING_MAX_LON = %u;
|
||||
|
||||
''' % (SAMPLING_RES,
|
||||
SAMPLING_MIN_LAT,
|
||||
SAMPLING_MAX_LAT,
|
||||
SAMPLING_MIN_LON,
|
||||
SAMPLING_MAX_LON))
|
||||
|
||||
|
||||
write_table(f,'declination_table', declination_table)
|
||||
# write_table(f,'inclination_table', inclination_table)
|
||||
# write_table(f,'intensity_table', intensity_table)
|
||||
|
||||
if args.check_error:
|
||||
print("Checking for maximum error")
|
||||
for lat in range(-60,60,1):
|
||||
for lon in range(-180,180,1):
|
||||
test_max_error(lat, lon)
|
||||
print("Generated with max error %.2f %s at (%.2f,%.2f)" % (
|
||||
max_error, max_error_field, max_error_pos[0], max_error_pos[1]))
|
||||
|
||||
print("Table generated in %s" % args.filename)
|
||||
@@ -0,0 +1,51 @@
|
||||
// this is an auto-generated file from the IGRF tables. Do not edit
|
||||
// To re-generate run generate/generate.py
|
||||
|
||||
#include "AP_Declination.h"
|
||||
|
||||
const float AP_Declination::SAMPLING_RES = 5;
|
||||
const float AP_Declination::SAMPLING_MIN_LAT = -90;
|
||||
const float AP_Declination::SAMPLING_MAX_LAT = 90;
|
||||
const float AP_Declination::SAMPLING_MIN_LON = -180;
|
||||
const float AP_Declination::SAMPLING_MAX_LON = 180;
|
||||
|
||||
const float AP_Declination::declination_table[37][73] = {
|
||||
{149.03407f,144.03407f,139.03406f,134.03407f,129.03407f,124.03407f,119.03407f,114.03407f,109.03407f,104.03407f,99.03407f,94.03407f,89.03407f,84.03407f,79.03407f,74.03407f,69.03407f,64.03407f,59.03407f,54.03407f,49.03407f,44.03407f,39.03407f,34.03407f,29.03407f,24.03407f,19.03407f,14.03407f,9.03407f,4.03407f,-0.96593f,-5.96593f,-10.96593f,-15.96593f,-20.96593f,-25.96593f,-30.96593f,-35.96593f,-40.96593f,-45.96593f,-50.96593f,-55.96593f,-60.96593f,-65.96593f,-70.96593f,-75.96593f,-80.96593f,-85.96593f,-90.96593f,-95.96593f,-100.96593f,-105.96593f,-110.96593f,-115.96593f,-120.96593f,-125.96593f,-130.96593f,-135.96593f,-140.96593f,-145.96593f,-150.96593f,-155.96593f,-160.96593f,-165.96593f,-170.96593f,-175.96593f,179.03407f,174.03407f,169.03407f,164.03407f,159.03407f,154.03407f,149.03407f},
|
||||
{141.51289f,135.86793f,130.30584f,124.83080f,119.44524f,114.14996f,108.94426f,103.82624f,98.79295f,93.84061f,88.96488f,84.16095f,79.42378f,74.74820f,70.12900f,65.56103f,61.03928f,56.55886f,52.11505f,47.70332f,43.31927f,38.95864f,34.61732f,30.29126f,25.97650f,21.66913f,17.36530f,13.06118f,8.75303f,4.43715f,0.10991f,-4.23218f,-8.59252f,-12.97431f,-17.38060f,-21.81426f,-26.27793f,-30.77408f,-35.30500f,-39.87280f,-44.47949f,-49.12697f,-53.81710f,-58.55174f,-63.33279f,-68.16223f,-73.04212f,-77.97468f,-82.96221f,-88.00713f,-93.11192f,-98.27905f,-103.51089f,-108.80960f,-114.17698f,-119.61431f,-125.12214f,-130.70013f,-136.34680f,-142.05937f,-147.83358f,-153.66357f,-159.54185f,-165.45931f,-171.40534f,-177.36808f,176.66527f,170.70806f,164.77375f,158.87542f,153.02536f,147.23465f,141.51289f},
|
||||
{129.30529f,123.04797f,117.07812f,111.38491f,105.95127f,100.75625f,95.77688f,90.98956f,86.37108f,81.89932f,77.55367f,73.31535f,69.16756f,65.09554f,61.08654f,57.12978f,53.21631f,49.33881f,45.49142f,41.66942f,37.86901f,34.08694f,30.32023f,26.56590f,22.82063f,19.08063f,15.34145f,11.59791f,7.84418f,4.07382f,0.28004f,-3.54407f,-7.40526f,-11.30984f,-15.26336f,-19.27045f,-23.33467f,-27.45845f,-31.64311f,-35.88907f,-40.19599f,-44.56309f,-48.98945f,-53.47437f,-58.01768f,-62.62012f,-67.28359f,-72.01144f,-76.80870f,-81.68223f,-86.64085f,-91.69542f,-96.85886f,-102.14600f,-107.57345f,-113.15916f,-118.92185f,-124.88005f,-131.05078f,-137.44774f,-144.07903f,-150.94447f,-158.03286f,-165.31961f,-172.76553f,179.68260f,172.08924f,164.52508f,157.06086f,149.76098f,142.67847f,135.85203f,129.30530f},
|
||||
{110.25505f,104.17460f,98.66441f,93.63327f,88.99997f,84.69384f,80.65388f,76.82752f,73.16943f,69.64048f,66.20704f,62.84045f,59.51673f,56.21635f,52.92405f,49.62874f,46.32321f,43.00388f,39.67033f,36.32482f,32.97161f,29.61614f,26.26424f,22.92114f,19.59060f,16.27413f,12.97028f,9.67439f,6.37844f,3.07147f,-0.25981f,-3.63001f,-7.05394f,-10.54543f,-14.11625f,-17.77518f,-21.52749f,-25.37470f,-29.31471f,-33.34230f,-37.44981f,-41.62809f,-45.86746f,-50.15868f,-54.49391f,-58.86750f,-63.27664f,-67.72194f,-72.20787f,-76.74318f,-81.34129f,-86.02075f,-90.80586f,-95.72746f,-100.82390f,-106.14225f,-111.73969f,-117.68467f,-124.05749f,-130.94884f,-138.45456f,-146.66322f,-155.63318f,-165.35821f,-175.72937f,173.48623f,162.62307f,152.04575f,142.06085f,132.86000f,124.51417f,117.00338f,110.25505f},
|
||||
{85.76021f,81.52735f,77.79070f,74.43416f,71.37193f,68.53750f,65.87661f,63.34292f,60.89542f,58.49697f,56.11379f,53.71552f,51.27574f,48.77254f,46.18931f,43.51519f,40.74548f,37.88166f,34.93116f,31.90679f,28.82578f,25.70857f,22.57709f,19.45288f,16.35486f,13.29723f,10.28748f,7.32498f,4.40043f,1.49621f,-1.41215f,-4.35362f,-7.35868f,-10.45619f,-13.67038f,-17.01862f,-20.50988f,-24.14424f,-27.91339f,-31.80187f,-35.78895f,-39.85088f,-43.96312f,-48.10247f,-52.24874f,-56.38605f,-60.50365f,-64.59643f,-68.66507f,-72.71624f,-76.76272f,-80.82387f,-84.92649f,-89.10641f,-93.41127f,-97.90497f,-102.67492f,-107.84353f,-113.58678f,-120.16345f,-127.95920f,-137.54085f,-149.67212f,-165.09730f,176.27330f,156.41262f,138.33488f,123.68413f,112.33089f,103.49538f,96.44591f,90.65322f,85.76021f},
|
||||
{63.49063f,61.53268f,59.70157f,57.98263f,56.36269f,54.82753f,53.36027f,51.94025f,50.54250f,49.13776f,47.69331f,46.17455f,44.54727f,42.78015f,40.84742f,38.73085f,36.42148f,33.92045f,31.23933f,28.39991f,25.43322f,22.37797f,19.27812f,16.17948f,13.12567f,10.15346f,7.28821f,4.54010f,1.90186f,-0.65133f,-3.15956f,-5.67358f,-8.24878f,-10.93849f,-13.78779f,-16.82860f,-20.07661f,-23.53033f,-27.17213f,-30.97101f,-34.88671f,-38.87435f,-42.88883f,-46.88860f,-50.83823f,-54.70981f,-58.48343f,-62.14677f,-65.69440f,-69.12673f,-72.44904f,-75.67052f,-78.80366f,-81.86412f,-84.87128f,-87.85003f,-90.83455f,-93.87592f,-97.05823f,-100.53633f,-104.63927f,-110.22502f,-120.43804f,-157.26638f,111.32321f,89.31102f,81.43675f,76.75453f,73.28001f,70.40353f,67.88022f,65.59556f,63.49063f},
|
||||
{48.02742f,47.38464f,46.65758f,45.89153f,45.11833f,44.35876f,43.62354f,42.91339f,42.21835f,41.51709f,40.77709f,39.95632f,39.00664f,37.87857f,36.52647f,34.91338f,33.01482f,30.82138f,28.34023f,25.59577f,22.62934f,19.49783f,16.27080f,13.02555f,9.84030f,6.78585f,3.91665f,1.26296f,-1.17444f,-3.42572f,-5.54885f,-7.62229f,-9.73470f,-11.97351f,-14.41415f,-17.11133f,-20.09307f,-23.35827f,-26.87804f,-30.60072f,-34.45961f,-38.38184f,-42.29652f,-46.14062f,-49.86226f,-53.42145f,-56.78910f,-59.94509f,-62.87577f,-65.57122f,-68.02220f,-70.21645f,-72.13432f,-73.74302f,-74.98855f,-75.78285f,-75.98080f,-75.33518f,-73.40197f,-69.32885f,-61.37994f,-46.16169f,-20.32594f,8.41772f,27.60663f,37.97792f,43.51550f,46.49369f,48.02544f,48.68241f,48.78368f,48.52307f,48.02742f},
|
||||
{38.02510f,37.95792f,37.73497f,37.41456f,37.04227f,36.65341f,36.27445f,35.92281f,35.60472f,35.31207f,35.01947f,34.68350f,34.24514f,33.63540f,32.78320f,31.62363f,30.10536f,28.19618f,25.88705f,23.19480f,20.16366f,16.86521f,13.39569f,9.86956f,6.40899f,3.12978f,0.12611f,-2.54307f,-4.86312f,-6.86630f,-8.62717f,-10.25201f,-11.86455f,-13.59071f,-15.54422f,-17.81359f,-20.45089f,-23.46424f,-26.81646f,-30.43193f,-34.21032f,-38.04311f,-41.82809f,-45.47842f,-48.92573f,-52.11873f,-55.01953f,-57.59932f,-59.83411f,-61.70043f,-63.17020f,-64.20414f,-64.74344f,-64.69915f,-63.93902f,-62.27080f,-59.42157f,-55.01852f,-48.59713f,-39.71493f,-28.30159f,-15.18866f,-2.13379f,9.17958f,18.04877f,24.61647f,29.33790f,32.66390f,34.94945f,36.45643f,37.37842f,37.86274f,38.02510f},
|
||||
{31.27326f,31.45631f,31.46505f,31.35356f,31.16598f,30.93929f,30.70590f,30.49422f,30.32590f,30.21035f,30.13820f,30.07626f,29.96635f,29.72894f,29.27146f,28.49916f,27.32664f,25.68808f,23.54579f,20.89697f,17.77899f,14.27223f,10.49883f,6.61468f,2.79343f,-0.79613f,-4.01459f,-6.77252f,-9.04210f,-10.85792f,-12.30759f,-13.51606f,-14.62889f,-15.79881f,-17.17497f,-18.89010f,-21.04162f,-23.66988f,-26.74447f,-30.16844f,-33.80096f,-37.48828f,-41.09001f,-44.49284f,-47.61215f,-50.38612f,-52.76791f,-54.71886f,-56.20341f,-57.18453f,-57.61810f,-57.44541f,-56.58460f,-54.92446f,-52.32546f,-48.63487f,-43.72300f,-37.54431f,-30.21313f,-22.05560f,-13.58090f,-5.35314f,2.16813f,8.71933f,14.22506f,18.73400f,22.35164f,25.19521f,27.37356f,28.98370f,30.11459f,30.85085f,31.27326f},
|
||||
{26.36786f,26.66604f,26.79441f,26.79796f,26.71209f,26.56667f,26.39096f,26.21608f,26.07315f,25.98705f,25.96761f,26.00071f,26.04227f,26.01707f,25.82354f,25.34414f,24.45955f,23.06449f,21.08311f,18.48260f,15.28457f,11.57281f,7.49449f,3.25012f,-0.93058f,-4.82388f,-8.24909f,-11.09616f,-13.33660f,-15.01667f,-16.23676f,-17.12582f,-17.82207f,-18.46870f,-19.22186f,-20.25387f,-21.73172f,-23.76952f,-26.38065f,-29.46427f,-32.83736f,-36.29065f,-39.63654f,-42.73095f,-45.47164f,-47.78552f,-49.61560f,-50.91277f,-51.63194f,-51.72970f,-51.16052f,-49.87079f,-47.79505f,-44.86273f,-41.02406f,-36.29488f,-30.80107f,-24.78755f,-18.56808f,-12.43986f,-6.61997f,-1.23543f,3.65142f,8.01587f,11.85648f,15.18803f,18.03396f,20.41861f,22.36396f,23.89345f,25.03898f,25.84526f,26.36786f},
|
||||
{22.56366f,22.90784f,23.10037f,23.17771f,23.16431f,23.07834f,22.93915f,22.77223f,22.60923f,22.48299f,22.41865f,22.42306f,22.47451f,22.51543f,22.45039f,22.15168f,21.47314f,20.27092f,18.42743f,15.87477f,12.61470f,8.73294f,4.40295f,-0.12806f,-4.57495f,-8.66629f,-12.19482f,-15.04947f,-17.22075f,-18.78121f,-19.84870f,-20.54584f,-20.97449f,-21.22186f,-21.39810f,-21.67766f,-22.29853f,-23.49197f,-25.37615f,-27.89268f,-30.83411f,-33.93295f,-36.94491f,-39.68626f,-42.03008f,-43.88606f,-45.18341f,-45.86344f,-45.87958f,-45.19889f,-43.79984f,-41.66647f,-38.78692f,-35.16910f,-30.87803f,-26.07520f,-21.01921f,-16.00229f,-11.25498f,-6.88718f,-2.90022f,0.75638f,4.13078f,7.24498f,10.09921f,12.68564f,14.99541f,17.01707f,18.73549f,20.13796f,21.22534f,22.01993f,22.56366f},
|
||||
{19.49262f,19.83667f,20.05379f,20.17527f,20.21649f,20.18386f,20.08413f,19.93130f,19.74919f,19.56928f,19.42437f,19.33850f,19.31400f,19.31803f,19.27228f,19.05075f,18.49032f,17.41469f,15.66762f,13.14805f,9.84083f,5.83792f,1.34298f,-3.35039f,-7.91048f,-12.03590f,-15.51510f,-18.25608f,-20.28181f,-21.69740f,-22.63861f,-23.21706f,-23.48665f,-23.45490f,-23.14501f,-22.68034f,-22.32882f,-22.44219f,-23.29750f,-24.95324f,-27.23460f,-29.83995f,-32.46585f,-34.87275f,-36.88902f,-38.39021f,-39.28263f,-39.49980f,-39.00659f,-37.80142f,-35.90945f,-33.36829f,-30.22080f,-26.53208f,-22.42894f,-18.12682f,-13.89950f,-9.99029f,-6.52460f,-3.48752f,-0.77184f,1.74639f,4.15087f,6.46808f,8.68408f,10.77078f,12.70017f,14.44314f,15.96584f,17.23560f,18.23538f,18.97548f,19.49262f},
|
||||
{16.97883f,17.28956f,17.49736f,17.63270f,17.70634f,17.71651f,17.65855f,17.53276f,17.34953f,17.13148f,16.91161f,16.72537f,16.59606f,16.51570f,16.42639f,16.21007f,15.69455f,14.67963f,12.97884f,10.46657f,7.11982f,3.04546f,-1.51809f,-6.23618f,-10.74800f,-14.74718f,-18.03985f,-20.56352f,-22.37111f,-23.58964f,-24.36165f,-24.78315f,-24.86525f,-24.54807f,-23.77488f,-22.60092f,-21.27530f,-20.21522f,-19.84302f,-20.38356f,-21.77840f,-23.76039f,-25.99255f,-28.16627f,-30.03382f,-31.40672f,-32.15138f,-32.19178f,-31.51307f,-30.15554f,-28.19296f,-25.70283f,-22.75199f,-19.41794f,-15.83548f,-12.22185f,-8.83494f,-5.87650f,-3.41043f,-1.35484f,0.45563f,2.18421f,3.92991f,5.71280f,7.50009f,9.24384f,10.90154f,12.43396f,13.79691f,14.94542f,15.85090f,16.51678f,16.97883f},
|
||||
{14.93100f,15.18635f,15.35662f,15.47564f,15.55491f,15.58961f,15.56703f,15.47431f,15.30624f,15.07283f,14.80278f,14.53815f,14.31797f,14.15367f,14.00355f,13.75659f,13.23581f,12.22632f,10.52389f,7.99180f,4.61181f,0.51478f,-4.02546f,-8.64514f,-12.97740f,-16.73466f,-19.75345f,-21.99705f,-23.53093f,-24.48178f,-24.98225f,-25.11094f,-24.85499f,-24.12625f,-22.83827f,-21.01546f,-18.87808f,-16.83573f,-15.35794f,-14.78584f,-15.21092f,-16.48364f,-18.30551f,-20.32696f,-22.21443f,-23.69113f,-24.56305f,-24.73343f,-24.20144f,-23.03963f,-21.35203f,-19.23024f,-16.73747f,-13.93871f,-10.95723f,-8.00232f,-5.32287f,-3.10447f,-1.38474f,-0.04693f,1.11102f,2.28032f,3.57005f,4.99114f,6.49051f,8.00055f,9.46670f,10.84386f,12.08259f,13.13004f,13.94893f,14.53733f,14.93100f},
|
||||
{13.28884f,13.47634f,13.58779f,13.66435f,13.72339f,13.76191f,13.76240f,13.70032f,13.55523f,13.32445f,13.03181f,12.72354f,12.44868f,12.22913f,12.02987f,11.74116f,11.18301f,10.13578f,8.39265f,5.82183f,2.42246f,-1.64568f,-6.07995f,-10.50575f,-14.57210f,-18.02426f,-20.72791f,-22.65695f,-23.86739f,-24.46549f,-24.56529f,-24.23837f,-23.48116f,-22.23069f,-20.43350f,-18.13422f,-15.53294f,-12.97246f,-10.85242f,-9.51146f,-9.13413f,-9.71333f,-11.06429f,-12.87374f,-14.77095f,-16.40698f,-17.52234f,-17.98440f,-17.78613f,-17.00868f,-15.76235f,-14.13365f,-12.17100f,-9.92237f,-7.49803f,-5.10053f,-2.97708f,-1.31256f,-0.13817f,0.67997f,1.36924f,2.14363f,3.12084f,4.30256f,5.61549f,6.97286f,8.30960f,9.57771f,10.72621f,11.69738f,12.44649f,12.96552f,13.28884f},
|
||||
{12.00282f,12.11837f,12.15757f,12.17280f,12.19200f,12.21760f,12.22925f,12.19160f,12.07031f,11.85230f,11.55870f,11.23943f,10.94829f,10.70784f,10.47867f,10.14549f,9.52682f,8.40876f,6.59772f,3.98239f,0.58868f,-3.39652f,-7.65593f,-11.82437f,-15.58129f,-18.70505f,-21.07796f,-22.66697f,-23.50437f,-23.67076f,-23.26901f,-22.38660f,-21.06689f,-19.31693f,-17.15424f,-14.65992f,-12.00100f,-9.41297f,-7.16151f,-5.50215f,-4.63707f,-4.66326f,-5.52593f,-7.00714f,-8.76960f,-10.44558f,-11.73428f,-12.46586f,-12.61021f,-12.23504f,-11.43824f,-10.29086f,-8.82422f,-7.07057f,-5.12746f,-3.18888f,-1.49978f,-0.24872f,0.52992f,0.97589f,1.33296f,1.83313f,2.60175f,3.63251f,4.83366f,6.10049f,7.35848f,8.55851f,9.65054f,10.57405f,11.27692f,11.74307f,12.00282f},
|
||||
{11.02739f,11.07622f,11.03933f,10.98421f,10.95339f,10.95728f,10.97332f,10.95552f,10.85678f,10.65621f,10.37470f,10.06586f,9.78388f,9.54402f,9.29421f,8.90939f,8.20943f,6.99576f,5.10152f,2.44783f,-0.90855f,-4.76162f,-8.79704f,-12.67530f,-16.11016f,-18.90310f,-20.93772f,-22.16415f,-22.59233f,-22.28944f,-21.36612f,-19.94707f,-18.14186f,-16.03876f,-13.72227f,-11.29101f,-8.85774f,-6.54187f,-4.47450f,-2.81373f,-1.73949f,-1.40345f,-1.85076f,-2.96503f,-4.47910f,-6.05292f,-7.37991f,-8.26855f,-8.66438f,-8.61274f,-8.19097f,-7.45133f,-6.40875f,-5.08083f,-3.55055f,-1.99810f,-0.66017f,0.27351f,0.76201f,0.94296f,1.06764f,1.38006f,2.01197f,2.95171f,4.09310f,5.31622f,6.53807f,7.70883f,8.78033f,9.68993f,10.37726f,10.81542f,11.02739f},
|
||||
{10.31472f,10.31453f,10.21064f,10.08863f,10.00885f,9.99151f,10.01233f,10.01480f,9.93992f,9.76023f,9.49687f,9.20560f,8.93764f,8.69695f,8.41641f,7.96184f,7.15858f,5.83066f,3.84646f,1.16583f,-2.12473f,-5.81072f,-9.59542f,-13.17310f,-16.28734f,-18.75062f,-20.43922f,-21.28955f,-21.30455f,-20.55871f,-19.18683f,-17.35463f,-15.22699f,-12.95067f,-10.64994f,-8.42037f,-6.31911f,-4.37056f,-2.60134f,-1.08831f,0.02271f,0.55979f,0.41503f,-0.37545f,-1.61611f,-3.01321f,-4.27793f,-5.21498f,-5.75467f,-5.92327f,-5.77955f,-5.35984f,-4.66555f,-3.70057f,-2.53081f,-1.31665f,-0.27875f,0.39794f,0.66647f,0.65655f,0.61794f,0.79941f,1.33742f,2.21923f,3.33090f,4.54215f,5.76289f,6.94188f,8.03189f,8.96743f,9.67821f,10.12326f,10.31472f},
|
||||
{9.80548f,9.79028f,9.64547f,9.47571f,9.36084f,9.33236f,9.36559f,9.39496f,9.35049f,9.19753f,8.95471f,8.67610f,8.40722f,8.14125f,7.79987f,7.24526f,6.31330f,4.85459f,2.77572f,0.07573f,-3.13325f,-6.63665f,-10.16260f,-13.43910f,-16.23268f,-18.36079f,-19.69530f,-20.17293f,-19.80955f,-18.70301f,-17.01472f,-14.93600f,-12.65708f,-10.34921f,-8.15147f,-6.15117f,-4.36707f,-2.76097f,-1.28854f,0.03431f,1.09685f,1.72693f,1.78209f,1.24223f,0.24367f,-0.96603f,-2.12362f,-3.04027f,-3.64042f,-3.93935f,-3.98483f,-3.80402f,-3.38988f,-2.73468f,-1.88594f,-0.97956f,-0.21314f,0.24019f,0.33045f,0.17587f,0.01620f,0.09717f,0.55809f,1.39034f,2.48092f,3.69622f,4.94135f,6.16231f,7.31051f,8.31580f,9.09607f,9.59342f,9.80548f},
|
||||
{9.42061f,9.44269f,9.30332f,9.12372f,9.00240f,8.98309f,9.04347f,9.11226f,9.11026f,8.99373f,8.77341f,8.49585f,8.19865f,7.86787f,7.42197f,6.72779f,5.63702f,4.02821f,1.84470f,-0.87850f,-4.00778f,-7.33151f,-10.60178f,-13.57573f,-16.03952f,-17.81956f,-18.79634f,-18.92464f,-18.24753f,-16.88942f,-15.02701f,-12.85350f,-10.55488f,-8.30042f,-6.23006f,-4.42721f,-2.89364f,-1.55859f,-0.33464f,0.80608f,1.78575f,2.44363f,2.62210f,2.26394f,1.45958f,0.41666f,-0.62651f,-1.49054f,-2.09912f,-2.46332f,-2.62771f,-2.61823f,-2.42568f,-2.03452f,-1.47479f,-0.85584f,-0.34717f,-0.10352f,-0.17336f,-0.45104f,-0.71330f,-0.72444f,-0.34439f,0.42720f,1.48729f,2.70684f,3.98949f,5.27733f,6.51771f,7.63398f,8.53135f,9.13351f,9.42061f},
|
||||
{9.06167f,9.18888f,9.12145f,8.99011f,8.90745f,8.92905f,9.03842f,9.16380f,9.21986f,9.15208f,8.95740f,8.66861f,8.31352f,7.87545f,7.27827f,6.40186f,5.11835f,3.33424f,1.02632f,-1.73833f,-4.80645f,-7.96686f,-10.98954f,-13.65607f,-15.77517f,-17.19456f,-17.82061f,-17.64000f,-16.72654f,-15.22262f,-13.30303f,-11.14274f,-8.90508f,-6.74344f,-4.79255f,-3.13707f,-1.77821f,-0.63553f,0.39969f,1.38495f,2.27215f,2.91947f,3.16817f,2.93852f,2.28580f,1.38356f,0.44726f,-0.35315f,-0.94224f,-1.33011f,-1.56318f,-1.67227f,-1.65234f,-1.48524f,-1.18690f,-0.84095f,-0.58630f,-0.55399f,-0.78557f,-1.18630f,-1.55248f,-1.66345f,-1.38052f,-0.69135f,0.31782f,1.52948f,2.84936f,4.21519f,5.56830f,6.82494f,7.87909f,8.63751f,9.06167f},
|
||||
{8.62618f,8.93281f,9.01770f,9.00969f,9.02653f,9.13240f,9.31934f,9.52089f,9.65030f,9.64274f,9.47763f,9.16936f,8.73421f,8.15557f,7.36859f,6.27213f,4.76201f,2.77242f,0.31104f,-2.52526f,-5.56242f,-8.58411f,-11.37133f,-13.72824f,-15.49367f,-16.55163f,-16.84874f,-16.40943f,-15.33257f,-13.76490f,-11.86461f,-9.77763f,-7.63798f,-5.57870f,-3.72678f,-2.17026f,-0.91907f,0.10149f,1.00480f,1.86583f,2.66250f,3.27711f,3.56090f,3.42364f,2.89396f,2.11484f,1.27906f,0.54622f,-0.00961f,-0.39793f,-0.66810f,-0.85870f,-0.97354f,-0.99760f,-0.93791f,-0.85565f,-0.85902f,-1.05158f,-1.46225f,-2.00284f,-2.48854f,-2.71585f,-2.54958f,-1.96518f,-1.02907f,0.15686f,1.50304f,2.94303f,4.41141f,5.81867f,7.05190f,8.00611f,8.62618f},
|
||||
{8.03598f,8.58889f,8.90722f,9.10460f,9.29148f,9.53362f,9.83144f,10.12834f,10.34195f,10.40095f,10.26805f,9.93790f,9.41372f,8.67870f,7.68051f,6.33877f,4.57424f,2.34883f,-0.29949f,-3.24365f,-6.28574f,-9.19890f,-11.77071f,-13.82904f,-15.25085f,-15.96729f,-15.97208f,-15.32534f,-14.14138f,-12.55967f,-10.71423f,-8.72028f,-6.68390f,-4.71835f,-2.94131f,-1.44306f,-0.24548f,0.71138f,1.53327f,2.30225f,3.01804f,3.59034f,3.88945f,3.82508f,3.40637f,2.74729f,2.01530f,1.35615f,0.84103f,0.46213f,0.16922f,-0.08321f,-0.30987f,-0.50356f,-0.66827f,-0.84729f,-1.11973f,-1.55925f,-2.17664f,-2.88418f,-3.51211f,-3.87370f,-3.83930f,-3.37449f,-2.52699f,-1.38270f,-0.02562f,1.47393f,3.04430f,4.59297f,6.00644f,7.17616f,8.03598f},
|
||||
{7.26404f,8.10965f,8.72719f,9.20363f,9.62912f,10.06111f,10.50424f,10.91333f,11.21567f,11.34011f,11.23825f,10.88772f,10.27813f,9.38958f,8.17873f,6.58333f,4.54783f,2.06192f,-0.80497f,-3.89324f,-6.97776f,-9.81797f,-12.20704f,-13.99879f,-15.11266f,-15.52991f,-15.28958f,-14.48074f,-13.22442f,-11.64711f,-9.85865f,-7.94820f,-5.99936f,-4.10802f,-2.38106f,-0.90774f,0.27922f,1.22103f,2.00764f,2.71959f,3.37356f,3.90798f,4.21895f,4.22501f,3.92204f,3.39553f,2.78286f,2.20986f,1.74142f,1.37229f,1.05518f,0.74140f,0.40656f,0.04769f,-0.34062f,-0.78836f,-1.34780f,-2.06152f,-2.91742f,-3.82180f,-4.61460f,-5.12394f,-5.22760f,-4.88549f,-4.13094f,-3.03660f,-1.68184f,-0.14054f,1.51063f,3.17937f,4.75791f,6.14267f,7.26404f},
|
||||
{6.34227f,7.49980f,8.45427f,9.25999f,9.97710f,10.64486f,11.26504f,11.80143f,12.19363f,12.37738f,12.30115f,11.93159f,11.24603f,10.21813f,8.80747f,6.96409f,4.65257f,1.88967f,-1.22203f,-4.48748f,-7.65190f,-10.46036f,-12.71276f,-14.29025f,-15.15384f,-15.33028f,-14.89618f,-13.96132f,-12.64812f,-11.07016f,-9.31841f,-7.46382f,-5.57411f,-3.73068f,-2.02814f,-0.55067f,0.66167f,1.63144f,2.42842f,3.12502f,3.74960f,4.26821f,4.60649f,4.69831f,4.53154f,4.16345f,3.69621f,3.22841f,2.81353f,2.44841f,2.09234f,1.69868f,1.23732f,0.69634f,0.06726f,-0.66937f,-1.54107f,-2.55884f,-3.68421f,-4.81193f,-5.78656f,-6.44890f,-6.68609f,-6.45751f,-5.78812f,-4.74191f,-3.39558f,-1.82633f,-0.11412f,1.65171f,3.37355f,4.95911f,6.34227f},
|
||||
{5.34603f,6.80801f,8.10430f,9.25699f,10.29292f,11.22674f,12.05003f,12.72994f,13.21683f,13.45645f,13.40063f,13.01148f,12.25724f,11.10344f,9.50726f,7.42475f,4.83602f,1.78342f,-1.59607f,-5.06954f,-8.35132f,-11.17309f,-13.34331f,-14.77010f,-15.45135f,-15.45050f,-14.87171f,-13.83788f,-12.47064f,-10.87411f,-9.12779f,-7.29256f,-5.42650f,-3.59932f,-1.89257f,-0.38187f,0.88983f,1.92927f,2.78537f,3.51820f,4.16232f,4.70759f,5.10911f,5.31921f,5.32149f,5.14594f,4.85608f,4.51693f,4.16472f,3.79586f,3.37783f,2.87062f,2.24401f,1.48161f,0.57414f,-0.48708f,-1.70528f,-3.05949f,-4.48253f,-5.85376f,-7.01874f,-7.82967f,-8.18534f,-8.05000f,-7.44677f,-6.43620f,-5.09470f,-3.50236f,-1.74074f,0.10559f,1.95098f,3.71728f,5.34602f},
|
||||
{4.36709f,6.10488f,7.71807f,9.20246f,10.55497f,11.76498f,12.80925f,13.65183f,14.24831f,14.55221f,14.52007f,14.11282f,13.29243f,12.01646f,10.23708f,7.91232f,5.03471f,1.67141f,-2.00440f,-5.71963f,-9.15722f,-12.03748f,-14.17954f,-15.51759f,-16.08076f,-15.96010f,-15.27810f,-14.16492f,-12.74067f,-11.10407f,-9.32963f,-7.47469f,-5.59264f,-3.74422f,-1.99849f,-0.42053f,0.94775f,2.10162f,3.07144f,3.90327f,4.63269f,5.26717f,5.78686f,6.16168f,6.37295f,6.42579f,6.34523f,6.15957f,5.88255f,5.50540f,5.00183f,4.34036f,3.49569f,2.45339f,1.20932f,-0.23035f,-1.84210f,-3.57115f,-5.31961f,-6.94925f,-8.30504f,-9.25096f,-9.70058f,-9.62856f,-9.06263f,-8.06501f,-6.71413f,-5.09269f,-3.28245f,-1.36256f,0.59288f,2.51929f,4.36709f},
|
||||
{3.48737f,5.45970f,7.34454f,9.11998f,10.76124f,12.23717f,13.50952f,14.53448f,15.26589f,15.65824f,15.66825f,15.25358f,14.36932f,12.96465f,10.98567f,8.39137f,5.18696f,1.46745f,-2.55250f,-6.55480f,-10.19002f,-13.17005f,-15.32853f,-16.62638f,-17.12030f,-16.92228f,-16.16643f,-14.98607f,-13.49940f,-11.80230f,-9.96824f,-8.05483f,-6.11394f,-4.20003f,-2.37140f,-0.68273f,0.82764f,2.14800f,3.29410f,4.29846f,5.19322f,5.99555f,6.70272f,7.29764f,7.75970f,8.07335f,8.22969f,8.22157f,8.03688f,7.65536f,7.05088f,6.19784f,5.07787f,3.68463f,2.02710f,0.13435f,-1.93649f,-4.09162f,-6.19897f,-8.10123f,-9.64329f,-10.70357f,-11.21502f,-11.16885f,-10.60359f,-9.58792f,-8.20440f,-6.53817f,-4.67039f,-2.67498f,-0.61648f,1.45157f,3.48737f},
|
||||
{2.75808f,4.92150f,7.02575f,9.03975f,10.92660f,12.64250f,14.13788f,15.35986f,16.25497f,16.77081f,16.85591f,16.45736f,15.51755f,13.97287f,11.76046f,8.83973f,5.23237f,1.07075f,-3.37550f,-7.73185f,-11.61289f,-14.72687f,-16.92941f,-18.21294f,-18.66191f,-18.40634f,-17.58827f,-16.34172f,-14.78238f,-13.00400f,-11.08009f,-9.06954f,-7.02398f,-4.99357f,-3.02836f,-1.17413f,0.53514f,2.08423f,3.47831f,4.73777f,5.88765f,6.94645f,7.91880f,8.79384f,9.54832f,10.15131f,10.56789f,10.76100f,10.69226f,10.32337f,9.61907f,8.55160f,7.10615f,5.28689f,3.12374f,0.67999f,-1.94104f,-4.59536f,-7.10950f,-9.30500f,-11.02932f,-12.18080f,-12.71873f,-12.65788f,-12.05373f,-10.98552f,-9.54170f,-7.80971f,-5.86969f,-3.79108f,-1.63131f,0.56380f,2.75808f},
|
||||
{2.18511f,4.50396f,6.78193f,8.98511f,11.07254f,12.99554f,14.69879f,16.12226f,17.20311f,17.87639f,18.07415f,17.72311f,16.74237f,15.04512f,12.55069f,9.21484f,5.08111f,0.33741f,-4.66447f,-9.46997f,-13.65027f,-16.91840f,-19.16613f,-20.42842f,-20.82260f,-20.49754f,-19.60237f,-18.27083f,-16.61550f,-14.72741f,-12.67917f,-10.52984f,-8.33033f,-6.12736f,-3.96470f,-1.88120f,0.09329f,1.94189f,3.66217f,5.26356f,6.76108f,8.16726f,9.48488f,10.70242f,11.79287f,12.71521f,13.41758f,13.84105f,13.92351f,13.60388f,12.82695f,11.54972f,9.74994f,7.43733f,4.66650f,1.54813f,-1.74846f,-5.00873f,-8.00386f,-10.53041f,-12.44318f,-13.66949f,-14.20444f,-14.09440f,-13.41755f,-12.26697f,-10.73807f,-8.92042f,-6.89286f,-4.72127f,-2.45833f,-0.14526f,2.18511f},
|
||||
{1.72053f,4.17297f,6.59438f,8.95134f,11.20364f,13.30308f,15.19344f,16.81081f,18.08389f,18.93317f,19.26896f,18.98863f,17.97530f,16.10272f,13.25527f,9.37362f,4.52871f,-1.00461f,-6.74269f,-12.11065f,-16.62809f,-20.03220f,-22.27725f,-23.45977f,-23.73964f,-23.28809f,-22.26206f,-20.79488f,-18.99544f,-16.95096f,-14.73125f,-12.39338f,-9.98585f,-7.55149f,-5.12863f,-2.75055f,-0.44367f,1.77430f,3.89494f,5.91760f,7.84559f,9.68075f,11.41785f,13.03988f,14.51531f,15.79744f,16.82551f,17.52703f,17.82085f,17.62109f,16.84267f,15.41052f,13.27453f,10.43204f,6.95518f,3.01260f,-1.13218f,-5.15905f,-8.75773f,-11.69139f,-13.82848f,-15.13789f,-15.66248f,-15.48890f,-14.72326f,-13.47499f,-11.84717f,-9.93137f,-7.80527f,-5.53214f,-3.16198f,-0.73388f,1.72053f},
|
||||
{1.25648f,3.83346f,6.38361f,8.87461f,11.26836f,13.51909f,15.57188f,17.36120f,18.80886f,19.82102f,20.28386f,20.05887f,18.98034f,16.86251f,13.53001f,8.89097f,3.05679f,-3.54903f,-10.22329f,-16.21814f,-21.01624f,-24.43081f,-26.52507f,-27.48288f,-27.51630f,-26.82066f,-25.55979f,-23.86524f,-21.84075f,-19.56791f,-17.11151f,-14.52415f,-11.84978f,-9.12619f,-6.38628f,-3.65838f,-0.96593f,1.67288f,4.24497f,6.74115f,9.15377f,11.47351f,13.68563f,15.76654f,17.68097f,19.38009f,20.80043f,21.86327f,22.47451f,22.52518f,21.89433f,20.45753f,18.10673f,14.78776f,10.55570f,5.62879f,0.39675f,-4.65483f,-9.08010f,-12.58457f,-15.05374f,-16.51320f,-17.06846f,-16.85678f,-16.01828f,-14.68187f,-12.95989f,-10.94687f,-8.72027f,-6.34224f,-3.86191f,-1.31827f,1.25648f},
|
||||
{0.61274f,3.30609f,5.97339f,8.58191f,11.09408f,13.46503f,15.63979f,17.55005f,19.10974f,20.20899f,20.70620f,20.41900f,19.11786f,16.53390f,12.40662f,6.61050f,-0.63393f,-8.60491f,-16.24674f,-22.64559f,-27.36876f,-30.42378f,-32.04157f,-32.50592f,-32.07420f,-30.95517f,-29.31063f,-27.26407f,-24.90971f,-22.32016f,-19.55232f,-16.65182f,-13.65621f,-10.59718f,-7.50200f,-4.39430f,-1.29461f,1.77927f,4.81167f,7.78814f,10.69402f,13.51268f,16.22331f,18.79876f,21.20313f,23.38945f,25.29705f,26.84843f,27.94548f,28.46508f,28.25526f,27.13565f,24.91066f,21.41082f,16.57999f,10.60085f,3.97961f,-2.54106f,-8.24894f,-12.70524f,-15.79057f,-17.60170f,-18.32929f,-18.17960f,-17.33926f,-15.96496f,-14.18379f,-12.09725f,-9.78580f,-7.31305f,-4.72954f,-2.07615f,0.61273f},
|
||||
{-0.51780f,2.25318f,4.99425f,7.66865f,10.23485f,12.64332f,14.83254f,16.72371f,18.21347f,19.16381f,19.38869f,18.63855f,16.59029f,12.86697f,7.14422f,-0.58466f,-9.65920f,-18.74956f,-26.50752f,-32.23849f,-35.92738f,-37.89259f,-38.51301f,-38.11560f,-36.95302f,-35.21298f,-33.03368f,-30.51738f,-27.74077f,-24.76235f,-21.62771f,-18.37317f,-15.02848f,-11.61858f,-8.16494f,-4.68647f,-1.20023f,2.27799f,5.73306f,9.14986f,12.51243f,15.80303f,19.00093f,22.08101f,25.01199f,27.75429f,30.25706f,32.45424f,34.25895f,35.55571f,36.19006f,35.95632f,34.58787f,31.76497f,27.17577f,20.68735f,12.62891f,3.95837f,-4.05513f,-10.47137f,-14.98139f,-17.73202f,-19.04365f,-19.24250f,-18.60128f,-17.33187f,-15.59484f,-13.51153f,-11.17436f,-8.65479f,-6.00927f,-3.28385f,-0.51781f},
|
||||
{-2.94630f,-0.27015f,2.35392f,4.87475f,7.23381f,9.35996f,11.16265f,12.52250f,13.27822f,13.20942f,12.01747f,9.31709f,4.67824f,-2.19503f,-11.07895f,-20.88160f,-29.97238f,-37.12710f,-41.99746f,-44.82656f,-46.03905f,-46.03089f,-45.11248f,-43.51276f,-41.39774f,-38.88807f,-36.07244f,-33.01710f,-29.77243f,-26.37748f,-22.86313f,-19.25438f,-15.57188f,-11.83313f,-8.05336f,-4.24613f,-0.42389f,3.40158f,7.21877f,11.01604f,14.78119f,18.50092f,22.16027f,25.74181f,29.22467f,32.58313f,35.78478f,38.78779f,41.53690f,43.95751f,45.94660f,47.35884f,47.98567f,47.52511f,45.54571f,41.47140f,34.68705f,24.97739f,13.30733f,1.91060f,-7.18982f,-13.36551f,-16.99104f,-18.69892f,-19.03825f,-18.41248f,-17.10439f,-15.31181f,-13.17571f,-10.79953f,-8.26212f,-5.62654f,-2.94631f},
|
||||
{-15.37669f,-14.26728f,-13.44233f,-13.07088f,-13.34672f,-14.49663f,-16.77455f,-20.42235f,-25.56851f,-32.05942f,-39.32137f,-46.45545f,-52.60312f,-57.27376f,-60.37929f,-62.07479f,-62.59922f,-62.18728f,-61.03725f,-59.30634f,-57.11599f,-54.55931f,-51.70786f,-48.61705f,-45.33014f,-41.88134f,-38.29801f,-34.60230f,-30.81233f,-26.94317f,-23.00747f,-19.01601f,-14.97808f,-10.90183f,-6.79450f,-2.66261f,1.48783f,5.65122f,9.82224f,13.99568f,18.16641f,22.32918f,26.47854f,30.60867f,34.71317f,38.78489f,42.81557f,46.79548f,50.71283f,54.55307f,58.29770f,61.92271f,65.39604f,68.67369f,71.69330f,74.36332f,76.54363f,78.00924f,78.37835f,76.96295f,72.45838f,62.44065f,43.87018f,19.37226f,-0.18356f,-11.16707f,-16.48422f,-18.69494f,-19.20397f,-18.76390f,-17.80785f,-16.61220f,-15.37669f},
|
||||
{-172.49255f,-167.49255f,-162.49255f,-157.49255f,-152.49255f,-147.49255f,-142.49256f,-137.49255f,-132.49255f,-127.49255f,-122.49255f,-117.49255f,-112.49255f,-107.49255f,-102.49255f,-97.49255f,-92.49255f,-87.49255f,-82.49255f,-77.49255f,-72.49256f,-67.49255f,-62.49256f,-57.49255f,-52.49256f,-47.49255f,-42.49255f,-37.49255f,-32.49255f,-27.49256f,-22.49255f,-17.49256f,-12.49255f,-7.49256f,-2.49256f,2.50744f,7.50745f,12.50745f,17.50744f,22.50744f,27.50744f,32.50744f,37.50744f,42.50744f,47.50744f,52.50745f,57.50744f,62.50744f,67.50744f,72.50744f,77.50744f,82.50744f,87.50744f,92.50744f,97.50744f,102.50744f,107.50744f,112.50744f,117.50744f,122.50744f,127.50744f,132.50744f,137.50744f,142.50744f,147.50744f,152.50744f,157.50744f,162.50744f,167.50744f,172.50744f,177.50744f,-177.49256f,-172.49256f}
|
||||
};
|
||||
|
||||
@@ -0,0 +1,72 @@
|
||||
#!/usr/bin/env python3
|
||||
'''
|
||||
generate field tables from IGRF12. Note that this requires python3
|
||||
'''
|
||||
|
||||
import igrf as igrf
|
||||
import numpy as np
|
||||
import datetime
|
||||
from pathlib import Path
|
||||
from pymavlink.rotmat import Vector3, Matrix3
|
||||
import math
|
||||
|
||||
import argparse
|
||||
parser = argparse.ArgumentParser(description='generate mag tables')
|
||||
parser.add_argument('--sampling-res', type=int, default=5, help='sampling resolution, degrees')
|
||||
parser.add_argument('--check-error', action='store_true', help='check max error')
|
||||
parser.add_argument('--filename', type=str, default='tables.cpp', help='tables file')
|
||||
|
||||
args = parser.parse_args()
|
||||
|
||||
if not Path("AP_Declination.h").is_file():
|
||||
raise OSError("Please run this tool from the AP_Declination directory")
|
||||
|
||||
|
||||
def write_table(f,name, table):
|
||||
'''write one table'''
|
||||
f.write("const float AP_Declination::%s[%u][%u] = {\n" %
|
||||
(name, NUM_LAT, NUM_LON))
|
||||
for i in range(NUM_LAT):
|
||||
f.write(" {")
|
||||
for j in range(NUM_LON):
|
||||
f.write("%.5ff" % table[i][j])
|
||||
if j != NUM_LON-1:
|
||||
f.write(",")
|
||||
f.write("}")
|
||||
if i != NUM_LAT-1:
|
||||
f.write(",")
|
||||
f.write("\n")
|
||||
f.write("};\n\n")
|
||||
|
||||
date = datetime.datetime.now()
|
||||
|
||||
SAMPLING_RES = args.sampling_res
|
||||
SAMPLING_MIN_LAT = -90
|
||||
SAMPLING_MAX_LAT = 90
|
||||
SAMPLING_MIN_LON = -180
|
||||
SAMPLING_MAX_LON = 180
|
||||
|
||||
lats = np.arange(SAMPLING_MIN_LAT, SAMPLING_MAX_LAT+SAMPLING_RES, SAMPLING_RES)
|
||||
lons = np.arange(SAMPLING_MIN_LON, SAMPLING_MAX_LON+SAMPLING_RES, SAMPLING_RES)
|
||||
|
||||
NUM_LAT = lats.size
|
||||
NUM_LON = lons.size
|
||||
|
||||
intensity_table = np.empty((NUM_LAT, NUM_LON))
|
||||
inclination_table = np.empty((NUM_LAT, NUM_LON))
|
||||
declination_table = np.empty((NUM_LAT, NUM_LON))
|
||||
|
||||
max_error = 0
|
||||
max_error_pos = None
|
||||
max_error_field = None
|
||||
|
||||
def get_igrf(lat, lon):
|
||||
'''return field as [declination_deg, inclination_deg, intensity_gauss]'''
|
||||
mag = igrf.igrf(date, glat=lat, glon=lon, alt_km=0., isv=0, itype=1)
|
||||
intensity = float(mag.total/1e5)
|
||||
inclination = float(mag.incl)
|
||||
declination = float(mag.decl)
|
||||
return [declination, inclination, intensity]
|
||||
|
||||
|
||||
print("%f" % get_igrf(40.5795,-74.1502)[0])
|
||||
@@ -0,0 +1,544 @@
|
||||
#include "sdkconfig.h"
|
||||
#if defined(CONFIG_BLUEDROID_ENABLED)
|
||||
#include <sstream>
|
||||
#include "atBLEAdvertisedDevice.h"
|
||||
#include "BLEUtils.h"
|
||||
#include "esp32-hal-log.h"
|
||||
|
||||
atBLEAdvertisedDevice::atBLEAdvertisedDevice() {
|
||||
m_adFlag = 0;
|
||||
m_appearance = 0;
|
||||
m_deviceType = 0;
|
||||
m_manufacturerData = "";
|
||||
m_name = "";
|
||||
m_rssi = -9999;
|
||||
m_serviceData = {};
|
||||
m_serviceDataUUIDs = {};
|
||||
m_txPower = 0;
|
||||
|
||||
m_haveAppearance = false;
|
||||
m_haveManufacturerData = false;
|
||||
m_haveName = false;
|
||||
m_haveRSSI = false;
|
||||
m_haveServiceData = false;
|
||||
m_haveServiceUUID = false;
|
||||
m_haveTXPower = false;
|
||||
|
||||
} // atBLEAdvertisedDevice
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the address.
|
||||
*
|
||||
* Every %BLE device exposes an address that is used to identify it and subsequently connect to it.
|
||||
* Call this function to obtain the address of the advertised device.
|
||||
*
|
||||
* @return The address of the advertised device.
|
||||
*/
|
||||
BLEAddress atBLEAdvertisedDevice::getAddress() {
|
||||
return m_address;
|
||||
} // getAddress
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the appearance.
|
||||
*
|
||||
* A %BLE device can declare its own appearance. The appearance is how it would like to be shown to an end user
|
||||
* typcially in the form of an icon.
|
||||
*
|
||||
* @return The appearance of the advertised device.
|
||||
*/
|
||||
uint16_t atBLEAdvertisedDevice::getAppearance() {
|
||||
return m_appearance;
|
||||
} // getAppearance
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the manufacturer data.
|
||||
* @return The manufacturer data of the advertised device.
|
||||
*/
|
||||
std::string atBLEAdvertisedDevice::getManufacturerData() {
|
||||
return m_manufacturerData;
|
||||
} // getManufacturerData
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the name.
|
||||
* @return The name of the advertised device.
|
||||
*/
|
||||
std::string atBLEAdvertisedDevice::getName() {
|
||||
return m_name;
|
||||
} // getName
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the RSSI.
|
||||
* @return The RSSI of the advertised device.
|
||||
*/
|
||||
int atBLEAdvertisedDevice::getRSSI() {
|
||||
return m_rssi;
|
||||
} // getRSSI
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the number of service data.
|
||||
* @return Number of service data discovered.
|
||||
*/
|
||||
int atBLEAdvertisedDevice::getServiceDataCount() {
|
||||
if (m_haveServiceData)
|
||||
return m_serviceData.size();
|
||||
else
|
||||
return 0;
|
||||
|
||||
} //getServiceDataCount
|
||||
|
||||
/**
|
||||
* @brief Get the service data.
|
||||
* @return The ServiceData of the advertised device.
|
||||
*/
|
||||
std::string atBLEAdvertisedDevice::getServiceData() {
|
||||
return m_serviceData[0];
|
||||
} //getServiceData
|
||||
|
||||
/**
|
||||
* @brief Get the service data.
|
||||
* @return The ServiceData of the advertised device.
|
||||
*/
|
||||
std::string atBLEAdvertisedDevice::getServiceData(int i) {
|
||||
return m_serviceData[i];
|
||||
} //getServiceData
|
||||
|
||||
/**
|
||||
* @brief Get the service data UUID.
|
||||
* @return The service data UUID.
|
||||
*/
|
||||
BLEUUID atBLEAdvertisedDevice::getServiceDataUUID() {
|
||||
return m_serviceDataUUIDs[0];
|
||||
} // getServiceDataUUID
|
||||
|
||||
/**
|
||||
* @brief Get the service data UUID.
|
||||
* @return The service data UUID.
|
||||
*/
|
||||
BLEUUID atBLEAdvertisedDevice::getServiceDataUUID(int i) {
|
||||
return m_serviceDataUUIDs[i];
|
||||
} // getServiceDataUUID
|
||||
|
||||
/**
|
||||
* @brief Get the Service UUID.
|
||||
* @return The Service UUID of the advertised device.
|
||||
*/
|
||||
BLEUUID atBLEAdvertisedDevice::getServiceUUID() {
|
||||
return m_serviceUUIDs[0];
|
||||
} // getServiceUUID
|
||||
|
||||
/**
|
||||
* @brief Get the Service UUID.
|
||||
* @return The Service UUID of the advertised device.
|
||||
*/
|
||||
BLEUUID atBLEAdvertisedDevice::getServiceUUID(int i) {
|
||||
return m_serviceUUIDs[i];
|
||||
} // getServiceUUID
|
||||
|
||||
/**
|
||||
* @brief Check advertised serviced for existence required UUID
|
||||
* @return Return true if service is advertised
|
||||
*/
|
||||
bool atBLEAdvertisedDevice::isAdvertisingService(BLEUUID uuid){
|
||||
for (int i = 0; i < m_serviceUUIDs.size(); i++) {
|
||||
if (m_serviceUUIDs[i].equals(uuid)) return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get the TX Power.
|
||||
* @return The TX Power of the advertised device.
|
||||
*/
|
||||
int8_t atBLEAdvertisedDevice::getTXPower() {
|
||||
return m_txPower;
|
||||
} // getTXPower
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief Does this advertisement have an appearance value?
|
||||
* @return True if there is an appearance value present.
|
||||
*/
|
||||
bool atBLEAdvertisedDevice::haveAppearance() {
|
||||
return m_haveAppearance;
|
||||
} // haveAppearance
|
||||
|
||||
|
||||
/**
|
||||
* @brief Does this advertisement have manufacturer data?
|
||||
* @return True if there is manufacturer data present.
|
||||
*/
|
||||
bool atBLEAdvertisedDevice::haveManufacturerData() {
|
||||
return m_haveManufacturerData;
|
||||
} // haveManufacturerData
|
||||
|
||||
|
||||
/**
|
||||
* @brief Does this advertisement have a name value?
|
||||
* @return True if there is a name value present.
|
||||
*/
|
||||
bool atBLEAdvertisedDevice::haveName() {
|
||||
return m_haveName;
|
||||
} // haveName
|
||||
|
||||
|
||||
/**
|
||||
* @brief Does this advertisement have a signal strength value?
|
||||
* @return True if there is a signal strength value present.
|
||||
*/
|
||||
bool atBLEAdvertisedDevice::haveRSSI() {
|
||||
return m_haveRSSI;
|
||||
} // haveRSSI
|
||||
|
||||
|
||||
/**
|
||||
* @brief Does this advertisement have a service data value?
|
||||
* @return True if there is a service data value present.
|
||||
*/
|
||||
bool atBLEAdvertisedDevice::haveServiceData() {
|
||||
return m_haveServiceData;
|
||||
} // haveServiceData
|
||||
|
||||
|
||||
/**
|
||||
* @brief Does this advertisement have a service UUID value?
|
||||
* @return True if there is a service UUID value present.
|
||||
*/
|
||||
bool atBLEAdvertisedDevice::haveServiceUUID() {
|
||||
return m_haveServiceUUID;
|
||||
} // haveServiceUUID
|
||||
|
||||
|
||||
/**
|
||||
* @brief Does this advertisement have a transmission power value?
|
||||
* @return True if there is a transmission power value present.
|
||||
*/
|
||||
bool atBLEAdvertisedDevice::haveTXPower() {
|
||||
return m_haveTXPower;
|
||||
} // haveTXPower
|
||||
|
||||
|
||||
/**
|
||||
* @brief Parse the advertising pay load.
|
||||
*
|
||||
* The pay load is a buffer of bytes that is either 31 bytes long or terminated by
|
||||
* a 0 length value. Each entry in the buffer has the format:
|
||||
* [length][type][data...]
|
||||
*
|
||||
* The length does not include itself but does include everything after it until the next record. A record
|
||||
* with a length value of 0 indicates a terminator.
|
||||
*
|
||||
* https://www.bluetooth.com/specifications/assigned-numbers/generic-access-profile
|
||||
*/
|
||||
void atBLEAdvertisedDevice::parseAdvertisement(uint8_t* payload, size_t total_len) {
|
||||
uint8_t length;
|
||||
uint8_t ad_type;
|
||||
uint8_t sizeConsumed = 0;
|
||||
bool finished = false;
|
||||
m_payload = payload;
|
||||
m_payloadLength = total_len;
|
||||
|
||||
while(!finished) {
|
||||
length = *payload; // Retrieve the length of the record.
|
||||
payload++; // Skip to type
|
||||
sizeConsumed += 1 + length; // increase the size consumed.
|
||||
|
||||
if (length != 0) { // A length of 0 indicates that we have reached the end.
|
||||
ad_type = *payload;
|
||||
payload++;
|
||||
length--;
|
||||
|
||||
char* pHex = BLEUtils::buildHexData(nullptr, payload, length);
|
||||
log_d("Type: 0x%.2x (%s), length: %d, data: %s",
|
||||
ad_type, BLEUtils::advTypeToString(ad_type), length, pHex);
|
||||
free(pHex);
|
||||
|
||||
switch(ad_type) {
|
||||
case ESP_BLE_AD_TYPE_NAME_CMPL: { // Adv Data Type: 0x09
|
||||
setName(std::string(reinterpret_cast<char*>(payload), length));
|
||||
break;
|
||||
} // ESP_BLE_AD_TYPE_NAME_CMPL
|
||||
|
||||
case ESP_BLE_AD_TYPE_TX_PWR: { // Adv Data Type: 0x0A
|
||||
setTXPower(*payload);
|
||||
break;
|
||||
} // ESP_BLE_AD_TYPE_TX_PWR
|
||||
|
||||
case ESP_BLE_AD_TYPE_APPEARANCE: { // Adv Data Type: 0x19
|
||||
setAppearance(*reinterpret_cast<uint16_t*>(payload));
|
||||
break;
|
||||
} // ESP_BLE_AD_TYPE_APPEARANCE
|
||||
|
||||
case ESP_BLE_AD_TYPE_FLAG: { // Adv Data Type: 0x01
|
||||
setAdFlag(*payload);
|
||||
break;
|
||||
} // ESP_BLE_AD_TYPE_FLAG
|
||||
|
||||
case ESP_BLE_AD_TYPE_16SRV_CMPL:
|
||||
case ESP_BLE_AD_TYPE_16SRV_PART: { // Adv Data Type: 0x02
|
||||
for (int var = 0; var < length/2; ++var) {
|
||||
setServiceUUID(BLEUUID(*reinterpret_cast<uint16_t*>(payload + var * 2)));
|
||||
}
|
||||
break;
|
||||
} // ESP_BLE_AD_TYPE_16SRV_PART
|
||||
|
||||
case ESP_BLE_AD_TYPE_32SRV_CMPL:
|
||||
case ESP_BLE_AD_TYPE_32SRV_PART: { // Adv Data Type: 0x04
|
||||
for (int var = 0; var < length/4; ++var) {
|
||||
setServiceUUID(BLEUUID(*reinterpret_cast<uint32_t*>(payload + var * 4)));
|
||||
}
|
||||
break;
|
||||
} // ESP_BLE_AD_TYPE_32SRV_PART
|
||||
|
||||
case ESP_BLE_AD_TYPE_128SRV_CMPL: { // Adv Data Type: 0x07
|
||||
setServiceUUID(BLEUUID(payload, 16, false));
|
||||
break;
|
||||
} // ESP_BLE_AD_TYPE_128SRV_CMPL
|
||||
|
||||
case ESP_BLE_AD_TYPE_128SRV_PART: { // Adv Data Type: 0x06
|
||||
setServiceUUID(BLEUUID(payload, 16, false));
|
||||
break;
|
||||
} // ESP_BLE_AD_TYPE_128SRV_PART
|
||||
|
||||
// See CSS Part A 1.4 Manufacturer Specific Data
|
||||
case ESP_BLE_AD_MANUFACTURER_SPECIFIC_TYPE: {
|
||||
setManufacturerData(std::string(reinterpret_cast<char*>(payload), length));
|
||||
break;
|
||||
} // ESP_BLE_AD_MANUFACTURER_SPECIFIC_TYPE
|
||||
|
||||
case ESP_BLE_AD_TYPE_SERVICE_DATA: { // Adv Data Type: 0x16 (Service Data) - 2 byte UUID
|
||||
if (length < 2) {
|
||||
log_e("Length too small for ESP_BLE_AD_TYPE_SERVICE_DATA");
|
||||
break;
|
||||
}
|
||||
uint16_t uuid = *(uint16_t*)payload;
|
||||
setServiceDataUUID(BLEUUID(uuid));
|
||||
if (length > 2) {
|
||||
setServiceData(std::string(reinterpret_cast<char*>(payload + 2), length - 2));
|
||||
}
|
||||
break;
|
||||
} //ESP_BLE_AD_TYPE_SERVICE_DATA
|
||||
|
||||
case ESP_BLE_AD_TYPE_32SERVICE_DATA: { // Adv Data Type: 0x20 (Service Data) - 4 byte UUID
|
||||
if (length < 4) {
|
||||
log_e("Length too small for ESP_BLE_AD_TYPE_32SERVICE_DATA");
|
||||
break;
|
||||
}
|
||||
uint32_t uuid = *(uint32_t*) payload;
|
||||
setServiceDataUUID(BLEUUID(uuid));
|
||||
if (length > 4) {
|
||||
setServiceData(std::string(reinterpret_cast<char*>(payload + 4), length - 4));
|
||||
}
|
||||
break;
|
||||
} //ESP_BLE_AD_TYPE_32SERVICE_DATA
|
||||
|
||||
case ESP_BLE_AD_TYPE_128SERVICE_DATA: { // Adv Data Type: 0x21 (Service Data) - 16 byte UUID
|
||||
if (length < 16) {
|
||||
log_e("Length too small for ESP_BLE_AD_TYPE_128SERVICE_DATA");
|
||||
break;
|
||||
}
|
||||
|
||||
setServiceDataUUID(BLEUUID(payload, (size_t)16, false));
|
||||
if (length > 16) {
|
||||
setServiceData(std::string(reinterpret_cast<char*>(payload + 16), length - 16));
|
||||
}
|
||||
break;
|
||||
} //ESP_BLE_AD_TYPE_32SERVICE_DATA
|
||||
|
||||
default: {
|
||||
log_d("Unhandled type: adType: %d - 0x%.2x", ad_type, ad_type);
|
||||
break;
|
||||
}
|
||||
} // switch
|
||||
payload += length;
|
||||
} // Length <> 0
|
||||
|
||||
|
||||
if (sizeConsumed >= total_len)
|
||||
finished = true;
|
||||
|
||||
} // !finished
|
||||
} // parseAdvertisement
|
||||
|
||||
/**
|
||||
* @brief Parse the advertising payload.
|
||||
* @param [in] payload The payload of the advertised device.
|
||||
* @param [in] total_len The length of payload
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setPayload(uint8_t* payload, size_t total_len) {
|
||||
m_payload = payload;
|
||||
m_payloadLength = total_len;
|
||||
} // setPayload
|
||||
|
||||
/**
|
||||
* @brief Set the address of the advertised device.
|
||||
* @param [in] address The address of the advertised device.
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setAddress(BLEAddress address) {
|
||||
m_address = address;
|
||||
} // setAddress
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the adFlag for this device.
|
||||
* @param [in] The discovered adFlag.
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setAdFlag(uint8_t adFlag) {
|
||||
m_adFlag = adFlag;
|
||||
} // setAdFlag
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the appearance for this device.
|
||||
* @param [in] The discovered appearance.
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setAppearance(uint16_t appearance) {
|
||||
m_appearance = appearance;
|
||||
m_haveAppearance = true;
|
||||
log_d("- appearance: %d", m_appearance);
|
||||
} // setAppearance
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the manufacturer data for this device.
|
||||
* @param [in] The discovered manufacturer data.
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setManufacturerData(std::string manufacturerData) {
|
||||
m_manufacturerData = manufacturerData;
|
||||
m_haveManufacturerData = true;
|
||||
char* pHex = BLEUtils::buildHexData(nullptr, (uint8_t*) m_manufacturerData.data(), (uint8_t) m_manufacturerData.length());
|
||||
log_d("- manufacturer data: %s", pHex);
|
||||
free(pHex);
|
||||
} // setManufacturerData
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the name for this device.
|
||||
* @param [in] name The discovered name.
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setName(std::string name) {
|
||||
m_name = name;
|
||||
m_haveName = true;
|
||||
log_d("- setName(): name: %s", m_name.c_str());
|
||||
} // setName
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the RSSI for this device.
|
||||
* @param [in] rssi The discovered RSSI.
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setRSSI(int rssi) {
|
||||
m_rssi = rssi;
|
||||
m_haveRSSI = true;
|
||||
log_d("- setRSSI(): rssi: %d", m_rssi);
|
||||
} // setRSSI
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the Service UUID for this device.
|
||||
* @param [in] serviceUUID The discovered serviceUUID
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setServiceUUID(const char* serviceUUID) {
|
||||
return setServiceUUID(BLEUUID(serviceUUID));
|
||||
} // setServiceUUID
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the Service UUID for this device.
|
||||
* @param [in] serviceUUID The discovered serviceUUID
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setServiceUUID(BLEUUID serviceUUID) {
|
||||
m_serviceUUIDs.push_back(serviceUUID);
|
||||
m_haveServiceUUID = true;
|
||||
log_d("- addServiceUUID(): serviceUUID: %s", serviceUUID.toString().c_str());
|
||||
} // setServiceUUID
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the ServiceData value.
|
||||
* @param [in] data ServiceData value.
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setServiceData(std::string serviceData) {
|
||||
m_haveServiceData = true; // Set the flag that indicates we have service data.
|
||||
m_serviceData.push_back(serviceData); // Save the service data that we received.
|
||||
} //setServiceData
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the ServiceDataUUID value.
|
||||
* @param [in] data ServiceDataUUID value.
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setServiceDataUUID(BLEUUID uuid) {
|
||||
m_haveServiceData = true; // Set the flag that indicates we have service data.
|
||||
m_serviceDataUUIDs.push_back(uuid);
|
||||
log_d("- addServiceDataUUID(): serviceDataUUID: %s", uuid.toString().c_str());
|
||||
} // setServiceDataUUID
|
||||
|
||||
|
||||
/**
|
||||
* @brief Set the power level for this device.
|
||||
* @param [in] txPower The discovered power level.
|
||||
*/
|
||||
void atBLEAdvertisedDevice::setTXPower(int8_t txPower) {
|
||||
m_txPower = txPower;
|
||||
m_haveTXPower = true;
|
||||
log_d("- txPower: %d", m_txPower);
|
||||
} // setTXPower
|
||||
|
||||
|
||||
/**
|
||||
* @brief Create a string representation of this device.
|
||||
* @return A string representation of this device.
|
||||
*/
|
||||
std::string atBLEAdvertisedDevice::toString() {
|
||||
std::string res = "Name: " + getName() + ", Address: " + getAddress().toString();
|
||||
if (haveAppearance()) {
|
||||
char val[6];
|
||||
snprintf(val, sizeof(val), "%d", getAppearance());
|
||||
res += ", appearance: ";
|
||||
res += val;
|
||||
}
|
||||
if (haveManufacturerData()) {
|
||||
char *pHex = BLEUtils::buildHexData(nullptr, (uint8_t*)getManufacturerData().data(), getManufacturerData().length());
|
||||
res += ", manufacturer data: ";
|
||||
res += pHex;
|
||||
free(pHex);
|
||||
}
|
||||
if (haveServiceUUID()) {
|
||||
for (int i=0; i < m_serviceUUIDs.size(); i++) {
|
||||
res += ", serviceUUID: " + getServiceUUID(i).toString();
|
||||
}
|
||||
}
|
||||
if (haveTXPower()) {
|
||||
char val[6];
|
||||
snprintf(val, sizeof(val), "%d", getTXPower());
|
||||
res += ", txPower: ";
|
||||
res += val;
|
||||
}
|
||||
return res;
|
||||
} // toString
|
||||
|
||||
uint8_t* atBLEAdvertisedDevice::getPayload() {
|
||||
return m_payload;
|
||||
}
|
||||
|
||||
esp_ble_addr_type_t atBLEAdvertisedDevice::getAddressType() {
|
||||
return m_addressType;
|
||||
}
|
||||
|
||||
void atBLEAdvertisedDevice::setAddressType(esp_ble_addr_type_t type) {
|
||||
m_addressType = type;
|
||||
}
|
||||
|
||||
size_t atBLEAdvertisedDevice::getPayloadLength() {
|
||||
return m_payloadLength;
|
||||
}
|
||||
|
||||
#endif /* CONFIG_BLUEDROID_ENABLED */
|
||||
|
||||
@@ -0,0 +1,89 @@
|
||||
#ifndef ATCOMPONENTS_CPP_UTILS_BLEADVERTISEDDEVICE_H_
|
||||
#define ATCOMPONENTS_CPP_UTILS_BLEADVERTISEDDEVICE_H_
|
||||
#include "sdkconfig.h"
|
||||
#if defined(CONFIG_BLUEDROID_ENABLED)
|
||||
#include <esp_gattc_api.h>
|
||||
|
||||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
#include "BLEAddress.h"
|
||||
#include "BLEUUID.h"
|
||||
|
||||
|
||||
class atBLEAdvertisedDevice {
|
||||
public:
|
||||
atBLEAdvertisedDevice();
|
||||
|
||||
BLEAddress getAddress();
|
||||
uint16_t getAppearance();
|
||||
std::string getManufacturerData();
|
||||
std::string getName();
|
||||
int getRSSI();
|
||||
std::string getServiceData();
|
||||
std::string getServiceData(int i);
|
||||
BLEUUID getServiceDataUUID();
|
||||
BLEUUID getServiceDataUUID(int i);
|
||||
BLEUUID getServiceUUID();
|
||||
BLEUUID getServiceUUID(int i);
|
||||
int getServiceDataCount();
|
||||
int8_t getTXPower();
|
||||
uint8_t* getPayload();
|
||||
size_t getPayloadLength();
|
||||
esp_ble_addr_type_t getAddressType();
|
||||
void setAddressType(esp_ble_addr_type_t type);
|
||||
|
||||
|
||||
bool isAdvertisingService(BLEUUID uuid);
|
||||
bool haveAppearance();
|
||||
bool haveManufacturerData();
|
||||
bool haveName();
|
||||
bool haveRSSI();
|
||||
bool haveServiceData();
|
||||
bool haveServiceUUID();
|
||||
bool haveTXPower();
|
||||
|
||||
std::string toString();
|
||||
|
||||
void parseAdvertisement(uint8_t* payload, size_t total_len=62);
|
||||
void setPayload(uint8_t* payload, size_t total_len=62);
|
||||
void setAddress(BLEAddress address);
|
||||
void setAdFlag(uint8_t adFlag);
|
||||
void setAdvertizementResult(uint8_t* payload);
|
||||
void setAppearance(uint16_t appearance);
|
||||
void setManufacturerData(std::string manufacturerData);
|
||||
void setName(std::string name);
|
||||
void setRSSI(int rssi);
|
||||
void setServiceData(std::string data);
|
||||
void setServiceDataUUID(BLEUUID uuid);
|
||||
void setServiceUUID(const char* serviceUUID);
|
||||
void setServiceUUID(BLEUUID serviceUUID);
|
||||
void setTXPower(int8_t txPower);
|
||||
|
||||
protected:
|
||||
bool m_haveAppearance;
|
||||
bool m_haveManufacturerData;
|
||||
bool m_haveName;
|
||||
bool m_haveRSSI;
|
||||
bool m_haveServiceData;
|
||||
bool m_haveServiceUUID;
|
||||
bool m_haveTXPower;
|
||||
|
||||
|
||||
BLEAddress m_address = BLEAddress((uint8_t*)"\0\0\0\0\0\0");
|
||||
uint8_t m_adFlag;
|
||||
uint16_t m_appearance;
|
||||
int m_deviceType;
|
||||
std::string m_manufacturerData;
|
||||
std::string m_name;
|
||||
int m_rssi;
|
||||
std::vector<BLEUUID> m_serviceUUIDs;
|
||||
int8_t m_txPower;
|
||||
std::vector<std::string> m_serviceData;
|
||||
std::vector<BLEUUID> m_serviceDataUUIDs;
|
||||
uint8_t* m_payload;
|
||||
size_t m_payloadLength = 0;
|
||||
esp_ble_addr_type_t m_addressType;
|
||||
};
|
||||
#endif
|
||||
#endif
|
||||
+219
@@ -0,0 +1,219 @@
|
||||
#pragma once
|
||||
//
|
||||
// FILE: FastTrig.h
|
||||
// AUTHOR: Rob Tillaart
|
||||
// VERSION: 0.1.6
|
||||
// PURPOSE: Arduino library for a faster approximation of sin() and cos()
|
||||
// DATE: 2011-08-18
|
||||
// URL: https://github.com/RobTillaart/FastTrig
|
||||
// https://forum.arduino.cc/index.php?topic=69723.0
|
||||
//
|
||||
// HISTORY:
|
||||
// 0.1.00 2011-08-18 initial version
|
||||
// 0.1.01 2011-08-18 improved tables a bit + changed param to float
|
||||
// 0.1.02 2011-08-20 added interpolation
|
||||
// eons passed
|
||||
// 0.1.1 2020-08-30 refactor, create a library out of it.
|
||||
// itan() approximation is bad.
|
||||
// 0.1.2 2020-09-06 optimize 16 bit table with example sketch
|
||||
// 0.1.3 2020-09-07 initial release.
|
||||
// 0.1.4 2020-09-08 rewrite itan() + cleanup + examples
|
||||
// 0.1.5 2020-09-11 fixed optimize, new table, added iasin() and iacos()
|
||||
// 0.1.6 2020-12-23 arduino-CI + unit tests
|
||||
|
||||
#include "Arduino.h"
|
||||
#define Pi 3.14159265359
|
||||
// 91 x 2 bytes ==> 182 bytes
|
||||
// use 65535.0 as divider
|
||||
static uint16_t isinTable16[] = {
|
||||
0,
|
||||
1145, 2289, 3435, 4572, 5716, 6853, 7989, 9125, 10255, 11385,
|
||||
12508, 13631, 14745, 15859, 16963, 18067, 19165, 20253, 21342, 22417,
|
||||
23489, 24553, 25610, 26659, 27703, 28731, 29755, 30773, 31777, 32772,
|
||||
33756, 34734, 35697, 36649, 37594, 38523, 39445, 40350, 41247, 42131,
|
||||
42998, 43856, 44701, 45528, 46344, 47147, 47931, 48708, 49461, 50205,
|
||||
50933, 51646, 52342, 53022, 53686, 54334, 54969, 55579, 56180, 56760,
|
||||
57322, 57866, 58394, 58908, 59399, 59871, 60327, 60768, 61184, 61584,
|
||||
61969, 62330, 62677, 63000, 63304, 63593, 63858, 64108, 64334, 64545,
|
||||
64731, 64903, 65049, 65177, 65289, 65377, 65449, 65501, 65527, 65535,
|
||||
65535
|
||||
};
|
||||
|
||||
|
||||
/* 0.1.4 table
|
||||
uint16_t isinTable16[] = {
|
||||
0,
|
||||
1145, 2289, 3435, 4571, 5715, 6852, 7988, 9125, 10254, 11385,
|
||||
12508, 13630, 14745, 15859, 16963, 18067, 19165, 20253, 21342, 22416,
|
||||
23488, 24553, 25610, 26659, 27699, 28730, 29754, 30773, 31777, 32771,
|
||||
33755, 34734, 35697, 36649, 37594, 38523, 39445, 40350, 41247, 42127,
|
||||
42998, 43856, 44697, 45527, 46344, 47146, 47931, 48708, 49461, 50205,
|
||||
50933, 51645, 52341, 53022, 53686, 54333, 54969, 55578, 56180, 56759,
|
||||
57322, 57866, 58394, 58908, 59399, 59871, 60327, 60767, 61184, 61584,
|
||||
61969, 62330, 62677, 63000, 63304, 63592, 63857, 64108, 64333, 64544,
|
||||
64731, 64902, 65049, 65177, 65289, 65376, 65449, 65501, 65527, 65535,
|
||||
65535
|
||||
};
|
||||
*/
|
||||
|
||||
|
||||
// use 255.0 as divider
|
||||
static uint8_t isinTable8[] = {
|
||||
0, 4, 9, 13, 18, 22, 27, 31, 35, 40, 44,
|
||||
49, 53, 57, 62, 66, 70, 75, 79, 83, 87,
|
||||
91, 96, 100, 104, 108, 112, 116, 120, 124, 128,
|
||||
131, 135, 139, 143, 146, 150, 153, 157, 160, 164,
|
||||
167, 171, 174, 177, 180, 183, 186, 190, 192, 195,
|
||||
198, 201, 204, 206, 209, 211, 214, 216, 219, 221,
|
||||
223, 225, 227, 229, 231, 233, 235, 236, 238, 240,
|
||||
241, 243, 244, 245, 246, 247, 248, 249, 250, 251,
|
||||
252, 253, 253, 254, 254, 254, 255, 255, 255, 255,
|
||||
255
|
||||
};
|
||||
|
||||
///////////////////////////////////////////////////////
|
||||
//
|
||||
// GONIO LOOKUP
|
||||
//
|
||||
static float isin(float f)
|
||||
{
|
||||
boolean pos = true; // positive
|
||||
if (f < 0)
|
||||
{
|
||||
f = -f;
|
||||
pos = !pos;
|
||||
}
|
||||
|
||||
long x = f;
|
||||
uint8_t r = (f - x) * 256;
|
||||
|
||||
if (x >= 360) x %= 360;
|
||||
|
||||
int y = x; // 16 bit math is faster than 32 bit
|
||||
|
||||
if (y >= 180)
|
||||
{
|
||||
y -= 180;
|
||||
pos = !pos;
|
||||
}
|
||||
|
||||
if (y >= 90)
|
||||
{
|
||||
y = 180 - y;
|
||||
if (r != 0)
|
||||
{
|
||||
r = 255 - r;
|
||||
y--;
|
||||
}
|
||||
}
|
||||
|
||||
// float v improves ~4% on avg error for ~60 bytes.
|
||||
uint16_t v = isinTable16[y];
|
||||
|
||||
// interpolate if needed
|
||||
if (r > 0)
|
||||
{
|
||||
v = v + ((isinTable16[y + 1] - v) / 8 * r) /32; // == * r / 256
|
||||
}
|
||||
float g = v * 0.0000152590219; // = /65535.0
|
||||
if (pos) return g;
|
||||
return -g;
|
||||
}
|
||||
|
||||
static float icos(float x)
|
||||
{
|
||||
// prevent modulo math if x in 0..360
|
||||
return isin(x - 270.0); // better than x + 90;
|
||||
}
|
||||
|
||||
static float itan(float f)
|
||||
{
|
||||
// reference
|
||||
// return isin(f)/icos(f);
|
||||
|
||||
// idea is to divide two (interpolated) values from the table
|
||||
// so no divide by 65535
|
||||
|
||||
// FOLDING
|
||||
bool neg = (f < 0);
|
||||
bool mir = false;
|
||||
if (neg) f = -f;
|
||||
|
||||
long x = f;
|
||||
float rem = f - x;
|
||||
float v = x % 180 + rem; // normalised value 0..179.9999
|
||||
if (v > 90)
|
||||
{
|
||||
v = 180 - v;
|
||||
neg = !neg;
|
||||
mir = true;
|
||||
}
|
||||
uint8_t d = v;
|
||||
if (d == 90) return 0;
|
||||
|
||||
// COS FIRST
|
||||
uint8_t p = 90 - d;
|
||||
float co = isinTable16[p];
|
||||
if (rem != 0)
|
||||
{
|
||||
float delta = (isinTable16[p] - isinTable16[p - 1]);
|
||||
if (mir) co = isinTable16[p - 1] + rem * delta;
|
||||
else co = isinTable16[p] - rem * delta;
|
||||
}
|
||||
if (co == 0) return 0;
|
||||
|
||||
float si = isinTable16[d];
|
||||
if (rem != 0) si += rem * (isinTable16[d + 1] - isinTable16[d]);
|
||||
|
||||
float ta = si/co;
|
||||
if (neg) return -ta;
|
||||
return ta;
|
||||
}
|
||||
|
||||
|
||||
///////////////////////////////////////////////////////
|
||||
//
|
||||
// INVERSE GONIO LOOKUP
|
||||
//
|
||||
static float iasin(float f)
|
||||
{
|
||||
bool neg = (f < 0);
|
||||
if (neg)
|
||||
{
|
||||
f = -f;
|
||||
neg = true;
|
||||
}
|
||||
uint16_t val = round(f * 65535);
|
||||
uint8_t lo = 0;
|
||||
uint8_t hi = 90;
|
||||
|
||||
while (hi - lo > 1)
|
||||
{
|
||||
uint8_t mi = (lo + hi) / 2;
|
||||
if (isinTable16[mi] == val)
|
||||
{
|
||||
if (neg) return -mi;
|
||||
return mi;
|
||||
}
|
||||
if (isinTable16[mi] < val) lo = mi;
|
||||
else hi = mi;
|
||||
}
|
||||
float delta = val - isinTable16[lo];
|
||||
uint16_t range = isinTable16[hi] - isinTable16[lo];
|
||||
delta /= range;
|
||||
if (neg) return -(lo + delta);
|
||||
return (lo + delta);
|
||||
}
|
||||
|
||||
static float iacos(float f)
|
||||
{
|
||||
return 90 - iasin(f);
|
||||
}
|
||||
|
||||
// PLACEHOLDER
|
||||
static float iatan(float f)
|
||||
{
|
||||
return 0 * f;
|
||||
}
|
||||
|
||||
// -- END OF FILE --
|
||||
+167
@@ -0,0 +1,167 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include "Status.h"
|
||||
#include "config.h"
|
||||
#include "atcommon.h"
|
||||
#include "AP_Declination/AP_Declination.h"
|
||||
|
||||
class GPSStatus : public Status
|
||||
{
|
||||
|
||||
private:
|
||||
CallbackObserver<GPSStatus, const GPSStatus *> statusObserver = CallbackObserver<GPSStatus, const GPSStatus *>(this, &GPSStatus::updateStatus);
|
||||
|
||||
bool hasLock = false; // default to false, until we complete our first read
|
||||
bool isConnected = false; // Do we have a GPS we are talking to
|
||||
int32_t latitude = 0, longitude = 0; // as an int mult by 1e-7 to get value as double
|
||||
int32_t altitude = 0;
|
||||
uint32_t hacc = 0;
|
||||
uint32_t dop = 0; // Diminution of position; PDOP where possible (UBlox), HDOP otherwise (TinyGPS) in 10^2 units (needs scaling before use)
|
||||
uint32_t heading = 0;
|
||||
uint32_t numSatellites = 0;
|
||||
uint32_t sequence = 0;
|
||||
double x = 0;
|
||||
double y = 0;
|
||||
float magDec = 0;
|
||||
|
||||
public:
|
||||
GPSStatus()
|
||||
{
|
||||
statusType = STATUS_TYPE_GPS;
|
||||
}
|
||||
GPSStatus(bool hasLock, bool isConnected, int32_t latitude, int32_t longitude, int32_t altitude, uint32_t dop, uint32_t hacc, uint32_t heading, uint32_t numSatellites) : Status()
|
||||
{
|
||||
this->hasLock = hasLock;
|
||||
this->isConnected = isConnected;
|
||||
this->latitude = latitude;
|
||||
this->longitude = longitude;
|
||||
this->altitude = altitude;
|
||||
this->dop = dop;
|
||||
this->heading = heading;
|
||||
this->numSatellites = numSatellites;
|
||||
this->hacc = hacc;
|
||||
if(hasLock)
|
||||
{
|
||||
magDec = AP_Declination::get_declination(latitude*1e-7,longitude*1e-7);
|
||||
//DEBUG_MSG("D %f\n",magDec);
|
||||
}
|
||||
this->updateXY();
|
||||
}
|
||||
|
||||
GPSStatus(const GPSStatus &);
|
||||
GPSStatus &operator=(const GPSStatus &);
|
||||
|
||||
float getMagDec()
|
||||
{
|
||||
return magDec;
|
||||
}
|
||||
void updateXY()
|
||||
{
|
||||
convertDegreeToMeter(longitude*1e-07,latitude*1e-07,x,y);
|
||||
}
|
||||
|
||||
void observe(Observable<const GPSStatus *> *source)
|
||||
{
|
||||
statusObserver.observe(source);
|
||||
}
|
||||
|
||||
bool getHasLock() const
|
||||
{
|
||||
return hasLock;
|
||||
}
|
||||
|
||||
bool getIsConnected() const
|
||||
{
|
||||
return isConnected;
|
||||
}
|
||||
|
||||
int32_t getLatitude() const
|
||||
{
|
||||
return latitude;
|
||||
}
|
||||
|
||||
int32_t getLongitude() const
|
||||
{
|
||||
return longitude;
|
||||
}
|
||||
|
||||
int32_t getAltitude() const
|
||||
{
|
||||
return altitude;
|
||||
}
|
||||
|
||||
uint32_t getDOP() const
|
||||
{
|
||||
return dop;
|
||||
}
|
||||
|
||||
uint32_t getHAcc() const
|
||||
{
|
||||
return hacc;
|
||||
}
|
||||
|
||||
uint32_t getHeading() const
|
||||
{
|
||||
return heading;
|
||||
}
|
||||
|
||||
uint32_t getNumSatellites() const
|
||||
{
|
||||
return numSatellites;
|
||||
}
|
||||
|
||||
uint32_t getSequence() const
|
||||
{
|
||||
return sequence;
|
||||
}
|
||||
|
||||
double getX() const
|
||||
{
|
||||
return x;
|
||||
}
|
||||
|
||||
double getY() const
|
||||
{
|
||||
return y;
|
||||
}
|
||||
|
||||
bool matches(const GPSStatus *newStatus) const
|
||||
{
|
||||
return (
|
||||
newStatus->hasLock != hasLock ||
|
||||
newStatus->isConnected != isConnected ||
|
||||
newStatus->latitude != latitude ||
|
||||
newStatus->longitude != longitude ||
|
||||
newStatus->altitude != altitude ||
|
||||
newStatus->dop != dop ||
|
||||
newStatus->hacc != hacc ||
|
||||
newStatus->heading != heading ||
|
||||
newStatus->numSatellites != numSatellites);
|
||||
}
|
||||
int updateStatus(const GPSStatus *newStatus)
|
||||
{
|
||||
// Only update the status if values have actually changed
|
||||
bool isDirty;
|
||||
{
|
||||
isDirty = matches(newStatus);
|
||||
initialized = true;
|
||||
hasLock = newStatus->hasLock;
|
||||
isConnected = newStatus->isConnected;
|
||||
latitude = newStatus->latitude;
|
||||
longitude = newStatus->longitude;
|
||||
altitude = newStatus->altitude;
|
||||
dop = newStatus->dop;
|
||||
hacc = newStatus->hacc;
|
||||
heading = newStatus->heading;
|
||||
numSatellites = newStatus->numSatellites;
|
||||
}
|
||||
if (isDirty)
|
||||
{
|
||||
//DEBUG_MSG("New GPS pos lat=%f, lon=%f, alt=%d, pdop=%f, heading=%f, sats=%d\n", latitude * 1e-7, longitude * 1e-7, altitude, dop * 1e-2, heading * 1e-5, numSatellites);
|
||||
updateXY();
|
||||
sequence++;
|
||||
onNewStatus.notifyObservers(this);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,65 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include "Status.h"
|
||||
#include "config.h"
|
||||
|
||||
class IMUStatus : public Status
|
||||
{
|
||||
|
||||
private:
|
||||
CallbackObserver<IMUStatus, const IMUStatus *> statusObserver = CallbackObserver<IMUStatus, const IMUStatus *>(this, &IMUStatus::updateStatus);
|
||||
|
||||
bool isConnected = false; // Do we have a GPS we are talking to
|
||||
int32_t heading;
|
||||
|
||||
public:
|
||||
IMUStatus()
|
||||
{
|
||||
statusType = STATUS_TYPE_GPS;
|
||||
}
|
||||
IMUStatus(bool isConnected,int32_t heading) : Status()
|
||||
{
|
||||
this->isConnected = isConnected;
|
||||
this->heading = heading;
|
||||
}
|
||||
IMUStatus(const IMUStatus &);
|
||||
IMUStatus &operator=(const IMUStatus &);
|
||||
|
||||
void observe(Observable<const IMUStatus *> *source)
|
||||
{
|
||||
statusObserver.observe(source);
|
||||
}
|
||||
|
||||
bool getIsConnected() const
|
||||
{
|
||||
return isConnected;
|
||||
}
|
||||
|
||||
int32_t getHeading() const
|
||||
{
|
||||
return heading;
|
||||
}
|
||||
|
||||
bool matches(const IMUStatus *newStatus) const
|
||||
{
|
||||
return (
|
||||
newStatus->isConnected != isConnected ||
|
||||
newStatus->heading != heading);
|
||||
}
|
||||
int updateStatus(const IMUStatus *newStatus)
|
||||
{
|
||||
// Only update the status if values have actually changed
|
||||
bool isDirty;
|
||||
{
|
||||
isDirty = matches(newStatus);
|
||||
initialized = true;
|
||||
isConnected = newStatus->isConnected;
|
||||
heading = newStatus->heading;
|
||||
}
|
||||
if (isDirty)
|
||||
{
|
||||
onNewStatus.notifyObservers(this);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,322 @@
|
||||
/*
|
||||
MPU9250.h
|
||||
Brian R Taylor
|
||||
brian.taylor@bolderflight.com
|
||||
|
||||
Copyright (c) 2017 Bolder Flight Systems
|
||||
|
||||
This program is free software: you can redistribute it and/or modify
|
||||
it under the terms of the GNU General Public License as published by
|
||||
the Free Software Foundation, either version 3 of the License, or
|
||||
(at your option) any later version.
|
||||
|
||||
This program 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 General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU General Public License
|
||||
along with this program. If not, see <https://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#ifndef MPU9250_h
|
||||
#define MPU9250_h
|
||||
|
||||
#include "Arduino.h"
|
||||
#include "Wire.h" // I2C library
|
||||
#include "SPI.h" // SPI library
|
||||
|
||||
class MPU9250{
|
||||
public:
|
||||
enum GyroRange
|
||||
{
|
||||
GYRO_RANGE_250DPS,
|
||||
GYRO_RANGE_500DPS,
|
||||
GYRO_RANGE_1000DPS,
|
||||
GYRO_RANGE_2000DPS
|
||||
};
|
||||
enum AccelRange
|
||||
{
|
||||
ACCEL_RANGE_2G,
|
||||
ACCEL_RANGE_4G,
|
||||
ACCEL_RANGE_8G,
|
||||
ACCEL_RANGE_16G
|
||||
};
|
||||
enum DlpfBandwidth
|
||||
{
|
||||
DLPF_BANDWIDTH_184HZ,
|
||||
DLPF_BANDWIDTH_92HZ,
|
||||
DLPF_BANDWIDTH_41HZ,
|
||||
DLPF_BANDWIDTH_20HZ,
|
||||
DLPF_BANDWIDTH_10HZ,
|
||||
DLPF_BANDWIDTH_5HZ
|
||||
};
|
||||
enum LpAccelOdr
|
||||
{
|
||||
LP_ACCEL_ODR_0_24HZ = 0,
|
||||
LP_ACCEL_ODR_0_49HZ = 1,
|
||||
LP_ACCEL_ODR_0_98HZ = 2,
|
||||
LP_ACCEL_ODR_1_95HZ = 3,
|
||||
LP_ACCEL_ODR_3_91HZ = 4,
|
||||
LP_ACCEL_ODR_7_81HZ = 5,
|
||||
LP_ACCEL_ODR_15_63HZ = 6,
|
||||
LP_ACCEL_ODR_31_25HZ = 7,
|
||||
LP_ACCEL_ODR_62_50HZ = 8,
|
||||
LP_ACCEL_ODR_125HZ = 9,
|
||||
LP_ACCEL_ODR_250HZ = 10,
|
||||
LP_ACCEL_ODR_500HZ = 11
|
||||
};
|
||||
MPU9250(TwoWire &bus,uint8_t address);
|
||||
MPU9250(SPIClass &bus,uint8_t csPin);
|
||||
int begin();
|
||||
int setAccelRange(AccelRange range);
|
||||
int setGyroRange(GyroRange range);
|
||||
int setDlpfBandwidth(DlpfBandwidth bandwidth);
|
||||
int setSrd(uint8_t srd);
|
||||
int enableDataReadyInterrupt();
|
||||
int disableDataReadyInterrupt();
|
||||
int enableWakeOnMotion(float womThresh_mg,LpAccelOdr odr);
|
||||
int readSensor();
|
||||
float getAccelX_mss();
|
||||
float getAccelY_mss();
|
||||
float getAccelZ_mss();
|
||||
float getGyroX_rads();
|
||||
float getGyroY_rads();
|
||||
float getGyroZ_rads();
|
||||
float getMagX_uT();
|
||||
float getMagY_uT();
|
||||
float getMagZ_uT();
|
||||
float getTemperature_C();
|
||||
|
||||
int calibrateGyro();
|
||||
float getGyroBiasX_rads();
|
||||
float getGyroBiasY_rads();
|
||||
float getGyroBiasZ_rads();
|
||||
void setGyroBiasX_rads(float bias);
|
||||
void setGyroBiasY_rads(float bias);
|
||||
void setGyroBiasZ_rads(float bias);
|
||||
int calibrateAccel();
|
||||
float getAccelBiasX_mss();
|
||||
float getAccelScaleFactorX();
|
||||
float getAccelBiasY_mss();
|
||||
float getAccelScaleFactorY();
|
||||
float getAccelBiasZ_mss();
|
||||
float getAccelScaleFactorZ();
|
||||
void setAccelCalX(float bias,float scaleFactor);
|
||||
void setAccelCalY(float bias,float scaleFactor);
|
||||
void setAccelCalZ(float bias,float scaleFactor);
|
||||
int calibrateMag();
|
||||
float getMagBiasX_uT();
|
||||
float getMagScaleFactorX();
|
||||
float getMagBiasY_uT();
|
||||
float getMagScaleFactorY();
|
||||
float getMagBiasZ_uT();
|
||||
float getMagScaleFactorZ();
|
||||
void setMagCalX(float bias,float scaleFactor);
|
||||
void setMagCalY(float bias,float scaleFactor);
|
||||
void setMagCalZ(float bias,float scaleFactor);
|
||||
void getMagMinMax(float &minX, float &maxX, float &minY, float &maxY, float &minZ, float &maxZ);
|
||||
void setMagMinMax(float minX, float maxX, float minY, float maxY, float minZ, float maxZ);
|
||||
void recalibrateMag();
|
||||
void recalibrateMagByBias();
|
||||
void updateMagMinMax(float mx, float my, float mz);
|
||||
float getRawMagX_uT();
|
||||
float getRawMagY_uT();
|
||||
float getRawMagZ_uT();
|
||||
void setAutoMag(bool v) { _autoMag = v; }
|
||||
bool getAutoMag() { return _autoMag; }
|
||||
protected:
|
||||
bool _autoMag = false;
|
||||
// i2c
|
||||
uint8_t _address;
|
||||
TwoWire *_i2c;
|
||||
const uint32_t _i2cRate = 400000; // 400 kHz
|
||||
size_t _numBytes; // number of bytes received from I2C
|
||||
// spi
|
||||
SPIClass *_spi;
|
||||
uint8_t _csPin;
|
||||
bool _useSPI;
|
||||
bool _useSPIHS;
|
||||
const uint8_t SPI_READ = 0x80;
|
||||
const uint32_t SPI_LS_CLOCK = 1000000; // 1 MHz
|
||||
const uint32_t SPI_HS_CLOCK = 15000000; // 15 MHz
|
||||
// track success of interacting with sensor
|
||||
int _status;
|
||||
// buffer for reading from sensor
|
||||
uint8_t _buffer[21];
|
||||
// data counts
|
||||
int16_t _axcounts,_aycounts,_azcounts;
|
||||
int16_t _gxcounts,_gycounts,_gzcounts;
|
||||
int16_t _hxcounts,_hycounts,_hzcounts;
|
||||
int16_t _tcounts;
|
||||
// data buffer
|
||||
float _ax, _ay, _az;
|
||||
float _gx, _gy, _gz;
|
||||
float _hx, _hy, _hz;
|
||||
float _hrx, _hry, _hrz;
|
||||
float _hrax, _hray, _hraz;
|
||||
float _t;
|
||||
// wake on motion
|
||||
uint8_t _womThreshold;
|
||||
// scale factors
|
||||
float _accelScale;
|
||||
float _gyroScale;
|
||||
float _magScaleX, _magScaleY, _magScaleZ;
|
||||
const float _tempScale = 333.87f;
|
||||
const float _tempOffset = 21.0f;
|
||||
// configuration
|
||||
AccelRange _accelRange;
|
||||
GyroRange _gyroRange;
|
||||
DlpfBandwidth _bandwidth;
|
||||
uint8_t _srd;
|
||||
// gyro bias estimation
|
||||
size_t _numSamples = 100;
|
||||
double _gxbD, _gybD, _gzbD;
|
||||
float _gxb, _gyb, _gzb;
|
||||
// accel bias and scale factor estimation
|
||||
double _axbD, _aybD, _azbD;
|
||||
float _axmax, _aymax, _azmax;
|
||||
float _axmin, _aymin, _azmin;
|
||||
float _axb, _ayb, _azb;
|
||||
float _axs = 1.0f;
|
||||
float _ays = 1.0f;
|
||||
float _azs = 1.0f;
|
||||
// magnetometer bias and scale factor estimation
|
||||
uint16_t _maxCounts = 1000;
|
||||
float _deltaThresh = 0.3f;
|
||||
uint8_t _coeff = 8;
|
||||
uint16_t _counter;
|
||||
float _framedelta, _delta;
|
||||
float _hxfilt, _hyfilt, _hzfilt;
|
||||
float _hxmax, _hymax, _hzmax;
|
||||
float _hxmin, _hymin, _hzmin;
|
||||
float _hxb, _hyb, _hzb;
|
||||
float _hxs = 1.0f;
|
||||
float _hys = 1.0f;
|
||||
float _hzs = 1.0f;
|
||||
float _avgs;
|
||||
// transformation matrix
|
||||
/* transform the accel and gyro axes to match the magnetometer axes */
|
||||
const int16_t tX[3] = {0, 1, 0};
|
||||
const int16_t tY[3] = {1, 0, 0};
|
||||
const int16_t tZ[3] = {0, 0, -1};
|
||||
// constants
|
||||
const float G = 9.807f;
|
||||
const float _d2r = 3.14159265359f/180.0f;
|
||||
// MPU9250 registers
|
||||
const uint8_t ACCEL_OUT = 0x3B;
|
||||
const uint8_t GYRO_OUT = 0x43;
|
||||
const uint8_t TEMP_OUT = 0x41;
|
||||
const uint8_t EXT_SENS_DATA_00 = 0x49;
|
||||
const uint8_t ACCEL_CONFIG = 0x1C;
|
||||
const uint8_t ACCEL_FS_SEL_2G = 0x00;
|
||||
const uint8_t ACCEL_FS_SEL_4G = 0x08;
|
||||
const uint8_t ACCEL_FS_SEL_8G = 0x10;
|
||||
const uint8_t ACCEL_FS_SEL_16G = 0x18;
|
||||
const uint8_t GYRO_CONFIG = 0x1B;
|
||||
const uint8_t GYRO_FS_SEL_250DPS = 0x00;
|
||||
const uint8_t GYRO_FS_SEL_500DPS = 0x08;
|
||||
const uint8_t GYRO_FS_SEL_1000DPS = 0x10;
|
||||
const uint8_t GYRO_FS_SEL_2000DPS = 0x18;
|
||||
const uint8_t ACCEL_CONFIG2 = 0x1D;
|
||||
const uint8_t ACCEL_DLPF_184 = 0x01;
|
||||
const uint8_t ACCEL_DLPF_92 = 0x02;
|
||||
const uint8_t ACCEL_DLPF_41 = 0x03;
|
||||
const uint8_t ACCEL_DLPF_20 = 0x04;
|
||||
const uint8_t ACCEL_DLPF_10 = 0x05;
|
||||
const uint8_t ACCEL_DLPF_5 = 0x06;
|
||||
const uint8_t CONFIG = 0x1A;
|
||||
const uint8_t GYRO_DLPF_184 = 0x01;
|
||||
const uint8_t GYRO_DLPF_92 = 0x02;
|
||||
const uint8_t GYRO_DLPF_41 = 0x03;
|
||||
const uint8_t GYRO_DLPF_20 = 0x04;
|
||||
const uint8_t GYRO_DLPF_10 = 0x05;
|
||||
const uint8_t GYRO_DLPF_5 = 0x06;
|
||||
const uint8_t SMPDIV = 0x19;
|
||||
const uint8_t INT_PIN_CFG = 0x37;
|
||||
const uint8_t INT_ENABLE = 0x38;
|
||||
const uint8_t INT_DISABLE = 0x00;
|
||||
const uint8_t INT_PULSE_50US = 0x00;
|
||||
const uint8_t INT_WOM_EN = 0x40;
|
||||
const uint8_t INT_RAW_RDY_EN = 0x01;
|
||||
const uint8_t PWR_MGMNT_1 = 0x6B;
|
||||
const uint8_t PWR_CYCLE = 0x20;
|
||||
const uint8_t PWR_RESET = 0x80;
|
||||
const uint8_t CLOCK_SEL_PLL = 0x01;
|
||||
const uint8_t PWR_MGMNT_2 = 0x6C;
|
||||
const uint8_t SEN_ENABLE = 0x00;
|
||||
const uint8_t DIS_GYRO = 0x07;
|
||||
const uint8_t USER_CTRL = 0x6A;
|
||||
const uint8_t I2C_MST_EN = 0x20;
|
||||
const uint8_t I2C_MST_CLK = 0x0D;
|
||||
const uint8_t I2C_MST_CTRL = 0x24;
|
||||
const uint8_t I2C_SLV0_ADDR = 0x25;
|
||||
const uint8_t I2C_SLV0_REG = 0x26;
|
||||
const uint8_t I2C_SLV0_DO = 0x63;
|
||||
const uint8_t I2C_SLV0_CTRL = 0x27;
|
||||
const uint8_t I2C_SLV0_EN = 0x80;
|
||||
const uint8_t I2C_READ_FLAG = 0x80;
|
||||
const uint8_t MOT_DETECT_CTRL = 0x69;
|
||||
const uint8_t ACCEL_INTEL_EN = 0x80;
|
||||
const uint8_t ACCEL_INTEL_MODE = 0x40;
|
||||
const uint8_t LP_ACCEL_ODR = 0x1E;
|
||||
const uint8_t WOM_THR = 0x1F;
|
||||
const uint8_t WHO_AM_I = 0x75;
|
||||
const uint8_t FIFO_EN = 0x23;
|
||||
const uint8_t FIFO_TEMP = 0x80;
|
||||
const uint8_t FIFO_GYRO = 0x70;
|
||||
const uint8_t FIFO_ACCEL = 0x08;
|
||||
const uint8_t FIFO_MAG = 0x01;
|
||||
const uint8_t FIFO_COUNT = 0x72;
|
||||
const uint8_t FIFO_READ = 0x74;
|
||||
// AK8963 registers
|
||||
const uint8_t AK8963_I2C_ADDR = 0x0C;
|
||||
const uint8_t AK8963_HXL = 0x03;
|
||||
const uint8_t AK8963_CNTL1 = 0x0A;
|
||||
const uint8_t AK8963_PWR_DOWN = 0x00;
|
||||
const uint8_t AK8963_CNT_MEAS1 = 0x12;
|
||||
const uint8_t AK8963_CNT_MEAS2 = 0x16;
|
||||
const uint8_t AK8963_FUSE_ROM = 0x0F;
|
||||
const uint8_t AK8963_CNTL2 = 0x0B;
|
||||
const uint8_t AK8963_RESET = 0x01;
|
||||
const uint8_t AK8963_ASA = 0x10;
|
||||
const uint8_t AK8963_WHO_AM_I = 0x00;
|
||||
// private functions
|
||||
int writeRegister(uint8_t subAddress, uint8_t data);
|
||||
int readRegisters(uint8_t subAddress, uint8_t count, uint8_t* dest);
|
||||
int writeAK8963Register(uint8_t subAddress, uint8_t data);
|
||||
int readAK8963Registers(uint8_t subAddress, uint8_t count, uint8_t* dest);
|
||||
int whoAmI();
|
||||
int whoAmIAK8963();
|
||||
};
|
||||
|
||||
class MPU9250FIFO: public MPU9250 {
|
||||
public:
|
||||
using MPU9250::MPU9250;
|
||||
int enableFifo(bool accel,bool gyro,bool mag,bool temp);
|
||||
int readFifo();
|
||||
void getFifoAccelX_mss(size_t *size,float* data);
|
||||
void getFifoAccelY_mss(size_t *size,float* data);
|
||||
void getFifoAccelZ_mss(size_t *size,float* data);
|
||||
void getFifoGyroX_rads(size_t *size,float* data);
|
||||
void getFifoGyroY_rads(size_t *size,float* data);
|
||||
void getFifoGyroZ_rads(size_t *size,float* data);
|
||||
void getFifoMagX_uT(size_t *size,float* data);
|
||||
void getFifoMagY_uT(size_t *size,float* data);
|
||||
void getFifoMagZ_uT(size_t *size,float* data);
|
||||
void getFifoTemperature_C(size_t *size,float* data);
|
||||
protected:
|
||||
// fifo
|
||||
bool _enFifoAccel,_enFifoGyro,_enFifoMag,_enFifoTemp;
|
||||
size_t _fifoSize,_fifoFrameSize;
|
||||
float _axFifo[85], _ayFifo[85], _azFifo[85];
|
||||
size_t _aSize;
|
||||
float _gxFifo[85], _gyFifo[85], _gzFifo[85];
|
||||
size_t _gSize;
|
||||
float _hxFifo[73], _hyFifo[73], _hzFifo[73];
|
||||
size_t _hSize;
|
||||
float _tFifo[256];
|
||||
size_t _tSize;
|
||||
};
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,98 @@
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <assert.h>
|
||||
#include <list>
|
||||
|
||||
template <class T> class Observable;
|
||||
|
||||
/**
|
||||
* An observer which can be mixed in as a baseclass. Implement onNotify as a method in your class.
|
||||
*/
|
||||
template <class T> class Observer
|
||||
{
|
||||
Observable<T> *observed = NULL;
|
||||
|
||||
public:
|
||||
virtual ~Observer();
|
||||
|
||||
void observe(Observable<T> *o);
|
||||
|
||||
private:
|
||||
friend class Observable<T>;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* returns 0 if other observers should continue to be called
|
||||
* returns !0 if the observe calls should be aborted and this result code returned for notifyObservers
|
||||
**/
|
||||
virtual int onNotify(T arg) = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* An observer that calls an arbitrary method
|
||||
*/
|
||||
template <class Callback, class T> class CallbackObserver : public Observer<T>
|
||||
{
|
||||
typedef int (Callback::*ObserverCallback)(T arg);
|
||||
|
||||
Callback *objPtr;
|
||||
ObserverCallback method;
|
||||
|
||||
public:
|
||||
CallbackObserver(Callback *_objPtr, ObserverCallback _method) : objPtr(_objPtr), method(_method) {}
|
||||
|
||||
protected:
|
||||
virtual int onNotify(T arg) { return (objPtr->*method)(arg); }
|
||||
};
|
||||
|
||||
/**
|
||||
* An observable class that will notify observers anytime notifyObservers is called. Argument type T can be any type, but for
|
||||
* performance reasons a pointer or word sized object is recommended.
|
||||
*/
|
||||
template <class T> class Observable
|
||||
{
|
||||
std::list<Observer<T> *> observers;
|
||||
|
||||
public:
|
||||
/**
|
||||
* Tell all observers about a change, observers can process arg as they wish
|
||||
*
|
||||
* returns !0 if an observer chose to abort processing by returning this code
|
||||
*/
|
||||
int notifyObservers(T arg)
|
||||
{
|
||||
for (typename std::list<Observer<T> *>::const_iterator iterator = observers.begin(); iterator != observers.end();
|
||||
++iterator) {
|
||||
int result = (*iterator)->onNotify(arg);
|
||||
if (result != 0)
|
||||
return result;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
private:
|
||||
friend class Observer<T>;
|
||||
|
||||
// Not called directly, instead call observer.observe
|
||||
void addObserver(Observer<T> *o) { observers.push_back(o); }
|
||||
|
||||
void removeObserver(Observer<T> *o) { observers.remove(o); }
|
||||
};
|
||||
|
||||
template <class T> Observer<T>::~Observer()
|
||||
{
|
||||
if (observed)
|
||||
observed->removeObserver(this);
|
||||
observed = NULL;
|
||||
}
|
||||
|
||||
template <class T> void Observer<T>::observe(Observable<T> *o)
|
||||
{
|
||||
// We can only watch one thing at a time
|
||||
assert(!observed);
|
||||
|
||||
observed = o;
|
||||
o->addObserver(this);
|
||||
}
|
||||
+238
@@ -0,0 +1,238 @@
|
||||
#include "power.h"
|
||||
#include "utils.h"
|
||||
|
||||
#ifdef HAS_AXP20X
|
||||
bool pmu_irq = false;
|
||||
#endif
|
||||
bool Power::setup()
|
||||
{
|
||||
#ifdef HAS_AXP20X
|
||||
axp192Init();
|
||||
#endif
|
||||
concurrency::PeriodicTask::setup(); // We don't start our periodic task unless we actually found the device
|
||||
setPeriod(1);
|
||||
|
||||
return true;
|
||||
}
|
||||
#ifdef BATTERY_PIN
|
||||
static float read_battery() {
|
||||
uint16_t v = analogRead(BATTERY_PIN);
|
||||
//float battery_voltage = (float)v/4095*2*3.3*1.1;
|
||||
float battery_voltage = (float)v*0.00246*1.19;
|
||||
|
||||
return battery_voltage;
|
||||
}
|
||||
#endif
|
||||
|
||||
/// Reads power status to powerStatus singleton.
|
||||
//
|
||||
// TODO(girts): move this and other axp stuff to power.h/power.cpp.
|
||||
void Power::readPowerStatus()
|
||||
{
|
||||
#ifdef HAS_AXP20X
|
||||
bool hasBattery = axp.isBatteryConnect();
|
||||
int batteryVoltageMv = 0;
|
||||
uint8_t batteryChargePercent = 0;
|
||||
if (hasBattery)
|
||||
{
|
||||
batteryVoltageMv = axp.getBattVoltage();
|
||||
// If the AXP192 returns a valid battery percentage, use it
|
||||
if (axp.getBattPercentage() >= 0)
|
||||
{
|
||||
batteryChargePercent = axp.getBattPercentage();
|
||||
}
|
||||
else
|
||||
{
|
||||
// If the AXP192 returns a percentage less than 0, the feature is either not supported or there is an error
|
||||
// In that case, we compute an estimate of the charge percent based on maximum and minimum voltages defined in power.h
|
||||
batteryChargePercent = clamp((int)(((batteryVoltageMv - BAT_MILLIVOLTS_EMPTY) * 1e2) / (BAT_MILLIVOLTS_FULL - BAT_MILLIVOLTS_EMPTY)), 0, 100);
|
||||
}
|
||||
}
|
||||
|
||||
// Notify any status instances that are observing us
|
||||
const PowerStatus powerStatus = PowerStatus(hasBattery, axp.isVBUSPlug(), axp.isChargeing(), batteryVoltageMv, batteryChargePercent);
|
||||
newStatus.notifyObservers(&powerStatus);
|
||||
|
||||
// If we have a battery at all and it is less than 10% full, force deep sleep
|
||||
if (powerStatus.getHasBattery() && !powerStatus.getHasUSB() &&
|
||||
axp.getBattVoltage() < MIN_BAT_MILLIVOLTS)
|
||||
{
|
||||
//powerFSM.trigger(EVENT_LOW_BATTERY);
|
||||
}
|
||||
#else
|
||||
int batteryVoltageMv = read_battery()*1000;
|
||||
uint8_t batteryChargePercent = clamp((int)(((batteryVoltageMv - BAT_MILLIVOLTS_EMPTY) * 1e2) / (BAT_MILLIVOLTS_FULL - BAT_MILLIVOLTS_EMPTY)), 0, 100);
|
||||
const PowerStatus powerStatus = PowerStatus(true, false, batteryVoltageMv>4200, batteryVoltageMv, batteryChargePercent);
|
||||
newStatus.notifyObservers(&powerStatus);
|
||||
#endif
|
||||
}
|
||||
|
||||
void Power::doTask()
|
||||
{
|
||||
readPowerStatus();
|
||||
|
||||
// Only read once every 20 seconds once the power status for the app has been initialized
|
||||
if (statusHandler && statusHandler->isInitialized())
|
||||
setPeriod(1000 * 20);
|
||||
}
|
||||
|
||||
void Power::gpsOff()
|
||||
{
|
||||
#ifdef HAS_AXP20X
|
||||
axp.setPowerOutPut(AXP192_LDO3, AXP202_OFF);
|
||||
#endif
|
||||
}
|
||||
|
||||
void Power::shutdown()
|
||||
{
|
||||
#ifdef HAS_AXP20X
|
||||
axp.shutdown();
|
||||
#endif
|
||||
}
|
||||
|
||||
void Power::gpsOn()
|
||||
{
|
||||
#ifdef HAS_AXP20X
|
||||
axp.setPowerOutPut(AXP192_LDO3, AXP202_ON);
|
||||
#endif
|
||||
}
|
||||
|
||||
#ifdef AXP192_SLAVE_ADDRESS
|
||||
/**
|
||||
* Init the power manager chip
|
||||
*
|
||||
* axp192 power
|
||||
DCDC1 0.7-3.5V @ 1200mA max -> OLED // If you turn this off you'll lose comms to the axp192 because the OLED and the axp192
|
||||
share the same i2c bus, instead use ssd1306 sleep mode DCDC2 -> unused DCDC3 0.7-3.5V @ 700mA max -> ESP32 (keep this on!) LDO1
|
||||
30mA -> charges GPS backup battery // charges the tiny J13 battery by the GPS to power the GPS ram (for a couple of days), can
|
||||
not be turned off LDO2 200mA -> LORA LDO3 200mA -> GPS
|
||||
*/
|
||||
|
||||
#ifdef HAS_AXP20X
|
||||
void Power::axp192Init()
|
||||
{
|
||||
if (!axp.begin(Wire, AXP192_SLAVE_ADDRESS))
|
||||
{
|
||||
DEBUG_MSG("AXP192 Begin PASS\n");
|
||||
|
||||
// axp.setChgLEDMode(LED_BLINK_4HZ);
|
||||
axp.setChgLEDModeCharging();
|
||||
DEBUG_MSG("DCDC1: %s\n", axp.isDCDC1Enable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("DCDC2: %s\n", axp.isDCDC2Enable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("LDO2: %s\n", axp.isLDO2Enable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("LDO3: %s\n", axp.isLDO3Enable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("DCDC3: %s\n", axp.isDCDC3Enable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("Exten: %s\n", axp.isExtenEnable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("----------------------------------------\n");
|
||||
axp.setDCDC1Voltage(3300); // for the OLED power
|
||||
axp.setPowerOutPut(AXP192_LDO2, AXP202_ON); // LORA radio
|
||||
axp.setPowerOutPut(AXP192_LDO3, AXP202_ON); // GPS main power
|
||||
axp.setPowerOutPut(AXP192_DCDC2, AXP202_ON);
|
||||
axp.setPowerOutPut(AXP192_EXTEN, AXP202_ON);
|
||||
axp.setPowerOutPut(AXP192_DCDC1, AXP202_ON);
|
||||
axp.setDCDC1Voltage(3300); // for the OLED power
|
||||
|
||||
DEBUG_MSG("DCDC1: %s\n", axp.isDCDC1Enable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("DCDC2: %s\n", axp.isDCDC2Enable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("LDO2: %s\n", axp.isLDO2Enable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("LDO3: %s\n", axp.isLDO3Enable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("DCDC3: %s\n", axp.isDCDC3Enable() ? "ENABLE" : "DISABLE");
|
||||
DEBUG_MSG("Exten: %s\n", axp.isExtenEnable() ? "ENABLE" : "DISABLE");
|
||||
|
||||
axp.setChargingTargetVoltage(AXP202_TARGET_VOL_4_2V);
|
||||
axp.setChargeControlCur(AXP1XX_CHARGE_CUR_1320MA); // actual limit (in HW) on the tbeam is 450mA
|
||||
axp.setStartupTime(2);
|
||||
axp.setShutdownTime(0);
|
||||
axp.EnableCoulombcounter();
|
||||
#if 0
|
||||
|
||||
// Not connected
|
||||
//val = 0xfc;
|
||||
//axp._writeByte(AXP202_VHTF_CHGSET, 1, &val); // Set temperature protection
|
||||
|
||||
//not used
|
||||
//val = 0x46;
|
||||
//axp._writeByte(AXP202_OFF_CTL, 1, &val); // enable bat detection
|
||||
#endif
|
||||
axp.debugCharging();
|
||||
|
||||
#ifdef PMU_IRQ
|
||||
pinMode(PMU_IRQ, INPUT);
|
||||
attachInterrupt(
|
||||
PMU_IRQ, [] { pmu_irq = true; }, FALLING);
|
||||
|
||||
axp.adc1Enable(AXP202_BATT_CUR_ADC1, 1);
|
||||
axp.enableIRQ(AXP202_BATT_REMOVED_IRQ | AXP202_BATT_CONNECT_IRQ | AXP202_CHARGING_FINISHED_IRQ | AXP202_CHARGING_IRQ |
|
||||
AXP202_VBUS_REMOVED_IRQ | AXP202_VBUS_CONNECT_IRQ | AXP202_PEK_SHORTPRESS_IRQ | AXP202_PEK_LONGPRESS_IRQ,
|
||||
1);
|
||||
|
||||
axp.clearIRQ();
|
||||
#endif
|
||||
readPowerStatus();
|
||||
}
|
||||
else
|
||||
{
|
||||
DEBUG_MSG("AXP192 Begin FAIL\n");
|
||||
delay(1000);
|
||||
abort();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
|
||||
int Power::loop()
|
||||
{
|
||||
int result = 0;
|
||||
#ifdef HAS_AXP20X
|
||||
#ifdef PMU_IRQ
|
||||
if (pmu_irq)
|
||||
{
|
||||
pmu_irq = false;
|
||||
axp.readIRQ();
|
||||
|
||||
DEBUG_MSG("pmu irq!\n");
|
||||
|
||||
if (axp.isChargingIRQ())
|
||||
{
|
||||
DEBUG_MSG("Battery start charging\n");
|
||||
}
|
||||
if (axp.isChargingDoneIRQ())
|
||||
{
|
||||
DEBUG_MSG("Battery fully charged\n");
|
||||
}
|
||||
if (axp.isVbusRemoveIRQ())
|
||||
{
|
||||
DEBUG_MSG("USB unplugged\n");
|
||||
}
|
||||
if (axp.isVbusPlugInIRQ())
|
||||
{
|
||||
DEBUG_MSG("USB plugged In\n");
|
||||
}
|
||||
if (axp.isBattPlugInIRQ())
|
||||
{
|
||||
DEBUG_MSG("Battery inserted\n");
|
||||
}
|
||||
if (axp.isBattRemoveIRQ())
|
||||
{
|
||||
DEBUG_MSG("Battery removed\n");
|
||||
}
|
||||
if (axp.isPEKShortPressIRQ())
|
||||
{
|
||||
DEBUG_MSG("PEK short button press\n");
|
||||
result |= 1;
|
||||
}
|
||||
if (axp.isPEKLongtPressIRQ())
|
||||
{
|
||||
DEBUG_MSG("PEK long button press\n");
|
||||
result |= 2;
|
||||
}
|
||||
|
||||
readPowerStatus();
|
||||
axp.clearIRQ();
|
||||
}
|
||||
#endif
|
||||
#else
|
||||
// readPowerStatus();
|
||||
#endif
|
||||
return result;
|
||||
}
|
||||
@@ -0,0 +1,96 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include "Status.h"
|
||||
#include "config.h"
|
||||
|
||||
class PowerStatus : public Status
|
||||
{
|
||||
|
||||
private:
|
||||
CallbackObserver<PowerStatus, const PowerStatus *> statusObserver = CallbackObserver<PowerStatus, const PowerStatus *>(this, &PowerStatus::updateStatus);
|
||||
|
||||
/// Whether we have a battery connected
|
||||
bool hasBattery;
|
||||
/// Battery voltage in mV, valid if haveBattery is true
|
||||
int batteryVoltageMv;
|
||||
/// Battery charge percentage, either read directly or estimated
|
||||
uint8_t batteryChargePercent;
|
||||
/// Whether USB is connected
|
||||
bool hasUSB;
|
||||
/// Whether we are charging the battery
|
||||
bool isCharging;
|
||||
|
||||
public:
|
||||
PowerStatus()
|
||||
{
|
||||
statusType = STATUS_TYPE_POWER;
|
||||
}
|
||||
PowerStatus(bool hasBattery, bool hasUSB, bool isCharging, int batteryVoltageMv, uint8_t batteryChargePercent) : Status()
|
||||
{
|
||||
this->hasBattery = hasBattery;
|
||||
this->hasUSB = hasUSB;
|
||||
this->isCharging = isCharging;
|
||||
this->batteryVoltageMv = batteryVoltageMv;
|
||||
this->batteryChargePercent = batteryChargePercent;
|
||||
}
|
||||
PowerStatus(const PowerStatus &);
|
||||
PowerStatus &operator=(const PowerStatus &);
|
||||
|
||||
void observe(Observable<const PowerStatus *> *source)
|
||||
{
|
||||
statusObserver.observe(source);
|
||||
}
|
||||
|
||||
bool getHasBattery() const
|
||||
{
|
||||
return hasBattery;
|
||||
}
|
||||
|
||||
bool getHasUSB() const
|
||||
{
|
||||
return hasUSB;
|
||||
}
|
||||
|
||||
bool getIsCharging() const
|
||||
{
|
||||
return isCharging;
|
||||
}
|
||||
|
||||
int getBatteryVoltageMv() const
|
||||
{
|
||||
return batteryVoltageMv;
|
||||
}
|
||||
|
||||
uint8_t getBatteryChargePercent() const
|
||||
{
|
||||
return batteryChargePercent;
|
||||
}
|
||||
|
||||
bool matches(const PowerStatus *newStatus) const
|
||||
{
|
||||
return (
|
||||
newStatus->getHasBattery() != hasBattery ||
|
||||
newStatus->getHasUSB() != hasUSB ||
|
||||
newStatus->getBatteryVoltageMv() != batteryVoltageMv);
|
||||
}
|
||||
int updateStatus(const PowerStatus *newStatus)
|
||||
{
|
||||
// Only update the status if values have actually changed
|
||||
bool isDirty;
|
||||
{
|
||||
isDirty = matches(newStatus);
|
||||
initialized = true;
|
||||
hasBattery = newStatus->getHasBattery();
|
||||
batteryVoltageMv = newStatus->getBatteryVoltageMv();
|
||||
batteryChargePercent = newStatus->getBatteryChargePercent();
|
||||
hasUSB = newStatus->getHasUSB();
|
||||
isCharging = newStatus->getIsCharging();
|
||||
}
|
||||
if (isDirty)
|
||||
{
|
||||
DEBUG_MSG("Battery %dmV %d%%\n", batteryVoltageMv, batteryChargePercent);
|
||||
onNewStatus.notifyObservers(this);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,66 @@
|
||||
#pragma once
|
||||
|
||||
#include "Observer.h"
|
||||
|
||||
// Constants for the various status types, so we can tell subclass instances apart
|
||||
#define STATUS_TYPE_BASE 0
|
||||
#define STATUS_TYPE_POWER 1
|
||||
#define STATUS_TYPE_GPS 2
|
||||
|
||||
// A base class for observable status
|
||||
class Status
|
||||
{
|
||||
protected:
|
||||
// Allows us to observe an Observable
|
||||
CallbackObserver<Status, const Status *> statusObserver = CallbackObserver<Status, const Status *>(this, &Status::updateStatus);
|
||||
bool initialized = false;
|
||||
// Workaround for no typeid support
|
||||
int statusType;
|
||||
|
||||
public:
|
||||
// Allows us to generate observable events
|
||||
Observable<const Status *> onNewStatus;
|
||||
|
||||
// Enable polymorphism ?
|
||||
virtual ~Status() = default;
|
||||
|
||||
Status()
|
||||
{
|
||||
if (!statusType)
|
||||
{
|
||||
statusType = STATUS_TYPE_BASE;
|
||||
}
|
||||
}
|
||||
|
||||
// Prevent object copy/move
|
||||
Status(const Status &) = delete;
|
||||
Status &operator=(const Status &) = delete;
|
||||
|
||||
// Start observing a source of data
|
||||
void observe(Observable<const Status *> *source)
|
||||
{
|
||||
statusObserver.observe(source);
|
||||
}
|
||||
|
||||
// Determines whether or not existing data matches the data in another Status instance
|
||||
bool matches(const Status *otherStatus) const
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
bool isInitialized() const
|
||||
{
|
||||
return initialized;
|
||||
}
|
||||
|
||||
int getStatusType() const
|
||||
{
|
||||
return statusType;
|
||||
}
|
||||
|
||||
// Called when the Observable we're observing generates a new notification
|
||||
int updateStatus(const Status *newStatus)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,81 @@
|
||||
#pragma once
|
||||
|
||||
#include <cassert>
|
||||
#include <type_traits>
|
||||
|
||||
#include "freertosinc.h"
|
||||
|
||||
#ifdef HAS_FREE_RTOS
|
||||
|
||||
/**
|
||||
* A wrapper for freertos queues. Note: each element object should be small
|
||||
* and POD (Plain Old Data type) as elements are memcpied by value.
|
||||
*/
|
||||
template <class T> class TypedQueue
|
||||
{
|
||||
static_assert(std::is_pod<T>::value, "T must be pod");
|
||||
QueueHandle_t h;
|
||||
|
||||
public:
|
||||
TypedQueue(int maxElements)
|
||||
{
|
||||
h = xQueueCreate(maxElements, sizeof(T));
|
||||
assert(h);
|
||||
}
|
||||
|
||||
~TypedQueue() { vQueueDelete(h); }
|
||||
|
||||
int numFree() { return uxQueueSpacesAvailable(h); }
|
||||
|
||||
bool isEmpty() { return uxQueueMessagesWaiting(h) == 0; }
|
||||
|
||||
bool enqueue(T x, TickType_t maxWait = portMAX_DELAY) { return xQueueSendToBack(h, &x, maxWait) == pdTRUE; }
|
||||
|
||||
bool enqueueFromISR(T x, BaseType_t *higherPriWoken) { return xQueueSendToBackFromISR(h, &x, higherPriWoken) == pdTRUE; }
|
||||
|
||||
bool dequeue(T *p, TickType_t maxWait = portMAX_DELAY) { return xQueueReceive(h, p, maxWait) == pdTRUE; }
|
||||
|
||||
bool dequeueFromISR(T *p, BaseType_t *higherPriWoken) { return xQueueReceiveFromISR(h, p, higherPriWoken); }
|
||||
};
|
||||
|
||||
#else
|
||||
|
||||
#include <queue>
|
||||
|
||||
/**
|
||||
* A wrapper for freertos queues. Note: each element object should be small
|
||||
* and POD (Plain Old Data type) as elements are memcpied by value.
|
||||
*/
|
||||
template <class T> class TypedQueue
|
||||
{
|
||||
std::queue<T> q;
|
||||
|
||||
public:
|
||||
TypedQueue(int maxElements) {}
|
||||
|
||||
// int numFree() { return uxQueueSpacesAvailable(h); }
|
||||
|
||||
bool isEmpty() { return q.empty(); }
|
||||
|
||||
bool enqueue(T x, TickType_t maxWait = portMAX_DELAY)
|
||||
{
|
||||
q.push(x);
|
||||
return true;
|
||||
}
|
||||
|
||||
// bool enqueueFromISR(T x, BaseType_t *higherPriWoken) { return xQueueSendToBackFromISR(h, &x, higherPriWoken) == pdTRUE; }
|
||||
|
||||
bool dequeue(T *p, TickType_t maxWait = portMAX_DELAY)
|
||||
{
|
||||
if (isEmpty())
|
||||
return false;
|
||||
else {
|
||||
*p = q.front();
|
||||
q.pop();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// bool dequeueFromISR(T *p, BaseType_t *higherPriWoken) { return xQueueReceiveFromISR(h, p, higherPriWoken); }
|
||||
};
|
||||
#endif
|
||||
@@ -0,0 +1,22 @@
|
||||
#pragma once
|
||||
|
||||
#define SERVICE_UUID_ADC "e9f2ee69-ee2e-49d2-a763-458db734b546"
|
||||
#define CHARACTERISTIC_UUID_ADC_POSITION_REPORT "697f4ebc-4a7e-4782-b15e-31dbee61b3e2"
|
||||
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct sADCPositionReport
|
||||
{
|
||||
unsigned long now;
|
||||
float latitude;
|
||||
float longitude;
|
||||
float altitude;
|
||||
bool hasLock;
|
||||
int sat;
|
||||
unsigned long lastSeen;
|
||||
float SNR;
|
||||
float RSSI;
|
||||
char status[32];
|
||||
uint32_t hacc;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
@@ -0,0 +1,13 @@
|
||||
#pragma once
|
||||
|
||||
#define LORA_TAG 0x93
|
||||
#define LORA_SCREEN_TAG 0x94
|
||||
#define LSID_GPS 1
|
||||
#define LSID_HASLOCK 2
|
||||
#define LSID_NSATS 3
|
||||
#define LSID_TIMER1 4
|
||||
#define LSID_TIMER2 5
|
||||
#define LSID_SWITCH 6
|
||||
#define LSID_TIMERS_MODE 7
|
||||
#define LSID_HACC 8
|
||||
|
||||
@@ -0,0 +1,88 @@
|
||||
#include "atLVEvents.h"
|
||||
#include "config.h"
|
||||
#include "atscreen.h"
|
||||
|
||||
static uint32_t keycode_to_ascii(uint32_t key);
|
||||
|
||||
static uint32_t last_key = 0;
|
||||
static lv_indev_state_t state ;
|
||||
void atlve_init(void)
|
||||
{
|
||||
/*Nothing to init*/
|
||||
}
|
||||
|
||||
bool atlve_read(lv_indev_drv_t * indev_drv, lv_indev_data_t * data)
|
||||
{
|
||||
(void) indev_drv; /*Unused*/
|
||||
data->state = state;
|
||||
data->key = keycode_to_ascii(last_key);
|
||||
lv_indev_t * indev = (lv_indev_t *)indev_drv->user_data;
|
||||
if(indev)
|
||||
{
|
||||
if(!lv_group_get_editing(indev->group))
|
||||
{
|
||||
switch(data->key)
|
||||
{
|
||||
case LV_KEY_UP:
|
||||
{
|
||||
data->key = LV_KEY_LEFT;
|
||||
break;
|
||||
}
|
||||
case LV_KEY_DOWN:
|
||||
{
|
||||
data->key = LV_KEY_RIGHT;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* It is called periodically from the SDL thread to check a key is pressed/released
|
||||
* @param event describes the event
|
||||
*/
|
||||
void atlve_handler(uint8_t event, uint32_t param1, uint32_t param2)
|
||||
{
|
||||
/* We only care about SDL_KEYDOWN and SDL_KEYUP events */
|
||||
switch(event) {
|
||||
case EV_BTN_ON: /*Button press*/
|
||||
last_key = param1; /*Save the pressed key*/
|
||||
state = LV_INDEV_STATE_PR; /*Save the key is pressed now*/
|
||||
break;
|
||||
case EV_BTN_OFF: /*Button release*/
|
||||
last_key = param1; /*Save the pressed key*/
|
||||
state = LV_INDEV_STATE_REL; /*Save the key is released but keep the last key*/
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
static uint32_t keycode_to_ascii(uint32_t key)
|
||||
{
|
||||
switch(key) {
|
||||
case 0:
|
||||
return LV_KEY_UP;
|
||||
|
||||
case 1:
|
||||
return LV_KEY_DOWN;
|
||||
|
||||
case 2:
|
||||
return LV_KEY_ESC;
|
||||
|
||||
case 3:
|
||||
return LV_KEY_LEFT;
|
||||
|
||||
case 4:
|
||||
return LV_KEY_ENTER;
|
||||
|
||||
case 5:
|
||||
return LV_KEY_RIGHT;
|
||||
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
#ifndef ATLVE_H
|
||||
#define ATLVE_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef LV_LVGL_H_INCLUDE_SIMPLE
|
||||
#include "lvgl.h"
|
||||
#else
|
||||
#include "lvgl/lvgl.h"
|
||||
#endif
|
||||
|
||||
void atlve_init(void);
|
||||
bool atlve_read(lv_indev_drv_t * indev_drv, lv_indev_data_t * data);
|
||||
void atlve_handler(uint8_t event, uint32_t param1, uint32_t param2);
|
||||
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,189 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include "config.h"
|
||||
#include "atNetworkStructs.h"
|
||||
#include "atglobal.h"
|
||||
#include <vector>
|
||||
#include <map>
|
||||
|
||||
#pragma pack(push, 1)
|
||||
struct sPeerTimeslot
|
||||
{
|
||||
uint8_t timeslot:7;
|
||||
uint8_t inUse:1;
|
||||
int64_t lastTime;
|
||||
};
|
||||
#pragma pack(pop)
|
||||
|
||||
struct sNetworkPacket
|
||||
{
|
||||
int64_t packetTime;
|
||||
int64_t preambleTime;
|
||||
std::vector<byte> data;
|
||||
};
|
||||
|
||||
class cNetworkProcessorInterface
|
||||
{
|
||||
public:
|
||||
virtual void sendPacket(const sNetworkPacket &packet)=0;
|
||||
};
|
||||
#define INVALID_TIME_OFFSET 0xFFFFFFFF
|
||||
#define CK_SIZE 32
|
||||
#define MAX_NETWORK_PACKET_SIZE 64
|
||||
class cCryptoEngine
|
||||
{
|
||||
protected:
|
||||
uint8_t m_key[CK_SIZE];
|
||||
public:
|
||||
cCryptoEngine();
|
||||
void initWithKey(const char *key, uint8_t size, uint8_t id, uint8_t netId);
|
||||
void encode(uint8_t *data, uint16_t size);
|
||||
bool decode(uint8_t *data, uint16_t size);
|
||||
};
|
||||
|
||||
class cPeer:public sPOI
|
||||
{
|
||||
protected:
|
||||
uint8_t m_id;
|
||||
cCryptoEngine m_cryptoEngine;
|
||||
int64_t m_lastPacketTime;
|
||||
int64_t m_lastRequestInfo;
|
||||
int64_t m_lastRetransmit;
|
||||
sNpReportItem m_report;
|
||||
sPeerTimeslot m_timeslots[TIMESLOTS_PER_PEER_MAX];
|
||||
int64_t m_peerTimeOffset=INVALID_TIME_OFFSET;
|
||||
public:
|
||||
cPeer();
|
||||
void init(const char *key, uint8_t size, uint8_t id, uint8_t netId);
|
||||
const char * getName()
|
||||
{
|
||||
return name.c_str();
|
||||
}
|
||||
|
||||
char getSymbol()
|
||||
{
|
||||
return symbol;
|
||||
}
|
||||
|
||||
void updateInfo(const sNpInfoResponse *info)
|
||||
{
|
||||
name = info->name;
|
||||
symbol = info->symbol;
|
||||
}
|
||||
|
||||
void updateLastPacketTime()
|
||||
{
|
||||
lastUpdate = m_lastPacketTime = millis();
|
||||
}
|
||||
|
||||
void updateLastRetransmit()
|
||||
{
|
||||
m_lastRetransmit = millis();
|
||||
}
|
||||
|
||||
int64_t getLastRetransmit()
|
||||
{
|
||||
return m_lastRetransmit;
|
||||
}
|
||||
|
||||
int64_t getLastPacketTime()
|
||||
{
|
||||
return m_lastPacketTime;
|
||||
}
|
||||
|
||||
uint8_t getId() const
|
||||
{
|
||||
return m_id;
|
||||
}
|
||||
|
||||
sNpReportItem & getReport()
|
||||
{
|
||||
return m_report;
|
||||
}
|
||||
|
||||
void updateReport(const sNpReportItem * report);
|
||||
|
||||
cCryptoEngine & geCryptoEngine()
|
||||
{
|
||||
return m_cryptoEngine;
|
||||
}
|
||||
|
||||
bool needRequestInfo(int64_t now, int64_t interval);
|
||||
|
||||
void updateLastRequestInfo(int64_t now)
|
||||
{
|
||||
m_lastRequestInfo = now;
|
||||
}
|
||||
|
||||
void updateTimeslot(uint8_t timeslot, int64_t now);
|
||||
void updateTimeslot(int64_t packetTime, uint16_t timeslotDuration, uint8_t timeslot,uint8_t timeslotNext, int64_t now);
|
||||
uint16_t getTimeslotOffset(uint8_t timeslot, uint16_t timeslotDuration);
|
||||
sPeerTimeslot *getTimeslots() { return m_timeslots; }
|
||||
int64_t getPeerTimeOffset() { return m_peerTimeOffset; }
|
||||
};
|
||||
|
||||
typedef std::map<uint8_t,cPeer> cPeers;
|
||||
|
||||
class cNetworkProcessor
|
||||
{
|
||||
protected:
|
||||
atGlobal &m_global;
|
||||
cNetworkProcessorInterface *m_interface;
|
||||
cCryptoEngine m_cryptoEngine;
|
||||
int64_t m_lastReportTime;
|
||||
int64_t m_lastInfoTime;
|
||||
uint16_t m_reportInterval;
|
||||
uint8_t m_buffer[MAX_NETWORK_PACKET_SIZE];
|
||||
uint16_t m_bufferSize;
|
||||
uint16_t m_packetItemHeaderPosition;
|
||||
bool m_infoRequested;
|
||||
cPeers m_peers;
|
||||
uint8_t m_bufferCRC;
|
||||
uint16_t m_timeslotDuration=0;
|
||||
uint16_t m_maxPacketDuration=0;
|
||||
uint8_t m_timeslotsMap[TIMESLOTS_MAX];
|
||||
sPeerTimeslot m_timeslots[TIMESLOTS_PER_PEER_MAX];
|
||||
int64_t m_timeBase=0;
|
||||
int64_t m_lastRebuildTimeslots = 0;
|
||||
int64_t m_lastSendTime = 0;
|
||||
uint8_t m_lastNeedTimeslots=0;
|
||||
uint32_t m_retransmitCounter = 0;
|
||||
sNpReportItem *m_lastPeerReport = NULL;
|
||||
|
||||
void startPacket();
|
||||
void endPacket();
|
||||
void startPacketItem(sNetworkPackeItemHeader &itemHeader);
|
||||
void endPacketItem();
|
||||
bool putPacketData(void *data,uint16_t size);
|
||||
|
||||
bool needRequestInfo(int64_t now);
|
||||
sNpReportItem buildReportPacket();
|
||||
void buildRemoteReportPacket(const sNpReportItem &report, cPeer &peer);
|
||||
void buildInfoPacket();
|
||||
void buildRequestInfoPacket(int64_t now);
|
||||
void sendPacket(uint8_t timeslot);
|
||||
void processPacketItem(cPeer *peer, const sNetworkPackeItemHeader *header, const uint8_t *data, uint16_t size);
|
||||
uint8_t getActivePeersCount();
|
||||
uint8_t calcMaxTimeslotsToUse();
|
||||
uint8_t getTimeslotsCount();
|
||||
void rebuildTimeslotsMap();
|
||||
void rebuildTimeslots();
|
||||
bool isTimeslotExist(uint8_t timeslot);
|
||||
bool checkTimeslot(int64_t now,uint8_t timeslot);
|
||||
uint8_t findTimeslotAround(uint8_t targetTimeslot);
|
||||
uint8_t getCurrentTimeslot(int64_t now);
|
||||
uint8_t getTimeslotNext(uint8_t timeslot);
|
||||
public:
|
||||
cNetworkProcessor(atGlobal &global, cNetworkProcessorInterface *interface);
|
||||
void updateConfig();
|
||||
void processPacket(const sNetworkPacket &packet, float RSSI, float SNR);
|
||||
void loop();
|
||||
void sendReport(uint8_t timeslot);
|
||||
uint16_t getTimeslotDuration() { return m_timeslotDuration; }
|
||||
uint16_t getMaxPacketDuration() { return m_maxPacketDuration; }
|
||||
uint32_t getTimeOnAir(size_t len);
|
||||
cPeers &getPeers()
|
||||
{
|
||||
return m_peers;
|
||||
}
|
||||
};
|
||||
@@ -0,0 +1,192 @@
|
||||
#pragma once
|
||||
#include <stdint.h>
|
||||
|
||||
#pragma pack(push, 1)
|
||||
|
||||
#define NETWORK_NODE_ID_MAX 255
|
||||
#define TIMESLOTS_PERIOD 5000
|
||||
#define TIMESLOTS_MAX 64
|
||||
#define TIMESLOTS_PER_PEER_MAX 20
|
||||
#define TIMESLOT_MIN_DURATION (TIMESLOTS_PERIOD/TIMESLOTS_MAX)
|
||||
#define TIMESLOT_TIMEOUT 30000
|
||||
#define TIMESLOT_LEARNING_TIME (TIMESLOTS_PERIOD+100)
|
||||
#define TIMESLOT_MAX_SYMBOLS_LEN 40
|
||||
|
||||
//Packet types
|
||||
#define PT_REPORT 0
|
||||
#define PT_INFO_REQUEST 1
|
||||
#define PT_INFO_RESPONSE 2
|
||||
#define PT_REPORT_REMOTE 3
|
||||
#define PT_REPORT_REMOTE_D1 4
|
||||
|
||||
// 4 bytes
|
||||
struct sNetworkPacketHeader
|
||||
{
|
||||
uint8_t src;
|
||||
uint16_t netId:4;
|
||||
uint16_t timeslot:6;
|
||||
uint16_t timeslotNext:6;
|
||||
uint8_t crc;
|
||||
};
|
||||
|
||||
// 1 byte
|
||||
struct sNetworkPackeItemHeader
|
||||
{
|
||||
uint8_t type:3;
|
||||
uint8_t size:5;
|
||||
};
|
||||
|
||||
#define MAX_NETWORK_USER_NAME 16
|
||||
|
||||
struct sNpInfoRequest
|
||||
{
|
||||
uint8_t count; //count of sNpInfoRequestItem
|
||||
};
|
||||
|
||||
struct sNpInfoRequestItem
|
||||
{
|
||||
uint8_t id; // src id
|
||||
};
|
||||
|
||||
|
||||
struct sNpInfoResponse
|
||||
{
|
||||
char name[MAX_NETWORK_USER_NAME];
|
||||
char symbol;
|
||||
};
|
||||
|
||||
|
||||
// 12 bytes
|
||||
struct sNpReportItem
|
||||
{
|
||||
int32_t lat:28;
|
||||
int32_t lng:28;
|
||||
int32_t alt:15;
|
||||
uint32_t heading:9;
|
||||
uint32_t status:4;
|
||||
uint32_t options:4;
|
||||
uint8_t hacc;
|
||||
};
|
||||
|
||||
#define RID1SL 16
|
||||
#define RID1SA 8
|
||||
// 9 bytes
|
||||
struct sNpReportItemD1
|
||||
{
|
||||
int32_t lat:RID1SL;
|
||||
int32_t lng:RID1SL;
|
||||
int32_t alt:RID1SA;
|
||||
uint32_t heading:9;
|
||||
uint32_t status:4;
|
||||
uint32_t options:4;
|
||||
uint8_t hacc;
|
||||
};
|
||||
|
||||
// 6 bytes
|
||||
struct sNpReportRemoteHeader
|
||||
{
|
||||
uint8_t src;
|
||||
union
|
||||
{
|
||||
// 5 bytes
|
||||
struct
|
||||
{
|
||||
uint8_t flag:1; // 0 - d4, 1 - d3
|
||||
uint8_t t0:6;
|
||||
uint8_t reserved:1;
|
||||
uint8_t t1:4;
|
||||
uint8_t t2:4;
|
||||
uint8_t t3:4;
|
||||
uint8_t t4:4;
|
||||
uint8_t t5:4;
|
||||
uint8_t t6:4;
|
||||
uint8_t t7:4;
|
||||
uint8_t t8:4;
|
||||
void set(uint8_t idx,uint8_t value)
|
||||
{
|
||||
switch(idx)
|
||||
{
|
||||
case 1: t1 = value;break;
|
||||
case 2: t2 = value;break;
|
||||
case 3: t3 = value;break;
|
||||
case 4: t4 = value;break;
|
||||
case 5: t5 = value;break;
|
||||
case 6: t6 = value;break;
|
||||
case 7: t7 = value;break;
|
||||
case 8: t8 = value;break;
|
||||
}
|
||||
}
|
||||
uint8_t get(uint8_t idx) const
|
||||
{
|
||||
switch(idx)
|
||||
{
|
||||
case 1: return t1;
|
||||
case 2: return t2;
|
||||
case 3: return t3;
|
||||
case 4: return t4;
|
||||
case 5: return t5;
|
||||
case 6: return t6;
|
||||
case 7: return t7;
|
||||
case 8: return t8;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
uint8_t size() const { return 8; }
|
||||
}d4;
|
||||
struct
|
||||
{
|
||||
uint8_t flag:1;
|
||||
uint8_t t0:6;
|
||||
uint8_t t1:3;
|
||||
uint8_t t2:3;
|
||||
uint8_t t3:3;
|
||||
uint8_t t4:3;
|
||||
uint8_t t5:3;
|
||||
uint8_t t6:3;
|
||||
uint8_t t7:3;
|
||||
uint8_t t8:3;
|
||||
uint8_t t9:3;
|
||||
uint8_t t10:3;
|
||||
uint8_t t11:3;
|
||||
void set(uint8_t idx,uint8_t value)
|
||||
{
|
||||
switch(idx)
|
||||
{
|
||||
case 1: t1 = value;break;
|
||||
case 2: t2 = value;break;
|
||||
case 3: t3 = value;break;
|
||||
case 4: t4 = value;break;
|
||||
case 5: t5 = value;break;
|
||||
case 6: t6 = value;break;
|
||||
case 7: t7 = value;break;
|
||||
case 8: t8 = value;break;
|
||||
case 9: t9 = value;break;
|
||||
case 10: t10 = value;break;
|
||||
case 11: t11 = value;break;
|
||||
}
|
||||
}
|
||||
uint8_t get(uint8_t idx) const
|
||||
{
|
||||
switch(idx)
|
||||
{
|
||||
case 1: return t1;
|
||||
case 2: return t2;
|
||||
case 3: return t3;
|
||||
case 4: return t4;
|
||||
case 5: return t5;
|
||||
case 6: return t6;
|
||||
case 7: return t7;
|
||||
case 8: return t8;
|
||||
case 9: return t9;
|
||||
case 10: return t10;
|
||||
case 11: return t11;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
uint8_t size() const { return 11; }
|
||||
}d3;
|
||||
}ts;
|
||||
};
|
||||
|
||||
|
||||
#pragma pack(pop)
|
||||
@@ -0,0 +1,16 @@
|
||||
#include "atscreen.h"
|
||||
|
||||
cATScreen::cATScreen(atGlobal &global):m_global(global)
|
||||
{
|
||||
m_screenState = SS_BOOT;
|
||||
}
|
||||
|
||||
cATScreen::~cATScreen()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
bool cATScreen::init()
|
||||
{
|
||||
return true;
|
||||
}
|
||||
+1052
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,133 @@
|
||||
#pragma once
|
||||
#include <stdint.h>
|
||||
#include "atScreenLVGL.h"
|
||||
extern "C"
|
||||
{
|
||||
#include <lvgl_tft/EVE_commands.h>
|
||||
}
|
||||
|
||||
bool EVE_loadImage(const char* filename, uint32_t ptr, uint32_t options);
|
||||
bool EVE_inflateImage(const char* filename, uint32_t ptr);
|
||||
|
||||
struct sEVEImage
|
||||
{
|
||||
uint32_t ptr;
|
||||
uint16_t w;
|
||||
uint16_t h;
|
||||
uint16_t fmt;
|
||||
sEVEImage()
|
||||
{
|
||||
ptr = 0;
|
||||
w = h = fmt = 0;
|
||||
}
|
||||
|
||||
uint32_t load(const char * filename,uint32_t _ptr)
|
||||
{
|
||||
uint32_t result=0;
|
||||
uint32_t _w, _h;
|
||||
ptr = _ptr;
|
||||
EVE_loadImage(filename,_ptr,EVE_OPT_NODL);
|
||||
EVE_LIB_GetProps(&result,&_w,&_h);
|
||||
fmt = EVE_ARGB4;
|
||||
w = _w;
|
||||
h = _h;
|
||||
return result;
|
||||
}
|
||||
|
||||
void setImage()
|
||||
{
|
||||
EVE_cmd_setbitmap(ptr,fmt, w, h);
|
||||
}
|
||||
}__attribute__((packed));
|
||||
|
||||
struct sMapsMemoryItem
|
||||
{
|
||||
uint32_t address;
|
||||
uint32_t memoryaddress;
|
||||
uint32_t size;
|
||||
int64_t lastUsed;
|
||||
uint32_t usageCounter;
|
||||
uint8_t inUse:1;
|
||||
uint8_t inCurrentList:1;
|
||||
};
|
||||
|
||||
#define MAX_MAP_IMAGE_SIZE 59536
|
||||
#define MAPS_MEMORY_ITEMS 9
|
||||
#define MAPS_MEMORY_BASE (EVE_RAM_G_SIZE-(MAPS_MEMORY_ITEMS*MAX_MAP_IMAGE_SIZE))
|
||||
class cMapsMemory
|
||||
{
|
||||
protected:
|
||||
sMapsMemoryItem *m_memory;
|
||||
uint8_t m_count;
|
||||
int8_t findItem(const sMapTile &tile);
|
||||
public:
|
||||
cMapsMemory(uint8_t count);
|
||||
~cMapsMemory();
|
||||
|
||||
void clearCurrent();
|
||||
void markCurrent(const sMapTile &tile);
|
||||
bool allocItem(const sMapTile &tile,int64_t now, uint32_t &address, bool &needLoad);
|
||||
};
|
||||
|
||||
|
||||
struct sLabelsRect
|
||||
{
|
||||
int32_t x1;
|
||||
int32_t y1;
|
||||
int32_t x2;
|
||||
int32_t y2;
|
||||
|
||||
int32_t getArea()
|
||||
{
|
||||
return abs((x2-x1)*(y2-y1));
|
||||
}
|
||||
};
|
||||
|
||||
class cATScreenEVE:public cATScreenLVGL
|
||||
{
|
||||
protected:
|
||||
sEVEImage m_gpsImage;
|
||||
uint32_t m_pointer;
|
||||
uint8_t m_backlight;
|
||||
double m_heading;
|
||||
cMapsMemory m_mapsMemory;
|
||||
std::list<sLabelsRect> m_labels;
|
||||
double m_distanceC = 1.0;
|
||||
|
||||
void loadAllImages();
|
||||
void beginDraw();
|
||||
void endDraw();
|
||||
void drawBattery();
|
||||
void drawGPS();
|
||||
void drawDateTime();
|
||||
void drawInfo();
|
||||
int32_t getMaxLabelOverlap(sLabelsRect r);
|
||||
public:
|
||||
cATScreenEVE(atGlobal &global);
|
||||
virtual ~cATScreenEVE();
|
||||
|
||||
virtual bool init();
|
||||
virtual void showBootScreen(const char * msg1,const char * msg2);
|
||||
virtual void closeBootScreen();
|
||||
virtual void beginMainScreen();
|
||||
virtual void drawPOI(sPOI &poi,int16_t count=0);
|
||||
virtual void endMainScreen();
|
||||
virtual void setBacklight(uint8_t level);
|
||||
virtual void screenOn();
|
||||
virtual void screenOff();
|
||||
virtual void drawSideButtons();
|
||||
virtual void showProgramMode();
|
||||
virtual uint32_t getFramesCount();
|
||||
virtual void showMenu();
|
||||
virtual void loop();
|
||||
virtual void setScreenState(uint8_t value);
|
||||
virtual void showShutdownScreen();
|
||||
virtual void drawTimers(bool forceAll);
|
||||
virtual uint16_t getRadius();
|
||||
|
||||
|
||||
virtual uint32_t flashProgrammStart();
|
||||
virtual void flashProgrammEnd();
|
||||
virtual void flashWrite(uint32_t address,uint16_t size, uint8_t *data);
|
||||
virtual void flashRead(uint32_t address,uint16_t size, uint8_t *data);
|
||||
};
|
||||
@@ -0,0 +1,991 @@
|
||||
#include "atScreenLVGL.h"
|
||||
#include <lvgl.h>
|
||||
#include "atLVEvents.h"
|
||||
#include "lv_settings/lv_settings.h"
|
||||
#include "global.h"
|
||||
|
||||
#define LV_SIDE_WIDTH 40
|
||||
#define LV_TOP_HEIGHT 10
|
||||
|
||||
void *lvgl_malloc(size_t size)
|
||||
{
|
||||
return heap_caps_malloc(size,MALLOC_CAP_SPIRAM);
|
||||
}
|
||||
|
||||
void lvgl_free(void *ptr)
|
||||
{
|
||||
heap_caps_free(ptr);
|
||||
}
|
||||
|
||||
#pragma GCC diagnostic ignored "-Wwrite-strings"
|
||||
static void lvIScreenEventCb(struct _lv_obj_t * obj, lv_event_t event)
|
||||
{
|
||||
void *userData = lv_obj_get_user_data(obj);
|
||||
if(userData)
|
||||
((cLVGLScreen*)userData)->lvIScreenEvent(obj,event);
|
||||
}
|
||||
|
||||
void cLVGLScreen::setEventHandler(lv_obj_t *obj)
|
||||
{
|
||||
lv_obj_set_user_data(obj,this);
|
||||
lv_obj_set_event_cb(obj,lvIScreenEventCb);
|
||||
}
|
||||
|
||||
class cLVGLScreenMainMenu:public cLVGLScreen
|
||||
{
|
||||
protected:
|
||||
lv_settings_item_t m_rootItem = {.name = "Main Menu", .value = ""};
|
||||
|
||||
lv_settings_item_t m_mainMenuItems[2] =
|
||||
{
|
||||
{.id = 10,.type = LV_SETTINGS_TYPE_LIST_BTN, .name="Settings", .value=""},
|
||||
{.type = LV_SETTINGS_TYPE_INV}, /*Mark the last item*/
|
||||
};
|
||||
|
||||
lv_settings_item_t m_mainSettingsItems[5] =
|
||||
{
|
||||
{.id = 4,.type = LV_SETTINGS_TYPE_LIST_BTN, .name="Timers settings", .value="Timers durations"},
|
||||
{.id = 1,.type = LV_SETTINGS_TYPE_LIST_BTN, .name="User settings", .value="ID, name, brightness, etc."},
|
||||
{.id = 2,.type = LV_SETTINGS_TYPE_LIST_BTN, .name="Network settings", .value="Frequency, bandwidth etc."},
|
||||
{.id = 3,.type = LV_SETTINGS_TYPE_LIST_BTN, .name="IMU settings", .value="Calibration, Settings"},
|
||||
{.type = LV_SETTINGS_TYPE_INV}, /*Mark the last item*/
|
||||
};
|
||||
|
||||
char imAutoCal[16];
|
||||
lv_settings_item_t m_mainIMUItems[5] =
|
||||
{
|
||||
{.id = 1,.type = LV_SETTINGS_TYPE_LIST_BTN, .name="Mag Calibration", .value=""},
|
||||
{.id = 2,.type = LV_SETTINGS_TYPE_SW, .name="Mag Auto Calibration", .value=imAutoCal},
|
||||
{.id = 3,.type = LV_SETTINGS_TYPE_LIST_BTN, .name="Save Calibration", .value=""},
|
||||
{.id = 4,.type = LV_SETTINGS_TYPE_LIST_BTN, .name="Reset Calibration", .value=""},
|
||||
{.type = LV_SETTINGS_TYPE_INV}, /*Mark the last item*/
|
||||
};
|
||||
|
||||
char nmName[16];
|
||||
char nmUserId[4];
|
||||
char nmSymbol[2];
|
||||
lv_settings_item_t m_userMenuItems[4] =
|
||||
{
|
||||
{.id = 1,.type = LV_SETTINGS_TYPE_EDIT, .name="Name", .value=nmName, .param1=sizeof(nmName)},
|
||||
{.id = 2,.type = LV_SETTINGS_TYPE_EDIT, .name="Symbol", .value=nmSymbol, .param1=sizeof(nmSymbol)},
|
||||
{.id = 3,.type = LV_SETTINGS_TYPE_NUMSET, .name="ID", .value=nmUserId},
|
||||
{.type = LV_SETTINGS_TYPE_INV}, /*Mark the last item*/
|
||||
};
|
||||
|
||||
char nmFrequency[16];
|
||||
char nmNetworkId[4];
|
||||
char nmSF[4];
|
||||
char nmCR[4];
|
||||
char nmPreamble[4];
|
||||
char nmSync[6];
|
||||
char nmPassword[16];
|
||||
lv_settings_item_t m_networkMenuItems[10] =
|
||||
{
|
||||
{.id = 1,.type = LV_SETTINGS_TYPE_NUMSET, .name="Frequency", .value=nmFrequency},
|
||||
{.id = 2,.type = LV_SETTINGS_TYPE_DDLIST, .name="Bandwidth", .value="500Mhz\n250Mhz"},
|
||||
{.id = 3,.type = LV_SETTINGS_TYPE_NUMSET, .name="Spreading Factor", .value=nmSF},
|
||||
{.id = 4,.type = LV_SETTINGS_TYPE_NUMSET, .name="Coding Rate", .value=nmCR},
|
||||
{.id = 5,.type = LV_SETTINGS_TYPE_NUMSET, .name="Preamble", .value=nmPreamble},
|
||||
{.id = 6,.type = LV_SETTINGS_TYPE_NUMSET, .name="Sync Word", .value=nmSync},
|
||||
{.id = 10,.type = LV_SETTINGS_TYPE_NUMSET, .name="Network ID", .value=nmNetworkId},
|
||||
{.id = 11,.type = LV_SETTINGS_TYPE_EDIT, .name="Password", .value=nmPassword, .param1=sizeof(nmPassword),.param2=LV_SETTINGS_EDIT_PWD},
|
||||
{.id = 20,.type = LV_SETTINGS_TYPE_LIST_BTN, .name="Save", .value="Save network settings"},
|
||||
{.type = LV_SETTINGS_TYPE_INV}, /*Mark the last item*/
|
||||
};
|
||||
|
||||
char nmTimers[AT_MAX_TIMERS][10];
|
||||
lv_settings_item_t m_timersSettings[5] =
|
||||
{
|
||||
{.id = 0,.type = LV_SETTINGS_TYPE_NUMSET, .name="Timer 1", .value=nmTimers[0], .param1 = 10},
|
||||
{.id = 1,.type = LV_SETTINGS_TYPE_NUMSET, .name="Timer 2", .value=nmTimers[1], .param1 = 10},
|
||||
{.id = 2,.type = LV_SETTINGS_TYPE_NUMSET, .name="Timer 3", .value=nmTimers[2], .param1 = 10},
|
||||
{.id = 3,.type = LV_SETTINGS_TYPE_NUMSET, .name="Timer 4", .value=nmTimers[3], .param1 = 10},
|
||||
{.type = LV_SETTINGS_TYPE_INV}, /*Mark the last item*/
|
||||
};
|
||||
|
||||
lv_obj_t *m_root;
|
||||
bool m_isMainPage = true;
|
||||
virtual void internalInit();
|
||||
virtual void internalDeinit();
|
||||
void updateIMUSettings();
|
||||
void updateNetworkSettings();
|
||||
void updateTimersSettings();
|
||||
void updateUserSettings();
|
||||
public:
|
||||
cLVGLScreenMainMenu(cATScreenLVGL *owner):cLVGLScreen(owner){};
|
||||
void mainMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e);
|
||||
void settingsMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e);
|
||||
void networkMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e);
|
||||
void userMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e);
|
||||
void timersMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e);
|
||||
void imuMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e);
|
||||
virtual void back()
|
||||
{
|
||||
lv_settings_back();
|
||||
}
|
||||
virtual bool isMainPage() { return m_isMainPage; }
|
||||
};
|
||||
|
||||
struct lv_mcal_axis_s
|
||||
{
|
||||
lv_obj_t* page;
|
||||
lv_obj_t* label;
|
||||
lv_obj_t* btnP;
|
||||
lv_obj_t* btnM;
|
||||
};
|
||||
class cLVGLScreenIMUCalibration:public cLVGLScreen
|
||||
{
|
||||
protected:
|
||||
int64_t m_updateTime;
|
||||
lv_obj_t *m_root;
|
||||
lv_style_t text48Style;
|
||||
lv_style_t text16Style;
|
||||
lv_style_t text14Style;
|
||||
lv_mcal_axis_s m_axisX;
|
||||
lv_mcal_axis_s m_axisY;
|
||||
lv_mcal_axis_s m_axisZ;
|
||||
lv_obj_t* m_headingLabel;
|
||||
lv_obj_t* m_autoButton;
|
||||
virtual void internalInit();
|
||||
virtual void internalDeinit();
|
||||
lv_obj_t* lv_button_create(lv_obj_t* parent, const char* caption);
|
||||
lv_mcal_axis_s lv_mcal_axis_create(lv_obj_t* parent);
|
||||
void updateIMU();
|
||||
public:
|
||||
cLVGLScreenIMUCalibration(cATScreenLVGL *owner):cLVGLScreen(owner){};
|
||||
virtual void back()
|
||||
{
|
||||
m_owner->setLVGLScreen(LVS_MAINMENU);
|
||||
}
|
||||
virtual void loop();
|
||||
virtual bool isMainPage() { return false; }
|
||||
virtual void lvIScreenEvent(struct _lv_obj_t * obj, lv_event_t event);
|
||||
};
|
||||
|
||||
cATScreenLVGL::cATScreenLVGL(atGlobal &global):cATScreen(global)
|
||||
{
|
||||
m_lvState = LVS_NULL;
|
||||
m_screen = NULL;
|
||||
m_keypadGroup = NULL;
|
||||
memset(m_screens,0,sizeof(m_screens));
|
||||
}
|
||||
|
||||
cATScreenLVGL::~cATScreenLVGL()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
bool cATScreenLVGL::init()
|
||||
{
|
||||
if(cATScreen::init())
|
||||
{
|
||||
lv_init();
|
||||
lvgl_driver_init();
|
||||
|
||||
static lv_color_t m_buffer1[DISP_BUF_SIZE];
|
||||
uint32_t size_in_px = DISP_BUF_SIZE;
|
||||
lv_disp_buf_init(&m_display_buffer, m_buffer1, NULL, size_in_px);
|
||||
|
||||
lv_disp_drv_t disp_drv;
|
||||
lv_disp_drv_init(&disp_drv);
|
||||
disp_drv.flush_cb = disp_driver_flush;
|
||||
|
||||
disp_drv.buffer = &m_display_buffer;
|
||||
lv_disp_drv_register(&disp_drv);
|
||||
|
||||
m_keypadGroup = lv_group_create();
|
||||
|
||||
lv_indev_drv_t kb_drv;
|
||||
lv_indev_drv_init(&kb_drv);
|
||||
kb_drv.type = LV_INDEV_TYPE_ENCODER;
|
||||
kb_drv.read_cb = atlve_read;
|
||||
lv_indev_t * kb_indev = lv_indev_drv_register(&kb_drv);
|
||||
kb_indev->driver.user_data = kb_indev;
|
||||
lv_indev_set_group(kb_indev, m_keypadGroup);
|
||||
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void cATScreenLVGL::showMenu()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void cATScreenLVGL::loop()
|
||||
{
|
||||
lv_task_handler();
|
||||
if(m_screen)
|
||||
m_screen->loop();
|
||||
}
|
||||
|
||||
void cATScreenLVGL::setLVGLScreen(uint8_t value)
|
||||
{
|
||||
if(value!=m_lvState)
|
||||
{
|
||||
m_lvState = value;
|
||||
if(m_screen)
|
||||
{
|
||||
m_screen->deinit();
|
||||
m_screen = NULL;
|
||||
}
|
||||
}
|
||||
if(m_screen==NULL)
|
||||
{
|
||||
m_screen = createScreen(m_lvState);
|
||||
if(m_screen)
|
||||
m_screen->init();
|
||||
}
|
||||
}
|
||||
|
||||
void cATScreenLVGL::setScreenState(uint8_t value)
|
||||
{
|
||||
if(value!=m_screenState)
|
||||
{
|
||||
cATScreen::setScreenState(value);
|
||||
switch(m_screenState)
|
||||
{
|
||||
case SS_MENU:
|
||||
{
|
||||
setLVGLScreen(LVS_MAINMENU);
|
||||
break;
|
||||
}
|
||||
}
|
||||
m_global.onScreenStateChange();
|
||||
}
|
||||
}
|
||||
|
||||
cLVGLScreen * cATScreenLVGL::createScreen(uint8_t lvState)
|
||||
{
|
||||
cLVGLScreen *result = m_screens[lvState-1];
|
||||
if(result)
|
||||
return result;
|
||||
switch(lvState)
|
||||
{
|
||||
case LVS_MAINMENU:
|
||||
{
|
||||
result = new cLVGLScreenMainMenu(this);
|
||||
break;
|
||||
}
|
||||
|
||||
case LVS_IMUCAL:
|
||||
{
|
||||
result = new cLVGLScreenIMUCalibration(this);
|
||||
break;
|
||||
}
|
||||
}
|
||||
m_screens[lvState-1] = result;
|
||||
return result;
|
||||
}
|
||||
|
||||
void cATScreenLVGL::processEvent(uint8_t event, uint32_t param1, uint32_t param2)
|
||||
{
|
||||
switch(event)
|
||||
{
|
||||
case EV_BTN_ON:
|
||||
{
|
||||
if(param1==2)
|
||||
{
|
||||
if(m_lvState > LVS_NULL)
|
||||
{
|
||||
if(m_screen)
|
||||
{
|
||||
if(m_screen->isMainPage())
|
||||
setScreenState(SS_MAIN);
|
||||
else
|
||||
m_screen->back();
|
||||
}
|
||||
else
|
||||
setScreenState(SS_MAIN);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
};
|
||||
atlve_handler(event,param1,param2);
|
||||
}
|
||||
|
||||
cLVGLScreen::cLVGLScreen(cATScreenLVGL *owner):m_owner(owner)
|
||||
{
|
||||
m_initialized = false;
|
||||
m_scr = NULL;
|
||||
m_mainCont = NULL;
|
||||
m_leftCont = NULL;
|
||||
m_rightCont = NULL;
|
||||
}
|
||||
|
||||
cLVGLScreen::~cLVGLScreen()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void cLVGLScreen::createRoots()
|
||||
{
|
||||
lv_style_init(&m_styleCont);
|
||||
lv_style_set_radius(&m_styleCont,LV_STATE_DEFAULT,0);
|
||||
lv_style_set_border_width(&m_styleCont, LV_STATE_DEFAULT, 0);
|
||||
|
||||
m_scr = lv_obj_create(NULL, NULL);
|
||||
lv_scr_load(m_scr);
|
||||
m_leftCont = lv_cont_create(lv_scr_act(), NULL);
|
||||
lv_obj_add_style(m_leftCont,LV_CONT_PART_MAIN, &m_styleCont);
|
||||
lv_obj_set_size(m_leftCont, LV_SIDE_WIDTH, lv_obj_get_height(lv_scr_act()));
|
||||
lv_obj_set_pos(m_leftCont,0,0);
|
||||
|
||||
m_mainCont = lv_cont_create(lv_scr_act(), NULL);
|
||||
lv_obj_add_style(m_mainCont,LV_CONT_PART_MAIN, &m_styleCont);
|
||||
lv_obj_set_size(m_mainCont, lv_obj_get_width(lv_scr_act())-LV_SIDE_WIDTH*2, lv_obj_get_height(lv_scr_act())-LV_TOP_HEIGHT);
|
||||
lv_obj_set_pos(m_mainCont,LV_SIDE_WIDTH,LV_TOP_HEIGHT);
|
||||
|
||||
m_rightCont = lv_cont_create(lv_scr_act(), NULL);
|
||||
lv_obj_add_style(m_rightCont,LV_CONT_PART_MAIN, &m_styleCont);
|
||||
lv_obj_set_size(m_rightCont, LV_SIDE_WIDTH, lv_obj_get_height(lv_scr_act()));
|
||||
lv_obj_set_pos(m_rightCont,lv_obj_get_width(lv_scr_act())-LV_SIDE_WIDTH,0);
|
||||
}
|
||||
|
||||
void cLVGLScreen::init()
|
||||
{
|
||||
if(!m_initialized)
|
||||
{
|
||||
internalInit();
|
||||
m_initialized = true;
|
||||
}
|
||||
}
|
||||
|
||||
void cLVGLScreen::deinit()
|
||||
{
|
||||
if(m_initialized)
|
||||
{
|
||||
internalDeinit();
|
||||
m_initialized = false;
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////////
|
||||
static cLVGLScreenMainMenu *menuObject=NULL;
|
||||
|
||||
#define DEF_SETTINGS_CB(name) \
|
||||
static void name##Cb(lv_obj_t * obj, lv_event_t e) \
|
||||
{\
|
||||
lv_settings_item_t * act_item = (lv_settings_item_t *)lv_event_get_data();\
|
||||
if(menuObject)\
|
||||
menuObject->name(act_item,obj,e);\
|
||||
}\
|
||||
|
||||
DEF_SETTINGS_CB(mainMenuEvent)
|
||||
DEF_SETTINGS_CB(settingsMenuEvent)
|
||||
DEF_SETTINGS_CB(networkMenuEvent)
|
||||
DEF_SETTINGS_CB(userMenuEvent)
|
||||
DEF_SETTINGS_CB(timersMenuEvent)
|
||||
DEF_SETTINGS_CB(imuMenuEvent)
|
||||
|
||||
|
||||
void cLVGLScreenMainMenu::internalInit()
|
||||
{
|
||||
if(m_scr)
|
||||
{
|
||||
lv_scr_load(m_scr);
|
||||
if(!lv_settings_reopen_last())
|
||||
lv_settings_open_page(&m_rootItem, mainMenuEventCb);
|
||||
}
|
||||
else
|
||||
{
|
||||
menuObject = this;
|
||||
|
||||
lv_settings_set_group(m_owner->getKeypadGroup());
|
||||
|
||||
createRoots();
|
||||
m_root = lv_settings_create(&m_rootItem, NULL);
|
||||
lv_obj_set_hidden(m_root,true);
|
||||
lv_settings_set_parent(m_mainCont);
|
||||
lv_settings_set_max_width(lv_obj_get_width(lv_scr_act())-LV_SIDE_WIDTH*2);
|
||||
lv_settings_open_page(&m_rootItem, mainMenuEventCb);
|
||||
}
|
||||
}
|
||||
|
||||
void cLVGLScreenMainMenu::internalDeinit()
|
||||
{
|
||||
}
|
||||
|
||||
void cLVGLScreenMainMenu::mainMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e)
|
||||
{
|
||||
if(e == LV_EVENT_REFRESH)
|
||||
{
|
||||
m_isMainPage = true;
|
||||
for(uint32_t i = 0; m_mainMenuItems[i].type != LV_SETTINGS_TYPE_INV; i++) {
|
||||
lv_settings_add(&m_mainMenuItems[i]);
|
||||
}
|
||||
}
|
||||
else if(e == LV_EVENT_CLICKED) {
|
||||
lv_settings_item_t * act_item = (lv_settings_item_t *)lv_event_get_data();
|
||||
switch(act_item->id)
|
||||
{
|
||||
case 10: //Settings
|
||||
{
|
||||
lv_settings_open_page(act_item, settingsMenuEventCb);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void cLVGLScreenMainMenu::settingsMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e)
|
||||
{
|
||||
if(e == LV_EVENT_REFRESH)
|
||||
{
|
||||
global->updateTimers();
|
||||
m_isMainPage = false;
|
||||
for(uint32_t i = 0; m_mainSettingsItems[i].type != LV_SETTINGS_TYPE_INV; i++) {
|
||||
lv_settings_add(&m_mainSettingsItems[i]);
|
||||
}
|
||||
}
|
||||
else if(e == LV_EVENT_CLICKED) {
|
||||
lv_settings_item_t * act_item = (lv_settings_item_t *)lv_event_get_data();
|
||||
switch(act_item->id)
|
||||
{
|
||||
case 1: //User settings
|
||||
{
|
||||
lv_settings_open_page(act_item, userMenuEventCb);
|
||||
break;
|
||||
}
|
||||
case 2: //Network settings
|
||||
{
|
||||
lv_settings_open_page(act_item, networkMenuEventCb);
|
||||
break;
|
||||
}
|
||||
case 3: //IMU settings
|
||||
{
|
||||
lv_settings_open_page(act_item, imuMenuEventCb);
|
||||
break;
|
||||
}
|
||||
|
||||
case 4: //Timers settings
|
||||
{
|
||||
lv_settings_open_page(act_item, timersMenuEventCb);
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void cLVGLScreenMainMenu::updateIMUSettings()
|
||||
{
|
||||
snprintf(imAutoCal,sizeof(imAutoCal),"%s",global->getIMU()->getAutoMag()?"Enabled":"Disabled");
|
||||
m_mainIMUItems[1].state = global->getIMU()->getAutoMag()?1:0;
|
||||
}
|
||||
|
||||
void cLVGLScreenMainMenu::imuMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e)
|
||||
{
|
||||
if(e == LV_EVENT_REFRESH)
|
||||
{
|
||||
updateIMUSettings();
|
||||
m_isMainPage = false;
|
||||
for(uint32_t i = 0; m_mainIMUItems[i].type != LV_SETTINGS_TYPE_INV; i++) {
|
||||
lv_settings_add(&m_mainIMUItems[i]);
|
||||
}
|
||||
}
|
||||
else if(e == LV_EVENT_CLICKED) {
|
||||
lv_settings_item_t * act_item = (lv_settings_item_t *)lv_event_get_data();
|
||||
switch(act_item->id)
|
||||
{
|
||||
case 1: //IMU Calibration
|
||||
{
|
||||
m_owner->setLVGLScreen(LVS_IMUCAL);
|
||||
break;
|
||||
}
|
||||
case 3: //Save
|
||||
{
|
||||
global->getIMU()->saveCalibration();
|
||||
break;
|
||||
}
|
||||
|
||||
case 4: //Reset
|
||||
{
|
||||
global->getIMU()->loadCalibration();
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
else if(e == LV_EVENT_VALUE_CHANGED)
|
||||
{
|
||||
lv_settings_item_t * act_item = (lv_settings_item_t *)lv_event_get_data();
|
||||
switch(act_item->id)
|
||||
{
|
||||
case 2: //Auto
|
||||
{
|
||||
global->getIMU()->setAutoMag(act_item->state);
|
||||
updateIMUSettings();
|
||||
lv_settings_refr(act_item);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
void cLVGLScreenMainMenu::networkMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e)
|
||||
{
|
||||
if(e == LV_EVENT_REFRESH)
|
||||
{
|
||||
m_isMainPage = false;
|
||||
updateNetworkSettings();
|
||||
for(uint32_t i = 0; m_networkMenuItems[i].type != LV_SETTINGS_TYPE_INV; i++) {
|
||||
lv_settings_add(&m_networkMenuItems[i]);
|
||||
}
|
||||
}
|
||||
else if(e == LV_EVENT_CLICKED) {
|
||||
lv_settings_item_t * act_item = (lv_settings_item_t *)lv_event_get_data();
|
||||
switch(act_item->id)
|
||||
{
|
||||
case 20: //Save
|
||||
{
|
||||
global->getNetworkConfig().saveConfig(NET_CONFIG_FILENAME);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
else if(e == LV_EVENT_VALUE_CHANGED) {
|
||||
lv_settings_item_t * act_item = (lv_settings_item_t *)lv_event_get_data();
|
||||
switch(act_item->id)
|
||||
{
|
||||
case 1: //Frequency
|
||||
{
|
||||
if(act_item->state>9300)
|
||||
act_item->state=9300;
|
||||
else
|
||||
if(act_item->state<8800)
|
||||
act_item->state=8800;
|
||||
global->getNetworkConfig().channelConfig.freq = act_item->state/10.0;
|
||||
updateNetworkSettings();
|
||||
break;
|
||||
}
|
||||
case 2: //BW
|
||||
{
|
||||
switch(act_item->state)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
global->getNetworkConfig().channelConfig.bw = 500.0;
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
global->getNetworkConfig().channelConfig.bw = 250.0;
|
||||
break;
|
||||
}
|
||||
}
|
||||
updateNetworkSettings();
|
||||
break;
|
||||
}
|
||||
case 3: //SF
|
||||
{
|
||||
if(act_item->state>10)
|
||||
act_item->state=10;
|
||||
else
|
||||
if(act_item->state<7)
|
||||
act_item->state=7;
|
||||
global->getNetworkConfig().channelConfig.sf = act_item->state;
|
||||
updateNetworkSettings();
|
||||
break;
|
||||
}
|
||||
|
||||
case 4: //CR
|
||||
{
|
||||
if(act_item->state>8)
|
||||
act_item->state=8;
|
||||
else
|
||||
if(act_item->state<5)
|
||||
act_item->state=5;
|
||||
global->getNetworkConfig().channelConfig.cr = act_item->state;
|
||||
updateNetworkSettings();
|
||||
break;
|
||||
}
|
||||
|
||||
case 5: //Preamble
|
||||
{
|
||||
if(act_item->state>16)
|
||||
act_item->state=16;
|
||||
else
|
||||
if(act_item->state<4)
|
||||
act_item->state=4;
|
||||
global->getNetworkConfig().channelConfig.preambleLength = act_item->state;
|
||||
updateNetworkSettings();
|
||||
break;
|
||||
}
|
||||
|
||||
case 6: //Sync
|
||||
{
|
||||
if(act_item->state>255)
|
||||
act_item->state=255;
|
||||
else
|
||||
if(act_item->state<1)
|
||||
act_item->state=1;
|
||||
global->getNetworkConfig().channelConfig.syncWord = act_item->state;
|
||||
updateNetworkSettings();
|
||||
break;
|
||||
}
|
||||
|
||||
case 10: //ID
|
||||
{
|
||||
if(act_item->state<0)
|
||||
act_item->state=0;
|
||||
if(act_item->state>=128)
|
||||
act_item->state=127;
|
||||
global->getNetworkConfig().networkId = act_item->state;
|
||||
updateNetworkSettings();
|
||||
break;
|
||||
}
|
||||
|
||||
case 11: //Password
|
||||
{
|
||||
global->getNetworkConfig().password = act_item->value;
|
||||
updateNetworkSettings();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void cLVGLScreenMainMenu::userMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e)
|
||||
{
|
||||
if(e == LV_EVENT_REFRESH)
|
||||
{
|
||||
m_isMainPage = false;
|
||||
updateUserSettings();
|
||||
for(uint32_t i = 0; m_userMenuItems[i].type != LV_SETTINGS_TYPE_INV; i++) {
|
||||
lv_settings_add(&m_userMenuItems[i]);
|
||||
}
|
||||
}
|
||||
else if(e == LV_EVENT_VALUE_CHANGED) {
|
||||
lv_settings_item_t * act_item = (lv_settings_item_t *)lv_event_get_data();
|
||||
switch(act_item->id)
|
||||
{
|
||||
case 1: //Name
|
||||
{
|
||||
global->getConfig().name = act_item->value;
|
||||
updateUserSettings();
|
||||
break;
|
||||
}
|
||||
case 2: //Symbol
|
||||
{
|
||||
int len = strnlen(act_item->value,act_item->param1);
|
||||
if(len>0)
|
||||
{
|
||||
if(global->getConfig().symbol!=act_item->value[len-1])
|
||||
{
|
||||
global->getConfig().symbol = act_item->value[len-1];
|
||||
}
|
||||
else
|
||||
if(len==1)
|
||||
break;
|
||||
}
|
||||
updateUserSettings();
|
||||
lv_settings_refr(act_item);
|
||||
break;
|
||||
}
|
||||
case 3: //ID
|
||||
{
|
||||
if(act_item->state<1)
|
||||
act_item->state=1;
|
||||
if(act_item->state>=128)
|
||||
act_item->state=127;
|
||||
global->getConfig().id = act_item->state;
|
||||
updateUserSettings();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#define LVGL_HOFFSET 40
|
||||
#define LVGL_VOFFSET 14
|
||||
#define LVGL_HPAD 10
|
||||
#define LVGL_VPAD 6
|
||||
|
||||
void cLVGLScreenIMUCalibration::internalInit()
|
||||
{
|
||||
int16_t width = LV_HOR_RES_MAX - LVGL_HOFFSET * 2;
|
||||
int16_t height = LV_VER_RES_MAX - LVGL_VOFFSET * 2;
|
||||
|
||||
m_updateTime = millis();
|
||||
createRoots();
|
||||
lv_group_remove_all_objs(m_owner->getKeypadGroup());
|
||||
lv_obj_t* mainPage = lv_cont_create(lv_scr_act(), NULL);
|
||||
m_root = mainPage;
|
||||
lv_obj_set_pos(mainPage, LVGL_HOFFSET, LVGL_VOFFSET);
|
||||
lv_obj_set_size(mainPage, width,height);
|
||||
|
||||
width = lv_obj_get_width(mainPage)-LVGL_HPAD * 2;
|
||||
height = lv_obj_get_height(mainPage) - LVGL_VPAD * 2;
|
||||
|
||||
lv_style_init(&text48Style);
|
||||
lv_style_set_text_font(&text48Style, LV_STATE_DEFAULT, &lv_font_montserrat_48);
|
||||
|
||||
lv_style_init(&text16Style);
|
||||
lv_style_set_text_font(&text16Style, LV_STATE_DEFAULT, &lv_font_montserrat_16);
|
||||
|
||||
lv_style_init(&text14Style);
|
||||
lv_style_set_text_font(&text14Style, LV_STATE_DEFAULT, &lv_font_montserrat_14);
|
||||
|
||||
lv_obj_t* captionLabel = lv_label_create(mainPage, NULL);
|
||||
lv_label_set_text(captionLabel, "Magnetometer Calibration");
|
||||
lv_label_set_align(captionLabel, LV_LABEL_ALIGN_CENTER);
|
||||
lv_obj_add_style(captionLabel, 0, &text16Style);
|
||||
lv_obj_align(captionLabel, mainPage, LV_ALIGN_IN_TOP_MID,0, LVGL_VPAD);
|
||||
|
||||
m_axisX = lv_mcal_axis_create(mainPage);
|
||||
lv_obj_set_pos(m_axisX.page, 4, LVGL_VPAD + 20);
|
||||
|
||||
m_axisY = lv_mcal_axis_create(mainPage);
|
||||
lv_obj_set_pos(m_axisY.page, 80, LVGL_VPAD + 20);
|
||||
|
||||
m_axisZ = lv_mcal_axis_create(mainPage);
|
||||
lv_obj_set_pos(m_axisZ.page, 156, LVGL_VPAD + 20);
|
||||
|
||||
// lv_obj_t* s90Button = lv_button_create(mainPage, "90°");
|
||||
// lv_obj_set_pos(s90Button, 20, 100);
|
||||
// lv_obj_set_size(s90Button, 60, 26);
|
||||
|
||||
m_autoButton = lv_button_create(mainPage, "Auto");
|
||||
lv_btn_set_checkable(m_autoButton, true);
|
||||
lv_obj_set_pos(m_autoButton, 90, 100);
|
||||
lv_obj_set_size(m_autoButton, 60, 26);
|
||||
setEventHandler(m_autoButton);
|
||||
|
||||
// lv_obj_t* s0Button = lv_button_create(mainPage, "0°");
|
||||
// lv_obj_set_pos(s0Button, 160, 100);
|
||||
// lv_obj_set_size(s0Button, 60, 26);
|
||||
|
||||
lv_obj_t* o = lv_cont_create(mainPage, NULL);
|
||||
lv_obj_set_pos(o, LVGL_HPAD, 130);
|
||||
lv_obj_set_size(o, width, 76);
|
||||
|
||||
m_headingLabel = lv_label_create(o, NULL);
|
||||
lv_label_set_text(m_headingLabel, "000°");
|
||||
lv_obj_add_style(m_headingLabel, 0, &text48Style);
|
||||
lv_obj_align(m_headingLabel, o, LV_ALIGN_CENTER, 0, 0);
|
||||
updateIMU();
|
||||
}
|
||||
|
||||
void cLVGLScreenIMUCalibration::internalDeinit()
|
||||
{
|
||||
lv_obj_del(m_scr);
|
||||
m_scr = NULL;
|
||||
}
|
||||
|
||||
lv_obj_t* cLVGLScreenIMUCalibration::lv_button_create(lv_obj_t* parent, const char* caption)
|
||||
{
|
||||
lv_obj_t* result = lv_btn_create(parent, NULL);
|
||||
lv_btn_set_fit(result, LV_FIT_NONE);
|
||||
lv_obj_t* label = lv_label_create(result, NULL);
|
||||
lv_label_set_text(label, caption);
|
||||
lv_obj_add_style(label, 0, &text14Style);
|
||||
lv_obj_align(label, result, LV_ALIGN_CENTER, 0, 0);
|
||||
lv_group_add_obj(m_owner->getKeypadGroup(),result);
|
||||
|
||||
return result;
|
||||
}
|
||||
lv_mcal_axis_s cLVGLScreenIMUCalibration::lv_mcal_axis_create(lv_obj_t* parent)
|
||||
{
|
||||
lv_obj_t* axisPage = lv_cont_create(parent, NULL);
|
||||
lv_obj_set_size(axisPage, 80, 65);
|
||||
|
||||
lv_obj_t* axislabel = lv_label_create(axisPage, NULL);
|
||||
lv_label_set_text(axislabel, "-00.00");
|
||||
lv_obj_align(axislabel, axisPage, LV_ALIGN_IN_TOP_MID, 0, 5);
|
||||
|
||||
lv_obj_t* axisPBtn = lv_btn_create(axisPage, NULL);
|
||||
lv_obj_set_pos(axisPBtn, 6, 30);
|
||||
lv_obj_set_size(axisPBtn, 30, 26);
|
||||
lv_btn_set_fit(axisPBtn, LV_FIT_NONE);
|
||||
setEventHandler(axisPBtn);
|
||||
lv_obj_t* label = lv_label_create(axisPBtn, NULL);
|
||||
lv_label_set_text(label, "+");
|
||||
lv_obj_add_style(label, 0, &text14Style);
|
||||
lv_obj_align(label, axisPBtn, LV_ALIGN_CENTER, 0, 0);
|
||||
lv_group_add_obj(m_owner->getKeypadGroup(),axisPBtn);
|
||||
|
||||
lv_obj_t* axisMBtn = lv_btn_create(axisPage, NULL);
|
||||
lv_obj_set_pos(axisMBtn, 42, 30);
|
||||
lv_obj_set_size(axisMBtn, 30, 26);
|
||||
lv_btn_set_fit(axisMBtn, LV_FIT_NONE);
|
||||
lv_obj_set_user_data(axisMBtn,this);
|
||||
setEventHandler(axisMBtn);
|
||||
label = lv_label_create(axisMBtn, NULL);
|
||||
lv_label_set_text(label, "-");
|
||||
lv_obj_add_style(label, 0, &text14Style);
|
||||
lv_obj_align(label, axisMBtn, LV_ALIGN_CENTER, 0, 0);
|
||||
lv_group_add_obj(m_owner->getKeypadGroup(),axisMBtn);
|
||||
|
||||
lv_mcal_axis_s result;
|
||||
result.page = axisPage;
|
||||
result.label = axislabel;
|
||||
result.btnP = axisPBtn;
|
||||
result.btnM = axisMBtn;
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
void cLVGLScreenIMUCalibration::updateIMU()
|
||||
{
|
||||
if(global->m_imuStatus)
|
||||
{
|
||||
char buffer[32];
|
||||
snprintf(buffer,sizeof(buffer),"%d°",(int)(global->m_imuStatus->getHeading()/100+global->m_gpsStatus->getMagDec()));
|
||||
lv_label_set_text(m_headingLabel,buffer);
|
||||
lv_obj_align(m_headingLabel, lv_obj_get_parent(m_headingLabel), LV_ALIGN_CENTER, 0, 0);
|
||||
|
||||
global->getIMU()->updateCalibration();
|
||||
sIMUCalibration &calibration = global->getIMU()->getCalibration();
|
||||
snprintf(buffer,sizeof(buffer),"X: %0.2f",calibration.magBiasX);
|
||||
lv_label_set_text(m_axisX.label,buffer);
|
||||
lv_obj_align(m_axisX.label, lv_obj_get_parent(m_axisX.label), LV_ALIGN_IN_TOP_MID, 0, 5);
|
||||
|
||||
snprintf(buffer,sizeof(buffer),"Y: %0.2f",calibration.magBiasY);
|
||||
lv_label_set_text(m_axisY.label,buffer);
|
||||
lv_obj_align(m_axisY.label, lv_obj_get_parent(m_axisY.label), LV_ALIGN_IN_TOP_MID, 0, 5);
|
||||
|
||||
snprintf(buffer,sizeof(buffer),"Z: %0.2f",calibration.magBiasZ);
|
||||
lv_label_set_text(m_axisZ.label,buffer);
|
||||
lv_obj_align(m_axisZ.label, lv_obj_get_parent(m_axisZ.label), LV_ALIGN_IN_TOP_MID, 0, 5);
|
||||
|
||||
if(global->getIMU()->getAutoMag())
|
||||
lv_obj_add_state(m_autoButton, LV_STATE_CHECKED);
|
||||
else
|
||||
lv_obj_clear_state(m_autoButton, LV_STATE_CHECKED);
|
||||
}
|
||||
}
|
||||
|
||||
void cLVGLScreenIMUCalibration::loop()
|
||||
{
|
||||
if((millis()-m_updateTime)>=150)
|
||||
{
|
||||
updateIMU();
|
||||
m_updateTime = millis();
|
||||
}
|
||||
}
|
||||
|
||||
#define MAG_ADJ_VAL 0.1
|
||||
|
||||
void cLVGLScreenIMUCalibration::lvIScreenEvent(struct _lv_obj_t * obj, lv_event_t event)
|
||||
{
|
||||
switch(event)
|
||||
{
|
||||
case LV_EVENT_CLICKED:
|
||||
{
|
||||
if(obj==m_autoButton)
|
||||
{
|
||||
global->getIMU()->setAutoMag(lv_obj_get_state(m_autoButton,LV_OBJ_PART_MAIN) & LV_STATE_CHECKED);
|
||||
}
|
||||
else
|
||||
if(obj==m_axisX.btnM)
|
||||
{
|
||||
global->getIMU()->adjustMagCalibrationBias(-MAG_ADJ_VAL,0);
|
||||
}
|
||||
else
|
||||
if(obj==m_axisX.btnP)
|
||||
{
|
||||
global->getIMU()->adjustMagCalibrationBias(MAG_ADJ_VAL,0);
|
||||
}
|
||||
else
|
||||
if(obj==m_axisY.btnM)
|
||||
{
|
||||
global->getIMU()->adjustMagCalibrationBias(-MAG_ADJ_VAL,1);
|
||||
}
|
||||
else
|
||||
if(obj==m_axisY.btnP)
|
||||
{
|
||||
global->getIMU()->adjustMagCalibrationBias(MAG_ADJ_VAL,1);
|
||||
}
|
||||
else
|
||||
if(obj==m_axisZ.btnM)
|
||||
{
|
||||
global->getIMU()->adjustMagCalibrationBias(-MAG_ADJ_VAL,2);
|
||||
}
|
||||
else
|
||||
if(obj==m_axisZ.btnP)
|
||||
{
|
||||
global->getIMU()->adjustMagCalibrationBias(MAG_ADJ_VAL,2);
|
||||
}
|
||||
updateIMU();
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void cLVGLScreenMainMenu::updateTimersSettings()
|
||||
{
|
||||
atMainConfig &c = global->getConfig();
|
||||
for(int i=0; i<AT_MAX_TIMERS; i++)
|
||||
{
|
||||
m_timersSettings[i].state = c.timers[i]/1000;
|
||||
snprintf(nmTimers[i],sizeof(nmTimers[i]),"%d",c.timers[i]/1000);
|
||||
}
|
||||
}
|
||||
|
||||
void cLVGLScreenMainMenu::updateNetworkSettings()
|
||||
{
|
||||
atNetworkConfig &nc = global->getNetworkConfig();
|
||||
m_networkMenuItems[0].state = nc.channelConfig.freq*10;
|
||||
snprintf(nmFrequency,sizeof(nmFrequency),"%03.1f Mhz",m_networkMenuItems[0].state/10.0);
|
||||
m_networkMenuItems[1].state = 0;
|
||||
if(nc.channelConfig.bw==250)
|
||||
m_networkMenuItems[1].state = 1;
|
||||
|
||||
m_networkMenuItems[2].state = nc.channelConfig.sf;
|
||||
snprintf(nmSF,sizeof(nmSF),"%d",nc.channelConfig.sf);
|
||||
|
||||
m_networkMenuItems[3].state = nc.channelConfig.cr;
|
||||
snprintf(nmCR,sizeof(nmCR),"%d",nc.channelConfig.cr);
|
||||
|
||||
m_networkMenuItems[4].state = nc.channelConfig.preambleLength;
|
||||
snprintf(nmPreamble,sizeof(nmPreamble),"%d",nc.channelConfig.preambleLength);
|
||||
|
||||
m_networkMenuItems[5].state = nc.channelConfig.syncWord;
|
||||
snprintf(nmSync,sizeof(nmSync),"0x%0X",nc.channelConfig.syncWord);
|
||||
|
||||
m_networkMenuItems[6].state = nc.networkId;
|
||||
snprintf(nmNetworkId,sizeof(nmNetworkId),"%d",nc.networkId);
|
||||
|
||||
snprintf(nmPassword,sizeof(nmPassword),"%s",nc.password.c_str());
|
||||
}
|
||||
|
||||
|
||||
void cLVGLScreenMainMenu::updateUserSettings()
|
||||
{
|
||||
atMainConfig &c = global->getConfig();
|
||||
snprintf(nmName,sizeof(nmName),"%s",c.name.c_str());
|
||||
snprintf(nmSymbol,sizeof(nmSymbol),"%c",c.symbol);
|
||||
m_userMenuItems[1].state = c.id;
|
||||
snprintf(nmUserId,sizeof(nmUserId),"%d",c.id);
|
||||
}
|
||||
|
||||
void cLVGLScreenMainMenu::timersMenuEvent(lv_settings_item_t * act_item, lv_obj_t * obj, lv_event_t e)
|
||||
{
|
||||
if(e == LV_EVENT_REFRESH)
|
||||
{
|
||||
m_isMainPage = false;
|
||||
updateTimersSettings();
|
||||
for(uint32_t i = 0; m_timersSettings[i].type != LV_SETTINGS_TYPE_INV; i++) {
|
||||
lv_settings_add(&m_timersSettings[i]);
|
||||
}
|
||||
}
|
||||
else if(e == LV_EVENT_VALUE_CHANGED) {
|
||||
lv_settings_item_t * act_item = (lv_settings_item_t *)lv_event_get_data();
|
||||
if(act_item->id<AT_MAX_TIMERS)
|
||||
{
|
||||
if(act_item->state<0)
|
||||
act_item->state = 0;
|
||||
global->getConfig().timers[act_item->id] = act_item->state*1000;
|
||||
updateTimersSettings();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
#pragma once
|
||||
#include "atscreen.h"
|
||||
#include <lvgl_helpers.h>
|
||||
#include "lvgl_mem.h"
|
||||
|
||||
#define LVS_NULL 0
|
||||
#define LVS_MAINMENU 1
|
||||
#define LVS_IMUCAL 2
|
||||
|
||||
#define LVS_MAX 2
|
||||
|
||||
class cATScreenLVGL;
|
||||
class cLVGLScreen
|
||||
{
|
||||
friend cATScreenLVGL;
|
||||
protected:
|
||||
cATScreenLVGL *m_owner;
|
||||
lv_obj_t *m_scr;
|
||||
lv_obj_t *m_mainCont;
|
||||
lv_obj_t *m_leftCont;
|
||||
lv_obj_t *m_rightCont;
|
||||
lv_style_t m_styleCont;
|
||||
|
||||
bool m_initialized;
|
||||
virtual void createRoots();
|
||||
virtual void internalInit() = 0;
|
||||
virtual void internalDeinit() = 0;
|
||||
void setEventHandler(lv_obj_t *obj);
|
||||
public:
|
||||
cLVGLScreen(cATScreenLVGL *owner);
|
||||
virtual ~cLVGLScreen();
|
||||
virtual void init();
|
||||
virtual void deinit();
|
||||
virtual void back()=0;
|
||||
virtual bool isMainPage()=0;
|
||||
virtual void loop(){};
|
||||
virtual void lvIScreenEvent(struct _lv_obj_t * obj, lv_event_t event) {}
|
||||
};
|
||||
|
||||
class cATScreenLVGL:public cATScreen
|
||||
{
|
||||
friend cLVGLScreen;
|
||||
protected:
|
||||
uint8_t m_lvState;
|
||||
lv_disp_buf_t m_display_buffer;
|
||||
cLVGLScreen *m_screen;
|
||||
lv_group_t *m_keypadGroup;
|
||||
cLVGLScreen *m_screens[LVS_MAX];
|
||||
|
||||
virtual cLVGLScreen * createScreen(uint8_t lvState);
|
||||
public:
|
||||
cATScreenLVGL(atGlobal &global);
|
||||
virtual ~cATScreenLVGL();
|
||||
|
||||
virtual bool init();
|
||||
virtual void showMenu();
|
||||
virtual void loop();
|
||||
virtual void setScreenState(uint8_t value);
|
||||
virtual void processEvent(uint8_t event, uint32_t param1, uint32_t param2);
|
||||
virtual void setLVGLScreen(uint8_t value);
|
||||
lv_group_t *getKeypadGroup() { return m_keypadGroup; }
|
||||
};
|
||||
+596
@@ -0,0 +1,596 @@
|
||||
#include "atble.h"
|
||||
#include "BLEDevice.h"
|
||||
#include "global.h"
|
||||
#include <nvs_flash.h>
|
||||
|
||||
static BLEUUID adcServiceUUID(SERVICE_UUID_ADC);
|
||||
static BLEUUID adcCharasteristicUUID(CHARACTERISTIC_UUID_ADC_POSITION_REPORT);
|
||||
|
||||
#define PROFILE_A_APP_ID 0
|
||||
|
||||
atBLE *bleDevice = NULL;
|
||||
|
||||
#define BS_IDLE 0
|
||||
#define BS_SCANING 1
|
||||
#define BS_CONNECTING 2
|
||||
#define BS_CONNECTING2 3
|
||||
#define BS_DISCOVER_SERVICES 4
|
||||
#define BS_CONNECTED 5
|
||||
#define BS_DICONNECTED 100
|
||||
#define BS_ERROR 101
|
||||
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
static void _gattc_profile_event_handler(esp_gattc_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gattc_cb_param_t *param)
|
||||
{
|
||||
if(bleDevice)
|
||||
bleDevice->gattc_profile_event_handler(event,gattc_if,param);
|
||||
};
|
||||
|
||||
void atBLE::gattc_profile_event_handler(esp_gattc_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gattc_cb_param_t *param)
|
||||
{
|
||||
m_lock.lock();
|
||||
esp_ble_gattc_cb_param_t *p_data = (esp_ble_gattc_cb_param_t *)param;
|
||||
switch (event) {
|
||||
case ESP_GATTC_REG_EVT:
|
||||
{
|
||||
// DEBUG_MSG("REG_EVT\n");
|
||||
static esp_ble_scan_params_t ble_scan_params = {
|
||||
.scan_type = BLE_SCAN_TYPE_ACTIVE,
|
||||
.own_addr_type = BLE_ADDR_TYPE_PUBLIC,
|
||||
.scan_filter_policy = BLE_SCAN_FILTER_ALLOW_ALL,
|
||||
.scan_interval = 0x4000,
|
||||
.scan_window = 312,
|
||||
.scan_duplicate = BLE_SCAN_DUPLICATE_DISABLE
|
||||
};
|
||||
esp_err_t scan_ret = esp_ble_gap_set_scan_params(&ble_scan_params);
|
||||
if (scan_ret){
|
||||
DEBUG_MSG("set scan params error, error code = %x\n", scan_ret);
|
||||
}
|
||||
break;
|
||||
}
|
||||
case ESP_GATTC_CONNECT_EVT:{
|
||||
if(m_bleState==BS_CONNECTING)
|
||||
{
|
||||
//DEBUG_MSG("ESP_GATTC_CONNECT_EVT conn_id %d, if %d\n", p_data->connect.conn_id, gattc_if);
|
||||
m_glProfileTab[PROFILE_A_APP_ID].conn_id = p_data->connect.conn_id;
|
||||
memcpy(m_glProfileTab[PROFILE_A_APP_ID].remote_bda, p_data->connect.remote_bda, sizeof(esp_bd_addr_t));
|
||||
esp_err_t mtu_ret = esp_ble_gattc_send_mtu_req (gattc_if, p_data->connect.conn_id);
|
||||
if (mtu_ret){
|
||||
DEBUG_MSG("config MTU error, error code = %x\n", mtu_ret);
|
||||
}
|
||||
m_bleState = BS_CONNECTING2;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case ESP_GATTC_CFG_MTU_EVT:
|
||||
{
|
||||
//DEBUG_MSG("ESP_GATTC_CFG_MTU_EVT\n");
|
||||
if(m_bleState==BS_CONNECTING2)
|
||||
{
|
||||
m_bleState = BS_DISCOVER_SERVICES;
|
||||
esp_ble_gattc_search_service(gattc_if, param->open.conn_id, adcServiceUUID.getNative());
|
||||
}
|
||||
else
|
||||
DEBUG_MSG("BLE Wrong State\n");
|
||||
break;
|
||||
}
|
||||
|
||||
case ESP_GATTC_OPEN_EVT:
|
||||
if(m_bleState==BS_CONNECTING2)
|
||||
{
|
||||
if (param->open.status != ESP_GATT_OK){
|
||||
DEBUG_MSG("open failed, status %d\n", p_data->open.status);
|
||||
m_bleState = BS_ERROR;
|
||||
break;
|
||||
}
|
||||
//DEBUG_MSG("open success\n");
|
||||
}
|
||||
break;
|
||||
|
||||
case ESP_GATTC_SEARCH_RES_EVT: {
|
||||
//DEBUG_MSG("SEARCH RES: conn_id = %x is primary service %d\n", p_data->search_res.conn_id, p_data->search_res.is_primary);
|
||||
//DEBUG_MSG("start handle %d end handle %d current handle value %d\n", p_data->search_res.start_handle, p_data->search_res.end_handle, p_data->search_res.srvc_id.inst_id);
|
||||
BLEUUID remoteService(p_data->search_res.srvc_id);
|
||||
if (remoteService.equals(adcServiceUUID)) {
|
||||
//DEBUG_MSG("service found\n");
|
||||
m_hasServer = true;
|
||||
m_glProfileTab[PROFILE_A_APP_ID].service_start_handle = p_data->search_res.start_handle;
|
||||
m_glProfileTab[PROFILE_A_APP_ID].service_end_handle = p_data->search_res.end_handle;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case ESP_GATTC_SEARCH_CMPL_EVT:
|
||||
{
|
||||
if (p_data->search_cmpl.status != ESP_GATT_OK){
|
||||
DEBUG_MSG("search service failed, error status = %x\n", p_data->search_cmpl.status);
|
||||
m_bleState = BS_ERROR;
|
||||
break;
|
||||
}
|
||||
if (m_hasServer){
|
||||
uint16_t count = 0;
|
||||
esp_gatt_status_t status = esp_ble_gattc_get_attr_count( gattc_if,
|
||||
p_data->search_cmpl.conn_id,
|
||||
ESP_GATT_DB_CHARACTERISTIC,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].service_start_handle,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].service_end_handle,
|
||||
0,
|
||||
&count);
|
||||
if (status != ESP_GATT_OK){
|
||||
DEBUG_MSG("esp_ble_gattc_get_attr_count error\n");
|
||||
}
|
||||
|
||||
if (count > 0)
|
||||
{
|
||||
esp_gattc_char_elem_t *char_elem_result = (esp_gattc_char_elem_t *)malloc(sizeof(esp_gattc_char_elem_t) * count);
|
||||
if (char_elem_result)
|
||||
{
|
||||
status = esp_ble_gattc_get_char_by_uuid( gattc_if,
|
||||
p_data->search_cmpl.conn_id,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].service_start_handle,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].service_end_handle,
|
||||
*adcCharasteristicUUID.getNative(),
|
||||
char_elem_result,
|
||||
&count);
|
||||
if (status != ESP_GATT_OK){
|
||||
DEBUG_MSG("esp_ble_gattc_get_char_by_uuid error\n");
|
||||
}
|
||||
|
||||
if(count>0)
|
||||
{
|
||||
m_glProfileTab[PROFILE_A_APP_ID].char_handle = char_elem_result[0].char_handle;
|
||||
if (char_elem_result[0].properties & ESP_GATT_CHAR_PROP_BIT_NOTIFY){
|
||||
esp_ble_gattc_register_for_notify(gattc_if, m_glProfileTab[PROFILE_A_APP_ID].remote_bda, char_elem_result[0].char_handle);
|
||||
}
|
||||
m_bleState = BS_CONNECTED;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_bleState = BS_ERROR;
|
||||
}
|
||||
free(char_elem_result);
|
||||
}
|
||||
else
|
||||
m_bleState = BS_ERROR;
|
||||
}else{
|
||||
DEBUG_MSG("no char found\n");
|
||||
m_bleState = BS_ERROR;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case ESP_GATTC_WRITE_DESCR_EVT:
|
||||
{
|
||||
if (p_data->write.status != ESP_GATT_OK){
|
||||
DEBUG_MSG("write descr failed, error status = %x\n", p_data->write.status);
|
||||
break;
|
||||
}
|
||||
//DEBUG_MSG("write descr success\n");
|
||||
uint8_t write_char_data[35];
|
||||
for (int i = 0; i < sizeof(write_char_data); ++i)
|
||||
{
|
||||
write_char_data[i] = i % 256;
|
||||
}
|
||||
esp_ble_gattc_write_char( gattc_if,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].conn_id,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].char_handle,
|
||||
sizeof(write_char_data),
|
||||
write_char_data,
|
||||
ESP_GATT_WRITE_TYPE_RSP,
|
||||
ESP_GATT_AUTH_REQ_NONE);
|
||||
break;
|
||||
}
|
||||
|
||||
case ESP_GATTC_REG_FOR_NOTIFY_EVT:
|
||||
{
|
||||
//DEBUG_MSG("ESP_GATTC_REG_FOR_NOTIFY_EVT\n");
|
||||
if (p_data->reg_for_notify.status != ESP_GATT_OK){
|
||||
DEBUG_MSG("REG FOR NOTIFY failed: error status = %d\n", p_data->reg_for_notify.status);
|
||||
}else{
|
||||
uint16_t count = 0;
|
||||
uint16_t notify_en = 1;
|
||||
esp_gatt_status_t ret_status = esp_ble_gattc_get_attr_count( gattc_if,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].conn_id,
|
||||
ESP_GATT_DB_DESCRIPTOR,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].service_start_handle,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].service_end_handle,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].char_handle,
|
||||
&count);
|
||||
if (ret_status != ESP_GATT_OK){
|
||||
DEBUG_MSG("esp_ble_gattc_get_attr_count error\n");
|
||||
}
|
||||
if (count > 0){
|
||||
esp_gattc_descr_elem_t *descr_elem_result = (esp_gattc_descr_elem_t *)malloc(sizeof(esp_gattc_descr_elem_t) * count);
|
||||
if (descr_elem_result)
|
||||
{
|
||||
esp_bt_uuid_t notify_descr_uuid = {
|
||||
.len = ESP_UUID_LEN_16,
|
||||
.uuid = {.uuid16 = ESP_GATT_UUID_CHAR_CLIENT_CONFIG,},
|
||||
};
|
||||
ret_status = esp_ble_gattc_get_descr_by_char_handle( gattc_if,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].conn_id,
|
||||
p_data->reg_for_notify.handle,
|
||||
notify_descr_uuid,
|
||||
descr_elem_result,
|
||||
&count);
|
||||
if (ret_status != ESP_GATT_OK){
|
||||
DEBUG_MSG("esp_ble_gattc_get_descr_by_char_handle error\n");
|
||||
}
|
||||
if (count > 0 && descr_elem_result[0].uuid.len == ESP_UUID_LEN_16 && descr_elem_result[0].uuid.uuid.uuid16 == ESP_GATT_UUID_CHAR_CLIENT_CONFIG){
|
||||
esp_err_t errRc = esp_ble_gattc_write_char_descr( gattc_if,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].conn_id,
|
||||
descr_elem_result[0].handle,
|
||||
sizeof(notify_en),
|
||||
(uint8_t *)¬ify_en,
|
||||
ESP_GATT_WRITE_TYPE_RSP,
|
||||
ESP_GATT_AUTH_REQ_NONE);
|
||||
if (errRc != ESP_OK)
|
||||
DEBUG_MSG("esp_ble_gattc_write_char_descr error\n");
|
||||
}
|
||||
free(descr_elem_result);
|
||||
}
|
||||
}
|
||||
else{
|
||||
DEBUG_MSG("decsr not found\n");
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case ESP_GATTC_NOTIFY_EVT:
|
||||
{
|
||||
//DEBUG_MSG("ESP_GATTC_NOTIFY_EVT LEN %d\n", (int)p_data->notify.value_len);
|
||||
if (p_data->notify.handle != m_glProfileTab[PROFILE_A_APP_ID].char_handle) break;
|
||||
m_newDataAvailable = true;
|
||||
m_data.resize(param->notify.value_len);
|
||||
memcpy(m_data.data(), param->notify.value, param->notify.value_len);
|
||||
break;
|
||||
}
|
||||
|
||||
case ESP_GATTC_READ_CHAR_EVT:
|
||||
{
|
||||
//DEBUG_MSG("ESP_GATTC_READ_CHAR_EVT %d %d\n",p_data->read.status,param->read.value_len);
|
||||
if (p_data->read.handle != m_glProfileTab[PROFILE_A_APP_ID].char_handle) break;
|
||||
|
||||
// At this point, we have determined that the event is for us, so now we save the value
|
||||
// and unlock the semaphore to ensure that the requestor of the data can continue.
|
||||
if (p_data->read.status == ESP_GATT_OK) {
|
||||
m_lastReadRequest = 0;
|
||||
m_data.resize(param->read.value_len);
|
||||
memcpy(m_data.data(), param->read.value, param->read.value_len);
|
||||
}
|
||||
else
|
||||
m_bleState = BS_ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
case ESP_GATTC_DISCONNECT_EVT:
|
||||
if(p_data->disconnect.conn_id==m_glProfileTab[PROFILE_A_APP_ID].conn_id)
|
||||
{
|
||||
m_hasServer = false;
|
||||
m_bleState = BS_DICONNECTED;
|
||||
}
|
||||
|
||||
//DEBUG_MSG("ESP_GATTC_DISCONNECT_EVT, reason = %d\n", p_data->disconnect.reason);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
};
|
||||
m_lock.unlock();
|
||||
}
|
||||
#endif
|
||||
|
||||
static void _esp_gap_cb(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param)
|
||||
{
|
||||
if(bleDevice)
|
||||
bleDevice->esp_gap_cb(event,param);
|
||||
}
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
static void _esp_gattc_cb(esp_gattc_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gattc_cb_param_t *param)
|
||||
{
|
||||
if(bleDevice)
|
||||
bleDevice->esp_gattc_cb(event,gattc_if,param);
|
||||
}
|
||||
|
||||
void atBLE::onDeviceFound(atBLEAdvertisedDevice &device)
|
||||
{
|
||||
if (device.haveServiceUUID() && device.isAdvertisingService(adcServiceUUID))
|
||||
{
|
||||
DEBUG_MSG("BLE device found. Connecting...\n");
|
||||
m_lastDeviceFound = millis();
|
||||
::esp_ble_gap_stop_scanning();
|
||||
m_bleState = BS_CONNECTING;
|
||||
esp_ble_gattc_open(m_glProfileTab[PROFILE_A_APP_ID].gattc_if, (uint8_t*) device.getAddress().getNative(), device.getAddressType(), true);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
void atBLE::esp_gap_cb(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param)
|
||||
{
|
||||
m_lock.lock();
|
||||
switch (event) {
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
case ESP_GAP_BLE_SCAN_RESULT_EVT: {
|
||||
esp_ble_gap_cb_param_t *scan_result = (esp_ble_gap_cb_param_t *)param;
|
||||
switch (scan_result->scan_rst.search_evt) {
|
||||
case ESP_GAP_SEARCH_INQ_RES_EVT:
|
||||
{
|
||||
if(m_bleState == BS_SCANING)
|
||||
{
|
||||
BLEAddress advertisedAddress(param->scan_rst.bda);
|
||||
atBLEAdvertisedDevice advertisedDevice;
|
||||
advertisedDevice.setAddress(advertisedAddress);
|
||||
advertisedDevice.setRSSI(scan_result->scan_rst.rssi);
|
||||
advertisedDevice.setAdFlag(scan_result->scan_rst.flag);
|
||||
advertisedDevice.parseAdvertisement((uint8_t*)scan_result->scan_rst.ble_adv, scan_result->scan_rst.adv_data_len + scan_result->scan_rst.scan_rsp_len);
|
||||
advertisedDevice.setAddressType(scan_result->scan_rst.ble_addr_type);
|
||||
|
||||
onDeviceFound(advertisedDevice);
|
||||
}
|
||||
break;
|
||||
}
|
||||
default:
|
||||
break;
|
||||
}
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
}
|
||||
m_lock.unlock();
|
||||
}
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
void atBLE::esp_gattc_cb(esp_gattc_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gattc_cb_param_t *param)
|
||||
{
|
||||
/* If event is register event, store the gattc_if for each profile */
|
||||
if (event == ESP_GATTC_REG_EVT) {
|
||||
if (param->reg.status == ESP_GATT_OK) {
|
||||
m_glProfileTab[param->reg.app_id].gattc_if = gattc_if;
|
||||
} else {
|
||||
DEBUG_MSG("reg app failed, app_id %04x, status %d\n",
|
||||
param->reg.app_id,
|
||||
param->reg.status);
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/* If the gattc_if equal to profile A, call profile A cb handler,
|
||||
* so here call each profile's callback */
|
||||
do {
|
||||
int idx;
|
||||
for (idx = 0; idx < PROFILE_NUM; idx++) {
|
||||
if (gattc_if == ESP_GATT_IF_NONE || /* ESP_GATT_IF_NONE, not specify a certain gatt_if, need to call every profile cb function */
|
||||
gattc_if == m_glProfileTab[idx].gattc_if) {
|
||||
if (m_glProfileTab[idx].gattc_cb) {
|
||||
m_glProfileTab[idx].gattc_cb(event, gattc_if, param);
|
||||
}
|
||||
}
|
||||
}
|
||||
} while (0);
|
||||
}
|
||||
#endif
|
||||
|
||||
atBLE::atBLE()
|
||||
{
|
||||
m_enableScan = false;
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
m_glProfileTab[PROFILE_A_APP_ID] = {
|
||||
.gattc_cb = _gattc_profile_event_handler,
|
||||
.gattc_if = ESP_GATT_IF_NONE, /* Not get the gatt_if, so initial is ESP_GATT_IF_NONE */
|
||||
};
|
||||
#endif
|
||||
}
|
||||
|
||||
void atBLE::begin(uint16_t poi)
|
||||
{
|
||||
m_lastReadRequest = 0;
|
||||
m_lastDeviceFound = 0;
|
||||
m_lastDisconnect = 0;
|
||||
m_hasServer = false;
|
||||
m_bleState = BS_IDLE;
|
||||
bleDevice = this;
|
||||
m_lastUpdate = 0;
|
||||
m_poi = poi;
|
||||
esp_err_t ret;
|
||||
/*
|
||||
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
if (!btStart()) {
|
||||
DEBUG_MSG("BLE Start failed\n");
|
||||
return;
|
||||
}
|
||||
#else
|
||||
ret = ::nvs_flash_init();
|
||||
if (ret != ESP_OK) {
|
||||
DEBUG_MSG("nvs_flash_init: rc=%d\n", ret);
|
||||
return;
|
||||
}
|
||||
|
||||
esp_bt_controller_config_t bt_cfg = BT_CONTROLLER_INIT_CONFIG_DEFAULT();
|
||||
ret = esp_bt_controller_init(&bt_cfg);
|
||||
if (ret) {
|
||||
DEBUG_MSG("%s initialize controller failed, error code = %x\n", __func__, ret);
|
||||
return;
|
||||
}
|
||||
|
||||
ret = esp_bt_controller_enable(ESP_BT_MODE_BLE);
|
||||
if (ret) {
|
||||
DEBUG_MSG("%s enable controller failed, error code = %x\n", __func__, ret);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
ret = esp_bluedroid_init();
|
||||
if (ret) {
|
||||
DEBUG_MSG("%s init bluetooth failed, error code = %x\n", __func__, ret);
|
||||
return;
|
||||
}
|
||||
|
||||
ret = esp_bluedroid_enable();
|
||||
if (ret) {
|
||||
DEBUG_MSG("%s enable bluetooth failed, error code = %x\n", __func__, ret);
|
||||
return;
|
||||
}
|
||||
|
||||
//register the callback function to the gap module
|
||||
ret = esp_ble_gap_register_callback(_esp_gap_cb);
|
||||
if (ret){
|
||||
DEBUG_MSG("%s gap register failed, error code = %x\n", __func__, ret);
|
||||
return;
|
||||
}
|
||||
|
||||
//register the callback function to the gattc module
|
||||
ret = esp_ble_gattc_register_callback(_esp_gattc_cb);
|
||||
if(ret){
|
||||
DEBUG_MSG("%s gattc register failed, error code = %x\n", __func__, ret);
|
||||
return;
|
||||
}
|
||||
*/
|
||||
char bleName[32];
|
||||
snprintf(bleName,sizeof(bleName),"TTracker %0X (%s)",global->getConfig().id,global->getConfig().name.c_str());
|
||||
BLEDevice::init(bleName);
|
||||
BLEDevice::setCustomGapHandler(_esp_gap_cb);
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
BLEDevice::setCustomGattcHandler(_esp_gattc_cb);
|
||||
ret = esp_ble_gattc_app_register(PROFILE_A_APP_ID);
|
||||
if (ret){
|
||||
DEBUG_MSG("%s gattc app register failed, error code = %x\n", __func__, ret);
|
||||
}
|
||||
#endif
|
||||
esp_err_t local_mtu_ret = esp_ble_gatt_set_local_mtu(500);
|
||||
if (local_mtu_ret){
|
||||
DEBUG_MSG("set local MTU failed, error code = %x\n", local_mtu_ret);
|
||||
}
|
||||
}
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
void atBLE::internalEnableScan()
|
||||
{
|
||||
m_lock.lock();
|
||||
static esp_ble_scan_params_t ble_scan_params = {
|
||||
.scan_type = BLE_SCAN_TYPE_ACTIVE,
|
||||
.own_addr_type = BLE_ADDR_TYPE_PUBLIC,
|
||||
.scan_filter_policy = BLE_SCAN_FILTER_ALLOW_ALL,
|
||||
.scan_interval = 0x4000,
|
||||
.scan_window = 312,
|
||||
.scan_duplicate = BLE_SCAN_DUPLICATE_DISABLE
|
||||
};
|
||||
esp_err_t scan_ret = esp_ble_gap_set_scan_params(&ble_scan_params);
|
||||
if (scan_ret){
|
||||
DEBUG_MSG("set scan params error, error code = %x\n", scan_ret);
|
||||
}
|
||||
::esp_ble_gap_start_scanning(0);
|
||||
if(m_bleState == BS_IDLE)
|
||||
m_bleState = BS_SCANING;
|
||||
m_lock.unlock();
|
||||
}
|
||||
|
||||
void atBLE::enableScan(bool en)
|
||||
{
|
||||
if(m_enableScan!=en)
|
||||
{
|
||||
m_enableScan = en;
|
||||
if(m_enableScan)
|
||||
{
|
||||
internalEnableScan();
|
||||
}
|
||||
else
|
||||
{
|
||||
::esp_ble_gap_stop_scanning();
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void atBLE::loop()
|
||||
{
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
m_lock.lock();
|
||||
|
||||
if(m_bleState==BS_IDLE && m_enableScan && (millis()-m_lastDisconnect)>5000)
|
||||
{
|
||||
m_bleState=BS_SCANING;
|
||||
m_lock.unlock();
|
||||
internalEnableScan();
|
||||
m_lock.lock();
|
||||
}
|
||||
|
||||
if(m_bleState==BS_DICONNECTED || m_bleState==BS_ERROR)
|
||||
{
|
||||
//DEBUG_MSG("BLE Deleting %d\n",m_bleState);
|
||||
m_data.clear();
|
||||
m_lastReadRequest = 0;
|
||||
m_lastDeviceFound = 0;
|
||||
//global->removePOI(m_poi);
|
||||
auto conn_id = m_glProfileTab[PROFILE_A_APP_ID].conn_id;
|
||||
m_glProfileTab[PROFILE_A_APP_ID].conn_id = 0xFFFF;
|
||||
m_lock.unlock();
|
||||
if(conn_id!=0xFFFF)
|
||||
::esp_ble_gattc_close(m_glProfileTab[PROFILE_A_APP_ID].gattc_if, conn_id);
|
||||
m_lock.lock();
|
||||
delay(10);
|
||||
m_bleState=BS_IDLE;
|
||||
m_lastDisconnect = millis();
|
||||
}
|
||||
else
|
||||
if(m_bleState==BS_CONNECTED)
|
||||
{
|
||||
if(m_data.size()>0)
|
||||
{
|
||||
if(m_data.size()==sizeof(sADCPositionReport))
|
||||
{
|
||||
m_lastUpdate = millis();
|
||||
m_newDataAvailable = false;
|
||||
sADCPositionReport *report = (sADCPositionReport *)m_data.data();
|
||||
sPOI poi;
|
||||
poi.lng = report->longitude;
|
||||
poi.lat = report->latitude;
|
||||
poi.alt = report->altitude;
|
||||
poi.lastUpdate = (int64_t)millis() - ((int64_t)report->now - (int64_t) report->lastSeen);
|
||||
//DEBUG_MSG("R %d %d %d\n",(int)poi.lastUpdate,(int)report->now,(int)report->lastSeen);
|
||||
poi.RSSI = report->RSSI;
|
||||
poi.SNR = report->SNR;
|
||||
poi.updateXY();
|
||||
poi.name = report->status;
|
||||
poi.symbol = 'D';
|
||||
poi.type = POIT_DYNAMIC;
|
||||
poi.positionChanged = true;
|
||||
poi.hacc = report->hacc;
|
||||
global->setPOI(m_poi,poi);
|
||||
}
|
||||
m_data.clear();
|
||||
}
|
||||
else
|
||||
if(m_lastReadRequest==0 && (m_newDataAvailable || (millis()-m_lastUpdate)>5500))
|
||||
{
|
||||
m_newDataAvailable = false;
|
||||
m_lastUpdate = millis();
|
||||
m_lastReadRequest = millis();
|
||||
esp_err_t errRc = ::esp_ble_gattc_read_char(
|
||||
m_glProfileTab[PROFILE_A_APP_ID].gattc_if,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].conn_id,
|
||||
m_glProfileTab[PROFILE_A_APP_ID].char_handle,
|
||||
ESP_GATT_AUTH_REQ_NONE); // Security
|
||||
|
||||
if (errRc != ESP_OK) {
|
||||
DEBUG_MSG("esp_ble_gattc_read_char: rc=%d\n", errRc);
|
||||
}
|
||||
}
|
||||
if(m_lastReadRequest!=0 && (millis()-m_lastReadRequest)>5000)
|
||||
{
|
||||
m_bleState = BS_ERROR;
|
||||
m_lastReadRequest = 0;
|
||||
DEBUG_MSG("BLE read timeout\n");
|
||||
}
|
||||
}
|
||||
if(m_lastDeviceFound!=0 && m_bleState<BS_DICONNECTED && m_bleState!=BS_CONNECTED && m_bleState>BS_SCANING && (millis()-m_lastDeviceFound)>10000)
|
||||
{
|
||||
m_bleState = BS_ERROR;
|
||||
m_lastDeviceFound = 0;
|
||||
DEBUG_MSG("BLE connect timeout\n");
|
||||
}
|
||||
m_lock.unlock();
|
||||
#endif
|
||||
}
|
||||
+64
@@ -0,0 +1,64 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
|
||||
#include <BLEDevice.h>
|
||||
#include <BLEClient.h>
|
||||
#include <BLEUtils.h>
|
||||
#include <BLE2902.h>
|
||||
#include "adcBLECommon.h"
|
||||
#include "config.h"
|
||||
#include "concurrency/Lock.h"
|
||||
#include "esp_gap_ble_api.h"
|
||||
#include "esp_gattc_api.h"
|
||||
#include "esp_gatt_defs.h"
|
||||
#include "esp_bt_main.h"
|
||||
#include "esp_gatt_common_api.h"
|
||||
#include "BLE/atBLEAdvertisedDevice.h"
|
||||
|
||||
#define PROFILE_NUM 1
|
||||
|
||||
struct sGattcProfile {
|
||||
esp_gattc_cb_t gattc_cb;
|
||||
uint16_t gattc_if;
|
||||
uint16_t app_id;
|
||||
uint16_t conn_id;
|
||||
uint16_t service_start_handle;
|
||||
uint16_t service_end_handle;
|
||||
uint16_t char_handle;
|
||||
esp_bd_addr_t remote_bda;
|
||||
};
|
||||
|
||||
class atBLE
|
||||
{
|
||||
protected:
|
||||
struct sGattcProfile m_glProfileTab[PROFILE_NUM];
|
||||
bool m_enableScan;
|
||||
bool m_newDataAvailable;
|
||||
uint16_t m_poi;
|
||||
int64_t m_lastUpdate;
|
||||
concurrency::Lock m_lock;
|
||||
int m_bleState;
|
||||
bool m_hasServer;
|
||||
std::vector<uint8_t> m_data;
|
||||
int64_t m_lastDisconnect;
|
||||
int64_t m_lastReadRequest;
|
||||
int64_t m_lastDeviceFound;
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
void internalEnableScan();
|
||||
void onDeviceFound(atBLEAdvertisedDevice &device);
|
||||
#endif
|
||||
public:
|
||||
atBLE();
|
||||
void begin(uint16_t poi);
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
void enableScan(bool en);
|
||||
bool getEnableScan() { return m_enableScan; }
|
||||
#endif
|
||||
void loop();
|
||||
|
||||
void esp_gap_cb(esp_gap_ble_cb_event_t event, esp_ble_gap_cb_param_t *param);
|
||||
#ifndef AD_BLE_NO_CLIENT
|
||||
void esp_gattc_cb(esp_gattc_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gattc_cb_param_t *param);
|
||||
void gattc_profile_event_handler(esp_gattc_cb_event_t event, esp_gatt_if_t gattc_if, esp_ble_gattc_cb_param_t *param);
|
||||
#endif
|
||||
};
|
||||
@@ -0,0 +1,73 @@
|
||||
#include "atcommon.h"
|
||||
#include <math.h>
|
||||
#include <Arduino.h>
|
||||
|
||||
void convertDegreeToMeter(double lng, double lat, double &x,double &y)
|
||||
{
|
||||
x = DEGREE_TO_METER_X(lng);
|
||||
y = DEGREE_TO_METER_Y(lat);
|
||||
}
|
||||
|
||||
void convertMeterToDegree(double x, double y, double &lng,double &lat)
|
||||
{
|
||||
lng = DEGREE_TO_METER_REVERSE_X(x);
|
||||
lat = DEGREE_TO_METER_REVERSE_Y(y);
|
||||
}
|
||||
|
||||
double distanceXYBetween(double x1, double y1, double x2, double y2)
|
||||
{
|
||||
return sqrt(pow(x2 - x1, 2) +pow(y2 - y1, 2));
|
||||
}
|
||||
|
||||
double courseXYTo(double x1, double y1, double x2, double y2)
|
||||
{
|
||||
double result = 270-degrees(atan2(y1 - y2, x1 - x2));
|
||||
if(result<0)
|
||||
result += 360;
|
||||
return result;
|
||||
}
|
||||
|
||||
double distanceBetween(double lat1, double long1, double lat2, double long2)
|
||||
{
|
||||
// returns distance in meters between two positions, both specified
|
||||
// as signed decimal-degrees latitude and longitude. Uses great-circle
|
||||
// distance computation for hypothetical sphere of radius 6372795 meters.
|
||||
// Because Earth is no exact sphere, rounding errors may be up to 0.5%.
|
||||
// Courtesy of Maarten Lamers
|
||||
double delta = radians(long1-long2);
|
||||
double sdlong = sin(delta);
|
||||
double cdlong = cos(delta);
|
||||
lat1 = radians(lat1);
|
||||
lat2 = radians(lat2);
|
||||
double slat1 = sin(lat1);
|
||||
double clat1 = cos(lat1);
|
||||
double slat2 = sin(lat2);
|
||||
double clat2 = cos(lat2);
|
||||
delta = (clat1 * slat2) - (slat1 * clat2 * cdlong);
|
||||
delta = sq(delta);
|
||||
delta += sq(clat2 * sdlong);
|
||||
delta = sqrt(delta);
|
||||
double denom = (slat1 * slat2) + (clat1 * clat2 * cdlong);
|
||||
delta = atan2(delta, denom);
|
||||
return delta * 6372795;
|
||||
}
|
||||
|
||||
double courseTo(double lat1, double long1, double lat2, double long2)
|
||||
{
|
||||
// returns course in degrees (North=0, West=270) from position 1 to position 2,
|
||||
// both specified as signed decimal-degrees latitude and longitude.
|
||||
// Because Earth is no exact sphere, calculated course may be off by a tiny fraction.
|
||||
// Courtesy of Maarten Lamers
|
||||
double dlon = radians(long2-long1);
|
||||
lat1 = radians(lat1);
|
||||
lat2 = radians(lat2);
|
||||
double a1 = sin(dlon) * cos(lat2);
|
||||
double a2 = sin(lat1) * cos(lat2) * cos(dlon);
|
||||
a2 = cos(lat1) * sin(lat2) - a2;
|
||||
a2 = atan2(a1, a2);
|
||||
if (a2 < 0.0)
|
||||
{
|
||||
a2 += TWO_PI;
|
||||
}
|
||||
return degrees(a2);
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
#pragma once
|
||||
|
||||
#define Pi M_PI
|
||||
|
||||
#define DEGREE_TO_METER_X(X) X*111319.490778
|
||||
#define DEGREE_TO_METER_Y(Y) (log(tan((90.0 + (Y)) * M_PI / 360.0)) / (M_PI / 180.0))*111319.490778
|
||||
#define DEGREE_TO_METER_REVERSE_Y(Y) (atan(pow(M_E, ((Y)/111319.490778)*M_PI/180.0))*360.0/M_PI-90.0)
|
||||
#define DEGREE_TO_METER_REVERSE_X(X) X/111319.490778
|
||||
void convertDegreeToMeter(double lng, double lat, double &x,double &y);
|
||||
void convertMeterToDegree(double x, double y, double &lng,double &lat);
|
||||
double distanceXYBetween(double x1, double y1, double x2, double y2);
|
||||
double courseXYTo(double x1, double y1, double x2, double y2);
|
||||
double distanceBetween(double lat1, double long1, double lat2, double long2);
|
||||
double courseTo(double lat1, double long1, double lat2, double long2);
|
||||
@@ -0,0 +1,417 @@
|
||||
#include "atglobal.h"
|
||||
#include <Arduino.h>
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include <fstream>
|
||||
|
||||
|
||||
struct PSRAMAllocator {
|
||||
void* allocate(size_t size) {
|
||||
return heap_caps_malloc(size,MALLOC_CAP_SPIRAM);
|
||||
}
|
||||
|
||||
void deallocate(void* ptr) {
|
||||
heap_caps_free(ptr);
|
||||
}
|
||||
|
||||
void* reallocate(void* ptr, size_t new_size) {
|
||||
return heap_caps_realloc(ptr, new_size,MALLOC_CAP_SPIRAM);
|
||||
}
|
||||
};
|
||||
|
||||
typedef BasicJsonDocument<PSRAMAllocator> PSRAMJsonDocument;
|
||||
|
||||
void atGlobal::onMapLoad(cMap *map)
|
||||
{
|
||||
m_poiMapStatic.clear();
|
||||
if(map)
|
||||
{
|
||||
char fileName[32];
|
||||
std::string mapName = map->getMap().name;
|
||||
for(auto& c : mapName)
|
||||
{
|
||||
c = tolower(c);
|
||||
}
|
||||
snprintf(fileName,sizeof(fileName),AT_POINTS_FILE_TEMPLATE,mapName.c_str());
|
||||
std::ifstream file(fileName,std::ifstream::binary);
|
||||
if(file.good())
|
||||
{
|
||||
DEBUG_MSG("Loading points from %s\n",fileName);
|
||||
PSRAMJsonDocument json(64*1024);
|
||||
DeserializationError error = deserializeJson(json,file);
|
||||
if(!error)
|
||||
{
|
||||
const JsonVariant &points = json.getMember("points");
|
||||
for(int i=0; i<points.size(); i++)
|
||||
{
|
||||
sPOI poi;
|
||||
const JsonVariant &poiJson = points.getElement(i);
|
||||
if(poi.loadFromJson(poiJson))
|
||||
{
|
||||
poi.updateXY();
|
||||
m_poiMapStatic.push_back(poi);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
DEBUG_MSG("Load maps error %s\n",error.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool sPOI::loadFromJson(const JsonVariant &json)
|
||||
{
|
||||
if(json.containsKey("type")&&json.containsKey("color")&&json.containsKey("name")&&json.containsKey("coordinates"))
|
||||
{
|
||||
name = (const char *) json["name"];
|
||||
uint8_t _t = json["type"].as<uint8_t>();
|
||||
switch(_t==0)
|
||||
{
|
||||
case 0:
|
||||
type = POIT_STATIC;
|
||||
break;
|
||||
default:
|
||||
type = POIT_STATIC;
|
||||
break;
|
||||
}
|
||||
color = json["color"].as<uint32_t>();
|
||||
lat = json["coordinates"][1].as<double>();
|
||||
lng = json["coordinates"][0].as<double>();
|
||||
positionChanged = true;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
cConfig::cConfig()
|
||||
{
|
||||
}
|
||||
|
||||
bool cConfig::loadConfig(std::istream &stream,bool binary)
|
||||
{
|
||||
try{
|
||||
StaticJsonDocument<1024> json;
|
||||
DeserializationError error;
|
||||
if(binary)
|
||||
error = deserializeMsgPack(json,stream);
|
||||
else
|
||||
error = deserializeJson(json,stream);
|
||||
if(error)
|
||||
{
|
||||
DEBUG_MSG("cConfig loading error %s\n",error.c_str());
|
||||
return false;
|
||||
}
|
||||
else
|
||||
{
|
||||
initDefaults();
|
||||
loadConfigValues(json);
|
||||
}
|
||||
}catch(...)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void cConfig::saveConfig(std::ostream &stream,bool binary)
|
||||
{
|
||||
StaticJsonDocument<1024> json;
|
||||
saveConfigValues(json);
|
||||
|
||||
try{
|
||||
if(binary)
|
||||
serializeMsgPack(json,stream);
|
||||
else
|
||||
serializeJson(json,stream);
|
||||
}catch(...)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void cConfig::loadConfig(const char * filename)
|
||||
{
|
||||
try{
|
||||
std::ifstream file(filename,std::ifstream::binary);
|
||||
if(!loadConfig(file))
|
||||
saveConfig(filename);
|
||||
}catch(...)
|
||||
{
|
||||
saveConfig(filename);
|
||||
}
|
||||
if(checkConfig())
|
||||
saveConfig(filename);
|
||||
}
|
||||
|
||||
void cConfig::saveConfig(const char * filename)
|
||||
{
|
||||
try{
|
||||
std::ofstream file(filename,std::ofstream::binary);
|
||||
saveConfig(file);
|
||||
}catch(...)
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
atMainConfig::atMainConfig()
|
||||
{
|
||||
initDefaults();
|
||||
}
|
||||
|
||||
void atMainConfig::initDefaults()
|
||||
{
|
||||
symbol = 0;
|
||||
id = MAX_CONFIG_ID;
|
||||
radius = 250;
|
||||
units = U_METRIC;
|
||||
reportInterval = 500;
|
||||
timeZone = DEFAULT_TZ;
|
||||
screenIdleTimeout = 90000;
|
||||
useVibration = true;
|
||||
vibrationTime = 200;
|
||||
maxPeerTimeout = 600;
|
||||
memset(&timers,0,sizeof(timers));
|
||||
checkConfig();
|
||||
}
|
||||
|
||||
bool atMainConfig::checkConfig()
|
||||
{
|
||||
bool changed = false;
|
||||
if(id==MAX_CONFIG_ID)
|
||||
{
|
||||
uint8_t mac[6];
|
||||
changed = true;
|
||||
esp_read_mac(mac, ESP_MAC_BT);
|
||||
id = mac[5];
|
||||
if(id==0)
|
||||
id = rand() & 0xFF;
|
||||
}
|
||||
if(name.empty())
|
||||
{
|
||||
changed = true;
|
||||
char buffer[16];
|
||||
snprintf(buffer,sizeof(buffer),"Dev%02X",(int)id);
|
||||
name = buffer;
|
||||
}
|
||||
if(reportInterval<200)
|
||||
{
|
||||
changed = true;
|
||||
reportInterval = 500;
|
||||
}
|
||||
if(reportInterval>30000)
|
||||
{
|
||||
changed = true;
|
||||
reportInterval = 30000;
|
||||
}
|
||||
if(timeZone.empty())
|
||||
{
|
||||
changed = true;
|
||||
timeZone = DEFAULT_TZ;
|
||||
}
|
||||
|
||||
if(screenIdleTimeout<10000 && screenIdleTimeout!=0)
|
||||
{
|
||||
screenIdleTimeout=10000;
|
||||
changed = true;
|
||||
}
|
||||
|
||||
if(vibrationTime<50)
|
||||
{
|
||||
vibrationTime = 50;
|
||||
changed = true;
|
||||
}
|
||||
else
|
||||
if(vibrationTime>2000)
|
||||
{
|
||||
vibrationTime = 2000;
|
||||
changed = true;
|
||||
}
|
||||
|
||||
if(maxPeerTimeout>86400)
|
||||
{
|
||||
maxPeerTimeout = 86400;
|
||||
changed = true;
|
||||
}
|
||||
return changed;
|
||||
}
|
||||
|
||||
void atMainConfig::loadConfigValues(JsonDocument &json)
|
||||
{
|
||||
const char * c = (const char*) json["tz"];
|
||||
testLat = json["testLat"];
|
||||
testLng = json["testLng"];
|
||||
id = json["id"];
|
||||
radius = json["radius"];
|
||||
symbol = json["symbol"];
|
||||
reportInterval = json["reportInterval"];
|
||||
screenIdleTimeout = json["screenIdleTimeout"];
|
||||
for(int i=0; i<AT_MAX_TIMERS; i++)
|
||||
{
|
||||
char buf[4];
|
||||
snprintf(buf,sizeof(buf),"t%d",i+1);
|
||||
timers[i] = json[buf];
|
||||
}
|
||||
if(json.containsKey("useVibration"))
|
||||
{
|
||||
useVibration = json["useVibration"];
|
||||
vibrationTime = json["vibrationTime"];
|
||||
}
|
||||
if(json.containsKey("maxPeerTimeout"))
|
||||
{
|
||||
maxPeerTimeout = json["maxPeerTimeout"];
|
||||
}
|
||||
c = (const char*) json["name"];
|
||||
if(c)
|
||||
name = c;
|
||||
units = json["units"];
|
||||
c = (const char*) json["tz"];
|
||||
if(c)
|
||||
timeZone = c;
|
||||
}
|
||||
|
||||
void atMainConfig::saveConfigValues(JsonDocument &json)
|
||||
{
|
||||
json["id"] = id;
|
||||
json["symbol"] = symbol;
|
||||
json["radius"] = radius;
|
||||
json["name"] = name.c_str();
|
||||
json["units"] = units;
|
||||
json["tz"] = timeZone.c_str();
|
||||
json["screenIdleTimeout"] = screenIdleTimeout;
|
||||
json["reportInterval"] = reportInterval;
|
||||
json["useVibration"] = useVibration;
|
||||
json["vibrationTime"] = vibrationTime;
|
||||
json["maxPeerTimeout"] = maxPeerTimeout;
|
||||
for(int i=0; i<AT_MAX_TIMERS; i++)
|
||||
{
|
||||
char buf[4];
|
||||
snprintf(buf,sizeof(buf),"t%d",i+1);
|
||||
json[buf] = timers[i];
|
||||
}
|
||||
}
|
||||
|
||||
////////////////////////////////////////
|
||||
void atNetworkConfig::initDefaults()
|
||||
{
|
||||
channelConfig = cLoraChannelCondfig();
|
||||
channelConfig.bw = LORA_BW;
|
||||
channelConfig.sf = LORA_SF;
|
||||
channelConfig.cr = LORA_CR;
|
||||
channelConfig.freq = LORA_FREQ;
|
||||
channelConfig.power = LORA_POWER;
|
||||
channelConfig.preambleLength = LORA_PRE;
|
||||
channelConfig.syncWord = LORA_SYNC;
|
||||
channelConfig.currentLimit = LORA_CURRENT;
|
||||
|
||||
password = "";
|
||||
networkId = 0;
|
||||
|
||||
requestInfoInterval = 10000;
|
||||
}
|
||||
|
||||
bool atNetworkConfig::checkConfig()
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
void atNetworkConfig::saveConfigValues(JsonDocument &json)
|
||||
{
|
||||
json["freq"] = channelConfig.freq;
|
||||
json["bw"] = channelConfig.bw;
|
||||
json["sf"] = channelConfig.sf;
|
||||
json["cr"] = channelConfig.cr;
|
||||
json["preambleLength"] = channelConfig.preambleLength;
|
||||
json["syncWord"] = channelConfig.syncWord;
|
||||
json["networkId"] = networkId;
|
||||
json["password"] = password.c_str();
|
||||
}
|
||||
|
||||
void atNetworkConfig::loadConfigValues(JsonDocument &json)
|
||||
{
|
||||
channelConfig.freq = json["freq"];
|
||||
channelConfig.bw = json["bw"];
|
||||
channelConfig.sf = json["sf"];
|
||||
channelConfig.cr = json["cr"];
|
||||
channelConfig.preambleLength = json["preambleLength"];
|
||||
channelConfig.syncWord = json["syncWord"];
|
||||
networkId = json["networkId"];
|
||||
const char *c = json["password"];
|
||||
if(c)
|
||||
password = c;
|
||||
}
|
||||
|
||||
atNetworkConfig::atNetworkConfig()
|
||||
{
|
||||
initDefaults();
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////
|
||||
void atAGDNetworkConfig::initDefaults()
|
||||
{
|
||||
channelConfig = cLoraChannelCondfig();
|
||||
channelConfig.bw = AGD_LORA_BW;
|
||||
channelConfig.sf = AGD_LORA_SF;
|
||||
channelConfig.cr = AGD_LORA_CR;
|
||||
channelConfig.freq = AGD_LORA_FREQ;
|
||||
channelConfig.power = LORA_POWER;
|
||||
channelConfig.preambleLength = AGD_LORA_PRE;
|
||||
channelConfig.syncWord = AGD_LORA_SYNC;
|
||||
channelConfig.currentLimit = LORA_CURRENT;
|
||||
updateTimeout = 10000;
|
||||
}
|
||||
|
||||
bool atAGDNetworkConfig::checkConfig()
|
||||
{
|
||||
bool changed = false;
|
||||
|
||||
if(updateTimeout<6000)
|
||||
{
|
||||
updateTimeout = 6000;
|
||||
changed = true;
|
||||
}
|
||||
else
|
||||
if(updateTimeout>60000)
|
||||
{
|
||||
updateTimeout = 60000;
|
||||
changed = true;
|
||||
}
|
||||
|
||||
return changed;
|
||||
}
|
||||
|
||||
void atAGDNetworkConfig::saveConfigValues(JsonDocument &json)
|
||||
{
|
||||
json["freq"] = channelConfig.freq;
|
||||
json["bw"] = channelConfig.bw;
|
||||
json["sf"] = channelConfig.sf;
|
||||
json["cr"] = channelConfig.cr;
|
||||
json["preambleLength"] = channelConfig.preambleLength;
|
||||
json["syncWord"] = channelConfig.syncWord;
|
||||
json["updateTimeout"] = updateTimeout;
|
||||
}
|
||||
|
||||
void atAGDNetworkConfig::loadConfigValues(JsonDocument &json)
|
||||
{
|
||||
channelConfig.freq = json["freq"];
|
||||
channelConfig.bw = json["bw"];
|
||||
channelConfig.sf = json["sf"];
|
||||
channelConfig.cr = json["cr"];
|
||||
channelConfig.preambleLength = json["preambleLength"];
|
||||
channelConfig.syncWord = json["syncWord"];
|
||||
updateTimeout = json["updateTimeout"];
|
||||
}
|
||||
|
||||
atAGDNetworkConfig::atAGDNetworkConfig()
|
||||
{
|
||||
initDefaults();
|
||||
}
|
||||
|
||||
cSideButtons::cSideButtons()
|
||||
{
|
||||
for(int i=0; i<SIDE_BUTTONS_COUNT; i++)
|
||||
{
|
||||
m_buttons[i].idx = i;
|
||||
m_buttons[i].action = 0;
|
||||
}
|
||||
}
|
||||
+507
@@ -0,0 +1,507 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include <power.h>
|
||||
#include <Wire.h>
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include "power.h"
|
||||
#include "GPSStatus.h"
|
||||
#include "IMUStatus.h"
|
||||
#include <ArduinoJson.h>
|
||||
#include "radio/LoraRadio.h"
|
||||
#include "mapTiles.h"
|
||||
#include "atstrings.h"
|
||||
#include <vector>
|
||||
|
||||
#define MAX_CONFIG_ID 0xFFFF
|
||||
#define U_METRIC 0
|
||||
#define U_US 1
|
||||
|
||||
#define POIT_PEER 0
|
||||
#define POIT_STATIC 1
|
||||
#define POIT_DYNAMIC 2
|
||||
#define POIT_ENEMY 3
|
||||
#define POIT_SELF 10
|
||||
|
||||
#define POIS_NORMAL 0
|
||||
#define POIS_DEAD 1
|
||||
|
||||
struct sPOI
|
||||
{
|
||||
bool positionChanged=false;
|
||||
uint8_t type=POIT_PEER;
|
||||
char symbol=0;
|
||||
std::string name;
|
||||
uint8_t status=0;
|
||||
uint8_t options=0;
|
||||
double lat=0;
|
||||
double lng=0;
|
||||
uint32_t hacc=0;
|
||||
double x=0;
|
||||
double y=0;
|
||||
int32_t alt=0;
|
||||
int16_t heading=0;
|
||||
double distance=0;
|
||||
double realDistance=0;
|
||||
double courseTo=0;
|
||||
double RSSI=0;
|
||||
double SNR=0;
|
||||
int32_t retransmitRating = 0;
|
||||
int64_t lastUpdate=0;
|
||||
uint8_t lastLabelIdx=0;
|
||||
uint32_t color=0;
|
||||
|
||||
sPOI()
|
||||
{
|
||||
}
|
||||
|
||||
bool isValidLocation() const
|
||||
{
|
||||
return (lat != 0) && (lng != 0) && (lat <= 90 && lat >= -90);
|
||||
}
|
||||
|
||||
void updateXY()
|
||||
{
|
||||
if(isValidLocation())
|
||||
{
|
||||
convertDegreeToMeter(lng,lat,x,y);
|
||||
}
|
||||
else
|
||||
{
|
||||
x = y = 0;
|
||||
}
|
||||
}
|
||||
bool loadFromJson(const JsonVariant &json);
|
||||
};
|
||||
|
||||
struct sSideButton
|
||||
{
|
||||
uint8_t idx=0;
|
||||
std::string caption;
|
||||
uint8_t action=0;
|
||||
uint8_t actionLong=0;
|
||||
};
|
||||
|
||||
class cSideButtons
|
||||
{
|
||||
protected:
|
||||
sSideButton m_buttons[SIDE_BUTTONS_COUNT];
|
||||
|
||||
public:
|
||||
cSideButtons();
|
||||
sSideButton *getButtons()
|
||||
{
|
||||
return m_buttons;
|
||||
}
|
||||
|
||||
uint8_t getButtonsCount()
|
||||
{
|
||||
return SIDE_BUTTONS_COUNT;
|
||||
}
|
||||
};
|
||||
|
||||
#define AT_MAX_TIMERS 4
|
||||
class cConfig
|
||||
{
|
||||
protected:
|
||||
virtual void initDefaults()=0;
|
||||
virtual bool checkConfig()=0;
|
||||
virtual void saveConfigValues(JsonDocument &json)=0;
|
||||
virtual void loadConfigValues(JsonDocument &json)=0;
|
||||
public:
|
||||
cConfig();
|
||||
virtual bool loadConfig(std::istream &stream,bool binary=false);
|
||||
virtual void saveConfig(std::ostream &stream,bool binary=false);
|
||||
|
||||
virtual void loadConfig(const char * filename);
|
||||
virtual void saveConfig(const char * filename);
|
||||
};
|
||||
|
||||
class atMainConfig:public cConfig
|
||||
{
|
||||
public:
|
||||
uint16_t id;
|
||||
std::string name;
|
||||
char symbol;
|
||||
uint16_t radius;
|
||||
uint8_t units;
|
||||
uint16_t reportInterval;
|
||||
std::string timeZone;
|
||||
uint32_t screenIdleTimeout;
|
||||
bool useVibration;
|
||||
uint32_t vibrationTime;
|
||||
uint32_t maxPeerTimeout;
|
||||
uint32_t timers[AT_MAX_TIMERS];
|
||||
int32_t testLat;
|
||||
int32_t testLng;
|
||||
protected:
|
||||
virtual void initDefaults();
|
||||
virtual bool checkConfig();
|
||||
virtual void saveConfigValues(JsonDocument &json);
|
||||
virtual void loadConfigValues(JsonDocument &json);
|
||||
public:
|
||||
atMainConfig();
|
||||
};
|
||||
|
||||
class atNetworkConfig:public cConfig
|
||||
{
|
||||
public:
|
||||
cLoraChannelCondfig channelConfig;
|
||||
uint8_t networkId;
|
||||
std::string password;
|
||||
int32_t requestInfoInterval;
|
||||
protected:
|
||||
virtual void initDefaults();
|
||||
virtual bool checkConfig();
|
||||
virtual void saveConfigValues(JsonDocument &json);
|
||||
virtual void loadConfigValues(JsonDocument &json);
|
||||
public:
|
||||
atNetworkConfig();
|
||||
};
|
||||
|
||||
class atAGDNetworkConfig:public cConfig
|
||||
{
|
||||
public:
|
||||
cLoraChannelCondfig channelConfig;
|
||||
uint32_t updateTimeout;
|
||||
protected:
|
||||
virtual void initDefaults();
|
||||
virtual bool checkConfig();
|
||||
virtual void saveConfigValues(JsonDocument &json);
|
||||
virtual void loadConfigValues(JsonDocument &json);
|
||||
public:
|
||||
atAGDNetworkConfig();
|
||||
};
|
||||
//Player Status
|
||||
#define PS_NORMAL 0
|
||||
#define PS_DEAD 1
|
||||
|
||||
struct sPlayerStatus
|
||||
{
|
||||
uint8_t status;
|
||||
uint8_t options;
|
||||
|
||||
sPlayerStatus()
|
||||
{
|
||||
status = PS_NORMAL;
|
||||
options = 0;
|
||||
}
|
||||
};
|
||||
|
||||
typedef std::map<uint16_t,sPOI> cPOIMap;
|
||||
typedef std::list<sPOI> cPOIList;
|
||||
class cTimersChain;
|
||||
class cTimer
|
||||
{
|
||||
friend cTimersChain;
|
||||
protected:
|
||||
int32_t m_duration;
|
||||
int64_t m_startTime;
|
||||
int64_t m_pauseTime;
|
||||
std::string m_name;
|
||||
public:
|
||||
cTimer():m_duration(0),m_startTime(0),m_pauseTime(0)
|
||||
{
|
||||
}
|
||||
~cTimer(){}
|
||||
int32_t getDuration() { return m_duration; }
|
||||
void setDuration(uint32_t value) { m_duration = value; }
|
||||
std::string getName() { return m_name; }
|
||||
void setName(const char *name) { m_name = name; }
|
||||
bool isEnabled() { return m_duration>0; }
|
||||
bool isStarted() { return m_startTime>0; }
|
||||
bool isPaused() { return isStarted() && m_pauseTime>0; }
|
||||
void start(int64_t now) { m_startTime = now; m_pauseTime = 0; }
|
||||
void reset() { m_startTime = m_pauseTime = 0; }
|
||||
void pause(int64_t now) { if(isStarted() && m_pauseTime==0) m_pauseTime = now; }
|
||||
void resume(int64_t now) {
|
||||
if(isPaused())
|
||||
{
|
||||
m_startTime = now-(m_pauseTime-m_startTime);
|
||||
m_pauseTime = 0;
|
||||
}
|
||||
}
|
||||
int64_t getCurrentDuration(int64_t now)
|
||||
{
|
||||
if(isStarted())
|
||||
{
|
||||
int64_t duration;
|
||||
if(m_pauseTime>0)
|
||||
duration = m_pauseTime-m_startTime;
|
||||
else
|
||||
duration = now-m_startTime;
|
||||
if(duration>=m_duration)
|
||||
return m_duration;
|
||||
else
|
||||
return duration;
|
||||
}
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
bool isFinished(int64_t now) {
|
||||
if(isStarted())
|
||||
return getCurrentDuration(now)>=m_duration;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
typedef std::vector<cTimer*> cTimersVector;
|
||||
typedef std::map<uint8_t,cTimer*> cTimersMap;
|
||||
|
||||
class cTimersChain
|
||||
{
|
||||
protected:
|
||||
cTimersVector m_timers;
|
||||
uint8_t m_currentIdx;
|
||||
std::string m_name;
|
||||
|
||||
void processTimers(int64_t now)
|
||||
{
|
||||
while(m_currentIdx<m_timers.size())
|
||||
{
|
||||
if(isStarted()&&!isPaused())
|
||||
{
|
||||
if(m_timers[m_currentIdx]->isFinished(now))
|
||||
{
|
||||
int64_t nowNew = m_timers[m_currentIdx]->m_startTime+m_timers[m_currentIdx]->m_duration;
|
||||
m_currentIdx++;
|
||||
if(m_currentIdx<m_timers.size())
|
||||
{
|
||||
m_timers[m_currentIdx]->start(nowNew);
|
||||
}
|
||||
}
|
||||
else
|
||||
break;
|
||||
}
|
||||
else
|
||||
break;
|
||||
}
|
||||
}
|
||||
public:
|
||||
cTimersChain():m_currentIdx(0){};
|
||||
~cTimersChain(){}
|
||||
std::string getName() { return m_name; }
|
||||
void setName(const char *name) { m_name = name; }
|
||||
cTimersVector &getTimers() { return m_timers; }
|
||||
void addTimer(cTimer* timer) {
|
||||
m_timers.push_back(timer);
|
||||
}
|
||||
|
||||
bool isEnabled()
|
||||
{
|
||||
for(auto itr=m_timers.begin(); itr!=m_timers.end(); itr++)
|
||||
if(!(*itr)->isEnabled())
|
||||
return false;
|
||||
return m_timers.size();
|
||||
}
|
||||
|
||||
bool isStarted()
|
||||
{
|
||||
if(m_currentIdx<m_timers.size())
|
||||
{
|
||||
return m_timers[m_currentIdx]->isStarted();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool isPaused()
|
||||
{
|
||||
if(m_currentIdx<m_timers.size())
|
||||
{
|
||||
return m_timers[m_currentIdx]->isPaused();
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void reset()
|
||||
{
|
||||
m_currentIdx = 0;
|
||||
for(auto itr=m_timers.begin(); itr!=m_timers.end(); itr++)
|
||||
(*itr)->reset();
|
||||
}
|
||||
|
||||
void start(int64_t now)
|
||||
{
|
||||
reset();
|
||||
if(m_timers.size())
|
||||
m_timers.front()->start(now);
|
||||
}
|
||||
|
||||
void pause(int64_t now)
|
||||
{
|
||||
processTimers(now);
|
||||
if(m_currentIdx<m_timers.size())
|
||||
{
|
||||
m_timers[m_currentIdx]->pause(now);
|
||||
}
|
||||
}
|
||||
|
||||
void resume(int64_t now)
|
||||
{
|
||||
if(m_currentIdx<m_timers.size())
|
||||
{
|
||||
m_timers[m_currentIdx]->resume(now);
|
||||
}
|
||||
}
|
||||
|
||||
bool isFinished(int64_t now)
|
||||
{
|
||||
processTimers(now);
|
||||
return m_currentIdx>=m_timers.size();
|
||||
}
|
||||
|
||||
void loop(int64_t now)
|
||||
{
|
||||
processTimers(now);
|
||||
}
|
||||
|
||||
uint8_t getCurrentIdx() { return m_currentIdx; }
|
||||
};
|
||||
|
||||
typedef std::list<cTimersChain*> cTimersChains;
|
||||
|
||||
class atGlobal:public cMapProcessorCallbacks
|
||||
{
|
||||
protected:
|
||||
atMainConfig m_config;
|
||||
atNetworkConfig m_networkConfig;
|
||||
atAGDNetworkConfig m_agdNetworkConfig;
|
||||
Power *m_power;
|
||||
|
||||
virtual void initPower()
|
||||
{
|
||||
m_mapCenterX = m_mapCenterY = 0;
|
||||
m_power = new Power();
|
||||
m_power->setup();
|
||||
m_power->setStatusHandler(m_powerStatus);
|
||||
m_powerStatus->observe(&m_power->newStatus);
|
||||
}
|
||||
uint16_t m_lastPOIID;
|
||||
public:
|
||||
bool m_has_axp192;
|
||||
bool m_has_oled;
|
||||
PowerStatus *m_powerStatus;
|
||||
GPSStatus *m_gpsStatus;
|
||||
IMUStatus *m_imuStatus;
|
||||
sPlayerStatus m_playerStatus;
|
||||
cSideButtons m_sideButtons;
|
||||
cMaps m_maps;
|
||||
cMapProcessor m_mapsProcessor;
|
||||
cPOIMap m_poiMap;
|
||||
cPOIList m_poiMapStatic;
|
||||
double m_mapCenterX,m_mapCenterY;
|
||||
cTimersMap m_timers;
|
||||
cTimersChains m_timersChains;
|
||||
|
||||
atGlobal():m_mapsProcessor(m_maps.getMaps(),this)
|
||||
{
|
||||
m_lastPOIID = 0;
|
||||
m_has_axp192 = m_has_oled = false;
|
||||
m_powerStatus = new PowerStatus();
|
||||
m_power = NULL;
|
||||
m_gpsStatus = new GPSStatus();
|
||||
m_imuStatus = new IMUStatus();
|
||||
m_config.loadConfig(MAIN_CONFIG_FILENAME);
|
||||
m_networkConfig.loadConfig(NET_CONFIG_FILENAME);
|
||||
m_agdNetworkConfig.loadConfig(AGD_NET_CONFIG_FILENAME);
|
||||
for(int i=0; i<AT_MAX_TIMERS; i++)
|
||||
{
|
||||
char buf[8];
|
||||
m_timers[i] = new cTimer();
|
||||
m_timers[i]->setDuration(m_config.timers[i]);
|
||||
snprintf(buf,sizeof(buf),"T%d",i+1);
|
||||
m_timers[i]->setName(buf);
|
||||
}
|
||||
cTimersChain *tc = new cTimersChain();
|
||||
tc->setName("T1->T2");
|
||||
tc->addTimer(m_timers[0]);
|
||||
tc->addTimer(m_timers[1]);
|
||||
m_timersChains.push_back(tc);
|
||||
|
||||
#ifndef AD_NOSCREEN
|
||||
m_maps.loadFromBinary(SCREEN_FLASH_META_BINARY_FILE);
|
||||
#endif
|
||||
}
|
||||
|
||||
virtual ~atGlobal()
|
||||
{
|
||||
delete m_imuStatus;
|
||||
delete m_gpsStatus;
|
||||
delete m_powerStatus;
|
||||
if(m_power)
|
||||
delete m_power;
|
||||
}
|
||||
|
||||
virtual void onMapLoad(cMap *map);
|
||||
|
||||
void updateTimers()
|
||||
{
|
||||
for(int i=0; i<AT_MAX_TIMERS; i++)
|
||||
{
|
||||
m_timers[i]->setDuration(m_config.timers[i]);
|
||||
}
|
||||
}
|
||||
|
||||
float getDirection()
|
||||
{
|
||||
return (18000-m_imuStatus->getHeading()+m_gpsStatus->getMagDec()*100)*PI/18000.0;
|
||||
}
|
||||
|
||||
float getDirectionDeg()
|
||||
{
|
||||
float result = m_imuStatus->getHeading()/100.0+m_gpsStatus->getMagDec();
|
||||
if(result<0)
|
||||
result += 360;
|
||||
if(result>=360)
|
||||
result -= 360;
|
||||
return result;
|
||||
}
|
||||
|
||||
uint16_t createPOI()
|
||||
{
|
||||
return ++m_lastPOIID;
|
||||
}
|
||||
|
||||
void setPOI(uint16_t id,const sPOI &poi)
|
||||
{
|
||||
m_poiMap[id] = poi;
|
||||
}
|
||||
|
||||
void removePOI(uint16_t id)
|
||||
{
|
||||
m_poiMap.erase(id);
|
||||
}
|
||||
|
||||
virtual void init()
|
||||
{
|
||||
initPower();
|
||||
}
|
||||
|
||||
virtual void loop()
|
||||
{
|
||||
concurrency::periodicScheduler.loop();
|
||||
}
|
||||
|
||||
Power *getPower() {
|
||||
return m_power;
|
||||
}
|
||||
|
||||
atMainConfig &getConfig()
|
||||
{
|
||||
return m_config;
|
||||
}
|
||||
|
||||
atNetworkConfig &getNetworkConfig()
|
||||
{
|
||||
return m_networkConfig;
|
||||
}
|
||||
|
||||
virtual void onScreenStateChange(){};
|
||||
|
||||
void processTimers(int64_t now)
|
||||
{
|
||||
for(auto itr=m_timersChains.begin(); itr!= m_timersChains.end(); itr++)
|
||||
(*itr)->loop(now);
|
||||
}
|
||||
};
|
||||
+1785
File diff suppressed because it is too large
Load Diff
+231
@@ -0,0 +1,231 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include <power.h>
|
||||
#include <Wire.h>
|
||||
#include "power.h"
|
||||
#include "atglobal.h"
|
||||
#include "concurrency/Thread.h"
|
||||
#include "utils.h"
|
||||
#include "gps/UBloxGPS.h"
|
||||
#include "radio/LoraRadio.h"
|
||||
#include "atcommon.h"
|
||||
#include "atscreen.h"
|
||||
#include "Adafruit_MCP23017.h"
|
||||
#include "imu.h"
|
||||
#include "atNetworkProcessor.h"
|
||||
#include "comm_prot_parser.h"
|
||||
#include "atble.h"
|
||||
|
||||
#define BA_NONE 0
|
||||
#define BA_STATUS 1
|
||||
#define BA_RADIUS 2
|
||||
#define BA_BLE 3
|
||||
#define BA_MENU 4
|
||||
#define BA_MAP 5
|
||||
#define BA_MAPLEFT 6
|
||||
#define BA_MAPRIGHT 7
|
||||
#define BA_MAPDOWN 8
|
||||
#define BA_MAPUP 9
|
||||
#define BA_MAPEXIT 10
|
||||
#define BA_MAPRESET 11
|
||||
#define BA_EXIT 12
|
||||
#define BA_TIMER 13
|
||||
#define BA_TIMERS 14
|
||||
#define BA_TIMERS_RST 15
|
||||
#define BA_AGD_UPDATE 16
|
||||
#define BA_SLEEP 17
|
||||
|
||||
struct sCommand
|
||||
{
|
||||
uint8_t id;
|
||||
uint8_t cmd;
|
||||
int64_t param1;
|
||||
int64_t param2;
|
||||
int64_t param3;
|
||||
};
|
||||
|
||||
typedef cProtectedQueue<sCommand> cCommands;
|
||||
|
||||
struct sLoraPacket
|
||||
{
|
||||
std::vector<byte> data;
|
||||
float SNR;
|
||||
float RSSI;
|
||||
int64_t time;
|
||||
int64_t preambleTime;
|
||||
};
|
||||
|
||||
struct sLoraPacketToSend
|
||||
{
|
||||
sNetworkPacket packet;
|
||||
};
|
||||
|
||||
|
||||
typedef cProtectedQueue<sLoraPacket> cLoraPackets;
|
||||
typedef cProtectedQueue<sLoraPacketToSend> cLoraPacketsToSend;
|
||||
|
||||
class cVMotor
|
||||
{
|
||||
protected:
|
||||
int64_t m_stopTime;
|
||||
int8_t m_pin;
|
||||
int8_t m_pin2;
|
||||
Adafruit_MCP23017 *m_expander;
|
||||
void setValue(bool b)
|
||||
{
|
||||
if(m_pin>=0)
|
||||
m_expander->digitalWrite(m_pin, b?HIGH:LOW);
|
||||
if(m_pin2>=0)
|
||||
m_expander->digitalWrite(m_pin2, b?HIGH:LOW);
|
||||
}
|
||||
public:
|
||||
cVMotor(int8_t pin, int8_t pin2, Adafruit_MCP23017* expander):m_stopTime(0),m_pin(pin), m_pin2(pin2),m_expander(expander)
|
||||
{
|
||||
}
|
||||
|
||||
void loop(int64_t now)
|
||||
{
|
||||
if(m_stopTime>0)
|
||||
{
|
||||
if(now>=m_stopTime)
|
||||
{
|
||||
m_stopTime = 0;
|
||||
setValue(false);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
loop(millis());
|
||||
}
|
||||
|
||||
void vibrate(uint32_t duration)
|
||||
{
|
||||
m_stopTime = millis()+duration;
|
||||
setValue(true);
|
||||
}
|
||||
|
||||
void stop()
|
||||
{
|
||||
m_stopTime = 0;
|
||||
setValue(false);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
#define AT_BUTTONS_COUNT 6
|
||||
//Click type
|
||||
#define ATCT_NORMAL 1
|
||||
#define ATCT_LONG 2
|
||||
|
||||
#define AT_LONG_CLICK_DURATION 1000
|
||||
|
||||
#define ATSBS_MAIN 0
|
||||
#define ATSBS_MAP 1
|
||||
#define ATSBS_TIMERS 2
|
||||
|
||||
class cAirsoftTracker:public atGlobal, public cNetworkProcessorInterface
|
||||
{
|
||||
protected:
|
||||
typedef concurrency::ThreadFunctionTemplate<cAirsoftTracker> cThreadFunction;
|
||||
UBloxGPS m_gps;
|
||||
cLoraDevice *m_lora;
|
||||
cThreadFunction *m_loraThread;
|
||||
cLoraPackets m_loraPackets;
|
||||
cLoraPacketsToSend m_loraPacketsToSend;
|
||||
cATScreen *m_screen;
|
||||
cThreadFunction *m_core0Thread;
|
||||
bool m_core0ThreadReady;
|
||||
cCommands m_commands;
|
||||
cIMU *m_imu;
|
||||
Adafruit_MCP23017 *m_expander;
|
||||
cVMotor *m_vmotor;
|
||||
uint16_t m_gpioExpander = 0;
|
||||
int64_t m_timer1 = 0;
|
||||
int64_t m_timer2 = 0;
|
||||
uint16_t m_ps = 0;
|
||||
int64_t m_idleTimer = 0;
|
||||
int64_t m_screenTimer = 0;
|
||||
bool m_displaySleep = false;
|
||||
std::string m_bootMsg1;
|
||||
std::string m_bootMsg2;
|
||||
cNetworkProcessor m_networkProcessor;
|
||||
uint32_t m_gpsSeq = 0xFFFFFFFF;
|
||||
uint8_t m_serialSignatureState=0;
|
||||
atBLE m_bleClient;
|
||||
int64_t m_buttonsTime[AT_BUTTONS_COUNT];
|
||||
int8_t m_buttonsIdx[AT_BUTTONS_COUNT];
|
||||
uint8_t m_sideButtonsState = ATSBS_MAIN;
|
||||
int64_t m_agdUpdateTimer = 0;
|
||||
uint16_t m_agdPOI=0;
|
||||
int64_t m_sideButtonsUpdateTime = 0;
|
||||
|
||||
void scanI2Cdevice(TwoWire &wd)
|
||||
{
|
||||
byte err, addr;
|
||||
int nDevices = 0;
|
||||
for (addr = 1; addr < 127; addr++)
|
||||
{
|
||||
wd.beginTransmission(addr);
|
||||
err = wd.endTransmission();
|
||||
if (err == 0)
|
||||
{
|
||||
DEBUG_MSG("I2C device found at address 0x%x\n", addr);
|
||||
|
||||
nDevices++;
|
||||
|
||||
if (addr == SSD1306_ADDRESS)
|
||||
{
|
||||
m_has_oled = true;
|
||||
DEBUG_MSG("ssd1306 display found\n");
|
||||
}
|
||||
#ifdef AXP192_SLAVE_ADDRESS
|
||||
if (addr == AXP192_SLAVE_ADDRESS)
|
||||
{
|
||||
m_has_axp192 = true;
|
||||
DEBUG_MSG("axp192 PMU found\n");
|
||||
}
|
||||
#endif
|
||||
}
|
||||
else if (err == 4)
|
||||
{
|
||||
DEBUG_MSG("Unknow error at address 0x%x\n", addr);
|
||||
}
|
||||
}
|
||||
if (nDevices == 0)
|
||||
DEBUG_MSG("No I2C devices found\n");
|
||||
else
|
||||
DEBUG_MSG("done\n");
|
||||
}
|
||||
|
||||
void loraThread(cThreadFunction *thread);
|
||||
void core0Thread(cThreadFunction *thread);
|
||||
void mainThread(cThreadFunction *thread);
|
||||
void processCommand(const sCommand &cmd);
|
||||
void processIMU();
|
||||
void processExpander();
|
||||
void updateBootScreen();
|
||||
void updateDistances(bool force=false);
|
||||
void updateSideButtons();
|
||||
void onButtonClick(uint8_t idx,uint8_t type);
|
||||
void programMode();
|
||||
void vibration();
|
||||
void startAGDUpdate();
|
||||
void stopAGDUpdate();
|
||||
bool isAGDUpdate()
|
||||
{
|
||||
return m_agdUpdateTimer!=0;
|
||||
}
|
||||
void processLoraPackets();
|
||||
void processLoraAGDPackets();
|
||||
public:
|
||||
cAirsoftTracker();
|
||||
virtual ~cAirsoftTracker(){};
|
||||
void init();
|
||||
void mainInit();
|
||||
void mainLoop();
|
||||
virtual void sendPacket(const sNetworkPacket &packet);
|
||||
cIMU *getIMU() { return m_imu; }
|
||||
virtual void onScreenStateChange();
|
||||
};
|
||||
+128
@@ -0,0 +1,128 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include "atglobal.h"
|
||||
#include "utils.h"
|
||||
#include "atcommon.h"
|
||||
|
||||
struct sColor
|
||||
{
|
||||
uint8_t r;
|
||||
uint8_t g;
|
||||
uint8_t b;
|
||||
uint8_t a;
|
||||
sColor(uint8_t _r, uint8_t _g, uint8_t _b, uint8_t _a=255):r(_r),g(_g),b(_b),a(_a)
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
sColor(uint32_t c):r((c >> 16)&0xFF),g((c >> 8)&0xFF),b(c&0xFF),a((c >> 24)&0xFF)
|
||||
{
|
||||
}
|
||||
|
||||
sColor operator+(const sColor& c)
|
||||
{
|
||||
sColor cc(std::min(255,(uint16_t)r+c.r),std::min(255,(uint16_t)g+c.g),std::min(255,(uint16_t)b+c.b),std::min(255,(uint16_t)a+c.a));
|
||||
return cc;
|
||||
}
|
||||
|
||||
uint8_t getA()
|
||||
{
|
||||
return a;
|
||||
}
|
||||
|
||||
uint32_t getRGB()
|
||||
{
|
||||
return (((uint32_t)r) << 16) | (((uint32_t)g) << 8) | b;
|
||||
}
|
||||
|
||||
uint32_t getRBG()
|
||||
{
|
||||
return (((uint32_t)r) << 16) | (((uint32_t)b) << 8) | g;
|
||||
}
|
||||
|
||||
uint32_t getEVEColor()
|
||||
{
|
||||
return ((4UL<<24)|(((r)&255UL)<<16)|(((g)&255UL)<<8)|b);
|
||||
}
|
||||
|
||||
uint32_t getEVEColorA()
|
||||
{
|
||||
return ((0x10UL<<24)|a);
|
||||
}
|
||||
|
||||
|
||||
} __attribute__ ((packed));
|
||||
|
||||
typedef sColor color_t;
|
||||
struct sScreenTheme
|
||||
{
|
||||
color_t bgColor;
|
||||
color_t textColor;
|
||||
color_t bootTextColor;
|
||||
color_t circleOuterColor;
|
||||
color_t circleLineColor;
|
||||
color_t circleBackgroundColor;
|
||||
color_t circleHeadingColor;
|
||||
color_t peerTextColor;
|
||||
color_t peerNormalColor;
|
||||
color_t peerDeadColor;
|
||||
color_t peerEnemyColor;
|
||||
color_t sideButtonsTextColor;
|
||||
color_t timerTextColor;
|
||||
color_t timerRectColor;
|
||||
color_t peerStaticColor;
|
||||
color_t peerDynamicColor;
|
||||
color_t peerAccuracyMask;
|
||||
uint8_t peerAccuracyA;
|
||||
|
||||
sScreenTheme():bgColor(0x0),textColor(0x208020),bootTextColor(255,255,255),circleOuterColor(0x404040),circleLineColor(8,23,153),circleBackgroundColor(89-40, 150-40, 145-40),circleHeadingColor(255,0,0),
|
||||
peerTextColor(255,255,255),peerNormalColor(0,255,0),peerDeadColor(255,0,0),peerEnemyColor(0xff4dff),sideButtonsTextColor(0x60A060),timerTextColor(255,0,0),timerRectColor(128,128,128),peerStaticColor(0,0,255),
|
||||
peerDynamicColor(0,0,255),peerAccuracyMask(64,64,64),peerAccuracyA(64)
|
||||
{
|
||||
}
|
||||
};
|
||||
|
||||
// Screen state
|
||||
#define SS_BOOT 1
|
||||
#define SS_MAIN 2
|
||||
#define SS_MENU 3
|
||||
|
||||
#define EV_BTN_ON 1
|
||||
#define EV_BTN_OFF 2
|
||||
class cATScreen
|
||||
{
|
||||
protected:
|
||||
atGlobal &m_global;
|
||||
sScreenTheme m_theme;
|
||||
uint8_t m_screenState;
|
||||
public:
|
||||
cATScreen(atGlobal &global);
|
||||
virtual ~cATScreen();
|
||||
|
||||
virtual bool init();
|
||||
virtual void showBootScreen(const char * msg1,const char * msg2)=0;
|
||||
virtual void showShutdownScreen() {};
|
||||
virtual void closeBootScreen()=0;
|
||||
virtual void beginMainScreen()=0;
|
||||
virtual void drawPOI(sPOI &poi,int16_t count=0)=0;
|
||||
virtual void endMainScreen()=0;
|
||||
virtual void setBacklight(uint8_t level)=0;
|
||||
virtual void screenOn()=0;
|
||||
virtual void screenOff()=0;
|
||||
virtual void drawSideButtons()=0;
|
||||
virtual void showProgramMode()=0;
|
||||
virtual void showMenu(){};
|
||||
virtual uint32_t getFramesCount()=0;
|
||||
virtual void loop() {};
|
||||
virtual uint8_t getScreenState() { return m_screenState; }
|
||||
virtual void setScreenState(uint8_t value) { m_screenState=value; }
|
||||
virtual void processEvent(uint8_t event, uint32_t param1, uint32_t param2){};
|
||||
virtual void drawTimers(bool forceAll) {};
|
||||
virtual uint16_t getRadius() = 0;
|
||||
|
||||
//Flash interface
|
||||
virtual uint32_t flashProgrammStart()=0;
|
||||
virtual void flashProgrammEnd()=0;
|
||||
virtual void flashWrite(uint32_t address,uint16_t size, uint8_t *data)=0;
|
||||
virtual void flashRead(uint32_t address,uint16_t size, uint8_t *data)=0;
|
||||
};
|
||||
@@ -0,0 +1,29 @@
|
||||
#pragma once
|
||||
#include "atscreen.h"
|
||||
|
||||
class cATScreenDummy: public cATScreen
|
||||
{
|
||||
public:
|
||||
cATScreenDummy(atGlobal &global):cATScreen(global) {};
|
||||
virtual ~cATScreenDummy(){};
|
||||
|
||||
virtual bool init() { return true; };
|
||||
virtual void showBootScreen(const char * msg1,const char * msg2) {};
|
||||
virtual void closeBootScreen() {};
|
||||
virtual void beginMainScreen() {};
|
||||
virtual void drawPOI(sPOI &poi,int16_t count=0) {};
|
||||
virtual void endMainScreen() {};
|
||||
virtual void setBacklight(uint8_t level) {};
|
||||
virtual void screenOn() {};
|
||||
virtual void screenOff() {};
|
||||
virtual void drawSideButtons() {};
|
||||
virtual void showProgramMode() {};
|
||||
virtual uint32_t getFramesCount() { return 0;};
|
||||
virtual uint16_t getRadius() { return m_global.getConfig().radius; }
|
||||
|
||||
//Flash interface
|
||||
virtual uint32_t flashProgrammStart() { return 0;};
|
||||
virtual void flashProgrammEnd() {};
|
||||
virtual void flashWrite(uint32_t address,uint16_t size, uint8_t *data) {};
|
||||
virtual void flashRead(uint32_t address,uint16_t size, uint8_t *data) {};
|
||||
};
|
||||
@@ -0,0 +1,10 @@
|
||||
#pragma once
|
||||
|
||||
#include <pgmspace.h>
|
||||
|
||||
#define ATS_DEAD PSTR("DEAD")
|
||||
#define ATS_ALIVE PSTR("ALIVE")
|
||||
#define ATS_BTN_DEAD PSTR("[L]->DEAD")
|
||||
#define ATS_BTN_ALIVE PSTR("[L]->ALIVE")
|
||||
#define ATS_MENU PSTR("MENU")
|
||||
#define ATS_MRADIUS PSTR("RADIUS/MAP")
|
||||
@@ -0,0 +1,317 @@
|
||||
/*Command Line: fnt_cvt.exe -f legacy -C BT81X -i Q:/fonts/calibri.ttf -s 14 -d 153600 -c setfont2 -l 32 -a -o D:/Projects/Embedded/AirsoftTracker/fonts*/
|
||||
|
||||
/*95 characters have been converted */
|
||||
|
||||
/* 148 Metric Block Begin +++ */
|
||||
/*('file properties ', 'format ', 'L4', ' stride ', 7, ' width ', 14, 'height', 14)*/
|
||||
{
|
||||
/* Widths */
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3,5,6,7,7,10,10,3,4,4,7,7,4,4,4,6,7,7,7,7,7,7,7,7,7,7,4,4,7,7,7,6,13,8,8,8,9,7,6,9,9,4,4,7,6,12,9,9,7,10,8,6,7,9,8,13,7,7,7,4,6,4,7,8,4,7,7,6,7,7,5,7,7,3,4,7,3,11,7,7,7,7,5,5,5,7,7,10,6,7,6,4,6,5,7,0,
|
||||
/* Format */
|
||||
2,0,0,0,
|
||||
/* Stride */
|
||||
7,0,0,0,
|
||||
/* Max Width */
|
||||
14,0,0,0,
|
||||
/* Max Height */
|
||||
14,0,0,0,
|
||||
/* Raw Data Address in Decimal: <153748> */
|
||||
148,88,2,0,
|
||||
|
||||
/* 148 Metric Block End --- */
|
||||
|
||||
/*Bitmap Raw Data begin +++*/
|
||||
/*The expected raw bitmap size is 9310 Bytes */
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,31,32,0,0,0,0,0,31,32,0,0,0,0,0,31,32,0,0,0,0,0,15,32,
|
||||
0,0,0,0,0,15,16,0,0,0,0,0,15,16,0,0,0,0,0,14,16,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,46,32,0,0,0,0,0,46,32,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,15,25,128,0,0,0,0,15,25,112,0,0,0,0,14,8,112,0,0,0,0,
|
||||
13,7,96,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,224,29,0,0,0,0,2,208,76,0,0,
|
||||
0,0,95,255,255,160,0,0,0,6,144,120,0,0,0,0,8,112,150,0,0,0,0,191,255,255,64,0,0,0,12,64,
|
||||
210,0,0,0,0,13,32,241,0,0,0,0,14,1,224,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,7,96,0,0,0,0,0,9,80,0,0,0,0,4,207,231,0,0,0,0,14,97,58,
|
||||
0,0,0,0,31,32,0,0,0,0,0,12,196,0,0,0,0,0,1,142,213,0,0,0,0,0,0,127,32,0,0,0,
|
||||
0,0,14,80,0,0,0,89,33,110,32,0,0,0,27,239,196,0,0,0,0,0,133,0,0,0,0,0,0,148,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,10,233,0,10,80,0,0,90,
|
||||
29,32,106,0,0,0,119,10,66,209,0,0,0,106,29,44,48,0,0,0,27,232,135,142,177,0,0,0,4,178,209,166,
|
||||
0,0,0,29,36,160,119,0,0,0,181,2,209,165,0,0,6,144,0,158,176,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,141,235,16,0,0,0,4,226,29,112,0,0,
|
||||
0,6,192,12,112,0,0,0,2,244,156,16,0,0,0,0,175,161,0,0,0,0,7,219,176,14,32,0,0,47,49,202,
|
||||
47,16,0,0,63,0,45,233,0,0,0,30,129,42,251,16,0,0,4,207,232,25,224,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,15,32,0,0,0,0,0,15,16,0,0,
|
||||
0,0,0,14,16,0,0,0,0,0,13,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,180,0,0,0,0,0,2,224,
|
||||
0,0,0,0,0,7,144,0,0,0,0,0,11,96,0,0,0,0,0,14,48,0,0,0,0,0,15,32,0,0,0,0,
|
||||
0,31,32,0,0,0,0,0,31,32,0,0,0,0,0,14,64,0,0,0,0,0,11,96,0,0,0,0,0,7,160,0,
|
||||
0,0,0,0,2,224,0,0,0,0,0,0,180,0,0,0,0,0,0,0,0,0,0,0,0,29,16,0,0,0,0,0,
|
||||
10,96,0,0,0,0,0,6,160,0,0,0,0,0,3,224,0,0,0,0,0,0,242,0,0,0,0,0,0,228,0,0,
|
||||
0,0,0,0,213,0,0,0,0,0,0,228,0,0,0,0,0,0,243,0,0,0,0,0,3,240,0,0,0,0,0,6,
|
||||
176,0,0,0,0,0,11,96,0,0,0,0,0,30,16,0,0,0,0,0,0,0,0,0,0,0,0,0,208,0,0,0,
|
||||
0,0,184,200,160,0,0,0,0,27,250,0,0,0,0,0,184,200,160,0,0,0,0,0,208,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,14,0,0,0,0,0,0,15,0,0,0,0,0,
|
||||
0,15,0,0,0,0,0,127,255,255,112,0,0,0,0,15,0,0,0,0,0,0,15,0,0,0,0,0,0,14,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,11,112,0,0,0,0,0,12,96,
|
||||
0,0,0,0,0,45,16,0,0,0,0,0,133,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,127,251,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,198,0,0,0,0,
|
||||
0,0,214,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,211,0,0,0,0,0,4,208,0,0,0,0,0,9,128,0,0,0,0,0,30,32,0,0,0,
|
||||
0,0,92,0,0,0,0,0,0,182,0,0,0,0,0,1,225,0,0,0,0,0,7,160,0,0,0,0,0,12,80,0,
|
||||
0,0,0,0,62,0,0,0,0,0,0,137,0,0,0,0,0,0,211,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,3,207,213,0,0,0,0,13,113,95,32,0,0,0,78,0,12,96,
|
||||
0,0,0,107,0,10,128,0,0,0,123,0,9,144,0,0,0,107,0,10,128,0,0,0,94,0,12,96,0,0,0,30,
|
||||
113,110,16,0,0,0,4,223,196,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5,231,0,0,0,0,0,155,199,0,0,0,0,0,48,167,
|
||||
0,0,0,0,0,0,167,0,0,0,0,0,0,167,0,0,0,0,0,0,167,0,0,0,0,0,0,167,0,0,0,0,
|
||||
0,0,167,0,0,0,0,0,191,255,244,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,222,178,0,0,0,0,27,33,203,0,0,0,0,
|
||||
0,0,125,0,0,0,0,0,0,155,0,0,0,0,0,2,228,0,0,0,0,0,28,128,0,0,0,0,0,186,0,0,
|
||||
0,0,0,11,160,0,0,0,0,0,63,255,255,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,6,222,178,0,0,0,0,11,33,186,0,0,
|
||||
0,0,0,0,124,0,0,0,0,0,3,214,0,0,0,0,5,255,194,0,0,0,0,0,2,157,16,0,0,0,0,0,
|
||||
47,48,0,0,0,73,33,142,16,0,0,0,26,238,179,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,7,246,0,0,0,0,0,45,199,
|
||||
0,0,0,0,0,167,183,0,0,0,0,3,208,183,0,0,0,0,12,64,183,0,0,0,0,106,0,183,0,0,0,0,
|
||||
175,255,255,144,0,0,0,0,0,183,0,0,0,0,0,0,166,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,12,255,250,0,0,0,0,13,
|
||||
48,0,0,0,0,0,13,48,0,0,0,0,0,13,254,179,0,0,0,0,1,1,158,16,0,0,0,0,0,31,48,0,
|
||||
0,0,0,0,31,32,0,0,0,72,17,172,0,0,0,0,27,238,162,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,125,252,16,0,0,
|
||||
0,7,179,3,0,0,0,0,14,32,0,0,0,0,0,47,174,232,0,0,0,0,79,81,62,64,0,0,0,78,0,10,
|
||||
128,0,0,0,47,32,11,96,0,0,0,13,145,94,32,0,0,0,3,207,212,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,79,255,255,96,
|
||||
0,0,0,0,0,30,48,0,0,0,0,0,123,0,0,0,0,0,0,213,0,0,0,0,0,5,208,0,0,0,0,0,
|
||||
12,112,0,0,0,0,0,63,16,0,0,0,0,0,170,0,0,0,0,0,1,243,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,206,
|
||||
214,0,0,0,0,14,96,95,16,0,0,0,31,32,31,32,0,0,0,9,196,184,0,0,0,0,1,206,210,0,0,0,
|
||||
0,29,112,126,32,0,0,0,108,0,11,112,0,0,0,78,48,62,80,0,0,0,7,223,215,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
5,206,195,0,0,0,0,46,65,125,0,0,0,0,108,0,14,48,0,0,0,78,48,62,80,0,0,0,8,238,190,80,
|
||||
0,0,0,0,0,14,64,0,0,0,0,0,79,16,0,0,0,19,3,216,0,0,0,0,29,253,112,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,153,0,0,0,0,0,0,153,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,153,0,0,0,0,0,0,153,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,9,144,0,0,0,0,0,
|
||||
9,144,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,8,160,0,0,0,0,0,9,144,0,0,
|
||||
0,0,0,13,32,0,0,0,0,0,88,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,59,64,0,0,0,0,41,213,0,0,0,0,24,231,0,0,0,
|
||||
0,0,142,32,0,0,0,0,0,24,214,0,0,0,0,0,0,41,212,0,0,0,0,0,0,59,64,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,79,255,255,
|
||||
64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,79,255,255,64,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,75,64,0,0,0,0,0,4,
|
||||
172,80,0,0,0,0,0,2,156,64,0,0,0,0,3,173,64,0,0,0,4,188,80,0,0,0,0,75,64,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,8,238,161,0,0,0,0,26,18,185,0,0,0,0,0,0,108,0,0,0,0,0,1,185,0,0,0,
|
||||
0,0,159,177,0,0,0,0,0,151,0,0,0,0,0,0,151,0,0,0,0,0,0,0,0,0,0,0,0,0,169,0,
|
||||
0,0,0,0,0,169,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,23,206,236,112,0,0,2,217,49,20,201,0,0,12,112,0,0,
|
||||
47,32,0,76,4,222,152,12,64,0,151,30,82,198,12,64,0,180,91,0,195,14,32,0,195,108,22,242,123,0,0,181,
|
||||
45,232,158,194,0,0,138,0,0,0,0,0,0,46,147,0,0,0,0,0,2,157,255,246,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,12,208,0,0,0,0,0,60,212,0,0,0,0,0,136,
|
||||
138,0,0,0,0,0,211,78,16,0,0,0,4,224,14,80,0,0,0,9,144,9,176,0,0,0,14,255,255,241,0,0,
|
||||
0,94,16,0,230,0,0,0,168,0,0,139,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,207,254,112,0,0,0,0,197,3,228,0,0,0,0,
|
||||
197,0,198,0,0,0,0,197,4,226,0,0,0,0,207,255,177,0,0,0,0,197,2,171,0,0,0,0,197,0,78,0,
|
||||
0,0,0,197,1,171,0,0,0,0,207,254,178,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,109,252,80,0,0,0,7,228,19,176,0,
|
||||
0,0,30,96,0,0,0,0,0,79,16,0,0,0,0,0,95,0,0,0,0,0,0,79,16,0,0,0,0,0,31,80,
|
||||
0,0,0,0,0,8,212,19,177,0,0,0,0,141,252,96,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,207,254,163,0,0,0,0,197,2,158,
|
||||
32,0,0,0,197,0,11,160,0,0,0,197,0,6,224,0,0,0,197,0,4,240,0,0,0,197,0,6,208,0,0,0,
|
||||
197,0,11,144,0,0,0,197,2,158,32,0,0,0,207,254,162,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,207,255,240,0,0,0,0,197,
|
||||
0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,207,255,128,0,0,0,0,197,0,0,0,0,
|
||||
0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,207,255,241,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,207,255,192,0,0,0,
|
||||
0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,207,255,144,0,0,0,0,197,0,0,
|
||||
0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,75,238,179,
|
||||
0,0,0,5,230,17,90,0,0,0,29,96,0,0,0,0,0,79,0,0,0,0,0,0,93,0,159,251,0,0,0,79,
|
||||
0,0,108,0,0,0,30,96,0,108,0,0,0,7,230,17,140,0,0,0,0,92,239,197,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197,0,
|
||||
9,128,0,0,0,197,0,9,128,0,0,0,197,0,9,128,0,0,0,197,0,9,128,0,0,0,207,255,255,128,0,0,
|
||||
0,197,0,9,128,0,0,0,197,0,9,128,0,0,0,197,0,9,128,0,0,0,197,0,9,128,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,
|
||||
0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,212,0,0,0,0,0,0,213,0,0,0,0,0,0,213,0,0,0,0,0,0,213,0,0,0,0,0,0,213,
|
||||
0,0,0,0,0,0,213,0,0,0,0,0,0,213,0,0,0,0,0,1,227,0,0,0,0,0,238,144,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,197,0,185,0,0,0,0,197,8,193,0,0,0,0,197,94,32,0,0,0,0,200,227,0,0,0,0,0,
|
||||
204,209,0,0,0,0,0,198,201,0,0,0,0,0,197,46,80,0,0,0,0,197,5,226,0,0,0,0,197,0,155,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,
|
||||
0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,197,0,0,0,0,0,0,207,255,
|
||||
176,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,191,64,0,3,235,0,0,204,160,0,10,204,0,0,198,242,0,30,108,0,0,197,183,0,
|
||||
121,92,0,0,197,93,0,211,92,0,0,197,14,69,192,92,0,0,197,8,171,96,92,0,0,197,2,254,16,92,0,0,
|
||||
196,0,185,0,92,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,190,32,3,208,0,0,0,205,144,4,208,0,0,0,199,243,4,208,0,0,0,196,
|
||||
155,4,208,0,0,0,196,30,68,208,0,0,0,196,8,196,208,0,0,0,196,1,233,208,0,0,0,196,0,111,208,0,
|
||||
0,0,196,0,12,192,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,108,254,161,0,0,0,7,228,2,188,0,0,0,30,80,0,31,80,0,
|
||||
0,63,0,0,11,112,0,0,78,0,0,10,144,0,0,63,0,0,12,112,0,0,31,64,0,31,48,0,0,9,211,2,
|
||||
203,0,0,0,0,125,253,145,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,191,253,96,0,0,0,0,197,4,244,0,0,0,0,197,0,184,0,
|
||||
0,0,0,197,0,183,0,0,0,0,197,5,243,0,0,0,0,207,252,80,0,0,0,0,197,0,0,0,0,0,0,197,
|
||||
0,0,0,0,0,0,197,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,108,254,145,0,0,0,7,228,2,188,0,0,0,30,80,
|
||||
0,31,64,0,0,63,0,0,11,112,0,0,78,0,0,10,144,0,0,63,0,0,12,112,0,0,31,64,0,31,64,0,
|
||||
0,9,211,2,203,0,0,0,0,141,253,174,113,0,0,0,0,0,3,169,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,207,254,128,0,0,0,0,197,3,229,0,0,0,0,
|
||||
197,0,184,0,0,0,0,197,4,228,0,0,0,0,207,255,80,0,0,0,0,197,26,176,0,0,0,0,197,1,243,0,
|
||||
0,0,0,197,0,169,0,0,0,0,197,0,78,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,5,222,161,0,0,0,0,47,65,118,0,0,
|
||||
0,0,79,16,0,0,0,0,0,29,179,0,0,0,0,0,2,175,161,0,0,0,0,0,2,202,0,0,0,0,0,0,
|
||||
93,0,0,0,0,119,17,185,0,0,0,0,42,238,145,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,223,255,255,160,0,0,0,0,63,0,
|
||||
0,0,0,0,0,63,0,0,0,0,0,0,63,0,0,0,0,0,0,63,0,0,0,0,0,0,63,0,0,0,0,0,
|
||||
0,63,0,0,0,0,0,0,63,0,0,0,0,0,0,47,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197,0,5,192,0,0,0,213,
|
||||
0,5,192,0,0,0,213,0,5,192,0,0,0,213,0,5,192,0,0,0,213,0,5,192,0,0,0,213,0,5,192,0,
|
||||
0,0,183,0,8,160,0,0,0,110,65,78,80,0,0,0,7,222,198,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,168,0,0,153,0,0,
|
||||
0,94,0,0,228,0,0,0,30,64,4,224,0,0,0,10,144,9,144,0,0,0,4,224,13,48,0,0,0,0,228,77,
|
||||
0,0,0,0,0,153,136,0,0,0,0,0,78,211,0,0,0,0,0,13,192,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,123,0,7,208,
|
||||
0,62,0,78,0,12,243,0,122,0,14,64,30,183,0,182,0,10,128,75,107,0,242,0,6,192,135,47,20,208,0,2,
|
||||
241,196,13,72,128,0,0,198,224,9,140,64,0,0,141,176,5,222,0,0,0,79,112,1,234,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,109,16,
|
||||
9,160,0,0,0,12,128,62,32,0,0,0,4,226,184,0,0,0,0,0,172,225,0,0,0,0,0,79,144,0,0,0,
|
||||
0,0,186,226,0,0,0,0,5,208,170,0,0,0,0,29,80,47,48,0,0,0,124,0,9,176,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
153,0,12,112,0,0,0,63,32,78,16,0,0,0,10,144,183,0,0,0,0,3,228,225,0,0,0,0,0,174,112,0,
|
||||
0,0,0,0,79,16,0,0,0,0,0,63,0,0,0,0,0,0,63,0,0,0,0,0,0,63,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,127,255,255,16,0,0,0,0,0,139,0,0,0,0,0,3,227,0,0,0,0,0,12,128,0,0,0,0,0,109,
|
||||
0,0,0,0,0,2,228,0,0,0,0,0,10,160,0,0,0,0,0,78,16,0,0,0,0,0,143,255,255,48,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,175,160,0,
|
||||
0,0,0,0,166,0,0,0,0,0,0,166,0,0,0,0,0,0,166,0,0,0,0,0,0,166,0,0,0,0,0,0,
|
||||
166,0,0,0,0,0,0,166,0,0,0,0,0,0,166,0,0,0,0,0,0,166,0,0,0,0,0,0,166,0,0,0,
|
||||
0,0,0,166,0,0,0,0,0,0,175,160,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,197,
|
||||
0,0,0,0,0,0,122,0,0,0,0,0,0,46,16,0,0,0,0,0,11,96,0,0,0,0,0,6,192,0,0,0,
|
||||
0,0,1,226,0,0,0,0,0,0,167,0,0,0,0,0,0,77,0,0,0,0,0,0,14,48,0,0,0,0,0,9,
|
||||
144,0,0,0,0,0,3,224,0,0,0,0,0,0,212,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,95,224,0,0,0,0,0,1,240,0,0,0,0,0,1,240,0,0,0,0,0,1,240,0,0,0,0,0,1,240,0,
|
||||
0,0,0,0,1,240,0,0,0,0,0,1,240,0,0,0,0,0,1,240,0,0,0,0,0,1,240,0,0,0,0,0,
|
||||
1,240,0,0,0,0,0,1,240,0,0,0,0,0,95,224,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,111,64,0,0,0,0,0,201,176,0,0,0,0,4,193,227,0,0,0,0,11,
|
||||
96,138,0,0,0,0,46,16,46,16,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,15,255,255,254,0,0,0,
|
||||
0,0,0,0,0,0,0,12,64,0,0,0,0,0,2,193,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,4,206,
|
||||
177,0,0,0,0,11,49,184,0,0,0,0,0,0,122,0,0,0,0,6,223,250,0,0,0,0,62,48,122,0,0,0,
|
||||
0,78,34,202,0,0,0,0,10,237,154,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,226,0,0,0,0,0,0,227,0,0,0,0,0,0,227,0,0,0,0,0,0,
|
||||
231,222,128,0,0,0,0,236,35,229,0,0,0,0,227,0,153,0,0,0,0,227,0,138,0,0,0,0,227,0,153,0,
|
||||
0,0,0,236,35,228,0,0,0,0,230,238,112,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,3,206,177,0,0,0,0,30,113,102,0,0,0,0,78,0,0,0,0,0,0,108,0,0,0,0,0,0,78,0,
|
||||
0,0,0,0,0,30,97,102,0,0,0,0,4,223,178,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,12,64,0,0,0,0,0,12,80,0,0,0,0,0,12,
|
||||
80,0,0,0,3,206,141,80,0,0,0,13,113,143,80,0,0,0,62,0,12,80,0,0,0,93,0,12,80,0,0,0,
|
||||
78,0,13,80,0,0,0,14,97,143,80,0,0,0,4,222,138,64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,2,190,195,0,0,0,0,13,113,109,0,0,0,0,62,0,13,48,0,0,0,95,255,255,48,0,
|
||||
0,0,77,0,0,0,0,0,0,29,113,0,0,0,0,0,3,206,255,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2,206,80,0,0,0,0,8,161,16,0,0,0,
|
||||
0,10,112,0,0,0,0,0,191,255,16,0,0,0,0,10,112,0,0,0,0,0,10,112,0,0,0,0,0,10,112,0,
|
||||
0,0,0,0,10,112,0,0,0,0,0,10,112,0,0,0,0,0,10,112,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,5,223,255,32,0,0,0,30,50,212,0,0,0,0,61,0,151,0,0,0,0,30,
|
||||
50,212,0,0,0,0,45,222,128,0,0,0,0,91,0,0,0,0,0,0,30,254,214,0,0,0,0,123,1,95,16,0,
|
||||
0,0,154,17,110,0,0,0,0,43,238,195,0,0,0,0,0,0,0,0,0,0,0,226,0,0,0,0,0,0,227,0,
|
||||
0,0,0,0,0,227,0,0,0,0,0,0,231,222,112,0,0,0,0,236,36,242,0,0,0,0,227,0,213,0,0,0,
|
||||
0,227,0,197,0,0,0,0,227,0,197,0,0,0,0,227,0,197,0,0,0,0,226,0,181,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3,0,0,0,0,0,0,
|
||||
14,48,0,0,0,0,0,0,0,0,0,0,0,0,14,32,0,0,0,0,0,14,48,0,0,0,0,0,14,48,0,0,
|
||||
0,0,0,14,48,0,0,0,0,0,14,48,0,0,0,0,0,14,48,0,0,0,0,0,14,32,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,33,0,0,0,
|
||||
0,0,0,213,0,0,0,0,0,0,0,0,0,0,0,0,0,196,0,0,0,0,0,0,196,0,0,0,0,0,0,196,
|
||||
0,0,0,0,0,0,196,0,0,0,0,0,0,196,0,0,0,0,0,0,196,0,0,0,0,0,0,196,0,0,0,0,
|
||||
0,0,212,0,0,0,0,0,1,226,0,0,0,0,0,78,144,0,0,0,0,0,0,0,0,0,0,0,0,226,0,0,
|
||||
0,0,0,0,227,0,0,0,0,0,0,227,0,0,0,0,0,0,227,10,160,0,0,0,0,227,156,16,0,0,0,0,
|
||||
233,193,0,0,0,0,0,235,192,0,0,0,0,0,227,200,0,0,0,0,0,227,62,64,0,0,0,0,226,6,208,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,226,
|
||||
0,0,0,0,0,0,227,0,0,0,0,0,0,227,0,0,0,0,0,0,227,0,0,0,0,0,0,227,0,0,0,0,
|
||||
0,0,227,0,0,0,0,0,0,227,0,0,0,0,0,0,227,0,0,0,0,0,0,227,0,0,0,0,0,0,226,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,229,222,101,222,80,0,0,236,38,252,
|
||||
38,224,0,0,227,0,243,0,242,0,0,227,0,227,0,243,0,0,227,0,227,0,243,0,0,227,0,227,0,243,0,0,
|
||||
226,0,226,0,226,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,229,222,112,0,0,0,0,236,
|
||||
36,242,0,0,0,0,227,0,213,0,0,0,0,227,0,197,0,0,0,0,227,0,197,0,0,0,0,227,0,197,0,0,
|
||||
0,0,226,0,181,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,2,191,215,0,0,0,
|
||||
0,13,129,78,80,0,0,0,78,0,8,160,0,0,0,92,0,7,176,0,0,0,78,0,9,144,0,0,0,30,113,78,
|
||||
48,0,0,0,3,206,213,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,228,222,128,0,
|
||||
0,0,0,236,35,229,0,0,0,0,227,0,153,0,0,0,0,227,0,138,0,0,0,0,227,0,153,0,0,0,0,236,
|
||||
35,228,0,0,0,0,232,238,112,0,0,0,0,227,0,0,0,0,0,0,227,0,0,0,0,0,0,226,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,3,206,
|
||||
154,64,0,0,0,13,113,143,80,0,0,0,62,0,12,80,0,0,0,93,0,12,80,0,0,0,78,0,13,80,0,0,
|
||||
0,14,97,143,80,0,0,0,4,222,140,80,0,0,0,0,0,12,80,0,0,0,0,0,12,80,0,0,0,0,0,12,
|
||||
64,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
230,233,0,0,0,0,0,236,17,0,0,0,0,0,228,0,0,0,0,0,0,227,0,0,0,0,0,0,227,0,0,0,
|
||||
0,0,0,227,0,0,0,0,0,0,226,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,8,239,128,0,0,0,0,61,16,0,0,0,0,0,46,64,0,0,0,0,0,5,220,48,0,0,0,0,0,8,
|
||||
192,0,0,0,0,0,7,192,0,0,0,0,111,252,48,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,10,96,0,0,0,0,0,10,112,0,
|
||||
0,0,0,0,191,255,48,0,0,0,0,10,112,0,0,0,0,0,10,112,0,0,0,0,0,10,112,0,0,0,0,0,
|
||||
10,112,0,0,0,0,0,9,145,16,0,0,0,0,3,222,48,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,226,0,196,0,0,0,0,242,0,197,0,0,0,0,242,0,197,0,0,0,0,242,0,197,0,0,
|
||||
0,0,227,0,197,0,0,0,0,200,24,245,0,0,0,0,61,232,180,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,168,0,46,0,0,0,0,109,0,123,0,0,0,0,31,32,197,0,0,0,0,11,113,225,
|
||||
0,0,0,0,5,198,160,0,0,0,0,1,253,80,0,0,0,0,0,174,16,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,137,0,153,0,136,0,0,77,0,221,0,197,0,0,31,34,206,33,225,0,0,11,
|
||||
101,154,101,176,0,0,6,169,85,169,96,0,0,2,236,17,236,32,0,0,0,204,0,204,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,108,0,167,0,0,0,0,13,100,209,0,0,0,0,4,237,80,0,0,0,
|
||||
0,0,222,0,0,0,0,0,6,204,112,0,0,0,0,30,68,225,0,0,0,0,138,0,169,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,168,0,46,0,0,0,0,93,0,123,0,0,0,0,30,48,198,0,
|
||||
0,0,0,10,130,241,0,0,0,0,5,214,160,0,0,0,0,0,237,80,0,0,0,0,0,158,16,0,0,0,0,0,
|
||||
122,0,0,0,0,0,0,198,0,0,0,0,0,2,226,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,79,255,176,0,0,0,0,0,11,112,0,0,0,0,0,93,
|
||||
0,0,0,0,0,0,213,0,0,0,0,0,7,176,0,0,0,0,0,46,32,0,0,0,0,0,95,255,224,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,190,0,
|
||||
0,0,0,0,2,225,0,0,0,0,0,3,208,0,0,0,0,0,3,192,0,0,0,0,0,7,160,0,0,0,0,0,
|
||||
110,32,0,0,0,0,0,8,144,0,0,0,0,0,3,192,0,0,0,0,0,3,208,0,0,0,0,0,3,208,0,0,
|
||||
0,0,0,2,224,0,0,0,0,0,0,174,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,75,
|
||||
0,0,0,0,0,0,91,0,0,0,0,0,0,91,0,0,0,0,0,0,91,0,0,0,0,0,0,91,0,0,0,0,
|
||||
0,0,91,0,0,0,0,0,0,91,0,0,0,0,0,0,91,0,0,0,0,0,0,91,0,0,0,0,0,0,91,0,
|
||||
0,0,0,0,0,91,0,0,0,0,0,0,91,0,0,0,0,0,0,75,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,47,128,0,0,0,0,0,3,224,0,0,0,0,0,1,240,0,0,0,0,0,1,240,0,0,0,0,0,0,227,0,
|
||||
0,0,0,0,0,95,32,0,0,0,0,0,198,0,0,0,0,0,0,240,0,0,0,0,0,1,240,0,0,0,0,0,
|
||||
1,240,0,0,0,0,0,3,224,0,0,0,0,0,46,112,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,28,229,5,128,0,0,0,105,61,41,96,0,0,0,133,
|
||||
5,235,16,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
|
||||
/*Bitmap Raw Data end ---*/
|
||||
}
|
||||
@@ -0,0 +1,202 @@
|
||||
#ifndef COMM_PROT_PARSER_H_
|
||||
#define COMM_PROT_PARSER_H_
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
#define __packed_ __attribute__((packed))
|
||||
#define CP_RESULT_NEED_MORE 0
|
||||
#define CP_RESULT_PACKET_READY 1
|
||||
#define CP_RESULT_ERROR_GENERAL -1
|
||||
#define CP_RESULT_ERROR_CSUM -2
|
||||
#define CP_RESULT_ERROR_PSIZE -3
|
||||
#define CP_RESULT_ERROR_SIZE -4
|
||||
#define CP_PREFIX_SIZE 4
|
||||
|
||||
typedef unsigned char byte_t;
|
||||
|
||||
#pragma pack(push)
|
||||
#pragma pack(1)
|
||||
typedef struct cp_prefix_t {
|
||||
cp_prefix_t(byte_t a, byte_t b, byte_t c, byte_t d) {
|
||||
data[0] = a;
|
||||
data[1] = b;
|
||||
data[2] = c;
|
||||
data[3] = d;
|
||||
}
|
||||
byte_t data[CP_PREFIX_SIZE];
|
||||
}__packed_ cp_prefix_t;
|
||||
#pragma pack(pop)
|
||||
|
||||
class comm_protocol_parser_c {
|
||||
protected:
|
||||
cp_prefix_t m_prefix;
|
||||
uint16_t m_max_cmd_size;
|
||||
byte_t* m_buffer;
|
||||
uint16_t m_buffer_pos;
|
||||
byte_t m_state;
|
||||
uint16_t m_payload_size;
|
||||
|
||||
public:
|
||||
comm_protocol_parser_c(cp_prefix_t prefix = cp_prefix_t(0xAA, 0xBB,0x33, 0x44),
|
||||
uint16_t max_cmd_size = 64) :
|
||||
m_prefix(prefix), m_max_cmd_size(max_cmd_size) {
|
||||
reset();
|
||||
m_buffer = (byte_t*) malloc(m_max_cmd_size);
|
||||
}
|
||||
|
||||
~comm_protocol_parser_c() {
|
||||
free(m_buffer);
|
||||
}
|
||||
|
||||
uint16_t get_expected_data_size() {
|
||||
if (m_state <= (CP_PREFIX_SIZE+1))
|
||||
return 1;
|
||||
else
|
||||
return m_payload_size - m_buffer_pos;
|
||||
}
|
||||
|
||||
void reset() {
|
||||
m_buffer_pos = 0;
|
||||
m_state = 0;
|
||||
m_payload_size = 0;
|
||||
}
|
||||
|
||||
byte_t csum(byte_t*data, int32_t size, const byte_t extra = 0) {
|
||||
byte_t result = extra;
|
||||
while (size > 0) {
|
||||
result ^= *data;
|
||||
data++;
|
||||
size--;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
byte_t *get_packet_buffer() {
|
||||
return m_buffer;
|
||||
}
|
||||
|
||||
uint16_t get_packet_size() {
|
||||
if (m_state == (CP_PREFIX_SIZE + 3))
|
||||
return m_payload_size - 1;
|
||||
else
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint16_t get_packet(void *data, uint16_t max_size) {
|
||||
uint16_t size = get_packet_size();
|
||||
if ((max_size >= size) && (max_size > 0)) {
|
||||
memcpy(data, m_buffer, size);
|
||||
return size;
|
||||
} else
|
||||
return 0;
|
||||
}
|
||||
|
||||
int8_t post_data(byte_t *data, const uint16_t size) {
|
||||
int8_t result = CP_RESULT_NEED_MORE;
|
||||
if ((size > get_expected_data_size())
|
||||
|| (m_buffer_pos > m_max_cmd_size))
|
||||
{
|
||||
result = CP_RESULT_ERROR_SIZE;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (m_state < CP_PREFIX_SIZE) {
|
||||
if (*data == m_prefix.data[m_state]) {
|
||||
m_state++;
|
||||
} else
|
||||
{
|
||||
if (*data == m_prefix.data[0])
|
||||
m_state = 1;
|
||||
else
|
||||
result = CP_RESULT_ERROR_GENERAL;
|
||||
}
|
||||
} else
|
||||
switch (m_state) {
|
||||
case CP_PREFIX_SIZE: {
|
||||
m_payload_size = *data;
|
||||
m_state++;
|
||||
break;
|
||||
}
|
||||
|
||||
case CP_PREFIX_SIZE + 1: {
|
||||
m_payload_size = m_payload_size + (((uint16_t)(*data))<<8);
|
||||
m_state++;
|
||||
if (m_payload_size == 0 || m_payload_size > m_max_cmd_size)
|
||||
result = CP_RESULT_ERROR_PSIZE;
|
||||
break;
|
||||
}
|
||||
|
||||
|
||||
case CP_PREFIX_SIZE + 2: {
|
||||
memcpy(&m_buffer[m_buffer_pos], data, size);
|
||||
m_buffer_pos += size;
|
||||
if (m_buffer_pos >= m_payload_size) {
|
||||
if (csum(m_buffer, m_payload_size - 1)
|
||||
== m_buffer[m_payload_size - 1]) {
|
||||
m_state++;
|
||||
result = CP_RESULT_PACKET_READY;
|
||||
} else
|
||||
result = CP_RESULT_ERROR_CSUM;
|
||||
}
|
||||
break;
|
||||
}
|
||||
default: {
|
||||
result = CP_RESULT_ERROR_GENERAL;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (result < 0)
|
||||
reset();
|
||||
return result;
|
||||
}
|
||||
|
||||
byte_t get_packet_extra() {
|
||||
return CP_PREFIX_SIZE + 3;
|
||||
}
|
||||
|
||||
uint16_t create_packet(void *buffer, const uint16_t max_size,
|
||||
const byte_t command, const void *data, const uint16_t data_size) {
|
||||
if (max_size >= (data_size + 1 + get_packet_extra())) {
|
||||
byte_t *bbuffer = (byte_t*) buffer;
|
||||
memcpy(bbuffer, &m_prefix, CP_PREFIX_SIZE);
|
||||
bbuffer += CP_PREFIX_SIZE;
|
||||
*bbuffer = (data_size + 2) & 0xFF;
|
||||
bbuffer++;
|
||||
*bbuffer = ((data_size + 2) >> 8) & 0xFF;
|
||||
bbuffer++;
|
||||
byte_t *csum_buffer = bbuffer;
|
||||
*bbuffer = command;
|
||||
bbuffer++;
|
||||
if (data_size > 0) {
|
||||
memcpy(bbuffer, data, data_size);
|
||||
bbuffer += data_size;
|
||||
}
|
||||
*bbuffer = csum(csum_buffer, data_size + 1);
|
||||
return data_size + get_packet_extra() + 1;
|
||||
} else
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint16_t create_packet(void *buffer, const uint16_t max_size, const void *data,
|
||||
const uint16_t data_size) {
|
||||
if (max_size >= (data_size + get_packet_extra())) {
|
||||
byte_t *bbuffer = (byte_t*) buffer;
|
||||
memcpy(bbuffer, &m_prefix, CP_PREFIX_SIZE);
|
||||
bbuffer += CP_PREFIX_SIZE;
|
||||
*bbuffer = (data_size + 1) & 0xFF;
|
||||
bbuffer++;
|
||||
*bbuffer = ((data_size + 1) >> 8) & 0xFF;
|
||||
bbuffer++;
|
||||
byte_t *csum_buffer = bbuffer;
|
||||
memcpy(bbuffer, data, data_size);
|
||||
bbuffer += data_size;
|
||||
*bbuffer = csum(csum_buffer, data_size);
|
||||
return data_size + get_packet_extra();
|
||||
} else
|
||||
return 0;
|
||||
}
|
||||
};
|
||||
|
||||
#endif /* COMM_PROT_PARSER_H_ */
|
||||
@@ -0,0 +1,23 @@
|
||||
#include "Lock.h"
|
||||
#include <cassert>
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
Lock::Lock()
|
||||
{
|
||||
handle = xSemaphoreCreateBinary();
|
||||
assert(handle);
|
||||
assert(xSemaphoreGive(handle));
|
||||
}
|
||||
|
||||
void Lock::lock()
|
||||
{
|
||||
assert(xSemaphoreTake(handle, portMAX_DELAY));
|
||||
}
|
||||
|
||||
void Lock::unlock()
|
||||
{
|
||||
assert(xSemaphoreGive(handle));
|
||||
}
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,33 @@
|
||||
#pragma once
|
||||
|
||||
#include "../freertosinc.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
/**
|
||||
* @brief Simple wrapper around FreeRTOS API for implementing a mutex lock
|
||||
*/
|
||||
class Lock
|
||||
{
|
||||
public:
|
||||
Lock();
|
||||
|
||||
Lock(const Lock &) = delete;
|
||||
Lock &operator=(const Lock &) = delete;
|
||||
|
||||
/// Locks the lock.
|
||||
//
|
||||
// Must not be called from an ISR.
|
||||
void lock();
|
||||
|
||||
// Unlocks the lock.
|
||||
//
|
||||
// Must not be called from an ISR.
|
||||
void unlock();
|
||||
|
||||
private:
|
||||
SemaphoreHandle_t handle;
|
||||
|
||||
};
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,15 @@
|
||||
#include "LockGuard.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
LockGuard::LockGuard(Lock *lock) : lock(lock)
|
||||
{
|
||||
lock->lock();
|
||||
}
|
||||
|
||||
LockGuard::~LockGuard()
|
||||
{
|
||||
lock->unlock();
|
||||
}
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,23 @@
|
||||
#pragma once
|
||||
|
||||
#include "Lock.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
/**
|
||||
* @brief RAII lock guard
|
||||
*/
|
||||
class LockGuard
|
||||
{
|
||||
public:
|
||||
LockGuard(Lock *lock);
|
||||
~LockGuard();
|
||||
|
||||
LockGuard(const LockGuard &) = delete;
|
||||
LockGuard &operator=(const LockGuard &) = delete;
|
||||
|
||||
private:
|
||||
Lock *lock;
|
||||
};
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,19 @@
|
||||
#include "NotifiedWorkerThread.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
/**
|
||||
* Notify this thread so it can run
|
||||
*/
|
||||
void NotifiedWorkerThread::notify(uint32_t v, eNotifyAction action)
|
||||
{
|
||||
xTaskNotify(taskHandle, v, action);
|
||||
}
|
||||
|
||||
void NotifiedWorkerThread::block()
|
||||
{
|
||||
xTaskNotifyWait(0, // don't clear notification on entry
|
||||
clearOnRead, ¬ification, portMAX_DELAY); // Wait forever
|
||||
}
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,47 @@
|
||||
#pragma once
|
||||
|
||||
#include "WorkerThread.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
/**
|
||||
* @brief A worker thread that waits on a freertos notification
|
||||
*/
|
||||
class NotifiedWorkerThread : public WorkerThread
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* Notify this thread so it can run
|
||||
*/
|
||||
void notify(uint32_t v = 0, eNotifyAction action = eNoAction);
|
||||
|
||||
/**
|
||||
* Notify from an ISR
|
||||
*
|
||||
* This must be inline or IRAM_ATTR on ESP32
|
||||
*/
|
||||
inline void notifyFromISR(BaseType_t *highPriWoken, uint32_t v = 0, eNotifyAction action = eNoAction)
|
||||
{
|
||||
xTaskNotifyFromISR(taskHandle, v, action, highPriWoken);
|
||||
}
|
||||
|
||||
protected:
|
||||
/**
|
||||
* The notification that was most recently used to wake the thread. Read from loop()
|
||||
*/
|
||||
uint32_t notification = 0;
|
||||
|
||||
/**
|
||||
* What notification bits should be cleared just after we read and return them in notification?
|
||||
*
|
||||
* Defaults to clear all of them.
|
||||
*/
|
||||
uint32_t clearOnRead = UINT32_MAX;
|
||||
|
||||
/**
|
||||
* A method that should block execution - either waiting ona queue/mutex or a "task notification"
|
||||
*/
|
||||
virtual void block();
|
||||
};
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,26 @@
|
||||
#pragma once
|
||||
|
||||
#include "PeriodicTask.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
/**
|
||||
* @brief Periodically invoke a callback. This just provides C-style callback conventions
|
||||
* rather than a virtual function - FIXME, remove?
|
||||
*/
|
||||
class Periodic : public PeriodicTask
|
||||
{
|
||||
uint32_t (*callback)();
|
||||
|
||||
public:
|
||||
// callback returns the period for the next callback invocation (or 0 if we should no longer be called)
|
||||
Periodic(uint32_t (*_callback)()) : callback(_callback) {}
|
||||
|
||||
protected:
|
||||
void doTask() {
|
||||
uint32_t p = callback();
|
||||
setPeriod(p);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,35 @@
|
||||
#include "PeriodicScheduler.h"
|
||||
#include "PeriodicTask.h"
|
||||
#include "LockGuard.h"
|
||||
#include "../timing.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
/// call this from loop
|
||||
void PeriodicScheduler::loop()
|
||||
{
|
||||
LockGuard lg(&lock);
|
||||
|
||||
uint32_t now = timing::millis();
|
||||
for (auto t : tasks) {
|
||||
if (t->period && (now - t->lastMsec) >= t->period) {
|
||||
|
||||
t->doTask();
|
||||
t->lastMsec = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void PeriodicScheduler::schedule(PeriodicTask *t)
|
||||
{
|
||||
LockGuard lg(&lock);
|
||||
tasks.insert(t);
|
||||
}
|
||||
|
||||
void PeriodicScheduler::unschedule(PeriodicTask *t)
|
||||
{
|
||||
LockGuard lg(&lock);
|
||||
tasks.erase(t);
|
||||
}
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,40 @@
|
||||
#pragma once
|
||||
|
||||
#include "Lock.h"
|
||||
#include <cstdint>
|
||||
#include <unordered_set>
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
class PeriodicTask;
|
||||
|
||||
/**
|
||||
* @brief Runs all PeriodicTasks in the system. Currently called from main loop()
|
||||
* but eventually should be its own thread blocked on a freertos timer.
|
||||
*/
|
||||
class PeriodicScheduler
|
||||
{
|
||||
friend class PeriodicTask;
|
||||
|
||||
/**
|
||||
* This really should be some form of heap, and when the period gets changed on a task it should get
|
||||
* rescheduled in that heap. Currently it is just a dumb array and everytime we run loop() we check
|
||||
* _every_ tasks. If it was a heap we'd only have to check the first task.
|
||||
*/
|
||||
std::unordered_set<PeriodicTask *> tasks;
|
||||
|
||||
// Protects the above variables.
|
||||
Lock lock;
|
||||
|
||||
public:
|
||||
/// Run any next tasks which are due for execution
|
||||
void loop();
|
||||
|
||||
private:
|
||||
void schedule(PeriodicTask *t);
|
||||
void unschedule(PeriodicTask *t);
|
||||
};
|
||||
|
||||
extern PeriodicScheduler periodicScheduler;
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,16 @@
|
||||
#include "PeriodicTask.h"
|
||||
#include "Periodic.h"
|
||||
#include "LockGuard.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
PeriodicScheduler periodicScheduler;
|
||||
|
||||
PeriodicTask::PeriodicTask(uint32_t initialPeriod) : period(initialPeriod) {}
|
||||
|
||||
void PeriodicTask::setup()
|
||||
{
|
||||
periodicScheduler.schedule(this);
|
||||
}
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,56 @@
|
||||
#pragma once
|
||||
|
||||
#include "PeriodicScheduler.h"
|
||||
#include "timing.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
/**
|
||||
* @brief A base class for tasks that want their doTask() method invoked periodically
|
||||
*
|
||||
* @todo currently just syntatic sugar for polling in loop (you must call .loop), but eventually
|
||||
* generalize with the freertos scheduler so we can save lots of power by having everything either in
|
||||
* something like this or triggered off of an irq.
|
||||
*/
|
||||
class PeriodicTask
|
||||
{
|
||||
friend class PeriodicScheduler;
|
||||
|
||||
uint32_t lastMsec = 0;
|
||||
uint32_t period = 1; // call soon after creation
|
||||
|
||||
public:
|
||||
virtual ~PeriodicTask() { periodicScheduler.unschedule(this); }
|
||||
|
||||
/**
|
||||
* Constructor (will schedule with the global PeriodicScheduler)
|
||||
*/
|
||||
PeriodicTask(uint32_t initialPeriod = 1);
|
||||
|
||||
/**
|
||||
* MUST be be called once at startup (but after threading is running - i.e. not from a constructor)
|
||||
*/
|
||||
void setup();
|
||||
|
||||
/**
|
||||
* Set a new period in msecs (can be called from doTask or elsewhere and the scheduler will cope)
|
||||
* While zero this task is disabled and will not run
|
||||
*/
|
||||
void setPeriod(uint32_t p)
|
||||
{
|
||||
lastMsec = timing::millis(); // reset starting from now
|
||||
period = p;
|
||||
}
|
||||
|
||||
uint32_t getPeriod() const { return period; }
|
||||
|
||||
/**
|
||||
* Syntatic sugar for suspending tasks
|
||||
*/
|
||||
void disable() { setPeriod(0); }
|
||||
|
||||
protected:
|
||||
virtual void doTask() = 0;
|
||||
};
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,25 @@
|
||||
#include "Thread.h"
|
||||
#include "timing.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
void Thread::start(const char *name, size_t stackSize, uint32_t priority, int8_t core)
|
||||
{
|
||||
if(core<0)
|
||||
{
|
||||
auto r = xTaskCreate(callRun, name, stackSize, this, priority, &taskHandle);
|
||||
assert(r == pdPASS);
|
||||
}
|
||||
else
|
||||
{
|
||||
auto r = xTaskCreatePinnedToCore(callRun, name, stackSize, this, priority, &taskHandle,core);
|
||||
assert(r == pdPASS);
|
||||
}
|
||||
}
|
||||
|
||||
void Thread::callRun(void *_this)
|
||||
{
|
||||
((Thread *)_this)->doRun();
|
||||
}
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,78 @@
|
||||
#pragma once
|
||||
|
||||
#include "freertosinc.h"
|
||||
#include "esp_task_wdt.h"
|
||||
|
||||
namespace concurrency
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Base threading
|
||||
*/
|
||||
class Thread
|
||||
{
|
||||
protected:
|
||||
TaskHandle_t taskHandle = NULL;
|
||||
|
||||
/**
|
||||
* set this to true to ask thread to cleanly exit asap
|
||||
*/
|
||||
volatile bool wantExit = false;
|
||||
|
||||
public:
|
||||
void start(const char *name, size_t stackSize = 1024, uint32_t priority = tskIDLE_PRIORITY, int8_t core=-1);
|
||||
|
||||
virtual ~Thread() { vTaskDelete(taskHandle); }
|
||||
|
||||
uint32_t getStackHighwaterMark() { return uxTaskGetStackHighWaterMark(taskHandle); }
|
||||
|
||||
protected:
|
||||
/**
|
||||
* The method that will be called when start is called.
|
||||
*/
|
||||
virtual void doRun() = 0;
|
||||
|
||||
/**
|
||||
* All thread run methods must periodically call serviceWatchdog, or the system will declare them hung and panic.
|
||||
*
|
||||
* this only applies after startWatchdog() has been called. If you need to sleep for a long time call stopWatchdog()
|
||||
*/
|
||||
void serviceWatchdog() { esp_task_wdt_reset(); }
|
||||
void startWatchdog()
|
||||
{
|
||||
auto r = esp_task_wdt_add(taskHandle);
|
||||
assert(r == ESP_OK);
|
||||
}
|
||||
void stopWatchdog()
|
||||
{
|
||||
auto r = esp_task_wdt_delete(taskHandle);
|
||||
assert(r == ESP_OK);
|
||||
}
|
||||
|
||||
private:
|
||||
static void callRun(void *_this);
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
class ThreadFunctionTemplate : public Thread
|
||||
{
|
||||
public:
|
||||
typedef void (T::*thread_func_t)(ThreadFunctionTemplate<T> *thread);
|
||||
|
||||
protected:
|
||||
thread_func_t m_func;
|
||||
T &m_owner;
|
||||
|
||||
public:
|
||||
ThreadFunctionTemplate(T &owner, thread_func_t func) : m_func(func), m_owner(owner)
|
||||
{
|
||||
}
|
||||
|
||||
protected:
|
||||
virtual void doRun()
|
||||
{
|
||||
(m_owner.*m_func)(this);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,32 @@
|
||||
#include "WorkerThread.h"
|
||||
#include "timing.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
void WorkerThread::doRun()
|
||||
{
|
||||
startWatchdog();
|
||||
|
||||
while (!wantExit) {
|
||||
stopWatchdog();
|
||||
block();
|
||||
startWatchdog();
|
||||
|
||||
// no need - startWatchdog is guaranteed to give us one full watchdog interval
|
||||
// serviceWatchdog(); // Let our loop worker have one full watchdog interval (at least) to run
|
||||
|
||||
#ifdef DEBUG_STACK
|
||||
static uint32_t lastPrint = 0;
|
||||
if (timing::millis() - lastPrint > 10 * 1000L) {
|
||||
lastPrint = timing::millis();
|
||||
meshtastic::printThreadInfo("net");
|
||||
}
|
||||
#endif
|
||||
|
||||
loop();
|
||||
}
|
||||
|
||||
stopWatchdog();
|
||||
}
|
||||
|
||||
} // namespace concurrency
|
||||
@@ -0,0 +1,29 @@
|
||||
#pragma once
|
||||
|
||||
#include "Thread.h"
|
||||
|
||||
namespace concurrency {
|
||||
|
||||
/**
|
||||
* @brief This wraps threading (FreeRTOS for now) with a blocking API intended for efficiently converting
|
||||
* old-school arduino loop() code. Use as a mixin base class for the classes you want to convert.
|
||||
*
|
||||
* @link https://www.freertos.org/RTOS_Task_Notification_As_Mailbox.html
|
||||
*/
|
||||
class WorkerThread : public Thread
|
||||
{
|
||||
protected:
|
||||
/**
|
||||
* A method that should block execution - either waiting ona queue/mutex or a "task notification"
|
||||
*/
|
||||
virtual void block() = 0;
|
||||
|
||||
virtual void loop() = 0;
|
||||
|
||||
/**
|
||||
* The method that will be called when start is called.
|
||||
*/
|
||||
virtual void doRun();
|
||||
};
|
||||
|
||||
} // namespace concurrency
|
||||
+181
@@ -0,0 +1,181 @@
|
||||
#ifndef CONFIG_H
|
||||
#define CONFIG_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <freertos\FreeRTOS.h>
|
||||
#include <freertos\semphr.h>
|
||||
|
||||
#define ARDUINOJSON_USE_DOUBLE 1
|
||||
|
||||
#define DEBUG_PORT Serial // Serial debug port
|
||||
#ifdef DEBUG_PORT
|
||||
extern SemaphoreHandle_t debugLock;
|
||||
#define DEBUG_MSG(...) ({xSemaphoreTake(debugLock, portMAX_DELAY); DEBUG_PORT.printf(__VA_ARGS__); xSemaphoreGive(debugLock);})
|
||||
#define LOCK_DEBUG_PORT() (xSemaphoreTake(debugLock, portMAX_DELAY))
|
||||
#define UNLOCK_DEBUG_PORT() (xSemaphoreGive(debugLock))
|
||||
#else
|
||||
#define DEBUG_MSG(...)
|
||||
#define LOCK_DEBUG_PORT()
|
||||
#define UNLOCK_DEBUG_PORT()
|
||||
#endif
|
||||
|
||||
#define xstr(s) ssstr(s)
|
||||
#define ssstr(s) #s
|
||||
|
||||
//AD_V100 - Lora RF95
|
||||
//#define AD_V100
|
||||
|
||||
//AD_V100 - Lora SX1262
|
||||
//#define AD_V101
|
||||
|
||||
#define APP_WATCHDOG_SECS_INIT 70
|
||||
#define APP_WATCHDOG_SECS_WORK 15
|
||||
|
||||
#define GPS_BAUDRATE 9600
|
||||
#define GPS_BAUDRATE2 115200
|
||||
#define GPS_FREQ 2
|
||||
#define LORA_SEND_LOCATION_PERIOD 10000
|
||||
|
||||
#define AC_WIRE Wire1
|
||||
#define AC_ADDRESS 0x68
|
||||
|
||||
#define FLIP_SCREEN_VERTICALLY
|
||||
#define FS SPIFFS
|
||||
#define SCK_GPIO 5
|
||||
#define MISO_GPIO 19
|
||||
#define MOSI_GPIO 27
|
||||
#define NSS_GPIO 18
|
||||
#define RESET_GPIO 14
|
||||
|
||||
#define GPS_SERIAL_NUM 1
|
||||
#define GPS_RX_PIN 34
|
||||
#define GPS_TX_PIN 12
|
||||
|
||||
#define RF95_IRQ_GPIO 26
|
||||
#define PMU_IRQ 35
|
||||
#define SSD1306_ADDRESS 0x3C
|
||||
|
||||
#define I2C_SDA 21
|
||||
#define I2C_SCL 22
|
||||
|
||||
#define I2C2_SDA 0
|
||||
#define I2C2_SCL 4
|
||||
|
||||
#define BUTTON_PIN 38 // The middle button GPIO on the T-Beam
|
||||
#define VIBRO_PIN 7
|
||||
#define VIBRO_PIN2 3
|
||||
|
||||
#define SX1262_E22
|
||||
|
||||
#define SX1262_IRQ 33
|
||||
#define SX1262_RST 23
|
||||
#define SX1262_BUSY 32
|
||||
#define SX1262_MAXPOWER 22
|
||||
|
||||
#define LORA_BW 500.0
|
||||
#define LORA_SF 8
|
||||
#define LORA_CR 7
|
||||
#define LORA_FREQ 912.0
|
||||
#define LORA_PRE 8
|
||||
#define LORA_SYNC 0x64
|
||||
#define LORA_POWER 17
|
||||
|
||||
#define AGD_LORA_BW 125.0
|
||||
#define AGD_LORA_SF 10
|
||||
#define AGD_LORA_CR 7
|
||||
#define AGD_LORA_FREQ 915.0
|
||||
#define AGD_LORA_PRE 8
|
||||
#define AGD_LORA_SYNC 0x31
|
||||
|
||||
#ifdef AD_V100
|
||||
#undef USE_SX1262
|
||||
#define HAS_AXP20X 1
|
||||
#define HAS_GPS 1
|
||||
#endif
|
||||
|
||||
#ifdef AD_V101
|
||||
#define USE_SX1262
|
||||
#undef LORA_POWER
|
||||
#define LORA_POWER SX1262_MAXPOWER
|
||||
#define HAS_AXP20X 1
|
||||
#define HAS_GPS 1
|
||||
#define LORA_CURRENT 140
|
||||
#endif
|
||||
|
||||
#ifdef AD_LORA32_V1
|
||||
#undef USE_SX1262
|
||||
#undef PMU_IRQ
|
||||
#define HAS_GPS
|
||||
#define AT_CAN_SLEEP
|
||||
#undef GPS_FREQ
|
||||
#define GPS_FREQ 1
|
||||
|
||||
#define LORA_SCK 5
|
||||
#define LORA_MISO 19
|
||||
#define LORA_MOSI 27
|
||||
#define LORA_CS 18
|
||||
#define LORA_RST 14
|
||||
#define LORA_IRQ 26
|
||||
#define LORA_CURRENT 240
|
||||
|
||||
#define BATTERY_PIN 39
|
||||
#define LED_PIN -1
|
||||
#undef I2C_SCL
|
||||
#define I2C_SCL 19
|
||||
#endif
|
||||
|
||||
#ifdef AD_NOSCREEN
|
||||
#define AD_BLE_NO_CLIENT
|
||||
#endif
|
||||
|
||||
#define SPI_DISP_SCK 2
|
||||
#define SPI_DISP_MISO 13
|
||||
#define SPI_DISP_MOSI 14
|
||||
#define SPI_DISP_CS 25
|
||||
#define SPI_DISP_RESET 4
|
||||
|
||||
#ifndef AD_NOSCREEN
|
||||
#define USE_SCREEN_EVE 1
|
||||
#endif
|
||||
|
||||
#define DW1000_IRQ 15
|
||||
#define DW1000_RST 4
|
||||
#define DW1000_CS 5
|
||||
|
||||
#define PSENSOR_PIN 36
|
||||
|
||||
#define BTN1_PIN 0
|
||||
#define BTN2_PIN 2
|
||||
#define BTN3_PIN 1
|
||||
#define BTN4_PIN 15
|
||||
#define BTN5_PIN 6
|
||||
#define BTN6_PIN 14
|
||||
|
||||
#define SIDE_BUTTONS_COUNT 6
|
||||
|
||||
#define GNSS_DBD_FILENAME "/config/gnss.dat"
|
||||
#define MAIN_CONFIG_FILENAME "/config/main.json"
|
||||
#define NET_CONFIG_FILENAME "/config/network.json"
|
||||
#define AGD_NET_CONFIG_FILENAME "/config/agdnet.json"
|
||||
|
||||
#define DEFAULT_TZ "EST5EDT,M3.2.0,M11.1.0"
|
||||
|
||||
//Packet Prefix
|
||||
#define SERIAL_APP_1 0x85
|
||||
#define SERIAL_APP_2 0x3B
|
||||
#define SERIAL_APP_3 0xDE
|
||||
#define SERIAL_APP_4 0x02
|
||||
#define MAX_SERIAL_PACKET_SIZE 4200
|
||||
|
||||
#define SERIAL_PROG_MODE_SIG1 0x92
|
||||
#define SERIAL_PROG_MODE_SIG2 0x5A
|
||||
|
||||
#define SERIAL_PROG_MODE_RESP 0x07
|
||||
|
||||
#define SERIAL_PROG Serial
|
||||
#define SCREEN_FLASH_META_FILE "/spiffs/sflash.json"
|
||||
#define SCREEN_FLASH_META_BINARY_FILE "/spiffs/sflash.dat"
|
||||
#define SCREEN_FLASH_META_FILE_PATH "/spiffs/"
|
||||
#define AT_POINTS_FILE_TEMPLATE "/spiffs/p_%s.json"
|
||||
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,608 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2015 by Thomas Trojer <thomas@trojer.net>
|
||||
* Decawave DW1000Ng library for arduino.
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*
|
||||
* @file DW1000Ng.h
|
||||
* Arduino driver library (header file) for the Decawave DW1000Ng UWB transceiver Module.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <Arduino.h>
|
||||
#include "Adafruit_MCP23017.h"
|
||||
#include "driver/spi_master.h"
|
||||
#include "DW1000NgConstants.hpp"
|
||||
#include "DW1000NgConfiguration.hpp"
|
||||
#include "DW1000NgCompileOptions.hpp"
|
||||
|
||||
namespace DW1000Ng {
|
||||
/**
|
||||
Initiates and starts a sessions with a DW1000. If rst is not set or value 0xff, a soft resets (i.e. command
|
||||
triggered) are used and it is assumed that no reset line is wired.
|
||||
|
||||
@param[in] ss The SPI Selection pin used to identify the specific connection
|
||||
@param[in] irq The interrupt line/pin that connects the Arduino.
|
||||
@param[in] rst The reset line/pin for hard resets of ICs that connect to the Arduino. Value 0xff means soft reset.
|
||||
*/
|
||||
void initialize(uint8_t ss, uint8_t irq, uint8_t rst, spi_host_device_t spihost,Adafruit_MCP23017 *expander);
|
||||
|
||||
/**
|
||||
Enable debounce Clock, used to clock the LED blinking
|
||||
*/
|
||||
void enableDebounceClock();
|
||||
|
||||
/**
|
||||
Enable led blinking feature
|
||||
*/
|
||||
void enableLedBlinking();
|
||||
|
||||
/**
|
||||
Set DW1000's GPIO pins mode
|
||||
*/
|
||||
void setGPIOMode(uint8_t msgp, uint8_t mode);
|
||||
|
||||
/**
|
||||
Applies the common sleep configuration and on-wake mode to the DW1000 for both DEEP_SLEEP and SLEEP modes.
|
||||
ONW_LLDO_BIT and ONW_LLDE_BIT are 1 to default.
|
||||
|
||||
@param [in] config struct The sleep/deepsleep configuration to apply to the DW1000
|
||||
*/
|
||||
void applySleepConfiguration(sleep_configuration_t sleep_config);
|
||||
|
||||
/**
|
||||
Enter in DeepSleep. applySleepConfiguration must be called first.
|
||||
Either spi wakeup or pin wakeup must be enabled.
|
||||
-- In case of future implementation of Sleep mode, you must reset proper antenna delay with setTxAntennaDelay() after wakeUp event. --
|
||||
*/
|
||||
void deepSleep();
|
||||
|
||||
/**
|
||||
Wake-up from deep sleep by toggle chip select pin
|
||||
*/
|
||||
void spiWakeup();
|
||||
|
||||
/**
|
||||
Resets all connected or the currently selected DW1000 chip.
|
||||
Uses hardware reset or in case the reset pin is not wired it falls back to software Reset.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/**
|
||||
Resets the currently selected DW1000 chip programmatically (via corresponding commands).
|
||||
*/
|
||||
void softwareReset();
|
||||
|
||||
/**
|
||||
(Re-)set the network identifier which the selected chip should be associated with. This
|
||||
setting is important for certain MAC address filtering rules.
|
||||
This is also referred as PanId
|
||||
|
||||
@param[in] val An arbitrary numeric network identifier.
|
||||
*/
|
||||
void setNetworkId(uint16_t val);
|
||||
|
||||
/**
|
||||
Gets the network identifier (a.k.a PAN id) set for the device
|
||||
|
||||
@param[out] id the bytes that represent the PAN id (2 bytes)
|
||||
*/
|
||||
void getNetworkId(byte id[]);
|
||||
|
||||
/**
|
||||
(Re-)set the device address (i.e. short address) for the currently selected chip. This
|
||||
setting is important for certain MAC address filtering rules.
|
||||
|
||||
@param[in] val An arbitrary numeric device address.
|
||||
*/
|
||||
void setDeviceAddress(uint16_t val);
|
||||
|
||||
/**
|
||||
Gets the short address identifier set for the device
|
||||
|
||||
@param[out] address the bytes that represent the short address of the device(2 bytes)
|
||||
*/
|
||||
void getDeviceAddress(byte address[]);
|
||||
|
||||
/**
|
||||
Sets the device Extended Unique Identifier.
|
||||
This is a long identifier of the device.
|
||||
|
||||
@param[in] eui A string containing the eui in its normal notation using columns.
|
||||
*/
|
||||
void setEUI(char eui[]);
|
||||
|
||||
/**
|
||||
Sets the device Extended Unique Identifier.
|
||||
This is a long identifier of the device.
|
||||
|
||||
@param[in] eui The raw bytes of the eui.
|
||||
*/
|
||||
void setEUI(byte eui[]);
|
||||
|
||||
/**
|
||||
Gets the device Extended Unique Identifier.
|
||||
|
||||
@param[out] eui The 8 bytes of the EUI.
|
||||
*/
|
||||
void getEUI(byte eui[]);
|
||||
|
||||
/**
|
||||
Sets the transmission power of the device.
|
||||
Be careful to respect your current country limitations.
|
||||
|
||||
@param[in] power Bytes that represent the power
|
||||
*/
|
||||
void setTXPower(byte power[]);
|
||||
|
||||
/**
|
||||
Sets the transmission power of the device.
|
||||
Be careful to respect your current country limitations.
|
||||
|
||||
@param[in] power Bytes (written as a 32-bit number) that represent the power
|
||||
*/
|
||||
void setTXPower(int32_t power);
|
||||
|
||||
/**
|
||||
Sets the transmission power of the device.
|
||||
Be careful to respect your current country limitations.
|
||||
|
||||
@param[in] driver_amplifier Base power amplifier
|
||||
@param[in] mixer Mixer power
|
||||
*/
|
||||
void setTXPower(DriverAmplifierValue driver_amplifier, TransmitMixerValue mixer);
|
||||
|
||||
/**
|
||||
Automatically sets power in respect to the current device settings.
|
||||
This should be guaranteed to set power under -41.3 dBm / MHz (legal limit in most countries).
|
||||
*/
|
||||
void setTXPowerAuto();
|
||||
|
||||
/**
|
||||
Sets the pulse generator delay value.
|
||||
You should use the setTCPGDelayAuto() function.
|
||||
*/
|
||||
void setTCPGDelay(byte tcpg_delay);
|
||||
|
||||
/**
|
||||
Automatically sets pulse generator delay value
|
||||
*/
|
||||
void setTCPGDelayAuto();
|
||||
|
||||
/**
|
||||
Enables transmit power spectrum test mode that is used for Transmit Power regulatory testing
|
||||
|
||||
@param [in] repeat_interval the interval to repeat the transmission
|
||||
*/
|
||||
void enableTransmitPowerSpectrumTestMode(int32_t repeat_interval);
|
||||
|
||||
/**
|
||||
Sets a delay for transmission and receive
|
||||
|
||||
@param [in] futureTimeBytes the timestamp in bytes of the time of the transmission (in UWB time)
|
||||
*/
|
||||
void setDelayedTRX(byte futureTimeBytes[]);
|
||||
|
||||
/**
|
||||
Sets the transmission bytes inside the tx buffer of the DW1000
|
||||
|
||||
@param [in] data the bytes to transmit
|
||||
@param [in] n the length of the array of bytes
|
||||
*/
|
||||
void setTransmitData(byte data[], uint16_t n);
|
||||
|
||||
/**
|
||||
Sets the transmission bytes inside the tx buffer of the DW1000 based on the input string
|
||||
|
||||
@param [in] data the string to transmit
|
||||
*/
|
||||
void setTransmitData(const String& data);
|
||||
|
||||
/**
|
||||
Gets the received bytes and stores them in a byte array
|
||||
|
||||
@param [out] data The array of byte to store the data
|
||||
@param [out] n The length of the byte array
|
||||
*/
|
||||
void getReceivedData(byte data[], uint16_t n);
|
||||
|
||||
/**
|
||||
Stores the received data inside a string
|
||||
|
||||
param [out] data the string that will contain the data
|
||||
*/
|
||||
void getReceivedData(String& data);
|
||||
|
||||
/**
|
||||
Calculates the length of the received data
|
||||
|
||||
returns the length of the data
|
||||
*/
|
||||
uint16_t getReceivedDataLength();
|
||||
|
||||
/**
|
||||
Calculates the latest transmission timestamp
|
||||
|
||||
return the last transmission timestamp
|
||||
*/
|
||||
uint64_t getTransmitTimestamp();
|
||||
|
||||
/**
|
||||
Calculates the latest receive timestamp
|
||||
|
||||
return the last receive timestamp
|
||||
*/
|
||||
uint64_t getReceiveTimestamp();
|
||||
|
||||
/**
|
||||
Calculates the current system timestamp inside the DW1000
|
||||
|
||||
return the system timestamp
|
||||
*/
|
||||
uint64_t getSystemTimestamp();
|
||||
|
||||
/* receive quality information. (RX_FSQUAL) - reg:0x12 */
|
||||
|
||||
/**
|
||||
Gets the receive power of the device (last receive)
|
||||
|
||||
returns the last receive power of the device
|
||||
*/
|
||||
float getReceivePower();
|
||||
|
||||
/**
|
||||
Gets the power of the first path
|
||||
|
||||
returns the first path power
|
||||
*/
|
||||
float getFirstPathPower();
|
||||
|
||||
/**
|
||||
Gets the last receive quality
|
||||
|
||||
returns last receive quality
|
||||
*/
|
||||
float getReceiveQuality();
|
||||
|
||||
/**
|
||||
Sets both tx and rx antenna delay value
|
||||
|
||||
@param [in] value the delay in UWB time
|
||||
*/
|
||||
void setAntennaDelay(uint16_t value);
|
||||
|
||||
#if defined(__AVR__)
|
||||
/**
|
||||
Sets both tx and rx antenna delay value, and saves it in the EEPROM for future use
|
||||
|
||||
@param [in] value the delay in UWB time
|
||||
@param [in] the EEPROM offset at which the delay is saved
|
||||
*/
|
||||
void setAndSaveAntennaDelay(uint16_t delay, uint8_t eeAddress = 0);
|
||||
|
||||
/**
|
||||
Gets the saved antenna delay value from EEPROM
|
||||
|
||||
returns the value of the delay saved in the EEPROM in UWB time
|
||||
|
||||
@param [in] the EEPROM offset at which the delay is saved
|
||||
*/
|
||||
uint16_t getSavedAntennaDelay(uint8_t eeAddress = 0);
|
||||
|
||||
/**
|
||||
Sets the saved antenna delay value from EEPROM as the configured delay
|
||||
|
||||
@param [in] the EEPROM offset at which the delay is saved
|
||||
*/
|
||||
uint16_t setAntennaDelayFromEEPROM(uint8_t eeAddress = 0);
|
||||
#endif
|
||||
|
||||
/**
|
||||
Sets the tx antenna delay value
|
||||
|
||||
@param [in] value the delay in UWB time
|
||||
*/
|
||||
void setTxAntennaDelay(uint16_t value);
|
||||
|
||||
/**
|
||||
Sets the rx antenna delay value
|
||||
|
||||
@param [in] value the delay in UWB time
|
||||
*/
|
||||
void setRxAntennaDelay(uint16_t value);
|
||||
|
||||
/**
|
||||
Gets the tx antenna delay value
|
||||
|
||||
returns the value of the delay in UWB time
|
||||
*/
|
||||
uint16_t getTxAntennaDelay();
|
||||
|
||||
/**
|
||||
Gets the rx antenna delay value
|
||||
|
||||
returns the value of the delay in UWB time
|
||||
*/
|
||||
uint16_t getRxAntennaDelay();
|
||||
|
||||
/**
|
||||
Sets the function for error event handling
|
||||
|
||||
@param [in] handleError the target function
|
||||
*/
|
||||
void attachErrorHandler(void (* handleError)(void));
|
||||
|
||||
/**
|
||||
Sets the function for end of transission event handling
|
||||
|
||||
@param [in] handleSent the target function
|
||||
*/
|
||||
void attachSentHandler(void (* handleSent)(void));
|
||||
|
||||
/**
|
||||
Sets the function for end of receive event handling
|
||||
|
||||
@param [in] handleReceived the target function
|
||||
*/
|
||||
void attachReceivedHandler(void (* handleReceived)(void));
|
||||
|
||||
/**
|
||||
Sets the function for receive error event handling
|
||||
|
||||
@param [in] handleReceiveFailed the target function
|
||||
*/
|
||||
void attachReceiveFailedHandler(void (* handleReceiveFailed)(void));
|
||||
|
||||
/**
|
||||
Sets the function for receive timeout event handling
|
||||
|
||||
@param [in] handleReceiveTimeout the target function
|
||||
*/
|
||||
void attachReceiveTimeoutHandler(void (* handleReceiveTimeout)(void));
|
||||
|
||||
/**
|
||||
Sets the function for receive timestamp availabe event handling
|
||||
|
||||
@param [in] handleReceiveTimestampAvailable the target function
|
||||
*/
|
||||
void attachReceiveTimestampAvailableHandler(void (* handleReceiveTimestampAvailable)(void));
|
||||
|
||||
/**
|
||||
Handles dw1000 events triggered by interrupt
|
||||
By default this is attached to the interrupt pin callback
|
||||
*/
|
||||
void interruptServiceRoutine();
|
||||
|
||||
boolean isTransmitDone();
|
||||
|
||||
void clearTransmitStatus();
|
||||
|
||||
boolean isReceiveDone();
|
||||
|
||||
void clearReceiveStatus();
|
||||
|
||||
boolean isReceiveFailed();
|
||||
|
||||
void clearReceiveFailedStatus();
|
||||
|
||||
boolean isReceiveTimeout();
|
||||
|
||||
void clearReceiveTimeoutStatus();
|
||||
|
||||
/**
|
||||
Stops the transceiver immediately, this actually sets the device in Idle mode.
|
||||
*/
|
||||
void forceTRxOff();
|
||||
|
||||
/**
|
||||
Sets the interrupt polarity
|
||||
|
||||
By default this is set to true by the DW1000
|
||||
|
||||
@param [in] val True here means active high
|
||||
*/
|
||||
void setInterruptPolarity(boolean val);
|
||||
|
||||
/**
|
||||
Applies the target configuration to the DW1000
|
||||
|
||||
@param [in] config the configuration to apply to the DW1000
|
||||
*/
|
||||
void applyConfiguration(device_configuration_t config);
|
||||
|
||||
/**
|
||||
Enables the interrupts for the target events
|
||||
|
||||
@param [in] interrupt_config the interrupt map to use
|
||||
*/
|
||||
void applyInterruptConfiguration(interrupt_configuration_t interrupt_config);
|
||||
|
||||
/**
|
||||
Gets the current channel in use
|
||||
|
||||
returns the current channel
|
||||
*/
|
||||
Channel getChannel();
|
||||
|
||||
/**
|
||||
Gets the current PRF of the device
|
||||
|
||||
returns the current PRF
|
||||
*/
|
||||
PulseFrequency getPulseFrequency();
|
||||
|
||||
/**
|
||||
Sets the timeout for Raceive Frame.
|
||||
|
||||
@param[in] Pac size based on current preamble lenght - 1
|
||||
*/
|
||||
void setPreambleDetectionTimeout(uint16_t pacSize);
|
||||
|
||||
/**
|
||||
Sets the timeout for SFD detection.
|
||||
The recommended value is: PreambleLenght + SFD + 1.
|
||||
The default value is 4096+64+1
|
||||
|
||||
@param[in] the sfd detection timeout
|
||||
*/
|
||||
void setSfdDetectionTimeout(uint16_t preambleSymbols);
|
||||
|
||||
/**
|
||||
Sets the timeout for Raceive Frame. Must be sets in idle mode.
|
||||
Allow the external microprocessor to enter a low power state awaiting a valid receive frame.
|
||||
|
||||
@param[in] time in μs. units = ~1μs(1.026μs). 0 to disable
|
||||
*/
|
||||
void setReceiveFrameWaitTimeoutPeriod(uint16_t timeMicroSeconds);
|
||||
|
||||
/**
|
||||
Sets the device in receive mode
|
||||
|
||||
@param [in] mode IMMEDIATE or DELAYED receive
|
||||
*/
|
||||
void startReceive(ReceiveMode mode = ReceiveMode::IMMEDIATE);
|
||||
|
||||
/**
|
||||
Sets the device in transmission mode
|
||||
|
||||
@param [in] mode IMMEDIATE or DELAYED transmission
|
||||
*/
|
||||
void startTransmit(TransmitMode mode = TransmitMode::IMMEDIATE);
|
||||
|
||||
/**
|
||||
Gets the temperature inside the DW1000 Device
|
||||
|
||||
returns The temperature
|
||||
*/
|
||||
float getTemperature();
|
||||
|
||||
/**
|
||||
Gets the voltage in input of the DW1000
|
||||
|
||||
returns The input voltage
|
||||
*/
|
||||
float getBatteryVoltage();
|
||||
|
||||
/**
|
||||
Gets both temperature and voltage with a single read
|
||||
|
||||
@param [out] temp the temperature
|
||||
@param [out] vbat the input voltage
|
||||
*/
|
||||
void getTemperatureAndBatteryVoltage(float& temp, float& vbat);
|
||||
|
||||
/**
|
||||
Enables the frame filtering functionality using the provided configuration.
|
||||
Messages must be formatted using 802.15.4-2011 format.
|
||||
|
||||
@param [in] config frame filtering configuration
|
||||
*/
|
||||
void enableFrameFiltering(frame_filtering_configuration_t config);
|
||||
|
||||
/**
|
||||
Disables the frame filtering functionality
|
||||
*/
|
||||
void disableFrameFiltering();
|
||||
|
||||
/**
|
||||
WARNING: this just sets the relative bits inside the register.
|
||||
You must refer to the DW1000 User manual to activate it properly.
|
||||
*/
|
||||
void setDoubleBuffering(boolean val);
|
||||
|
||||
/**
|
||||
Enables frames up to 1023 byte length
|
||||
|
||||
@param [in] val true or false
|
||||
*/
|
||||
void useExtendedFrameLength(boolean val);
|
||||
|
||||
/**
|
||||
Sets the time before the device enters receive after a transmission.
|
||||
Use 0 here to deactivate it.
|
||||
|
||||
@param[in] time in μs. units = ~1μs(1.026μs)
|
||||
*/
|
||||
void setWait4Response(uint32_t timeMicroSeconds);
|
||||
|
||||
#if DW1000NG_PRINTABLE
|
||||
|
||||
/* ##### Print device id, address, etc. ###################################### */
|
||||
/**
|
||||
Generates a String representation of the device identifier of the chip. That usually
|
||||
are the letters "DECA" plus the version and revision numbers of the chip.
|
||||
|
||||
@param[out] msgBuffer The String buffer to be filled with printable device information.
|
||||
Provide 128 bytes, this should be sufficient.
|
||||
*/
|
||||
void getPrintableDeviceIdentifier(char msgBuffer[]);
|
||||
|
||||
/**
|
||||
Generates a String representation of the extended unique identifier (EUI) of the chip.
|
||||
|
||||
@param[out] msgBuffer The String buffer to be filled with printable device information.
|
||||
Provide 128 bytes, this should be sufficient.
|
||||
*/
|
||||
void getPrintableExtendedUniqueIdentifier(char msgBuffer[]);
|
||||
|
||||
/**
|
||||
Generates a String representation of the short address and network identifier currently
|
||||
defined for the respective chip.
|
||||
|
||||
@param[out] msgBuffer The String buffer to be filled with printable device information.
|
||||
Provide 128 bytes, this should be sufficient.
|
||||
*/
|
||||
void getPrintableNetworkIdAndShortAddress(char msgBuffer[]);
|
||||
|
||||
/**
|
||||
Generates a String representation of the main operational settings of the chip. This
|
||||
includes data rate, pulse repetition frequency, preamble and channel settings.
|
||||
|
||||
@param[out] msgBuffer The String buffer to be filled with printable device information.
|
||||
Provide 128 bytes, this should be sufficient.
|
||||
*/
|
||||
void getPrintableDeviceMode(char msgBuffer[]);
|
||||
#endif
|
||||
|
||||
#if DW1000NG_DEBUG
|
||||
void getPrettyBytes(byte data[], char msgBuffer[], uint16_t n);
|
||||
void getPrettyBytes(byte cmd, uint16_t offset, char msgBuffer[], uint16_t n);
|
||||
#endif
|
||||
};
|
||||
@@ -0,0 +1,70 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
/**
|
||||
* Copyright (c) 2016 by Ludwig Grill (www.rotzbua.de)
|
||||
* Decawave DW1000 library for arduino.
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*
|
||||
* @file DW1000CompileOptions.h
|
||||
* Here are some options to optimize code and save some ram and rom
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* Adds printing functions to base driver
|
||||
*
|
||||
*/
|
||||
#define DW1000NG_PRINTABLE true
|
||||
/**
|
||||
* Adds debug functionalities
|
||||
*
|
||||
*/
|
||||
#define DW1000NG_DEBUG false
|
||||
|
||||
/**
|
||||
* Optimizes code for the DWM1000 Module
|
||||
*/
|
||||
#define DWM1000_OPTIMIZED false
|
||||
|
||||
/**
|
||||
* Printable DW1000NgDeviceConfiguration about: rom:2494 byte ; ram 256 byte
|
||||
* This option is needed because compiler can not optimize unused codes from inheritanced methods
|
||||
* Some examples or debug code use this
|
||||
* Set false if you do not need it and have to save some space
|
||||
*/
|
||||
#define DW1000NGCONFIGURATION_H_PRINTABLE false
|
||||
@@ -0,0 +1,73 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "DW1000NgConstants.hpp"
|
||||
|
||||
typedef struct device_configuration_t {
|
||||
boolean extendedFrameLength;
|
||||
boolean receiverAutoReenable;
|
||||
boolean smartPower;
|
||||
boolean frameCheck;
|
||||
boolean nlos;
|
||||
SFDMode sfd;
|
||||
Channel channel;
|
||||
DataRate dataRate;
|
||||
PulseFrequency pulseFreq;
|
||||
PreambleLength preambleLen;
|
||||
PreambleCode preaCode;
|
||||
} device_configuration_t;
|
||||
|
||||
typedef struct interrupt_configuration_t {
|
||||
boolean interruptOnSent;
|
||||
boolean interruptOnReceived;
|
||||
boolean interruptOnReceiveFailed;
|
||||
boolean interruptOnReceiveTimeout;
|
||||
boolean interruptOnReceiveTimestampAvailable;
|
||||
boolean interruptOnAutomaticAcknowledgeTrigger;
|
||||
} interrupt_configuration_t;
|
||||
|
||||
typedef struct frame_filtering_configuration_t {
|
||||
boolean behaveAsCoordinator;
|
||||
boolean allowBeacon;
|
||||
boolean allowData;
|
||||
boolean allowAcknowledgement;
|
||||
boolean allowMacCommand;
|
||||
boolean allowAllReserved;
|
||||
boolean allowReservedFour;
|
||||
boolean allowReservedFive;
|
||||
} frame_filtering_configuration_t;
|
||||
|
||||
typedef struct sleep_configuration_t {
|
||||
boolean onWakeUpRunADC;
|
||||
boolean onWakeUpReceive;
|
||||
boolean onWakeUpLoadEUI;
|
||||
boolean onWakeUpLoadL64Param;
|
||||
boolean preserveSleep;
|
||||
boolean enableSLP;
|
||||
boolean enableWakePIN;
|
||||
boolean enableWakeSPI;
|
||||
} sleep_configuration_t;
|
||||
@@ -0,0 +1,235 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
#define GPIO_MODE 0
|
||||
#define LED_MODE 1
|
||||
|
||||
|
||||
constexpr float TIME_RES = 0.000015650040064103f;
|
||||
constexpr float TIME_RES_INV = 63897.6f;
|
||||
|
||||
/* Speed of radio waves (light) [m/s] * timestamp resolution [~15.65ps] of DW1000Ng */
|
||||
constexpr float DISTANCE_OF_RADIO = 0.0046917639786159f;
|
||||
constexpr float DISTANCE_OF_RADIO_INV = 213.139451293f;
|
||||
|
||||
// timestamp byte length - 40 bit -> 5 byte
|
||||
constexpr uint8_t LENGTH_TIMESTAMP = 5;
|
||||
|
||||
// timer/counter overflow (40 bits) -> 4overflow approx. every 17.2 seconds
|
||||
constexpr int64_t TIME_OVERFLOW = 0x10000000000; //1099511627776LL
|
||||
constexpr int64_t TIME_MAX = 0xffffffffff;
|
||||
|
||||
// time factors (relative to [us]) for setting delayed transceive
|
||||
// TODO use non float
|
||||
constexpr float SECONDS = 1e6;
|
||||
constexpr float MILLISECONDS = 1e3;
|
||||
constexpr float MICROSECONDS = 1;
|
||||
constexpr float NANOSECONDS = 1e-3;
|
||||
|
||||
/* preamble codes (CHAN_CTRL - RX & TX _CODE) - reg:0x1F, bits:31-27,26-22 */
|
||||
|
||||
enum class PreambleCode : byte {
|
||||
CODE_1 = 1,
|
||||
CODE_2,
|
||||
CODE_3,
|
||||
CODE_4,
|
||||
CODE_5,
|
||||
CODE_6,
|
||||
CODE_7,
|
||||
CODE_8,
|
||||
CODE_9,
|
||||
CODE_10,
|
||||
CODE_11,
|
||||
CODE_12,
|
||||
CODE_17 = 17,
|
||||
CODE_18,
|
||||
CODE_19,
|
||||
CODE_20
|
||||
};
|
||||
|
||||
/* Validity matrix for 16 MHz PRF preamble codes */
|
||||
constexpr byte preamble_validity_matrix_PRF16[8][2] = {
|
||||
{0,0}, /* Channel 0 doesn't exist */
|
||||
{1, 2},
|
||||
{3, 4},
|
||||
{5, 6},
|
||||
{7, 8},
|
||||
{3, 4},
|
||||
{0,0}, /* Channel 5 doesn't exist */
|
||||
{7, 8}
|
||||
};
|
||||
|
||||
/* Validity matrix for 64 MHz PRF preamble codes */
|
||||
constexpr byte preamble_validity_matrix_PRF64[8][4] = {
|
||||
{0,0,0,0}, /* Channel 0 doesn't exist */
|
||||
{9, 10, 11, 12},
|
||||
{9, 10, 11, 12},
|
||||
{9, 10, 11, 12},
|
||||
{17, 18, 19, 20},
|
||||
{9, 10, 11, 12},
|
||||
{0,0,0,0}, /* Channel 5 doesn't exist */
|
||||
{17, 18, 19, 20}
|
||||
};
|
||||
|
||||
/* transmission/reception bit rate (TXBR) - reg:0x08, bits:14,13 */
|
||||
enum class DataRate : byte {
|
||||
RATE_110KBPS,
|
||||
RATE_850KBPS,
|
||||
RATE_6800KBPS
|
||||
};
|
||||
|
||||
/* transmission pulse frequency (TXPRF) - reg:0x08, bits:17,16
|
||||
* 0x00 is 4MHZ, but receiver in DW1000Ng does not support it (!??) */
|
||||
enum class PulseFrequency : byte {
|
||||
FREQ_16MHZ = 0x01,
|
||||
FREQ_64MHZ
|
||||
};
|
||||
|
||||
/* preamble length (PE + TXPSR) - reg:0x08, bits:21,20,19,18 - table 16 */
|
||||
enum class PreambleLength : byte {
|
||||
LEN_64 = 0x01,
|
||||
LEN_128 = 0x05,
|
||||
LEN_256 = 0x09,
|
||||
LEN_512 = 0x0D,
|
||||
LEN_1024 = 0x02,
|
||||
LEN_1536 = 0x06,
|
||||
LEN_2048 = 0x0A,
|
||||
LEN_4096 = 0x03
|
||||
};
|
||||
|
||||
|
||||
/* PAC size (DRX_TUNE2) - reg:0x08, sub-reg:0x27, bits:26,25 - table 33
|
||||
* The value to program the sub-register changes in based of RXPRF */
|
||||
enum class PacSize : byte {
|
||||
SIZE_8 = 8,
|
||||
SIZE_16 = 16,
|
||||
SIZE_32 = 32,
|
||||
SIZE_64 = 64
|
||||
};
|
||||
|
||||
/* channel of operation (CHAN_CTRL - TX & RX _CHAN) - reg:0x1F, bits:3-0,7-4 */
|
||||
enum class Channel : byte {
|
||||
CHANNEL_1 = 1,
|
||||
CHANNEL_2,
|
||||
CHANNEL_3,
|
||||
CHANNEL_4,
|
||||
CHANNEL_5,
|
||||
CHANNEL_7 = 7
|
||||
};
|
||||
|
||||
/* Register is 6 bit, 7 = write, 6 = sub-adressing, 5-0 = register value
|
||||
* Total header with sub-adressing can be 15 bit. */
|
||||
constexpr byte WRITE = 0x80; // regular write
|
||||
constexpr byte WRITE_SUB = 0xC0; // write with sub address
|
||||
constexpr byte READ = 0x00; // regular read
|
||||
constexpr byte READ_SUB = 0x40; // read with sub address
|
||||
constexpr byte RW_SUB_EXT = 0x80; // R/W with sub address extension
|
||||
|
||||
/* clocks available. */
|
||||
constexpr byte SYS_AUTO_CLOCK = 0x00;
|
||||
constexpr byte SYS_XTI_CLOCK = 0x01;
|
||||
constexpr byte SYS_PLL_CLOCK = 0x02;
|
||||
constexpr byte TX_PLL_CLOCK = 0x20;
|
||||
constexpr byte LDE_CLOCK = 0x03;
|
||||
|
||||
/* range bias tables - APS011*/
|
||||
|
||||
constexpr double BIAS_TABLE[18][5] = {
|
||||
{61, -198, -110, -275, -295},
|
||||
{63, -187, -105, -244, -266},
|
||||
{65, -179, -100, -210, -235},
|
||||
{67, -163, -93, -176, -199},
|
||||
{69, -143, -82, -138, -150},
|
||||
{71, -127, -69, -95, -100},
|
||||
{73, -109, -51, -51, -58},
|
||||
{75, -84, -27, 0, 0},
|
||||
{77, -59, 0, 42, 49},
|
||||
{79, -31, 21, 97, 91},
|
||||
{81, 0, 35, 158, 127},
|
||||
{83, 36, 42, 210, 153},
|
||||
{85, 65, 49, 254, 175},
|
||||
{87, 84, 62, 294, 197},
|
||||
{89, 97, 71, 321, 233},
|
||||
{91, 106, 76, 339, 245},
|
||||
{93, 110, 81, 356, 264},
|
||||
{95, 112, 86, 394, 284}
|
||||
};
|
||||
|
||||
enum class DriverAmplifierValue : byte {
|
||||
dB_18,
|
||||
dB_15,
|
||||
dB_12,
|
||||
dB_9,
|
||||
dB_6,
|
||||
dB_3,
|
||||
dB_0,
|
||||
OFF
|
||||
};
|
||||
|
||||
enum class TransmitMixerValue : byte {
|
||||
dB_0,
|
||||
dB_0_5,
|
||||
dB_1,
|
||||
dB_1_5,
|
||||
dB_2,
|
||||
dB_2_5,
|
||||
dB_3,
|
||||
dB_3_5,
|
||||
dB_4,
|
||||
dB_4_5,
|
||||
dB_5,
|
||||
dB_5_5,
|
||||
dB_6,
|
||||
dB_6_5,
|
||||
dB_7,
|
||||
dB_7_5,
|
||||
dB_8,
|
||||
dB_8_5,
|
||||
dB_9,
|
||||
dB_9_5,
|
||||
dB_10,
|
||||
dB_10_5,
|
||||
dB_11,
|
||||
dB_11_5,
|
||||
dB_12,
|
||||
dB_12_5,
|
||||
dB_13,
|
||||
dB_13_5,
|
||||
dB_14,
|
||||
dB_14_5,
|
||||
dB_15,
|
||||
dB_15_5
|
||||
};
|
||||
|
||||
enum class SFDMode {STANDARD_SFD, DECAWAVE_SFD};
|
||||
|
||||
enum class TransmitMode {IMMEDIATE, DELAYED};
|
||||
|
||||
enum class ReceiveMode {IMMEDIATE, DELAYED};
|
||||
|
||||
enum class SPIClock {SLOW, FAST};
|
||||
@@ -0,0 +1,349 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "DW1000NgRTLS.hpp"
|
||||
#include "DW1000Ng.hpp"
|
||||
#include "DW1000NgUtils.hpp"
|
||||
#include "DW1000NgTime.hpp"
|
||||
#include "DW1000NgRanging.hpp"
|
||||
|
||||
static byte SEQ_NUMBER = 0;
|
||||
|
||||
namespace DW1000NgRTLS {
|
||||
|
||||
byte increaseSequenceNumber(){
|
||||
return ++SEQ_NUMBER;
|
||||
}
|
||||
|
||||
void transmitTwrShortBlink() {
|
||||
byte Blink[] = {BLINK, SEQ_NUMBER++, 0,0,0,0,0,0,0,0, NO_BATTERY_STATUS | NO_EX_ID, TAG_LISTENING_NOW};
|
||||
DW1000Ng::getEUI(&Blink[2]);
|
||||
DW1000Ng::setTransmitData(Blink, sizeof(Blink));
|
||||
DW1000Ng::startTransmit();
|
||||
}
|
||||
|
||||
void transmitRangingInitiation(byte tag_eui[], byte tag_short_address[]) {
|
||||
byte RangingInitiation[] = {DATA, SHORT_SRC_LONG_DEST, SEQ_NUMBER++, 0,0, 0,0,0,0,0,0,0,0, 0,0, RANGING_INITIATION, 0,0};
|
||||
DW1000Ng::getNetworkId(&RangingInitiation[3]);
|
||||
memcpy(&RangingInitiation[5], tag_eui, 8);
|
||||
DW1000Ng::getDeviceAddress(&RangingInitiation[13]);
|
||||
memcpy(&RangingInitiation[16], tag_short_address, 2);
|
||||
DW1000Ng::setTransmitData(RangingInitiation, sizeof(RangingInitiation));
|
||||
DW1000Ng::startTransmit();
|
||||
}
|
||||
|
||||
void transmitPoll(byte anchor_address[]){
|
||||
byte Poll[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0 , RANGING_TAG_POLL};
|
||||
DW1000Ng::getNetworkId(&Poll[3]);
|
||||
memcpy(&Poll[5], anchor_address, 2);
|
||||
DW1000Ng::getDeviceAddress(&Poll[7]);
|
||||
DW1000Ng::setTransmitData(Poll, sizeof(Poll));
|
||||
DW1000Ng::startTransmit();
|
||||
}
|
||||
|
||||
void transmitResponseToPoll(byte tag_short_address[]) {
|
||||
byte pollAck[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONTINUE, 0, 0};
|
||||
DW1000Ng::getNetworkId(&pollAck[3]);
|
||||
memcpy(&pollAck[5], tag_short_address, 2);
|
||||
DW1000Ng::getDeviceAddress(&pollAck[7]);
|
||||
DW1000Ng::setTransmitData(pollAck, sizeof(pollAck));
|
||||
DW1000Ng::startTransmit();
|
||||
}
|
||||
|
||||
void transmitFinalMessage(byte anchor_address[], uint16_t reply_delay, uint64_t timePollSent, uint64_t timeResponseToPollReceived) {
|
||||
/* Calculation of future time */
|
||||
byte futureTimeBytes[LENGTH_TIMESTAMP];
|
||||
|
||||
uint64_t timeFinalMessageSent = DW1000Ng::getSystemTimestamp();
|
||||
timeFinalMessageSent += DW1000NgTime::microsecondsToUWBTime(reply_delay);
|
||||
DW1000NgUtils::writeValueToBytes(futureTimeBytes, timeFinalMessageSent, LENGTH_TIMESTAMP);
|
||||
DW1000Ng::setDelayedTRX(futureTimeBytes);
|
||||
timeFinalMessageSent += DW1000Ng::getTxAntennaDelay();
|
||||
|
||||
byte finalMessage[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, RANGING_TAG_FINAL_RESPONSE_EMBEDDED,
|
||||
0,0,0,0,0,0,0,0,0,0,0,0
|
||||
};
|
||||
|
||||
DW1000Ng::getNetworkId(&finalMessage[3]);
|
||||
memcpy(&finalMessage[5], anchor_address, 2);
|
||||
DW1000Ng::getDeviceAddress(&finalMessage[7]);
|
||||
|
||||
DW1000NgUtils::writeValueToBytes(finalMessage + 10, (uint32_t) timePollSent, 4);
|
||||
DW1000NgUtils::writeValueToBytes(finalMessage + 14, (uint32_t) timeResponseToPollReceived, 4);
|
||||
DW1000NgUtils::writeValueToBytes(finalMessage + 18, (uint32_t) timeFinalMessageSent, 4);
|
||||
DW1000Ng::setTransmitData(finalMessage, sizeof(finalMessage));
|
||||
DW1000Ng::startTransmit(TransmitMode::DELAYED);
|
||||
}
|
||||
|
||||
void transmitRangingConfirm(byte tag_short_address[], byte next_anchor[]) {
|
||||
byte rangingConfirm[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONFIRM, next_anchor[0], next_anchor[1]};
|
||||
DW1000Ng::getNetworkId(&rangingConfirm[3]);
|
||||
memcpy(&rangingConfirm[5], tag_short_address, 2);
|
||||
DW1000Ng::getDeviceAddress(&rangingConfirm[7]);
|
||||
DW1000Ng::setTransmitData(rangingConfirm, sizeof(rangingConfirm));
|
||||
DW1000Ng::startTransmit();
|
||||
}
|
||||
|
||||
void transmitActivityFinished(byte tag_short_address[], byte blink_rate[]) {
|
||||
/* I send the new blink rate to the tag */
|
||||
byte rangingConfirm[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, ACTIVITY_FINISHED, blink_rate[0], blink_rate[1]};
|
||||
DW1000Ng::getNetworkId(&rangingConfirm[3]);
|
||||
memcpy(&rangingConfirm[5], tag_short_address, 2);
|
||||
DW1000Ng::getDeviceAddress(&rangingConfirm[7]);
|
||||
DW1000Ng::setTransmitData(rangingConfirm, sizeof(rangingConfirm));
|
||||
DW1000Ng::startTransmit();
|
||||
}
|
||||
|
||||
static uint32_t calculateNewBlinkRate(byte frame[]) {
|
||||
uint32_t blinkRate = frame[11] + static_cast<uint32_t>(((frame[12] & 0x3F) << 8));
|
||||
byte multiplier = ((frame[12] & 0xC0) >> 6);
|
||||
if(multiplier == 0x01) {
|
||||
blinkRate *= 25;
|
||||
} else if(multiplier == 0x02) {
|
||||
blinkRate *= 1000;
|
||||
}
|
||||
|
||||
return blinkRate;
|
||||
}
|
||||
|
||||
void waitForTransmission() {
|
||||
while(!DW1000Ng::isTransmitDone()) {
|
||||
#if defined(ESP8266)
|
||||
yield();
|
||||
#endif
|
||||
}
|
||||
DW1000Ng::clearTransmitStatus();
|
||||
}
|
||||
|
||||
boolean receiveFrame() {
|
||||
DW1000Ng::startReceive();
|
||||
while(!DW1000Ng::isReceiveDone()) {
|
||||
if(DW1000Ng::isReceiveTimeout() ) {
|
||||
DW1000Ng::clearReceiveTimeoutStatus();
|
||||
return false;
|
||||
}
|
||||
#if defined(ESP8266)
|
||||
yield();
|
||||
#endif
|
||||
}
|
||||
DW1000Ng::clearReceiveStatus();
|
||||
return true;
|
||||
}
|
||||
|
||||
static boolean waitForNextRangingStep() {
|
||||
DW1000NgRTLS::waitForTransmission();
|
||||
if(!DW1000NgRTLS::receiveFrame()) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
RangeRequestResult tagRangeRequest() {
|
||||
DW1000NgRTLS::transmitTwrShortBlink();
|
||||
|
||||
if(!DW1000NgRTLS::waitForNextRangingStep()) return {false, 0};
|
||||
|
||||
size_t init_len = DW1000Ng::getReceivedDataLength();
|
||||
byte init_recv[init_len];
|
||||
DW1000Ng::getReceivedData(init_recv, init_len);
|
||||
|
||||
if(!(init_len > 17 && init_recv[15] == RANGING_INITIATION)) {
|
||||
return { false, 0};
|
||||
}
|
||||
|
||||
DW1000Ng::setDeviceAddress(DW1000NgUtils::bytesAsValue(&init_recv[16], 2));
|
||||
return { true, static_cast<uint16_t>(DW1000NgUtils::bytesAsValue(&init_recv[13], 2)) };
|
||||
}
|
||||
|
||||
static RangeResult tagFinishRange(uint16_t anchor, uint16_t replyDelayUs) {
|
||||
RangeResult returnValue;
|
||||
|
||||
byte target_anchor[2];
|
||||
DW1000NgUtils::writeValueToBytes(target_anchor, anchor, 2);
|
||||
DW1000NgRTLS::transmitPoll(target_anchor);
|
||||
/* Start of poll control for range */
|
||||
if(!DW1000NgRTLS::waitForNextRangingStep()) {
|
||||
returnValue = {false, false, 0, 0};
|
||||
} else {
|
||||
|
||||
size_t cont_len = DW1000Ng::getReceivedDataLength();
|
||||
byte cont_recv[cont_len];
|
||||
DW1000Ng::getReceivedData(cont_recv, cont_len);
|
||||
|
||||
if (cont_len > 10 && cont_recv[9] == ACTIVITY_CONTROL && cont_recv[10] == RANGING_CONTINUE) {
|
||||
/* Received Response to poll */
|
||||
DW1000NgRTLS::transmitFinalMessage(
|
||||
&cont_recv[7],
|
||||
replyDelayUs,
|
||||
DW1000Ng::getTransmitTimestamp(), // Poll transmit time
|
||||
DW1000Ng::getReceiveTimestamp() // Response to poll receive time
|
||||
);
|
||||
|
||||
if(!DW1000NgRTLS::waitForNextRangingStep()) {
|
||||
returnValue = {false, false, 0, 0};
|
||||
} else {
|
||||
|
||||
size_t act_len = DW1000Ng::getReceivedDataLength();
|
||||
byte act_recv[act_len];
|
||||
DW1000Ng::getReceivedData(act_recv, act_len);
|
||||
|
||||
if(act_len > 10 && act_recv[9] == ACTIVITY_CONTROL) {
|
||||
if (act_len > 12 && act_recv[10] == RANGING_CONFIRM) {
|
||||
returnValue = {true, true, static_cast<uint16_t>(DW1000NgUtils::bytesAsValue(&act_recv[11], 2)), 0};
|
||||
} else if(act_len > 12 && act_recv[10] == ACTIVITY_FINISHED) {
|
||||
returnValue = {true, false, 0, calculateNewBlinkRate(act_recv)};
|
||||
}
|
||||
} else {
|
||||
returnValue = {false, false, 0, 0};
|
||||
}
|
||||
}
|
||||
} else {
|
||||
returnValue = {false, false, 0, 0};
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
return returnValue;
|
||||
}
|
||||
|
||||
RangeInfrastructureResult tagRangeInfrastructure(uint16_t target_anchor, uint16_t finalMessageDelay) {
|
||||
RangeInfrastructureResult returnValue;
|
||||
|
||||
RangeResult result = tagFinishRange(target_anchor, finalMessageDelay);
|
||||
byte keep_going = 1;
|
||||
|
||||
if(!result.success) {
|
||||
keep_going = 0;
|
||||
returnValue = {false , 0};
|
||||
} else {
|
||||
while(result.success && result.next) {
|
||||
result = tagFinishRange(result.next_anchor, finalMessageDelay);
|
||||
if(!result.success) {
|
||||
keep_going = 0;
|
||||
returnValue = {false , 0};
|
||||
break;
|
||||
}
|
||||
|
||||
#if defined(ESP8266)
|
||||
if (keep_going == 1) {
|
||||
yield();
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
if (keep_going == 1) {
|
||||
|
||||
if(result.success && result.new_blink_rate != 0) {
|
||||
keep_going = 0;
|
||||
returnValue = { true, static_cast<uint16_t>(result.new_blink_rate) };
|
||||
} else {
|
||||
if(!result.success) {
|
||||
keep_going = 0;
|
||||
returnValue = { false , 0 };
|
||||
} else {
|
||||
// TODO. Handle this condition?
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return returnValue;
|
||||
}
|
||||
|
||||
RangeInfrastructureResult tagTwrLocalize(uint16_t finalMessageDelay) {
|
||||
RangeRequestResult request_result = DW1000NgRTLS::tagRangeRequest();
|
||||
|
||||
if(request_result.success) {
|
||||
|
||||
RangeInfrastructureResult result = DW1000NgRTLS::tagRangeInfrastructure(request_result.target_anchor, finalMessageDelay);
|
||||
|
||||
if(result.success)
|
||||
return result;
|
||||
}
|
||||
return {false, 0};
|
||||
}
|
||||
|
||||
RangeAcceptResult anchorRangeAccept(NextActivity next, uint16_t value) {
|
||||
RangeAcceptResult returnValue;
|
||||
|
||||
double range;
|
||||
if(!DW1000NgRTLS::receiveFrame()) {
|
||||
returnValue = {false, 0};
|
||||
} else {
|
||||
|
||||
size_t poll_len = DW1000Ng::getReceivedDataLength();
|
||||
byte poll_data[poll_len];
|
||||
DW1000Ng::getReceivedData(poll_data, poll_len);
|
||||
|
||||
if(poll_len > 9 && poll_data[9] == RANGING_TAG_POLL) {
|
||||
uint64_t timePollReceived = DW1000Ng::getReceiveTimestamp();
|
||||
DW1000NgRTLS::transmitResponseToPoll(&poll_data[7]);
|
||||
DW1000NgRTLS::waitForTransmission();
|
||||
uint64_t timeResponseToPoll = DW1000Ng::getTransmitTimestamp();
|
||||
delayMicroseconds(1500);
|
||||
|
||||
if(!DW1000NgRTLS::receiveFrame()) {
|
||||
returnValue = {false, 0};
|
||||
} else {
|
||||
|
||||
size_t rfinal_len = DW1000Ng::getReceivedDataLength();
|
||||
byte rfinal_data[rfinal_len];
|
||||
DW1000Ng::getReceivedData(rfinal_data, rfinal_len);
|
||||
if(rfinal_len > 18 && rfinal_data[9] == RANGING_TAG_FINAL_RESPONSE_EMBEDDED) {
|
||||
uint64_t timeFinalMessageReceive = DW1000Ng::getReceiveTimestamp();
|
||||
|
||||
byte finishValue[2];
|
||||
DW1000NgUtils::writeValueToBytes(finishValue, value, 2);
|
||||
|
||||
if(next == NextActivity::RANGING_CONFIRM) {
|
||||
DW1000NgRTLS::transmitRangingConfirm(&rfinal_data[7], finishValue);
|
||||
} else {
|
||||
DW1000NgRTLS::transmitActivityFinished(&rfinal_data[7], finishValue);
|
||||
}
|
||||
|
||||
DW1000NgRTLS::waitForTransmission();
|
||||
|
||||
range = DW1000NgRanging::computeRangeAsymmetric(
|
||||
DW1000NgUtils::bytesAsValue(rfinal_data + 10, LENGTH_TIMESTAMP), // Poll send time
|
||||
timePollReceived,
|
||||
timeResponseToPoll, // Response to poll sent time
|
||||
DW1000NgUtils::bytesAsValue(rfinal_data + 14, LENGTH_TIMESTAMP), // Response to Poll Received
|
||||
DW1000NgUtils::bytesAsValue(rfinal_data + 18, LENGTH_TIMESTAMP), // Final Message send time
|
||||
timeFinalMessageReceive // Final message receive time
|
||||
);
|
||||
|
||||
range = DW1000NgRanging::correctRange(range);
|
||||
|
||||
/* In case of wrong read due to bad device calibration */
|
||||
if(range <= 0)
|
||||
range = 0.000001;
|
||||
|
||||
returnValue = {true, range};
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return returnValue;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,133 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
/* Frame control */
|
||||
constexpr byte BLINK = 0xC5;
|
||||
constexpr byte DATA = 0x41;
|
||||
constexpr byte SHORT_SRC_AND_DEST = 0x88;
|
||||
constexpr byte LONG_SRC_AND_DEST = 0xCC;
|
||||
constexpr byte SHORT_SRC_LONG_DEST = 0x8C;
|
||||
constexpr byte LONG_SRC_SHORT_DEST = 0xC8;
|
||||
|
||||
/* Application ID */
|
||||
constexpr byte RTLS_APP_ID_LOW = 0x9A;
|
||||
constexpr byte RTLS_APP_ID_HIGH = 0x60;
|
||||
constexpr uint16_t RTLS_APP_ID = RTLS_APP_ID_LOW | ((uint16_t) (RTLS_APP_ID_HIGH << 8));
|
||||
|
||||
/* Function code */
|
||||
constexpr byte ACTIVITY_CONTROL = 0x10;
|
||||
constexpr byte RANGING_INITIATION = 0x20;
|
||||
constexpr byte RANGING_TAG_POLL = 0x21;
|
||||
constexpr byte RANGING_TAG_FINAL_RESPONSE_EMBEDDED = 0x23;
|
||||
constexpr byte RANGING_TAG_FINAL_RESPONSE_NO_EMBEDDED = 0x25;
|
||||
constexpr byte RANGING_TAG_FINAL_SEND_TIME = 0x27;
|
||||
|
||||
/* Activity code */
|
||||
constexpr byte ACTIVITY_FINISHED = 0x00;
|
||||
constexpr byte RANGING_CONFIRM = 0x01;
|
||||
constexpr byte RANGING_CONTINUE = 0x02;
|
||||
|
||||
/* BLINK Encoding Header */
|
||||
constexpr byte BATTERY_GOOD = 0x00;
|
||||
constexpr byte BATTERY_10_30_PERCENT = 0x02;
|
||||
constexpr byte BATTERY_0_10_PERCENT = 0x01;
|
||||
constexpr byte NO_BATTERY_STATUS = 0x03;
|
||||
|
||||
constexpr byte TEMPERATURE_DATA = 0x20;
|
||||
|
||||
constexpr byte EX_ID = 0x80;
|
||||
constexpr byte NO_EX_ID = 0x40;
|
||||
|
||||
/* BLINK Ext Header */
|
||||
constexpr byte BLINK_RATE_AND_LISTENING = 0x01;
|
||||
constexpr byte TAG_LISTENING_NOW = 0x02;
|
||||
|
||||
enum class NextActivity {
|
||||
ACTIVITY_FINISHED,
|
||||
RANGING_CONFIRM
|
||||
};
|
||||
|
||||
typedef struct RangeRequestResult {
|
||||
boolean success;
|
||||
uint16_t target_anchor;
|
||||
} RangeRequestResult;
|
||||
|
||||
typedef struct RangeResult {
|
||||
boolean success;
|
||||
boolean next;
|
||||
uint16_t next_anchor;
|
||||
uint32_t new_blink_rate;
|
||||
} RangeResult;
|
||||
|
||||
typedef struct RangeInfrastructureResult {
|
||||
boolean success;
|
||||
uint16_t new_blink_rate;
|
||||
} RangeInfrastructureResult;
|
||||
|
||||
typedef struct RangeAcceptResult {
|
||||
boolean success;
|
||||
double range;
|
||||
} RangeAcceptResult;
|
||||
|
||||
namespace DW1000NgRTLS {
|
||||
/*** TWR functions used in ISO/IEC 24730-62:2013, refer to the standard or the decawave manual for details about TWR ***/
|
||||
byte increaseSequenceNumber();
|
||||
void transmitTwrShortBlink();
|
||||
void transmitRangingInitiation(byte tag_eui[], byte tag_short_address[]);
|
||||
void transmitPoll(byte anchor_address[]);
|
||||
void transmitResponseToPoll(byte tag_short_address[]);
|
||||
void transmitFinalMessage(byte anchor_address[], uint16_t reply_delay, uint64_t timePollSent, uint64_t timeResponseToPollReceived);
|
||||
void transmitRangingConfirm(byte tag_short_address[], byte next_anchor[]);
|
||||
void transmitActivityFinished(byte tag_short_address[], byte blink_rate[]);
|
||||
|
||||
boolean receiveFrame();
|
||||
void waitForTransmission();
|
||||
/*** End of TWR functions ***/
|
||||
|
||||
/* Send a request range from tag to the rtls infrastructure */
|
||||
RangeRequestResult tagRangeRequest();
|
||||
|
||||
/* Used by an anchor to accept an incoming tagRangeRequest by means of the infrastructure
|
||||
NextActivity is used to indicate the tag what to do next after the ranging process (Activity finished is to return to blink (range request),
|
||||
Continue range is to tell the tag to range a new anchor)
|
||||
value is the value relative to the next activity (Activity finished = new blink rante, continue range = new anchor address)
|
||||
*/
|
||||
RangeAcceptResult anchorRangeAccept(NextActivity next, uint16_t value);
|
||||
|
||||
/* Used by tag to range after range request accept of the infrastructure
|
||||
Target anchor is given after a range request success
|
||||
Finalmessagedelay is used in the process of TWR, a value of 1500 works on 8mhz-80mhz range devices,
|
||||
you could try to decrease it to improve system performance.
|
||||
*/
|
||||
RangeInfrastructureResult tagRangeInfrastructure(uint16_t target_anchor, uint16_t finalMessageDelay);
|
||||
|
||||
/* Can be used as a single function start the localization process from the tag.
|
||||
Finalmessagedelay is the same as in function tagRangeInfrastructure
|
||||
*/
|
||||
RangeInfrastructureResult tagTwrLocalize(uint16_t finalMessageDelay);
|
||||
}
|
||||
@@ -0,0 +1,88 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "DW1000Ng.hpp"
|
||||
#include "DW1000NgConstants.hpp"
|
||||
#include "DW1000NgRanging.hpp"
|
||||
#include "DW1000NgConstants.hpp"
|
||||
#include "DW1000NgRTLS.hpp"
|
||||
|
||||
namespace DW1000NgRanging {
|
||||
|
||||
/* asymmetric two-way ranging (more computation intense, less error prone) */
|
||||
double computeRangeAsymmetric(
|
||||
uint64_t timePollSent,
|
||||
uint64_t timePollReceived,
|
||||
uint64_t timePollAckSent,
|
||||
uint64_t timePollAckReceived,
|
||||
uint64_t timeRangeSent,
|
||||
uint64_t timeRangeReceived
|
||||
)
|
||||
{
|
||||
uint32_t timePollSent_32 = static_cast<uint32_t>(timePollSent);
|
||||
uint32_t timePollReceived_32 = static_cast<uint32_t>(timePollReceived);
|
||||
uint32_t timePollAckSent_32 = static_cast<uint32_t>(timePollAckSent);
|
||||
uint32_t timePollAckReceived_32 = static_cast<uint32_t>(timePollAckReceived);
|
||||
uint32_t timeRangeSent_32 = static_cast<uint32_t>(timeRangeSent);
|
||||
uint32_t timeRangeReceived_32 = static_cast<uint32_t>(timeRangeReceived);
|
||||
|
||||
double round1 = static_cast<double>(timePollAckReceived_32 - timePollSent_32);
|
||||
double reply1 = static_cast<double>(timePollAckSent_32 - timePollReceived_32);
|
||||
double round2 = static_cast<double>(timeRangeReceived_32 - timePollAckSent_32);
|
||||
double reply2 = static_cast<double>(timeRangeSent_32 - timePollAckReceived_32);
|
||||
|
||||
int64_t tof_uwb = static_cast<int64_t>((round1 * round2 - reply1 * reply2) / (round1 + round2 + reply1 + reply2));
|
||||
double distance = tof_uwb * DISTANCE_OF_RADIO;
|
||||
|
||||
return distance;
|
||||
}
|
||||
|
||||
double correctRange(double range) {
|
||||
double result = 0;
|
||||
|
||||
Channel currentChannel = DW1000Ng::getChannel();
|
||||
double rxPower = -(static_cast<double>(DW1000Ng::getReceivePower()));
|
||||
|
||||
size_t index = DW1000Ng::getPulseFrequency() == PulseFrequency::FREQ_16MHZ ? 1 : 2;
|
||||
if(currentChannel == Channel::CHANNEL_4 || currentChannel == Channel::CHANNEL_7)
|
||||
index+=2;
|
||||
|
||||
if (rxPower < BIAS_TABLE[0][0]) {
|
||||
result = range += BIAS_TABLE[0][index]*0.001;
|
||||
} else if (rxPower >= BIAS_TABLE[17][0]) {
|
||||
result = range += BIAS_TABLE[17][index]*0.001;
|
||||
} else {
|
||||
for(auto i=0; i < 17; i++) {
|
||||
if (rxPower >= BIAS_TABLE[i][0] && rxPower < BIAS_TABLE[i+1][0]){
|
||||
result = range += BIAS_TABLE[i][index]*0.001;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,59 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
namespace DW1000NgRanging {
|
||||
|
||||
/**
|
||||
Asymmetric two-way ranging algorithm (more computation intense, less error prone)
|
||||
|
||||
@param [in] timePollSent timestamp of poll transmission
|
||||
@param [in] timePollReceived timestamp of poll receive
|
||||
@param [in] timePollAckSent timestamp of response to poll transmission
|
||||
@param [in] timePollAckReceived timestamp of response to poll receive
|
||||
@param [in] timeRangeSent timestamp of final message transmission
|
||||
@param [in] timeRangeReceived timestamp of final message receive
|
||||
|
||||
returns the range in meters
|
||||
*/
|
||||
double computeRangeAsymmetric(
|
||||
uint64_t timePollSent,
|
||||
uint64_t timePollReceived,
|
||||
uint64_t timePollAckSent,
|
||||
uint64_t timePollAckReceived,
|
||||
uint64_t timeRangeSent,
|
||||
uint64_t timeRangeReceived
|
||||
);
|
||||
//TODO Symmetric
|
||||
|
||||
/**
|
||||
Removes bias from the target range
|
||||
|
||||
returns the unbiased range
|
||||
*/
|
||||
double correctRange(double range);
|
||||
}
|
||||
@@ -0,0 +1,361 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
// no sub-address for register write
|
||||
constexpr uint16_t NO_SUB = 0xFF;
|
||||
|
||||
// device id register
|
||||
constexpr uint16_t DEV_ID = 0x00;
|
||||
constexpr uint16_t LEN_DEV_ID = 4;
|
||||
|
||||
// extended unique identifier register
|
||||
constexpr uint16_t EUI = 0x01;
|
||||
constexpr uint16_t LEN_EUI = 8;
|
||||
|
||||
// PAN identifier, short address register
|
||||
constexpr uint16_t PANADR = 0x03;
|
||||
constexpr uint16_t LEN_PANADR = 4;
|
||||
|
||||
// device configuration register
|
||||
constexpr uint16_t SYS_CFG = 0x04;
|
||||
constexpr uint16_t FFEN_BIT = 0;
|
||||
constexpr uint16_t FFBC_BIT = 1;
|
||||
constexpr uint16_t FFAB_BIT = 2;
|
||||
constexpr uint16_t FFAD_BIT = 3;
|
||||
constexpr uint16_t FFAA_BIT = 4;
|
||||
constexpr uint16_t FFAM_BIT = 5;
|
||||
constexpr uint16_t FFAR_BIT = 6;
|
||||
constexpr uint16_t FFA4_BIT = 7;
|
||||
constexpr uint16_t FFA5_BIT = 8;
|
||||
constexpr uint16_t HIRQ_POL_BIT = 9;
|
||||
constexpr uint16_t SPI_EDGE_BIT = 10;
|
||||
constexpr uint16_t DIS_FCE_BIT = 11;
|
||||
constexpr uint16_t DIS_DRXB_BIT = 12;
|
||||
constexpr uint16_t DIS_PHE_BIT = 13;
|
||||
constexpr uint16_t DIS_RSDE_BIT = 14;
|
||||
constexpr uint16_t FCS_INIT2F_BIT = 15;
|
||||
constexpr uint16_t PHR_MODE_0_BIT = 16;
|
||||
constexpr uint16_t PHR_MODE_1_BIT = 17;
|
||||
constexpr uint16_t DIS_STXP_BIT = 18;
|
||||
constexpr uint16_t RXM110K_BIT = 22;
|
||||
constexpr uint16_t RXWTOE_BIT = 28;
|
||||
constexpr uint16_t RXAUTR_BIT = 29;
|
||||
constexpr uint16_t AUTOACK_BIT = 30;
|
||||
constexpr uint16_t AACKPEND_BIT = 31;
|
||||
constexpr uint16_t LEN_SYS_CFG = 4;
|
||||
|
||||
// device control register
|
||||
constexpr uint16_t SYS_CTRL = 0x0D;
|
||||
constexpr uint16_t LEN_SYS_CTRL = 4;
|
||||
constexpr uint16_t SFCST_BIT = 0;
|
||||
constexpr uint16_t TXSTRT_BIT = 1;
|
||||
constexpr uint16_t TXDLYS_BIT = 2;
|
||||
constexpr uint16_t TRXOFF_BIT = 6;
|
||||
constexpr uint16_t WAIT4RESP_BIT = 7;
|
||||
constexpr uint16_t RXENAB_BIT = 8;
|
||||
constexpr uint16_t RXDLYS_BIT = 9;
|
||||
|
||||
// system event status register
|
||||
constexpr uint16_t SYS_STATUS = 0x0F;
|
||||
constexpr uint16_t SYS_STATUS_SUB = 0x04;
|
||||
constexpr uint16_t IRQS_BIT = 0;
|
||||
constexpr uint16_t CPLOCK_BIT = 1;
|
||||
constexpr uint16_t ESYNCR_BIT = 2;
|
||||
constexpr uint16_t AAT_BIT = 3;
|
||||
constexpr uint16_t TXFRB_BIT = 4;
|
||||
constexpr uint16_t TXPRS_BIT = 5;
|
||||
constexpr uint16_t TXPHS_BIT = 6;
|
||||
constexpr uint16_t TXFRS_BIT = 7;
|
||||
constexpr uint16_t RXPRD_BIT = 8;
|
||||
constexpr uint16_t RXSFDD_BIT = 9;
|
||||
constexpr uint16_t LDEDONE_BIT = 10;
|
||||
constexpr uint16_t RXPHD_BIT = 11;
|
||||
constexpr uint16_t RXPHE_BIT = 12;
|
||||
constexpr uint16_t RXDFR_BIT = 13;
|
||||
constexpr uint16_t RXFCG_BIT = 14;
|
||||
constexpr uint16_t RXFCE_BIT = 15;
|
||||
constexpr uint16_t RXRFSL_BIT = 16;
|
||||
constexpr uint16_t RXRFTO_BIT = 17;
|
||||
constexpr uint16_t LDEERR_BIT = 18;
|
||||
constexpr uint16_t RXOVRR_BIT = 20;
|
||||
constexpr uint16_t RXPTO_BIT = 21;
|
||||
constexpr uint16_t GPIOIRQ_BIT = 22;
|
||||
constexpr uint16_t SLP2INIT_BIT = 23;
|
||||
constexpr uint16_t RFPLL_LL_BIT = 24;
|
||||
constexpr uint16_t CLKPLL_LL_BIT = 25;
|
||||
constexpr uint16_t RXSFDTO_BIT = 26;
|
||||
constexpr uint16_t HPDWARN_BIT = 27;
|
||||
constexpr uint16_t TXBERR_BIT = 28;
|
||||
constexpr uint16_t AFFREJ_BIT = 29;
|
||||
constexpr uint16_t HSRBP_BIT = 30;
|
||||
constexpr uint16_t ICRBP_BIT = 31;
|
||||
constexpr uint16_t RXRSCS_BIT = 0;
|
||||
constexpr uint16_t RXPREJ_BIT = 1;
|
||||
constexpr uint16_t TXPUTE_BIT = 2;
|
||||
constexpr uint16_t LEN_SYS_STATUS = 4;
|
||||
constexpr uint16_t LEN_SYS_STATUS_SUB = 1;
|
||||
|
||||
// system event mask register
|
||||
// NOTE: uses the bit definitions of SYS_STATUS (below 32)
|
||||
constexpr uint16_t SYS_MASK = 0x0E;
|
||||
constexpr uint16_t LEN_SYS_MASK = 4;
|
||||
|
||||
// system time counter
|
||||
constexpr uint16_t SYS_TIME = 0x06;
|
||||
constexpr uint16_t LEN_SYS_TIME = 5;
|
||||
|
||||
// RX timestamp register
|
||||
constexpr uint16_t RX_TIME = 0x15;
|
||||
constexpr uint16_t LEN_RX_TIME = 14;
|
||||
constexpr uint16_t RX_STAMP_SUB = 0x00;
|
||||
constexpr uint16_t FP_AMPL1_SUB = 0x07;
|
||||
constexpr uint16_t LEN_RX_STAMP = 5;
|
||||
constexpr uint16_t LEN_FP_AMPL1 = 2;
|
||||
|
||||
// RX frame quality
|
||||
constexpr uint16_t RX_FQUAL = 0x12;
|
||||
constexpr uint16_t LEN_RX_FQUAL = 8;
|
||||
constexpr uint16_t STD_NOISE_SUB = 0x00;
|
||||
constexpr uint16_t FP_AMPL2_SUB = 0x02;
|
||||
constexpr uint16_t FP_AMPL3_SUB = 0x04;
|
||||
constexpr uint16_t CIR_PWR_SUB = 0x06;
|
||||
constexpr uint16_t LEN_STD_NOISE = 2;
|
||||
constexpr uint16_t LEN_FP_AMPL2 = 2;
|
||||
constexpr uint16_t LEN_FP_AMPL3 = 2;
|
||||
constexpr uint16_t LEN_CIR_PWR = 2;
|
||||
|
||||
// TX timestamp register
|
||||
constexpr uint16_t TX_TIME = 0x17;
|
||||
constexpr uint16_t LEN_TX_TIME = 10;
|
||||
constexpr uint16_t TX_STAMP_SUB = 0;
|
||||
constexpr uint16_t LEN_TX_STAMP = 5;
|
||||
|
||||
// timing register (for delayed RX/TX)
|
||||
constexpr uint16_t DX_TIME = 0x0A;
|
||||
constexpr uint16_t LEN_DX_TIME = 5;
|
||||
|
||||
// Receive Frame Wait Timeout Period
|
||||
constexpr uint16_t RX_WFTO = 0x0C;
|
||||
constexpr uint16_t LEN_RX_WFTO = 2;
|
||||
|
||||
// transmit data buffer
|
||||
constexpr uint16_t TX_BUFFER = 0x09;
|
||||
constexpr uint16_t LEN_TX_BUFFER = 1024;
|
||||
constexpr uint16_t LEN_UWB_FRAMES = 127;
|
||||
constexpr uint16_t LEN_EXT_UWB_FRAMES = 1023;
|
||||
|
||||
// RX frame info
|
||||
constexpr uint16_t RX_FINFO = 0x10;
|
||||
constexpr uint16_t LEN_RX_FINFO = 4;
|
||||
|
||||
// receive data buffer
|
||||
constexpr uint16_t RX_BUFFER = 0x11;
|
||||
constexpr uint16_t LEN_RX_BUFFER = 1024;
|
||||
|
||||
// transmit control
|
||||
constexpr uint16_t TX_FCTRL = 0x08;
|
||||
constexpr uint16_t LEN_TX_FCTRL = 5;
|
||||
|
||||
// channel control
|
||||
constexpr uint16_t CHAN_CTRL = 0x1F;
|
||||
constexpr uint16_t LEN_CHAN_CTRL = 4;
|
||||
constexpr uint16_t DWSFD_BIT = 17;
|
||||
constexpr uint16_t TNSSFD_BIT = 20;
|
||||
constexpr uint16_t RNSSFD_BIT = 21;
|
||||
|
||||
// user-defined SFD
|
||||
constexpr uint16_t USR_SFD = 0x21;
|
||||
constexpr uint16_t LEN_USR_SFD = 41;
|
||||
constexpr uint16_t SFD_LENGTH_SUB = 0x00;
|
||||
constexpr uint16_t LEN_SFD_LENGTH = 1;
|
||||
|
||||
// OTP control (for LDE micro code loading only)
|
||||
constexpr uint16_t OTP_IF = 0x2D;
|
||||
constexpr uint16_t OTP_ADDR_SUB = 0x04;
|
||||
constexpr uint16_t OTP_CTRL_SUB = 0x06;
|
||||
constexpr uint16_t OTP_RDAT_SUB = 0x0A;
|
||||
constexpr uint16_t LEN_OTP_ADDR = 2;
|
||||
constexpr uint16_t LEN_OTP_CTRL = 2;
|
||||
constexpr uint16_t LEN_OTP_RDAT = 4;
|
||||
|
||||
// AGC_TUNE1/2/3 (for re-tuning only)
|
||||
constexpr uint16_t AGC_TUNE = 0x23;
|
||||
constexpr uint16_t AGC_TUNE1_SUB = 0x04;
|
||||
constexpr uint16_t AGC_TUNE2_SUB = 0x0C;
|
||||
constexpr uint16_t AGC_TUNE3_SUB = 0x12;
|
||||
constexpr uint16_t LEN_AGC_TUNE1 = 2;
|
||||
constexpr uint16_t LEN_AGC_TUNE2 = 4;
|
||||
constexpr uint16_t LEN_AGC_TUNE3 = 2;
|
||||
|
||||
// EXT_SYNC (External Synchronization Control)
|
||||
constexpr uint16_t EXT_SYNC = 0x24;
|
||||
constexpr uint16_t EC_CTRL_SUB = 0x00;
|
||||
constexpr uint16_t PLLLDT_BIT = 2;
|
||||
constexpr uint16_t EC_RXTC_SUB = 0x04;
|
||||
constexpr uint16_t EC_GOLP_SUB = 0x08;
|
||||
constexpr uint16_t LEN_EC_CTRL = 4;
|
||||
constexpr uint16_t LEN_EC_RXTC = 4;
|
||||
constexpr uint16_t LEN_EC_GOLP = 4;
|
||||
|
||||
// DRX_TUNE2 (for re-tuning only)
|
||||
constexpr uint16_t DRX_TUNE = 0x27;
|
||||
constexpr uint16_t DRX_TUNE0b_SUB = 0x02;
|
||||
constexpr uint16_t DRX_TUNE1a_SUB = 0x04;
|
||||
constexpr uint16_t DRX_TUNE1b_SUB = 0x06;
|
||||
constexpr uint16_t DRX_TUNE2_SUB = 0x08;
|
||||
constexpr uint16_t DRX_SFDTOC_SUB = 0x20;
|
||||
constexpr uint16_t DRX_PRETOC_SUB = 0x24;
|
||||
constexpr uint16_t DRX_TUNE4H_SUB = 0x26;
|
||||
constexpr uint16_t DRX_CAR_INT_SUB = 0x28;
|
||||
constexpr uint16_t RXPACC_NOSAT_SUB = 0x2C;
|
||||
constexpr uint16_t LEN_DRX_TUNE0b = 2;
|
||||
constexpr uint16_t LEN_DRX_TUNE1a = 2;
|
||||
constexpr uint16_t LEN_DRX_TUNE1b = 2;
|
||||
constexpr uint16_t LEN_DRX_TUNE2 = 4;
|
||||
constexpr uint16_t LEN_DRX_SFDTOC = 2;
|
||||
constexpr uint16_t LEN_DRX_PRETOC = 2;
|
||||
constexpr uint16_t LEN_DRX_TUNE4H = 2;
|
||||
constexpr uint16_t LEN_DRX_CAR_INT = 3;
|
||||
constexpr uint16_t LEN_RXPACC_NOSAT = 2;
|
||||
|
||||
// LDE_CFG1 (for re-tuning only)
|
||||
constexpr uint16_t LDE_IF = 0x2E;
|
||||
constexpr uint16_t LDE_CFG1_SUB = 0x0806;
|
||||
constexpr uint16_t LDE_RXANTD_SUB = 0x1804;
|
||||
constexpr uint16_t LDE_CFG2_SUB = 0x1806;
|
||||
constexpr uint16_t LDE_REPC_SUB = 0x2804;
|
||||
constexpr uint16_t LEN_LDE_CFG1 = 1;
|
||||
constexpr uint16_t LEN_LDE_CFG2 = 2;
|
||||
constexpr uint16_t LEN_LDE_REPC = 2;
|
||||
constexpr uint16_t LEN_LDE_RXANTD = 2;
|
||||
|
||||
// DIG_DIAG (Digital Diagnostics Interface)
|
||||
constexpr uint16_t DIG_DIAG = 0x2F;
|
||||
constexpr uint16_t EVC_CTRL_SUB = 0x00;
|
||||
constexpr uint16_t EVC_STO_SUB = 0x10;
|
||||
constexpr uint16_t EVC_PTO_SUB = 0x12;
|
||||
constexpr uint16_t EVC_FWTO_SUB = 0x14;
|
||||
constexpr uint16_t DIAG_TMC_SUB = 0x24;
|
||||
constexpr uint16_t LEN_EVC_CTRL = 4;
|
||||
constexpr uint16_t LEN_EVC_STO = 2;
|
||||
constexpr uint16_t LEN_EVC_PTO = 2;
|
||||
constexpr uint16_t LEN_EVC_FWTO = 2;
|
||||
constexpr uint16_t LEN_DIAG_TMC = 2;
|
||||
|
||||
// TX_POWER (for re-tuning only)
|
||||
constexpr uint16_t TX_POWER = 0x1E;
|
||||
constexpr uint16_t LEN_TX_POWER = 4;
|
||||
|
||||
// RF_CONF (for re-tuning only)
|
||||
constexpr uint16_t RF_CONF = 0x28;
|
||||
constexpr uint16_t RF_CONF_SUB = 0x00;
|
||||
constexpr uint16_t RF_RXCTRLH_SUB = 0x0B;
|
||||
constexpr uint16_t RF_TXCTRL_SUB = 0x0C;
|
||||
constexpr uint16_t LEN_RX_CONF_SUB = 4;
|
||||
constexpr uint16_t LEN_RF_RXCTRLH = 1;
|
||||
constexpr uint16_t LEN_RF_TXCTRL = 4;
|
||||
|
||||
// TX_CAL (for re-tuning only)
|
||||
constexpr uint16_t TX_CAL = 0x2A;
|
||||
constexpr uint16_t TC_PGDELAY_SUB = 0x0B;
|
||||
constexpr uint16_t LEN_TC_PGDELAY = 1;
|
||||
constexpr uint16_t TC_SARC = 0x00;
|
||||
constexpr uint16_t TC_SARL = 0x03;
|
||||
|
||||
// FS_CTRL (for re-tuning only)
|
||||
constexpr uint16_t FS_CTRL = 0x2B;
|
||||
constexpr uint16_t FS_PLLCFG_SUB = 0x07;
|
||||
constexpr uint16_t FS_PLLTUNE_SUB = 0x0B;
|
||||
constexpr uint16_t FS_XTALT_SUB = 0x0E;
|
||||
constexpr uint16_t LEN_FS_PLLCFG = 4;
|
||||
constexpr uint16_t LEN_FS_PLLTUNE = 1;
|
||||
constexpr uint16_t LEN_FS_XTALT = 1;
|
||||
|
||||
// AON
|
||||
constexpr uint16_t AON = 0x2C;
|
||||
constexpr uint16_t AON_WCFG_SUB = 0x00;
|
||||
constexpr uint16_t ONW_RADC_BIT = 0;
|
||||
constexpr uint16_t ONW_RX_BIT = 1;
|
||||
constexpr uint16_t ONW_LEUI_BIT = 3;
|
||||
constexpr uint16_t ONW_LDC_BIT = 6;
|
||||
constexpr uint16_t ONW_L64P_BIT = 7;
|
||||
constexpr uint16_t ONW_PRES_SLEEP_BIT = 8;
|
||||
constexpr uint16_t ONW_LLDE_BIT = 11;
|
||||
constexpr uint16_t ONW_LLDO_BIT = 12;
|
||||
constexpr uint16_t LEN_AON_WCFG = 2;
|
||||
|
||||
constexpr uint16_t AON_CTRL_SUB = 0x02;
|
||||
constexpr uint16_t RESTORE_BIT = 0;
|
||||
constexpr uint16_t SAVE_BIT = 1;
|
||||
constexpr uint16_t UPL_CFG_BIT = 2;
|
||||
constexpr uint16_t LEN_AON_CTRL = 1;
|
||||
|
||||
constexpr uint16_t AON_CFG0_SUB = 0x06;
|
||||
constexpr uint16_t SLEEP_EN_BIT = 0;
|
||||
constexpr uint16_t WAKE_PIN_BIT = 1;
|
||||
constexpr uint16_t WAKE_SPI_BIT = 2;
|
||||
constexpr uint16_t WAKE_CNT_BIT = 3;
|
||||
constexpr uint16_t LPDIV_EN_BIT = 4;
|
||||
constexpr uint16_t LEN_AON_CFG0 = 4;
|
||||
|
||||
constexpr uint16_t AON_CFG1_SUB = 0x0A;
|
||||
constexpr uint16_t SLEEP_CEN_BIT = 0;
|
||||
constexpr uint16_t SMXX_BIT = 1;
|
||||
constexpr uint16_t LPOSC_CAL_BIT = 2;
|
||||
constexpr uint16_t LEN_AON_CFG1 = 2;
|
||||
|
||||
// PMSC
|
||||
constexpr uint16_t PMSC = 0x36;
|
||||
constexpr uint16_t PMSC_CTRL0_SUB = 0x00;
|
||||
constexpr uint16_t GPDCE_BIT = 18;
|
||||
constexpr uint16_t KHZCLKEN_BIT = 23;
|
||||
constexpr uint16_t PMSC_SOFTRESET_SUB = 0x03;
|
||||
constexpr uint16_t PMSC_CTRL1_SUB = 0x04;
|
||||
constexpr uint16_t ATXSLP_BIT = 11;
|
||||
constexpr uint16_t ARXSLP_BIT = 12;
|
||||
constexpr uint16_t PMSC_LEDC_SUB = 0x28;
|
||||
constexpr uint16_t BLNKEN = 8;
|
||||
constexpr uint16_t LEN_PMSC_CTRL0 = 4;
|
||||
constexpr uint16_t LEN_PMSC_SOFTRESET = 1;
|
||||
constexpr uint16_t LEN_PMSC_CTRL1 = 4;
|
||||
constexpr uint16_t LEN_PMSC_LEDC = 4;
|
||||
|
||||
// TX_ANTD Antenna delays
|
||||
constexpr uint16_t TX_ANTD = 0x18;
|
||||
constexpr uint16_t LEN_TX_ANTD = 2;
|
||||
|
||||
// Acknowledgement time and response time
|
||||
constexpr uint16_t ACK_RESP_T = 0x1A;
|
||||
constexpr uint16_t ACK_RESP_T_W4R_TIME_SUB = 0x00;
|
||||
constexpr uint16_t LEN_ACK_RESP_T_W4R_TIME_SUB = 3;
|
||||
constexpr uint16_t LEN_ACK_RESP_T = 4;
|
||||
|
||||
// GPIO
|
||||
constexpr uint16_t GPIO_CTRL = 0x26;
|
||||
constexpr uint16_t GPIO_MODE_SUB = 0x00;
|
||||
constexpr uint16_t LEN_GPIO_MODE = 4;
|
||||
@@ -0,0 +1,33 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "DW1000NgTime.hpp"
|
||||
#include "DW1000NgConstants.hpp"
|
||||
|
||||
namespace DW1000NgTime {
|
||||
uint64_t microsecondsToUWBTime(uint64_t microSeconds) {
|
||||
return static_cast<uint64_t>(microSeconds * TIME_RES_INV);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
namespace DW1000NgTime {
|
||||
uint64_t microsecondsToUWBTime(uint64_t microSeconds);
|
||||
}
|
||||
@@ -0,0 +1,141 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2015 by Thomas Trojer <thomas@trojer.net>
|
||||
* Decawave DW1000 library for arduino.
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*
|
||||
* @file DW1000.cpp
|
||||
* Helper functions.
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "DW1000NgUtils.hpp"
|
||||
#include "DW1000NgConstants.hpp"
|
||||
#include "DW1000NgRegisters.hpp"
|
||||
|
||||
namespace DW1000NgUtils {
|
||||
|
||||
/*
|
||||
* Set the value of a bit in an array of bytes that are considered
|
||||
* consecutive and stored from MSB to LSB.
|
||||
* @param data
|
||||
* The number as byte array.
|
||||
* @param n
|
||||
* The number of bytes in the array.
|
||||
* @param bit
|
||||
* The position of the bit to be set.
|
||||
* @param val
|
||||
* The boolean value to be set to the given bit position.
|
||||
*/
|
||||
void setBit(byte data[], uint16_t n, uint16_t bit, boolean val) {
|
||||
uint16_t idx;
|
||||
uint8_t shift;
|
||||
|
||||
idx = bit/8;
|
||||
if(idx >= n) {
|
||||
return; // TODO proper error handling: out of bounds
|
||||
}
|
||||
byte* targetByte = &data[idx];
|
||||
shift = bit%8;
|
||||
if(val) {
|
||||
bitSet(*targetByte, shift);
|
||||
} else {
|
||||
bitClear(*targetByte, shift);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Check the value of a bit in an array of bytes that are considered
|
||||
* consecutive and stored from MSB to LSB.
|
||||
* @param data
|
||||
* The number as byte array.
|
||||
* @param n
|
||||
* The number of bytes in the array.
|
||||
* @param bit
|
||||
* The position of the bit to be checked.
|
||||
*/
|
||||
boolean getBit(byte data[], uint16_t n, uint16_t bit) {
|
||||
uint16_t idx;
|
||||
uint8_t shift;
|
||||
|
||||
idx = bit/8;
|
||||
if(idx >= n) {
|
||||
return false; // TODO proper error handling: out of bounds
|
||||
}
|
||||
byte targetByte = data[idx];
|
||||
shift = bit%8;
|
||||
|
||||
return bitRead(targetByte, shift); // TODO wrong type returned byte instead of boolean
|
||||
}
|
||||
|
||||
void writeValueToBytes(byte data[], uint64_t val, uint8_t n) {
|
||||
for(auto i = 0; i < n; i++) {
|
||||
data[i] = ((val >> (i*8)) & 0xFF);
|
||||
}
|
||||
}
|
||||
|
||||
uint64_t bytesAsValue(byte data[], uint8_t n) {
|
||||
uint64_t value = 0;
|
||||
for(auto i = 0; i < n; i++) {
|
||||
value |= ((uint64_t)data[i] << (i*8));
|
||||
}
|
||||
return value;
|
||||
}
|
||||
|
||||
uint8_t nibbleFromChar(char c) {
|
||||
if(c >= '0' && c <= '9') {
|
||||
return c-'0';
|
||||
}
|
||||
if(c >= 'a' && c <= 'f') {
|
||||
return c-'a'+10;
|
||||
}
|
||||
if(c >= 'A' && c <= 'F') {
|
||||
return c-'A'+10;
|
||||
}
|
||||
return 255;
|
||||
}
|
||||
|
||||
void convertToByte(char string[], byte* bytes) {
|
||||
byte eui_byte[LEN_EUI];
|
||||
// we fill it with the char array under the form of "AA:FF:1C:...."
|
||||
for(uint16_t i = 0; i < LEN_EUI; i++) {
|
||||
eui_byte[i] = (nibbleFromChar(string[i*3]) << 4)+nibbleFromChar(string[i*3+1]);
|
||||
}
|
||||
memcpy(bytes, eui_byte, LEN_EUI);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,110 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
/*
|
||||
* Copyright (c) 2015 by Thomas Trojer <thomas@trojer.net>
|
||||
* Decawave DW1000 library for arduino.
|
||||
*
|
||||
* Licensed under the Apache License, Version 2.0 (the "License");
|
||||
* you may not use this file except in compliance with the License.
|
||||
* You may obtain a copy of the License at
|
||||
*
|
||||
* http://www.apache.org/licenses/LICENSE-2.0
|
||||
*
|
||||
* Unless required by applicable law or agreed to in writing, software
|
||||
* distributed under the License is distributed on an "AS IS" BASIS,
|
||||
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
* See the License for the specific language governing permissions and
|
||||
* limitations under the License.
|
||||
*
|
||||
* @file DW1000.h
|
||||
* Helper functions.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "DW1000NgConstants.hpp"
|
||||
|
||||
namespace DW1000NgUtils {
|
||||
|
||||
/**
|
||||
Returns target bit value inside a byte array
|
||||
|
||||
@param [in] data the byte array
|
||||
@param [in] n the length of the byte array
|
||||
@param [in] bit the bit position
|
||||
|
||||
returns bit value (true = 1, false = 0)
|
||||
*/
|
||||
boolean getBit(byte data[], uint16_t n, uint16_t bit);
|
||||
|
||||
/**
|
||||
Sets the target bit value inside an array of bytes
|
||||
|
||||
@param [in] data the byte array
|
||||
@param [in] n the length of the byte array
|
||||
@param [in] bit the bit position
|
||||
@param [in] val the bit value
|
||||
|
||||
*/
|
||||
void setBit(byte data[], uint16_t n, uint16_t bit, boolean val);
|
||||
|
||||
/**
|
||||
Writes the target value inside a given byte array.
|
||||
|
||||
@param [in] data the byte array
|
||||
@param [in] val the value to insert
|
||||
@param [in] n the length of the byte array
|
||||
*/
|
||||
void writeValueToBytes(byte data[], uint64_t val, uint8_t n);
|
||||
|
||||
/**
|
||||
Gets the target byte array value
|
||||
|
||||
@param [in] data the byte array
|
||||
@param [in] n the length of the byte array
|
||||
|
||||
returns the byte array value
|
||||
*/
|
||||
uint64_t bytesAsValue(byte data[], uint8_t n);
|
||||
|
||||
/**
|
||||
Converts from char to 4 bits (hexadecimal)
|
||||
|
||||
@param [in] c the character
|
||||
|
||||
returns target value
|
||||
*/
|
||||
|
||||
uint8_t nibbleFromChar(char c);
|
||||
|
||||
/**
|
||||
Converts the target string to eui bytes
|
||||
|
||||
@param [in] string The eui string (in format XX:XX:XX:XX:XX:XX:XX:XX)
|
||||
@param [out] eui_byte The eui bytes
|
||||
*/
|
||||
void convertToByte(char string[], byte* eui_byte);
|
||||
}
|
||||
@@ -0,0 +1,148 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*
|
||||
* @file SPIporting.hpp
|
||||
* Arduino porting for the SPI interface.
|
||||
*/
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <SPI.h>
|
||||
#include "SPIporting.hpp"
|
||||
#include "DW1000NgConstants.hpp"
|
||||
#include "DW1000NgRegisters.hpp"
|
||||
|
||||
uint32_t m_spiClock = 0;
|
||||
spi_host_device_t m_spiHost;
|
||||
spi_device_handle_t m_spiHandle = 0;
|
||||
Adafruit_MCP23017 *m_expander = NULL;
|
||||
|
||||
namespace SPIporting {
|
||||
|
||||
void SPIInitWithClock(uint32_t clock)
|
||||
{
|
||||
if(m_spiHandle)
|
||||
{
|
||||
spi_bus_remove_device(m_spiHandle);
|
||||
m_spiHandle = 0;
|
||||
}
|
||||
spi_device_interface_config_t devcfg={
|
||||
.command_bits = 0,
|
||||
.address_bits = 0,
|
||||
.dummy_bits = 0,
|
||||
.mode=0,
|
||||
.clock_speed_hz=clock,
|
||||
.spics_io_num=-1,
|
||||
.flags = 0,
|
||||
.queue_size=7,
|
||||
.pre_cb=NULL,
|
||||
.post_cb=NULL,
|
||||
};
|
||||
m_spiClock = clock;
|
||||
spi_bus_add_device(m_spiHost, &devcfg, &m_spiHandle);
|
||||
}
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr uint32_t EspSPImaximumSpeed = 20000000; //20MHz
|
||||
constexpr uint32_t SPIminimumSpeed = 2000000; //2MHz
|
||||
|
||||
const SPISettings _fastSPI = SPISettings(EspSPImaximumSpeed, MSBFIRST, SPI_MODE0);
|
||||
const SPISettings _slowSPI = SPISettings(SPIminimumSpeed, MSBFIRST, SPI_MODE0);
|
||||
const SPISettings* _currentSPI = &_fastSPI;
|
||||
|
||||
void _openSPI(uint8_t slaveSelectPIN) {
|
||||
if(_currentSPI->_clock!=m_spiClock)
|
||||
{
|
||||
SPIInitWithClock(_currentSPI->_clock);
|
||||
}
|
||||
m_expander->digitalWrite(slaveSelectPIN, LOW);
|
||||
}
|
||||
|
||||
void _closeSPI(uint8_t slaveSelectPIN) {
|
||||
m_expander->digitalWrite(slaveSelectPIN, HIGH);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void SPIinit(spi_host_device_t spihost, Adafruit_MCP23017 *expander) {
|
||||
m_spiHost = spihost;
|
||||
m_expander = expander;
|
||||
SPIInitWithClock(EspSPImaximumSpeed);
|
||||
}
|
||||
|
||||
void SPIend() {
|
||||
spi_bus_remove_device(m_spiHandle);
|
||||
m_spiHandle = 0;
|
||||
}
|
||||
|
||||
void SPIselect(uint8_t slaveSelectPIN, uint8_t irq) {
|
||||
m_expander->pinMode(slaveSelectPIN, OUTPUT);
|
||||
m_expander->digitalWrite(slaveSelectPIN, HIGH);
|
||||
}
|
||||
|
||||
uint8_t spi_transfer(uint8_t data)
|
||||
{
|
||||
esp_err_t ret;
|
||||
spi_transaction_t t;
|
||||
memset(&t, 0, sizeof(t));
|
||||
t.flags = SPI_TRANS_USE_RXDATA;
|
||||
t.length=8;
|
||||
t.tx_buffer=&data;
|
||||
ret=spi_device_transmit(m_spiHandle, &t);
|
||||
assert(ret==ESP_OK);
|
||||
return t.rx_data[0];
|
||||
}
|
||||
|
||||
|
||||
void writeToSPI(uint8_t slaveSelectPIN, uint8_t headerLen, byte header[], uint16_t dataLen, byte data[]) {
|
||||
_openSPI(slaveSelectPIN);
|
||||
for(auto i = 0; i < headerLen; i++) {
|
||||
spi_transfer(header[i]); // send header
|
||||
}
|
||||
for(auto i = 0; i < dataLen; i++) {
|
||||
spi_transfer(data[i]); // write values
|
||||
}
|
||||
delayMicroseconds(5);
|
||||
_closeSPI(slaveSelectPIN);
|
||||
}
|
||||
|
||||
void readFromSPI(uint8_t slaveSelectPIN, uint8_t headerLen, byte header[], uint16_t dataLen, byte data[]){
|
||||
_openSPI(slaveSelectPIN);
|
||||
for(auto i = 0; i < headerLen; i++) {
|
||||
spi_transfer(header[i]); // send header
|
||||
}
|
||||
for(auto i = 0; i < dataLen; i++) {
|
||||
data[i] = spi_transfer(0x00); // read values
|
||||
}
|
||||
delayMicroseconds(5);
|
||||
_closeSPI(slaveSelectPIN);
|
||||
}
|
||||
|
||||
void setSPIspeed(SPIClock speed) {
|
||||
if(speed == SPIClock::FAST) {
|
||||
_currentSPI = &_fastSPI;
|
||||
} else if(speed == SPIClock::SLOW) {
|
||||
_currentSPI = &_slowSPI;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,82 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*
|
||||
* @file SPIporting.hpp
|
||||
* Arduino porting for the SPI interface.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
#include "DW1000NgConstants.hpp"
|
||||
#include "Adafruit_MCP23017.h"
|
||||
#include "driver/spi_master.h"
|
||||
|
||||
namespace SPIporting{
|
||||
|
||||
/**
|
||||
Initializes the SPI bus.
|
||||
*/
|
||||
void SPIinit(spi_host_device_t spihost, Adafruit_MCP23017 *expander);
|
||||
|
||||
/**
|
||||
Tells the driver library that no communication to a DW1000 will be required anymore.
|
||||
This basically just frees SPI and the previously used pins.
|
||||
*/
|
||||
void SPIend();
|
||||
|
||||
/**
|
||||
(Re-)selects a specific DW1000 chip for communication. Used in case you switched SPI to another device.
|
||||
*/
|
||||
void SPIselect(uint8_t slaveSelectPIN, uint8_t irq = 0xff);
|
||||
|
||||
/**
|
||||
Arduino function to write to the SPI.
|
||||
Takes two separate byte buffers for write header and write data
|
||||
|
||||
@param [in] Header lenght
|
||||
@param [in] Header array built before
|
||||
@param [in] Data lenght
|
||||
@param [in] Data array
|
||||
*/
|
||||
void writeToSPI(uint8_t slaveSelectPIN, uint8_t headerLen, byte header[], uint16_t dataLen, byte data[]);
|
||||
|
||||
/**
|
||||
Arduino function to read from the SPI.
|
||||
Takes two separate byte buffers for write header and write data
|
||||
|
||||
@param [in] Header lenght
|
||||
@param [in] Header array built before
|
||||
@param [in] Data lenght
|
||||
@param [out] Data array
|
||||
*/
|
||||
void readFromSPI(uint8_t slaveSelectPIN, uint8_t headerLen, byte header[], uint16_t dataLen, byte data[]);
|
||||
|
||||
/**
|
||||
Sets speed of SPI clock, fast or slow(20MHz or 2MHz)
|
||||
|
||||
@param [in] SPIClock FAST or SLOW
|
||||
*/
|
||||
void setSPIspeed(SPIClock speed);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,35 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef __has_cpp_attribute
|
||||
#if __has_cpp_attribute(deprecated)
|
||||
#define DEPRECATED [[deprecated]]
|
||||
#define DEPRECATED_MSG(msg) [[deprecated(msg)]]
|
||||
#endif // __has_cpp_attribute(deprecated)
|
||||
#else
|
||||
#define DEPRECATED __attribute__((deprecated))
|
||||
#define DEPRECATED_MSG(msg) __attribute__((deprecated(msg)))
|
||||
#endif // __has_cpp_attribute
|
||||
@@ -0,0 +1,29 @@
|
||||
/*
|
||||
* MIT License
|
||||
*
|
||||
* Copyright (c) 2018 Michele Biondi, Andrea Salvatori
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in all
|
||||
* copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
* SOFTWARE.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#if __cplusplus < 201103L
|
||||
#error "You need Arduino IDE version >=1.6.6 to use this project as it needs C++11 support."
|
||||
#endif
|
||||
@@ -0,0 +1,30 @@
|
||||
#pragma once
|
||||
|
||||
// The FreeRTOS includes are in a different directory on ESP32 and I can't figure out how to make that work with platformio gcc
|
||||
// options so this is my quick hack to make things work
|
||||
|
||||
#ifdef ARDUINO_ARCH_ESP32
|
||||
#define HAS_FREE_RTOS
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include <freertos/queue.h>
|
||||
#include <freertos/semphr.h>
|
||||
#include <freertos/task.h>
|
||||
#else
|
||||
// not yet supported on cubecell
|
||||
#ifndef CubeCell_BoardPlus
|
||||
#define HAS_FREE_RTOS
|
||||
#include <FreeRTOS.h>
|
||||
#include <queue.h>
|
||||
#include <semphr.h>
|
||||
#include <task.h>
|
||||
#else
|
||||
|
||||
#include <Arduino.h>
|
||||
|
||||
typedef uint32_t TickType_t;
|
||||
typedef uint32_t BaseType_t;
|
||||
|
||||
#define portMAX_DELAY UINT32_MAX
|
||||
|
||||
#endif
|
||||
#endif
|
||||
@@ -0,0 +1,4 @@
|
||||
#pragma once
|
||||
#include "atracker.h"
|
||||
|
||||
extern cAirsoftTracker *global;
|
||||
+100
@@ -0,0 +1,100 @@
|
||||
|
||||
#include "GPS.h"
|
||||
#include "config.h"
|
||||
#include "timing.h"
|
||||
#include <assert.h>
|
||||
#include <time.h>
|
||||
|
||||
#ifdef GPS_RX_PIN
|
||||
HardwareSerial _serial_gps_real(GPS_SERIAL_NUM);
|
||||
HardwareSerial &GPS::_serial_gps = _serial_gps_real;
|
||||
#else
|
||||
// Assume NRF52
|
||||
HardwareSerial &GPS::_serial_gps = Serial1;
|
||||
#endif
|
||||
|
||||
bool timeSetFromGPS; // We try to set our time from GPS each time we wake from sleep
|
||||
|
||||
GPS *gps;
|
||||
|
||||
// stuff that really should be in in the instance instead...
|
||||
static uint32_t
|
||||
timeStartMsec; // Once we have a GPS lock, this is where we hold the initial msec clock that corresponds to that time
|
||||
static uint64_t zeroOffsetSecs; // GPS based time in secs since 1970 - only updated once on initial lock
|
||||
|
||||
void readFromRTC()
|
||||
{
|
||||
struct timeval tv; /* btw settimeofday() is helpfull here too*/
|
||||
|
||||
if (!gettimeofday(&tv, NULL)) {
|
||||
uint32_t now = timing::millis();
|
||||
|
||||
DEBUG_MSG("Read RTC time as %ld (cur millis %u) valid=%d\n", tv.tv_sec, now, timeSetFromGPS);
|
||||
timeStartMsec = now;
|
||||
zeroOffsetSecs = tv.tv_sec;
|
||||
}
|
||||
}
|
||||
|
||||
/// If we haven't yet set our RTC this boot, set it from a GPS derived time
|
||||
void perhapsSetRTC(const struct timeval *tv)
|
||||
{
|
||||
if (!timeSetFromGPS) {
|
||||
timeSetFromGPS = true;
|
||||
DEBUG_MSG("Setting RTC %ld secs\n", tv->tv_sec);
|
||||
#ifndef NO_ESP32
|
||||
settimeofday(tv, NULL);
|
||||
#else
|
||||
DEBUG_MSG("ERROR TIME SETTING NOT IMPLEMENTED!\n");
|
||||
#endif
|
||||
readFromRTC();
|
||||
}
|
||||
}
|
||||
|
||||
time_t mktimegm(struct tm *tm)
|
||||
{
|
||||
time_t t;
|
||||
|
||||
int y = tm->tm_year + 1900, m = tm->tm_mon + 1, d = tm->tm_mday;
|
||||
|
||||
if (m < 3) {
|
||||
m += 12;
|
||||
y--;
|
||||
}
|
||||
|
||||
t = 86400 *
|
||||
(d + (153 * m - 457) / 5 + 365 * y + y / 4 - y / 100 + y / 400 - 719469);
|
||||
|
||||
t += 3600 * tm->tm_hour + 60 * tm->tm_min + tm->tm_sec;
|
||||
|
||||
return t;
|
||||
}
|
||||
void perhapsSetRTC(struct tm &t, uint32_t ms)
|
||||
{
|
||||
/* Convert to unix time
|
||||
The Unix epoch (or Unix time or POSIX time or Unix timestamp) is the number of seconds that have elapsed since January 1, 1970
|
||||
(midnight UTC/GMT), not counting leap seconds (in ISO 8601: 1970-01-01T00:00:00Z).
|
||||
*/
|
||||
time_t res = mktimegm(&t);
|
||||
struct timeval tv;
|
||||
tv.tv_sec = res;
|
||||
if(ms>0)
|
||||
tv.tv_usec = ms*1000;
|
||||
else
|
||||
tv.tv_usec = 0;
|
||||
|
||||
//DEBUG_MSG("Got time from GPS month=%d, year=%d, unixtime=%ld, ms=%d\n", t.tm_mon, t.tm_year, tv.tv_sec,ms);
|
||||
if (t.tm_year < 120 || t.tm_year >= 300)
|
||||
DEBUG_MSG("Ignoring invalid GPS time\n");
|
||||
else
|
||||
perhapsSetRTC(&tv);
|
||||
}
|
||||
|
||||
uint32_t getTime()
|
||||
{
|
||||
return ((timing::millis() - timeStartMsec) / 1000) + zeroOffsetSecs;
|
||||
}
|
||||
|
||||
uint32_t getValidTime()
|
||||
{
|
||||
return timeSetFromGPS ? getTime() : 0;
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
#pragma once
|
||||
|
||||
#include "Observer.h"
|
||||
#include "GPSStatus.h"
|
||||
#include "../concurrency/PeriodicTask.h"
|
||||
#include "sys/time.h"
|
||||
|
||||
#define GPS_DIVIDER 10000000
|
||||
|
||||
/// If we haven't yet set our RTC this boot, set it from a GPS derived time
|
||||
void perhapsSetRTC(const struct timeval *tv);
|
||||
void perhapsSetRTC(struct tm &t,uint32_t ms);
|
||||
|
||||
// Generate a string representation of DOP
|
||||
const char *getDOPString(uint32_t dop);
|
||||
|
||||
/// Return time since 1970 in secs. Until we have a GPS lock we will be returning time based at zero
|
||||
uint32_t getTime();
|
||||
|
||||
/// Return time since 1970 in secs. If we don't have a GPS lock return zero
|
||||
uint32_t getValidTime();
|
||||
|
||||
void readFromRTC();
|
||||
|
||||
/**
|
||||
* A gps class that only reads from the GPS periodically (and FIXME - eventually keeps the gps powered down except when reading)
|
||||
*
|
||||
* When new data is available it will notify observers.
|
||||
*/
|
||||
class GPS : public Observable<void *>
|
||||
{
|
||||
protected:
|
||||
bool hasValidLocation = false; // default to false, until we complete our first read
|
||||
|
||||
static HardwareSerial &_serial_gps;
|
||||
|
||||
public:
|
||||
int32_t latitude = 0, longitude = 0; // as an int mult by 1e-7 to get value as double
|
||||
int32_t altitude = 0;
|
||||
uint32_t hacc = 0;
|
||||
uint32_t dop = 0; // Diminution of position; PDOP where possible (UBlox), HDOP otherwise (TinyGPS) in 10^2 units (needs scaling before use)
|
||||
uint32_t heading = 0; // Heading of motion, in degrees * 10^-5
|
||||
uint32_t numSatellites = 0;
|
||||
|
||||
bool isConnected = false; // Do we have a GPS we are talking to
|
||||
|
||||
virtual ~GPS() {}
|
||||
|
||||
Observable<const GPSStatus *> newStatus;
|
||||
|
||||
/**
|
||||
* Returns true if we succeeded
|
||||
*/
|
||||
virtual bool setup() { return true; }
|
||||
|
||||
/// A loop callback for subclasses that need it. FIXME, instead just block on serial reads
|
||||
virtual void loop() {}
|
||||
|
||||
/// Returns ture if we have acquired GPS lock.
|
||||
bool hasLock() const { return hasValidLocation; }
|
||||
|
||||
/**
|
||||
* Restart our lock attempt - try to get and broadcast a GPS reading ASAP
|
||||
* called after the CPU wakes from light-sleep state */
|
||||
virtual void startLock() {}
|
||||
|
||||
virtual void reset() {};
|
||||
virtual bool canSleep() { return true; }
|
||||
virtual void shutdown() {}
|
||||
HardwareSerial &getSerial() { return _serial_gps; }
|
||||
};
|
||||
@@ -0,0 +1,341 @@
|
||||
#include "UBloxGPS.h"
|
||||
#include <assert.h>
|
||||
#include "global.h"
|
||||
|
||||
#define GPS_DEBUG
|
||||
#define GPS_DISPLAY_HW
|
||||
|
||||
UBloxGPS::UBloxGPS()
|
||||
{
|
||||
}
|
||||
|
||||
bool UBloxGPS::internalSetup()
|
||||
{
|
||||
bool ok;
|
||||
ok = ublox.setUART1Output(COM_TYPE_UBX, 3000); // Use native API
|
||||
if(!ok) return false;
|
||||
ok = ublox.setNavigationFrequency(GPS_FREQ+1, 3000); // Produce 4x/sec to keep the amount of time we stall in getPVT low
|
||||
if(!ok) return false;
|
||||
ok = ublox.setDynamicModel(DYN_MODEL_PEDESTRIAN,3000); // probably PEDESTRIAN but just in case assume bike speeds
|
||||
if(!ok) return false;
|
||||
//ok = ublox.powerSaveMode(false, 2000); // use power save mode, the default timeout (1100ms seems a bit too tight)
|
||||
//if(!ok) return false;
|
||||
|
||||
uint8_t payloadGNSS[] = {0x00,0x00,0x20,0x07,
|
||||
0x00,0x08,0x10,0x00,0x01,0x00,0x01,0x01,
|
||||
0x01,0x01,0x03,0x00,0x01,0x00,0x01,0x01,
|
||||
0x02,0x04,0x08,0x00,0x01,0x00,0x01,0x01,
|
||||
0x03,0x08,0x10,0x00,0x00,0x00,0x01,0x01,
|
||||
0x04,0x00,0x08,0x00,0x00,0x00,0x01,0x01,
|
||||
0x05,0x00,0x03,0x00,0x01,0x00,0x01,0x01,
|
||||
0x06,0x08,0x0E,0x00,0x01,0x00,0x01,0x01};
|
||||
ubxPacket packetCfg = {0, 0, 0, 0, 0, payloadGNSS, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED,SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
|
||||
packetCfg.cls = UBX_CLASS_CFG;
|
||||
packetCfg.id = UBX_CFG_GNSS;
|
||||
packetCfg.len = 0x3C;
|
||||
packetCfg.startingSpot = 0;
|
||||
ublox.sendCommand(&packetCfg,0);
|
||||
delay(50);
|
||||
|
||||
uint8_t payloadNMEA[] = {0x00,0x41,0x00,0x02,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
|
||||
packetCfg.payload = payloadNMEA;
|
||||
packetCfg.cls = UBX_CLASS_CFG;
|
||||
packetCfg.id = UBX_CFG_NMEA;
|
||||
packetCfg.len = 0x14;
|
||||
packetCfg.startingSpot = 0;
|
||||
ublox.sendCommand(&packetCfg,0);
|
||||
delay(50);
|
||||
|
||||
#ifdef HAS_AXP20X
|
||||
ublox.setSerialRate(GPS_BAUDRATE2,1,3000);
|
||||
delay(2);
|
||||
_serial_gps.updateBaudRate(GPS_BAUDRATE2);
|
||||
delay(50);
|
||||
|
||||
restoreDatabase(GNSS_DBD_FILENAME);
|
||||
#else
|
||||
ublox.setSerialRate(GPS_BAUDRATE);
|
||||
_serial_gps.setRxBufferSize(800);
|
||||
#endif
|
||||
//ok = ublox.saveConfiguration(3000);
|
||||
//assert(ok);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool UBloxGPS::setup()
|
||||
{
|
||||
if(serialInitialized)
|
||||
{
|
||||
_serial_gps.end();
|
||||
delay(10);
|
||||
serialInitialized = false;
|
||||
}
|
||||
if(!serialInitialized)
|
||||
{
|
||||
_serial_gps.setRxBufferSize(800);
|
||||
#ifdef GPS_RX_PIN
|
||||
_serial_gps.begin(GPS_BAUDRATE, SERIAL_8N1, GPS_RX_PIN, GPS_TX_PIN);
|
||||
#else
|
||||
_serial_gps.begin(GPS_BAUDRATE);
|
||||
#endif
|
||||
serialInitialized = true;
|
||||
delay(30);
|
||||
}
|
||||
//ublox.enableDebugging(Serial);
|
||||
|
||||
// note: the lib's implementation has the wrong docs for what the return val is
|
||||
// it is not a bool, it returns zero for success
|
||||
isConnected = ublox.begin(_serial_gps);
|
||||
|
||||
// try a second time, the ublox lib serial parsing is buggy?
|
||||
if (!isConnected)
|
||||
{
|
||||
isConnected = ublox.begin(_serial_gps);
|
||||
}
|
||||
|
||||
if (isConnected)
|
||||
{
|
||||
DEBUG_MSG("Connected to UBLOX GPS successfully\n");
|
||||
|
||||
if(!internalSetup())
|
||||
{
|
||||
DEBUG_MSG("UBLOX Init error\n");
|
||||
return false;
|
||||
}
|
||||
concurrency::PeriodicTask::setup(); // We don't start our periodic task unless we actually found the device
|
||||
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void UBloxGPS::reset()
|
||||
{
|
||||
uint8_t payloadCfg[4];
|
||||
ubxPacket packetCfg = {0, 0, 0, 0, 0, payloadCfg, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED,SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
|
||||
packetCfg.cls = UBX_CLASS_CFG;
|
||||
packetCfg.id = UBX_CFG_RST;
|
||||
packetCfg.len = 4;
|
||||
packetCfg.startingSpot = 0;
|
||||
payloadCfg[0] = 0x1;
|
||||
payloadCfg[1] = 0x0;
|
||||
payloadCfg[2] = 0; // 0=HW reset
|
||||
payloadCfg[3] = 0; // reserved
|
||||
ublox.sendCommand(&packetCfg, 0); // don't expect ACK
|
||||
}
|
||||
|
||||
void UBloxGPS::loop()
|
||||
{
|
||||
ublox.checkUblox();
|
||||
if(lastPvtSeqId!=ublox.pvtSeqId)
|
||||
{
|
||||
processPVT();
|
||||
}
|
||||
}
|
||||
|
||||
void UBloxGPS::processPVT()
|
||||
{
|
||||
uint8_t fixtype;
|
||||
|
||||
missPVT = 0;
|
||||
lastPvtSeqId=ublox.pvtSeqId;
|
||||
fixtype = ublox.getFixType(0);
|
||||
#ifdef GPS_DEBUG
|
||||
DEBUG_MSG("GPS fix type %d\n", fixtype);
|
||||
#endif
|
||||
|
||||
//DEBUG_MSG("lat %d\n", ublox.getLatitude());
|
||||
|
||||
// any fix that has time
|
||||
if (ublox.moduleQueried.gpsSecond)
|
||||
{
|
||||
/* Convert to unix time
|
||||
The Unix epoch (or Unix time or POSIX time or Unix timestamp) is the number of seconds that have elapsed since January 1, 1970
|
||||
(midnight UTC/GMT), not counting leap seconds (in ISO 8601: 1970-01-01T00:00:00Z).
|
||||
*/
|
||||
struct tm t;
|
||||
t.tm_sec = ublox.getSecond(0);
|
||||
t.tm_min = ublox.getMinute(0);
|
||||
t.tm_hour = ublox.getHour(0);
|
||||
t.tm_mday = ublox.getDay(0);
|
||||
t.tm_mon = ublox.getMonth(0) - 1;
|
||||
t.tm_year = ublox.getYear(0) - 1900;
|
||||
t.tm_isdst = false;
|
||||
perhapsSetRTC(t,ublox.getNanosecond(0)/1000000);
|
||||
}
|
||||
|
||||
if(ublox.moduleQueried.horizontalAccEst)
|
||||
hacc = ublox.getHorizontalAccEst(0);
|
||||
|
||||
if(ublox.moduleQueried.pDOP)
|
||||
dop = ublox.getPDOP(0);
|
||||
|
||||
if(ublox.moduleQueried.SIV)
|
||||
numSatellites = ublox.getSIV(0);
|
||||
|
||||
#ifdef GPS_DEBUG
|
||||
DEBUG_MSG("GPS numSatellites %d\n", numSatellites);
|
||||
#endif
|
||||
|
||||
#ifdef GPS_DISPLAY_HW
|
||||
ubxHwState hwState;
|
||||
if(ublox.getHwState(hwState))
|
||||
{
|
||||
DEBUG_MSG("HW State Noise %d, AGC %0.2f (%d)\n", hwState.noisePerMS,hwState.agcCnt*100/8192.0,hwState.agcCnt);
|
||||
}
|
||||
#endif
|
||||
if(global->getConfig().testLat&&global->getConfig().testLng)
|
||||
{
|
||||
latitude = global->getConfig().testLat;
|
||||
longitude = global->getConfig().testLng;
|
||||
altitude = 0;
|
||||
heading = ublox.getHeading(0);
|
||||
|
||||
// bogus lat lon is reported as 0 or 0 (can be bogus just for one)
|
||||
// Also: apparently when the GPS is initially reporting lock it can output a bogus latitude > 90 deg!
|
||||
hasValidLocation = (latitude != 0) && (longitude != 0) && (latitude <= 900000000 && latitude >= -900000000);
|
||||
if (hasValidLocation)
|
||||
{
|
||||
wantNewLocation = false;
|
||||
notifyObservers(NULL);
|
||||
}
|
||||
}
|
||||
else
|
||||
if ((fixtype >= 3 && fixtype <= 4) && ublox.moduleQueried.latitude) // rd fixes only
|
||||
{
|
||||
// we only notify if position has changed
|
||||
latitude = ublox.getLatitude(0);
|
||||
longitude = ublox.getLongitude(0);
|
||||
//longitude = -74.6294672*10000000; //OTP GPS
|
||||
//latitude = 39.9381124*10000000;
|
||||
//longitude = -74.41993781254924*10000000; //SANJ GPS
|
||||
//latitude = 39.99472549579622*10000000;
|
||||
//longitude = -73.98029099999999*10000000; //MSATO GPS
|
||||
//latitude = 42.207361*10000000;
|
||||
//longitude = -74.55166537552641*10000000; //R14 GPS
|
||||
//latitude = 39.9980604701216*10000000;
|
||||
|
||||
//longitude = -76.19249075700209*10000000; //BR2022 GPS
|
||||
//latitude = 40.94618445451866*10000000;
|
||||
//longitude = -74.9177937*10000000; //Picasso GPS
|
||||
//latitude = 39.7361251*10000000;
|
||||
|
||||
altitude = ublox.getAltitude(0) / 1000; // in mm convert to meters
|
||||
heading = ublox.getHeading(0);
|
||||
|
||||
// bogus lat lon is reported as 0 or 0 (can be bogus just for one)
|
||||
// Also: apparently when the GPS is initially reporting lock it can output a bogus latitude > 90 deg!
|
||||
hasValidLocation = (latitude != 0) && (longitude != 0) && (latitude <= 900000000 && latitude >= -900000000);
|
||||
if (hasValidLocation)
|
||||
{
|
||||
wantNewLocation = false;
|
||||
notifyObservers(NULL);
|
||||
}
|
||||
}
|
||||
else // we didn't get a location update, go back to sleep and hope the characters show up
|
||||
wantNewLocation = true;
|
||||
|
||||
// Notify any status instances that are observing us
|
||||
const GPSStatus status = GPSStatus(hasLock(), isConnected, latitude, longitude, altitude, dop, hacc, heading, numSatellites);
|
||||
newStatus.notifyObservers(&status);
|
||||
ublox.flushPVT();
|
||||
waitPVT = false;
|
||||
}
|
||||
|
||||
void UBloxGPS::doTask()
|
||||
{
|
||||
loop();
|
||||
if(waitPVT)
|
||||
{
|
||||
missPVT++;
|
||||
if(missPVT>4)
|
||||
{
|
||||
missPVT = 0;
|
||||
}
|
||||
}
|
||||
waitPVT = true;
|
||||
ublox.getPVT(0);
|
||||
#ifdef GPS_DISPLAY_HW
|
||||
ublox.queryHwState(0);
|
||||
#endif
|
||||
|
||||
setPeriod(hasValidLocation && !wantNewLocation ? 1000/GPS_FREQ : 1000);
|
||||
}
|
||||
|
||||
bool UBloxGPS::canSleep()
|
||||
{
|
||||
#ifdef AT_CAN_SLEEP
|
||||
return !waitPVT;
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
|
||||
void UBloxGPS::startLock()
|
||||
{
|
||||
DEBUG_MSG("Looking for GPS lock\n");
|
||||
wantNewLocation = true;
|
||||
setPeriod(1);
|
||||
}
|
||||
|
||||
void UBloxGPS::backupDatabase(const char *filename)
|
||||
{
|
||||
uint8_t *data = (uint8_t *) malloc(8192);
|
||||
int16_t size=8192;
|
||||
ublox.pollNavigationDatabase(data,size,5000);
|
||||
if(size>0)
|
||||
{
|
||||
FILE *file;
|
||||
file = fopen(filename,"wb+");
|
||||
fwrite(data,size,1,file);
|
||||
fflush(file);
|
||||
fclose(file);
|
||||
}
|
||||
free(data);
|
||||
}
|
||||
|
||||
void UBloxGPS::restoreDatabase(const char *filename)
|
||||
{
|
||||
FILE *file;
|
||||
file = fopen(filename,"rb");
|
||||
if(file==NULL)
|
||||
return;
|
||||
fseek(file, 0L, SEEK_END);
|
||||
size_t size = ftell(file);
|
||||
fseek(file, 0L, SEEK_SET);
|
||||
if(size>0)
|
||||
{
|
||||
int ss = 64;
|
||||
uint8_t *data = (uint8_t *) malloc(ss);
|
||||
while(true)
|
||||
{
|
||||
int readed = fread(data,ss,1,file);
|
||||
if(readed!=0)
|
||||
ublox.sendNavigationDatabase(data,readed);
|
||||
if(readed!=ss)
|
||||
break;
|
||||
delay(1);
|
||||
}
|
||||
free(data);
|
||||
}
|
||||
fclose(file);
|
||||
}
|
||||
|
||||
void UBloxGPS::shutdown()
|
||||
{
|
||||
//ublox.enableDebugging(Serial);
|
||||
uint8_t *payloadCfg = ublox.payloadCfg;
|
||||
ubxPacket packetCfg = {0, 0, 0, 0, 0, payloadCfg, 0, 0, SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED,SFE_UBLOX_PACKET_VALIDITY_NOT_DEFINED};
|
||||
packetCfg.cls = UBX_CLASS_CFG;
|
||||
packetCfg.id = UBX_CFG_RST;
|
||||
packetCfg.len = 4;
|
||||
packetCfg.startingSpot = 0;
|
||||
payloadCfg[0] = 0x0;
|
||||
payloadCfg[1] = 0x0;
|
||||
payloadCfg[2] = 0x8; // 0 = Controlled GNSS stop
|
||||
payloadCfg[3] = 0; // reserved
|
||||
ublox.sendCommand(&packetCfg,0); // don't expect ACK
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
#pragma once
|
||||
|
||||
#include "GPS.h"
|
||||
#include "Observer.h"
|
||||
#include "../concurrency/PeriodicTask.h"
|
||||
#include "SparkFun_Ublox_Arduino_Library.h"
|
||||
|
||||
/**
|
||||
* A gps class that only reads from the GPS periodically (and FIXME - eventually keeps the gps powered down except when reading)
|
||||
*
|
||||
* When new data is available it will notify observers.
|
||||
*/
|
||||
class UBloxGPS : public GPS, public concurrency::PeriodicTask
|
||||
{
|
||||
SFE_UBLOX_GPS ublox;
|
||||
uint32_t lastPvtSeqId=0xFFFFFFFF;
|
||||
bool wantNewLocation = true;
|
||||
bool internalSetup();
|
||||
bool serialInitialized = false;
|
||||
bool waitPVT=false;
|
||||
uint8_t missPVT = 0;
|
||||
void processPVT();
|
||||
public:
|
||||
UBloxGPS();
|
||||
|
||||
/**
|
||||
* Returns true if we succeeded
|
||||
*/
|
||||
virtual bool setup();
|
||||
|
||||
virtual void doTask();
|
||||
|
||||
/**
|
||||
* Restart our lock attempt - try to get and broadcast a GPS reading ASAP
|
||||
* called after the CPU wakes from light-sleep state */
|
||||
virtual void startLock();
|
||||
virtual void reset();
|
||||
virtual void backupDatabase(const char *filename);
|
||||
virtual void restoreDatabase(const char *filename);
|
||||
virtual bool canSleep();
|
||||
virtual void loop();
|
||||
virtual void shutdown();
|
||||
};
|
||||
+262
@@ -0,0 +1,262 @@
|
||||
#include "imu.h"
|
||||
#include "EEPROM.h"
|
||||
|
||||
#define Pi 3.14159265359
|
||||
//#define IMU_DEBUG
|
||||
|
||||
cIMU::cIMU()
|
||||
{
|
||||
m_imu = NULL;
|
||||
m_isConnected = false;
|
||||
m_lastUpdate = 0;
|
||||
}
|
||||
|
||||
void cIMU::adjustMagCalibrationBias(float value,uint8_t axis)
|
||||
{
|
||||
float o,s;
|
||||
switch(axis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
o = m_imu->getMagBiasX_uT();
|
||||
s = m_imu->getMagScaleFactorX();
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
o = m_imu->getMagBiasY_uT();
|
||||
s = m_imu->getMagScaleFactorY();
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
o = m_imu->getMagBiasZ_uT();
|
||||
s = m_imu->getMagScaleFactorZ();
|
||||
break;
|
||||
}
|
||||
}
|
||||
o+=value;
|
||||
switch(axis)
|
||||
{
|
||||
case 0:
|
||||
{
|
||||
m_imu->setMagCalX(o,s);
|
||||
break;
|
||||
}
|
||||
case 1:
|
||||
{
|
||||
m_imu->setMagCalY(o,s);
|
||||
break;
|
||||
}
|
||||
case 2:
|
||||
{
|
||||
m_imu->setMagCalZ(o,s);
|
||||
break;
|
||||
}
|
||||
}
|
||||
m_imu->recalibrateMagByBias();
|
||||
updateCalibration();
|
||||
DEBUG_MSG("Mag X: %0.2f/%0.2f, Y: %0.2f/%0.2f, Z: %0.2f/%0.2f\n",m_calibration.magBiasX,m_calibration.magScaleX,m_calibration.magBiasY,m_calibration.magScaleY,m_calibration.magBiasZ,m_calibration.magScaleZ);
|
||||
}
|
||||
|
||||
void cIMU::updateCalibration()
|
||||
{
|
||||
m_calibration.signature = CAL_SIG;
|
||||
m_calibration.magBiasX = m_imu->getMagBiasX_uT();
|
||||
m_calibration.magBiasY = m_imu->getMagBiasY_uT();
|
||||
m_calibration.magBiasZ = m_imu->getMagBiasZ_uT();
|
||||
m_calibration.magScaleX = m_imu->getMagScaleFactorX();
|
||||
m_calibration.magScaleY = m_imu->getMagScaleFactorY();
|
||||
m_calibration.magScaleZ = m_imu->getMagScaleFactorZ();
|
||||
m_imu->getMagMinMax(m_calibration.magMinX,m_calibration.magMaxX,m_calibration.magMinY,m_calibration.magMaxY,m_calibration.magMinZ,m_calibration.magMaxZ);
|
||||
|
||||
m_calibration.gyroBiasX = m_imu->getGyroBiasX_rads();
|
||||
m_calibration.gyroBiasY = m_imu->getGyroBiasY_rads();
|
||||
m_calibration.gyroBiasZ = m_imu->getGyroBiasZ_rads();
|
||||
|
||||
m_calibration.accBiasX = m_imu->getAccelBiasX_mss();
|
||||
m_calibration.accScaleX = m_imu->getAccelScaleFactorX();
|
||||
m_calibration.accBiasY = m_imu->getAccelBiasY_mss();
|
||||
m_calibration.accScaleY = m_imu->getAccelScaleFactorY();
|
||||
m_calibration.accBiasZ = m_imu->getAccelBiasZ_mss();
|
||||
m_calibration.accScaleZ = m_imu->getAccelScaleFactorZ();
|
||||
}
|
||||
|
||||
void cIMU::saveCalibration()
|
||||
{
|
||||
updateCalibration();
|
||||
EEPROM.writeBytes(CAL_OFFISET,&m_calibration,sizeof(m_calibration));
|
||||
EEPROM.commit();
|
||||
}
|
||||
|
||||
void cIMU::loadCalibration()
|
||||
{
|
||||
EEPROM.readBytes(CAL_OFFISET,&m_calibration,sizeof(m_calibration));
|
||||
if(m_calibration.signature==CAL_SIG)
|
||||
{
|
||||
m_imu->setGyroBiasX_rads(m_calibration.gyroBiasX);
|
||||
m_imu->setGyroBiasY_rads(m_calibration.gyroBiasY);
|
||||
m_imu->setGyroBiasZ_rads(m_calibration.gyroBiasZ);
|
||||
|
||||
m_imu->setMagMinMax(m_calibration.magMinX,m_calibration.magMaxX,m_calibration.magMinY,m_calibration.magMaxY,m_calibration.magMinZ,m_calibration.magMaxZ);
|
||||
m_imu->setMagCalX(m_calibration.magBiasX,m_calibration.magScaleX);
|
||||
m_imu->setMagCalY(m_calibration.magBiasY,m_calibration.magScaleY);
|
||||
m_imu->setMagCalZ(m_calibration.magBiasZ,m_calibration.magScaleZ);
|
||||
|
||||
m_imu->setAccelCalX(m_calibration.accBiasX,m_calibration.accScaleX);
|
||||
m_imu->setAccelCalY(m_calibration.accBiasY,m_calibration.accScaleY);
|
||||
m_imu->setAccelCalZ(m_calibration.accBiasZ,m_calibration.accScaleZ);
|
||||
}
|
||||
}
|
||||
|
||||
void cIMU::shutdown()
|
||||
{
|
||||
}
|
||||
|
||||
void cIMU::calibrate()
|
||||
{
|
||||
DEBUG_MSG("Calibrating Gyro\n");
|
||||
m_imu->calibrateGyro();
|
||||
delay(100);
|
||||
DEBUG_MSG("Calibrating Accel\n");
|
||||
m_imu->calibrateAccel();
|
||||
delay(100);
|
||||
DEBUG_MSG("Calibrating MAG\n");
|
||||
m_imu->calibrateMag();
|
||||
saveCalibration();
|
||||
}
|
||||
bool cIMU::setup()
|
||||
{
|
||||
EEPROM.readBytes(CAL_OFFISET,&m_calibration,sizeof(m_calibration));
|
||||
m_filter = new Madgwick();// new Adafruit_Mahony();
|
||||
m_filter->begin(50);
|
||||
m_lastUpdate = 0;
|
||||
m_isConnected = false;
|
||||
void *ptr = heap_caps_calloc(1,sizeof(MPU9250),MALLOC_CAP_8BIT);
|
||||
m_imu = new(ptr) MPU9250(Wire,0x68);
|
||||
auto status = m_imu->begin();
|
||||
if(status<0)
|
||||
{
|
||||
DEBUG_MSG("IMU initialization unsuccessful, %d\n",status);
|
||||
}
|
||||
else
|
||||
{
|
||||
DEBUG_MSG("IMU initialization successful, %d\n",status);
|
||||
m_isConnected = true;
|
||||
if(m_calibration.signature!=CAL_SIG)
|
||||
{
|
||||
calibrate();
|
||||
}
|
||||
else
|
||||
{
|
||||
DEBUG_MSG("Use calibration data\n");
|
||||
m_imu->setGyroBiasX_rads(m_calibration.gyroBiasX);
|
||||
m_imu->setGyroBiasY_rads(m_calibration.gyroBiasY);
|
||||
m_imu->setGyroBiasZ_rads(m_calibration.gyroBiasZ);
|
||||
DEBUG_MSG("Mag X: %0.2f/%0.2f, Y: %0.2f/%0.2f, Z: %0.2f/%0.2f\n",m_calibration.magBiasX,m_calibration.magScaleX,m_calibration.magBiasY,m_calibration.magScaleY,m_calibration.magBiasZ,m_calibration.magScaleZ);
|
||||
DEBUG_MSG("Mag Min/Max X: %0.2f/%0.2f, Y: %0.2f/%0.2f, Z: %0.2f/%0.2f\n",m_calibration.magMinX,m_calibration.magMaxX,m_calibration.magMinY,m_calibration.magMaxY,m_calibration.magMinZ,m_calibration.magMaxZ);
|
||||
m_imu->setMagMinMax(m_calibration.magMinX,m_calibration.magMaxX,m_calibration.magMinY,m_calibration.magMaxY,m_calibration.magMinZ,m_calibration.magMaxZ);
|
||||
m_imu->setMagCalX(m_calibration.magBiasX,m_calibration.magScaleX);
|
||||
m_imu->setMagCalY(m_calibration.magBiasY,m_calibration.magScaleY);
|
||||
m_imu->setMagCalZ(m_calibration.magBiasZ,m_calibration.magScaleZ);
|
||||
|
||||
m_imu->setAccelCalX(m_calibration.accBiasX,m_calibration.accScaleX);
|
||||
m_imu->setAccelCalY(m_calibration.accBiasY,m_calibration.accScaleY);
|
||||
m_imu->setAccelCalZ(m_calibration.accBiasZ,m_calibration.accScaleZ);
|
||||
}
|
||||
m_imu->setAccelRange(MPU9250::ACCEL_RANGE_2G);
|
||||
// setting the gyroscope full scale range to +/-500 deg/s
|
||||
m_imu->setGyroRange(MPU9250::GYRO_RANGE_500DPS);
|
||||
//m_imu->setGyroRange(MPU9250::GYRO_RANGE_500DPS);
|
||||
// setting DLPF bandwidth
|
||||
m_imu->setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_20HZ);
|
||||
// setting SRD to 19 for a 50 Hz update rate
|
||||
m_imu->setSrd(19);
|
||||
}
|
||||
return m_isConnected;
|
||||
}
|
||||
|
||||
cIMU::~cIMU()
|
||||
{
|
||||
}
|
||||
|
||||
#define GYRO_SPEED 1
|
||||
|
||||
#ifdef IMU_DEBUG
|
||||
int vvv=0;
|
||||
#endif
|
||||
void cIMU::loop()
|
||||
{
|
||||
if(m_isConnected)
|
||||
{
|
||||
int64_t now = millis();
|
||||
if((now-m_lastUpdate)>=20)
|
||||
{
|
||||
float delta = (now-m_lastUpdate)/1000.0;
|
||||
if(delta>0.5f)
|
||||
delta = 0.5f;
|
||||
m_lastUpdate = now;
|
||||
m_imu->readSensor();
|
||||
m_filter->update(m_imu->getGyroX_rads()/GYRO_SPEED,m_imu->getGyroY_rads()/GYRO_SPEED,m_imu->getGyroZ_rads()/GYRO_SPEED,m_imu->getAccelX_mss(), m_imu->getAccelY_mss(),m_imu->getAccelZ_mss(),m_imu->getMagX_uT(), m_imu->getMagY_uT(),m_imu->getMagZ_uT(),delta);
|
||||
/*
|
||||
float xM = m_imu->getMagX_uT();
|
||||
float yM = m_imu->getMagY_uT();
|
||||
float zM = m_imu->getMagZ_uT();
|
||||
|
||||
float g = 9.8;
|
||||
float xG = (m_imu->getAccelX_mss()) / g;
|
||||
float yG = (m_imu->getAccelY_mss()) / g;
|
||||
float zG = (m_imu->getAccelZ_mss()) / g;
|
||||
float pitch = atan2(-xG, sqrt(yG * yG + zG * zG));
|
||||
float roll = atan2(yG, zG);
|
||||
|
||||
//float pitch = m_filter.getPitchRadians();
|
||||
//float roll = m_filter.getRollRadians();
|
||||
float xM2 = xM * cos(pitch) + zM * sin(pitch);
|
||||
float yM2 = xM * sin(roll) * sin(pitch) + yM * cos(roll) - zM * sin(roll) * cos(pitch);
|
||||
float compHeading2 = (atan2(yM2, xM2) * 180 / Pi)-90;
|
||||
if (compHeading2 < 0) {
|
||||
compHeading2 = 360 + compHeading2;
|
||||
}
|
||||
*/
|
||||
float compHeading = 90-m_filter->getYaw();
|
||||
if(compHeading<0) compHeading+=360;
|
||||
const IMUStatus status = IMUStatus(true,compHeading*100);
|
||||
newStatus.notifyObservers(&status);
|
||||
#ifdef IMU_DEBUG
|
||||
vvv++;
|
||||
if(vvv%5==0)
|
||||
{
|
||||
m_imu->getMagMinMax(m_calibration.magMinX,m_calibration.magMaxX,m_calibration.magMinY,m_calibration.magMaxY,m_calibration.magMinZ,m_calibration.magMaxZ);
|
||||
Serial.print(m_imu->getAccelX_mss(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getAccelY_mss(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getAccelZ_mss(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getGyroX_rads(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getGyroY_rads(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getGyroZ_rads(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getMagX_uT(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getMagY_uT(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getMagZ_uT(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getRawMagX_uT(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getRawMagY_uT(),6);
|
||||
Serial.print("\t");
|
||||
Serial.print(m_imu->getRawMagZ_uT(),6);
|
||||
|
||||
//Serial.print("\t");
|
||||
//Serial.print(m_imu->getTemperature_C(),6);
|
||||
DEBUG_MSG("\tHeading %0.2f %0.2f/%0.2f, Y: %0.2f/%0.2f, Z: %0.2f/%0.2f\n",compHeading,m_calibration.magMinX,m_calibration.magMaxX,m_calibration.magMinY,m_calibration.magMaxY,m_calibration.magMinZ,m_calibration.magMaxZ);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,71 @@
|
||||
#pragma once
|
||||
#include <Arduino.h>
|
||||
#include "MPU9250/MPU9250.h"
|
||||
#include "Observer.h"
|
||||
#include "IMUStatus.h"
|
||||
#include "AHRS/MadgwickAHRS.h"
|
||||
#include "AHRS/Adafruit_AHRS_Mahony.h"
|
||||
|
||||
#define CAL_SIG 0x2452
|
||||
#define CAL_OFFISET 0
|
||||
struct sIMUCalibration
|
||||
{
|
||||
int16_t signature;
|
||||
float magMinX;
|
||||
float magMaxX;
|
||||
float magMinY;
|
||||
float magMaxY;
|
||||
float magMinZ;
|
||||
float magMaxZ;
|
||||
float magBiasX;
|
||||
float magBiasY;
|
||||
float magBiasZ;
|
||||
float magScaleX;
|
||||
float magScaleY;
|
||||
float magScaleZ;
|
||||
|
||||
float accBiasX;
|
||||
float accBiasY;
|
||||
float accBiasZ;
|
||||
float accScaleX;
|
||||
float accScaleY;
|
||||
float accScaleZ;
|
||||
|
||||
float gyroBiasX;
|
||||
float gyroBiasY;
|
||||
float gyroBiasZ;
|
||||
sIMUCalibration()
|
||||
{
|
||||
signature = 0;
|
||||
magMaxX = magMaxY = magMaxZ = magMinX = magMinY = magMinZ = 0;
|
||||
magBiasX = magBiasY = magBiasZ = magScaleX = magScaleY = magScaleZ = 0;
|
||||
accBiasX = accBiasY = accBiasZ = accScaleX = accScaleY = accScaleZ = 0;
|
||||
gyroBiasX = gyroBiasY = gyroBiasZ = 0;
|
||||
}
|
||||
};
|
||||
|
||||
class cIMU : public Observable<void *>
|
||||
{
|
||||
protected:
|
||||
MPU9250 *m_imu;
|
||||
bool m_isConnected;
|
||||
int64_t m_lastUpdate;
|
||||
//Adafruit_AHRS_FusionInterface *m_filter;
|
||||
Madgwick *m_filter;
|
||||
sIMUCalibration m_calibration;
|
||||
public:
|
||||
Observable<const IMUStatus *> newStatus;
|
||||
cIMU();
|
||||
virtual ~cIMU();
|
||||
virtual bool setup();
|
||||
virtual void loop();
|
||||
void calibrate();
|
||||
void shutdown();
|
||||
void updateCalibration();
|
||||
void saveCalibration();
|
||||
void loadCalibration();
|
||||
void setAutoMag(bool v) { m_imu->setAutoMag(v); }
|
||||
bool getAutoMag() { return m_imu->getAutoMag(); }
|
||||
void adjustMagCalibrationBias(float value,uint8_t axis);
|
||||
sIMUCalibration &getCalibration() { return m_calibration; }
|
||||
};
|
||||
+789
@@ -0,0 +1,789 @@
|
||||
#ifndef LV_CONF_H
|
||||
#define LV_CONF_H
|
||||
/* clang-format off */
|
||||
#include <stdint.h>
|
||||
|
||||
#include "config.h"
|
||||
|
||||
#define CONFIG_LV_TOUCH_CONTROLLER_NONE 1
|
||||
//#ifdef USE_SCREEN_EVE
|
||||
#define CONFIG_LV_TFT_DISPLAY_CONTROLLER_FT81X 1
|
||||
//#endif
|
||||
#define CONFIG_LV_TFT_DISPLAY_PROTOCOL_SPI 1
|
||||
#define CONFIG_LV_TFT_DISPLAY_SPI_FULL_DUPLEX 1
|
||||
#define CONFIG_LV_TFT_DISPLAY_SPI_TRANS_MODE_SIO 1
|
||||
#define CONFIG_LV_DISPLAY_ORIENTATION_LANDSCAPE 1
|
||||
#define CONFIG_LV_FT81X_CONFIG_EVE_EVE3_35 1
|
||||
//#define CONFIG_LV_TFT_DISPLAY_SPI_HSPI 1
|
||||
#define CONFIG_LV_TFT_DISPLAY_SPI_HSPI 1
|
||||
#define CONFIG_LV_DISPLAY_WIDTH 320
|
||||
#define CONFIG_LV_DISPLAY_HEIGHT 240
|
||||
#define CONFIG_LV_TFT_USE_CUSTOM_SPI_CLK_DIVIDER 1
|
||||
#define CONFIG_LV_TFT_CUSTOM_SPI_CLK_DIVIDER 5
|
||||
|
||||
#define CONFIG_LV_DISP_SPI_MOSI (SPI_DISP_MOSI)
|
||||
#define CONFIG_LV_DISPLAY_USE_SPI_MISO 1
|
||||
#define CONFIG_LV_DISP_SPI_MISO (SPI_DISP_MISO)
|
||||
#define CONFIG_LV_DISP_SPI_INPUT_DELAY_NS 0
|
||||
#define CONFIG_LV_DISP_SPI_IO2 -1
|
||||
#define CONFIG_LV_DISP_SPI_IO3 -1
|
||||
#define CONFIG_LV_DISP_SPI_CLK (SPI_DISP_SCK)
|
||||
#define CONFIG_LV_DISPLAY_USE_SPI_CS 1
|
||||
#define CONFIG_LV_DISP_SPI_CS (SPI_DISP_CS)
|
||||
#define CONFIG_LV_DISP_PIN_RST (SPI_DISP_RESET)
|
||||
#undef CONFIG_LV_TFT_DISPLAY_SPI_HALF_DUPLEX
|
||||
|
||||
#define FT81X_FULL 1
|
||||
|
||||
#define FT81X_ARGB4 1
|
||||
|
||||
/*====================
|
||||
Graphical settings
|
||||
*====================*/
|
||||
|
||||
/* Maximal horizontal and vertical resolution to support by the library.*/
|
||||
#define LV_HOR_RES_MAX (CONFIG_LV_DISPLAY_WIDTH)
|
||||
#define LV_VER_RES_MAX (CONFIG_LV_DISPLAY_HEIGHT)
|
||||
|
||||
/* Color depth:
|
||||
* - 1: 1 byte per pixel
|
||||
* - 8: RGB332
|
||||
* - 16: RGB565
|
||||
* - 32: ARGB8888
|
||||
*/
|
||||
#define LV_COLOR_DEPTH 32
|
||||
|
||||
/* Swap the 2 bytes of RGB565 color.
|
||||
* Useful if the display has a 8 bit interface (e.g. SPI)*/
|
||||
#define LV_COLOR_16_SWAP 0
|
||||
|
||||
/* 1: Enable screen transparency.
|
||||
* Useful for OSD or other overlapping GUIs.
|
||||
* Requires `LV_COLOR_DEPTH = 32` colors and the screen's style should be modified: `style.body.opa = ...`*/
|
||||
#define LV_COLOR_SCREEN_TRANSP 1
|
||||
|
||||
/*Images pixels with this color will not be drawn (with chroma keying)*/
|
||||
#define LV_COLOR_TRANSP LV_COLOR_LIME /*LV_COLOR_LIME: pure green*/
|
||||
|
||||
/* Enable anti-aliasing (lines, and radiuses will be smoothed) */
|
||||
#define LV_ANTIALIAS 1
|
||||
|
||||
/* Default display refresh period.
|
||||
* Can be changed in the display driver (`lv_disp_drv_t`).*/
|
||||
#define LV_DISP_DEF_REFR_PERIOD 30 /*[ms]*/
|
||||
|
||||
/* Dot Per Inch: used to initialize default sizes.
|
||||
* E.g. a button with width = LV_DPI / 2 -> half inch wide
|
||||
* (Not so important, you can adjust it to modify default sizes and spaces)*/
|
||||
#define LV_DPI 40 /*[px]*/
|
||||
|
||||
/* The the real width of the display changes some default values:
|
||||
* default object sizes, layout of examples, etc.
|
||||
* According to the width of the display (hor. res. / dpi)
|
||||
* the displays fall in 4 categories.
|
||||
* The 4th is extra large which has no upper limit so not listed here
|
||||
* The upper limit of the categories are set below in 0.1 inch unit.
|
||||
*/
|
||||
#define LV_DISP_SMALL_LIMIT 30
|
||||
#define LV_DISP_MEDIUM_LIMIT 50
|
||||
#define LV_DISP_LARGE_LIMIT 70
|
||||
|
||||
/* Type of coordinates. Should be `int16_t` (or `int32_t` for extreme cases) */
|
||||
typedef int16_t lv_coord_t;
|
||||
|
||||
/*=========================
|
||||
Memory manager settings
|
||||
*=========================*/
|
||||
|
||||
/* LittelvGL's internal memory manager's settings.
|
||||
* The graphical objects and other related data are stored here. */
|
||||
|
||||
/* 1: use custom malloc/free, 0: use the built-in `lv_mem_alloc` and `lv_mem_free` */
|
||||
#define LV_MEM_CUSTOM 1
|
||||
#if LV_MEM_CUSTOM == 0
|
||||
/* Size of the memory used by `lv_mem_alloc` in bytes (>= 2kB)*/
|
||||
# define LV_MEM_SIZE (32U * 1024U)
|
||||
|
||||
/* Compiler prefix for a big array declaration */
|
||||
# define LV_MEM_ATTR
|
||||
|
||||
/* Set an address for the memory pool instead of allocating it as an array.
|
||||
* Can be in external SRAM too. */
|
||||
# define LV_MEM_ADR 0
|
||||
|
||||
/* Automatically defrag. on free. Defrag. means joining the adjacent free cells. */
|
||||
# define LV_MEM_AUTO_DEFRAG 1
|
||||
#else /*LV_MEM_CUSTOM*/
|
||||
//# define LV_MEM_CUSTOM_INCLUDE <stdlib.h> /*Header for the dynamic memory function*/
|
||||
//# define LV_MEM_CUSTOM_ALLOC malloc /*Wrapper to malloc*/
|
||||
//# define LV_MEM_CUSTOM_FREE free /*Wrapper to free*/
|
||||
|
||||
# define LV_MEM_CUSTOM_INCLUDE "lvgl_mem.h" /*Header for the dynamic memory function*/
|
||||
# define LV_MEM_CUSTOM_ALLOC lvgl_malloc /*Wrapper to malloc*/
|
||||
# define LV_MEM_CUSTOM_FREE lvgl_free /*Wrapper to free*/
|
||||
|
||||
#endif /*LV_MEM_CUSTOM*/
|
||||
|
||||
/* Use the standard memcpy and memset instead of LVGL's own functions.
|
||||
* The standard functions might or might not be faster depending on their implementation. */
|
||||
#define LV_MEMCPY_MEMSET_STD 0
|
||||
|
||||
/* Garbage Collector settings
|
||||
* Used if lvgl is binded to higher level language and the memory is managed by that language */
|
||||
#define LV_ENABLE_GC 0
|
||||
#if LV_ENABLE_GC != 0
|
||||
# define LV_GC_INCLUDE "gc.h" /*Include Garbage Collector related things*/
|
||||
# define LV_MEM_CUSTOM_REALLOC your_realloc /*Wrapper to realloc*/
|
||||
# define LV_MEM_CUSTOM_GET_SIZE your_mem_get_size /*Wrapper to lv_mem_get_size*/
|
||||
#endif /* LV_ENABLE_GC */
|
||||
|
||||
/*=======================
|
||||
Input device settings
|
||||
*=======================*/
|
||||
|
||||
/* Input device default settings.
|
||||
* Can be changed in the Input device driver (`lv_indev_drv_t`)*/
|
||||
|
||||
/* Input device read period in milliseconds */
|
||||
#define LV_INDEV_DEF_READ_PERIOD 30
|
||||
|
||||
/* Drag threshold in pixels */
|
||||
#define LV_INDEV_DEF_DRAG_LIMIT 10
|
||||
|
||||
/* Drag throw slow-down in [%]. Greater value -> faster slow-down */
|
||||
#define LV_INDEV_DEF_DRAG_THROW 10
|
||||
|
||||
/* Long press time in milliseconds.
|
||||
* Time to send `LV_EVENT_LONG_PRESSED`) */
|
||||
#define LV_INDEV_DEF_LONG_PRESS_TIME 400
|
||||
|
||||
/* Repeated trigger period in long press [ms]
|
||||
* Time between `LV_EVENT_LONG_PRESSED_REPEAT */
|
||||
#define LV_INDEV_DEF_LONG_PRESS_REP_TIME 100
|
||||
|
||||
|
||||
/* Gesture threshold in pixels */
|
||||
#define LV_INDEV_DEF_GESTURE_LIMIT 50
|
||||
|
||||
/* Gesture min velocity at release before swipe (pixels)*/
|
||||
#define LV_INDEV_DEF_GESTURE_MIN_VELOCITY 3
|
||||
|
||||
/*==================
|
||||
* Feature usage
|
||||
*==================*/
|
||||
|
||||
/*1: Enable the Animations */
|
||||
#define LV_USE_ANIMATION 1
|
||||
#if LV_USE_ANIMATION
|
||||
|
||||
/*Declare the type of the user data of animations (can be e.g. `void *`, `int`, `struct`)*/
|
||||
typedef void * lv_anim_user_data_t;
|
||||
|
||||
#endif
|
||||
|
||||
/* 1: Enable shadow drawing on rectangles*/
|
||||
#define LV_USE_SHADOW 1
|
||||
#if LV_USE_SHADOW
|
||||
/* Allow buffering some shadow calculation
|
||||
* LV_SHADOW_CACHE_SIZE is the max. shadow size to buffer,
|
||||
* where shadow size is `shadow_width + radius`
|
||||
* Caching has LV_SHADOW_CACHE_SIZE^2 RAM cost*/
|
||||
#define LV_SHADOW_CACHE_SIZE 0
|
||||
#endif
|
||||
|
||||
/*1: enable outline drawing on rectangles*/
|
||||
#define LV_USE_OUTLINE 1
|
||||
|
||||
/*1: enable pattern drawing on rectangles*/
|
||||
#define LV_USE_PATTERN 1
|
||||
|
||||
/*1: enable value string drawing on rectangles*/
|
||||
#define LV_USE_VALUE_STR 1
|
||||
|
||||
/* 1: Use other blend modes than normal (`LV_BLEND_MODE_...`)*/
|
||||
#define LV_USE_BLEND_MODES 1
|
||||
|
||||
/* 1: Use the `opa_scale` style property to set the opacity of an object and its children at once*/
|
||||
#define LV_USE_OPA_SCALE 1
|
||||
|
||||
/* 1: Use image zoom and rotation*/
|
||||
#define LV_USE_IMG_TRANSFORM 1
|
||||
|
||||
/* 1: Enable object groups (for keyboard/encoder navigation) */
|
||||
#define LV_USE_GROUP 1
|
||||
#if LV_USE_GROUP
|
||||
typedef void * lv_group_user_data_t;
|
||||
#endif /*LV_USE_GROUP*/
|
||||
|
||||
/* 1: Enable GPU interface*/
|
||||
#define LV_USE_GPU 1 /*Only enables `gpu_fill_cb` and `gpu_blend_cb` in the disp. drv- */
|
||||
#define LV_USE_GPU_STM32_DMA2D 0
|
||||
/*If enabling LV_USE_GPU_STM32_DMA2D, LV_GPU_DMA2D_CMSIS_INCLUDE must be defined to include path of CMSIS header of target processor
|
||||
e.g. "stm32f769xx.h" or "stm32f429xx.h" */
|
||||
#define LV_GPU_DMA2D_CMSIS_INCLUDE
|
||||
|
||||
/*1: Use PXP for CPU off-load on NXP RTxxx platforms */
|
||||
#define LV_USE_GPU_NXP_PXP 0
|
||||
|
||||
/*1: Add default bare metal and FreeRTOS interrupt handling routines for PXP (lv_gpu_nxp_pxp_osa.c)
|
||||
* and call lv_gpu_nxp_pxp_init() automatically during lv_init(). Note that symbol FSL_RTOS_FREE_RTOS
|
||||
* has to be defined in order to use FreeRTOS OSA, otherwise bare-metal implementation is selected.
|
||||
*0: lv_gpu_nxp_pxp_init() has to be called manually before lv_init()
|
||||
* */
|
||||
#define LV_USE_GPU_NXP_PXP_AUTO_INIT 0
|
||||
|
||||
/*1: Use VG-Lite for CPU offload on NXP RTxxx platforms */
|
||||
#define LV_USE_GPU_NXP_VG_LITE 0
|
||||
|
||||
/* 1: Enable file system (might be required for images */
|
||||
#define LV_USE_FILESYSTEM 1
|
||||
#if LV_USE_FILESYSTEM
|
||||
/*Declare the type of the user data of file system drivers (can be e.g. `void *`, `int`, `struct`)*/
|
||||
typedef void * lv_fs_drv_user_data_t;
|
||||
#endif
|
||||
|
||||
/*1: Add a `user_data` to drivers and objects*/
|
||||
#define LV_USE_USER_DATA 1
|
||||
|
||||
/*1: Show CPU usage and FPS count in the right bottom corner*/
|
||||
#define LV_USE_PERF_MONITOR 0
|
||||
|
||||
/*1: Use the functions and types from the older API if possible */
|
||||
#define LV_USE_API_EXTENSION_V6 1
|
||||
#define LV_USE_API_EXTENSION_V7 1
|
||||
|
||||
/*========================
|
||||
* Image decoder and cache
|
||||
*========================*/
|
||||
|
||||
/* 1: Enable indexed (palette) images */
|
||||
#define LV_IMG_CF_INDEXED 1
|
||||
|
||||
/* 1: Enable alpha indexed images */
|
||||
#define LV_IMG_CF_ALPHA 1
|
||||
|
||||
/* Default image cache size. Image caching keeps the images opened.
|
||||
* If only the built-in image formats are used there is no real advantage of caching.
|
||||
* (I.e. no new image decoder is added)
|
||||
* With complex image decoders (e.g. PNG or JPG) caching can save the continuous open/decode of images.
|
||||
* However the opened images might consume additional RAM.
|
||||
* Set it to 0 to disable caching */
|
||||
#define LV_IMG_CACHE_DEF_SIZE 1
|
||||
|
||||
/*Declare the type of the user data of image decoder (can be e.g. `void *`, `int`, `struct`)*/
|
||||
typedef void * lv_img_decoder_user_data_t;
|
||||
|
||||
/*=====================
|
||||
* Compiler settings
|
||||
*====================*/
|
||||
|
||||
/* For big endian systems set to 1 */
|
||||
#define LV_BIG_ENDIAN_SYSTEM 0
|
||||
|
||||
/* Define a custom attribute to `lv_tick_inc` function */
|
||||
#define LV_ATTRIBUTE_TICK_INC
|
||||
|
||||
/* Define a custom attribute to `lv_task_handler` function */
|
||||
#define LV_ATTRIBUTE_TASK_HANDLER
|
||||
|
||||
/* Define a custom attribute to `lv_disp_flush_ready` function */
|
||||
#define LV_ATTRIBUTE_FLUSH_READY
|
||||
|
||||
/* Required alignment size for buffers */
|
||||
#define LV_ATTRIBUTE_MEM_ALIGN_SIZE
|
||||
|
||||
/* With size optimization (-Os) the compiler might not align data to
|
||||
* 4 or 8 byte boundary. Some HW may need even 32 or 64 bytes.
|
||||
* This alignment will be explicitly applied where needed.
|
||||
* LV_ATTRIBUTE_MEM_ALIGN_SIZE should be used to specify required align size.
|
||||
* E.g. __attribute__((aligned(LV_ATTRIBUTE_MEM_ALIGN_SIZE))) */
|
||||
#define LV_ATTRIBUTE_MEM_ALIGN
|
||||
|
||||
/* Attribute to mark large constant arrays for example
|
||||
* font's bitmaps */
|
||||
#define LV_ATTRIBUTE_LARGE_CONST
|
||||
|
||||
/* Prefix performance critical functions to place them into a faster memory (e.g RAM)
|
||||
* Uses 15-20 kB extra memory */
|
||||
#define LV_ATTRIBUTE_FAST_MEM
|
||||
|
||||
/* Export integer constant to binding.
|
||||
* This macro is used with constants in the form of LV_<CONST> that
|
||||
* should also appear on lvgl binding API such as Micropython
|
||||
*
|
||||
* The default value just prevents a GCC warning.
|
||||
*/
|
||||
#define LV_EXPORT_CONST_INT(int_value) struct _silence_gcc_warning
|
||||
|
||||
/* Prefix variables that are used in GPU accelerated operations, often these need to be
|
||||
* placed in RAM sections that are DMA accessible */
|
||||
#define LV_ATTRIBUTE_DMA
|
||||
|
||||
/*===================
|
||||
* HAL settings
|
||||
*==================*/
|
||||
|
||||
/* 1: use a custom tick source.
|
||||
* It removes the need to manually update the tick with `lv_tick_inc`) */
|
||||
#define LV_TICK_CUSTOM 1
|
||||
#if LV_TICK_CUSTOM == 1
|
||||
#define LV_TICK_CUSTOM_INCLUDE "Arduino.h" /*Header for the system time function*/
|
||||
#define LV_TICK_CUSTOM_SYS_TIME_EXPR (millis()) /*Expression evaluating to current system time in ms*/
|
||||
#endif /*LV_TICK_CUSTOM*/
|
||||
|
||||
typedef void * lv_disp_drv_user_data_t; /*Type of user data in the display driver*/
|
||||
typedef void * lv_indev_drv_user_data_t; /*Type of user data in the input device driver*/
|
||||
|
||||
/*================
|
||||
* Log settings
|
||||
*===============*/
|
||||
|
||||
/*1: Enable the log module*/
|
||||
#define LV_USE_LOG 0
|
||||
#if LV_USE_LOG
|
||||
/* How important log should be added:
|
||||
* LV_LOG_LEVEL_TRACE A lot of logs to give detailed information
|
||||
* LV_LOG_LEVEL_INFO Log important events
|
||||
* LV_LOG_LEVEL_WARN Log if something unwanted happened but didn't cause a problem
|
||||
* LV_LOG_LEVEL_ERROR Only critical issue, when the system may fail
|
||||
* LV_LOG_LEVEL_NONE Do not log anything
|
||||
*/
|
||||
# define LV_LOG_LEVEL LV_LOG_LEVEL_WARN
|
||||
|
||||
/* 1: Print the log with 'printf';
|
||||
* 0: user need to register a callback with `lv_log_register_print_cb`*/
|
||||
# define LV_LOG_PRINTF 0
|
||||
#endif /*LV_USE_LOG*/
|
||||
|
||||
/*=================
|
||||
* Debug settings
|
||||
*================*/
|
||||
|
||||
/* If Debug is enabled LittelvGL validates the parameters of the functions.
|
||||
* If an invalid parameter is found an error log message is printed and
|
||||
* the MCU halts at the error. (`LV_USE_LOG` should be enabled)
|
||||
* If you are debugging the MCU you can pause
|
||||
* the debugger to see exactly where the issue is.
|
||||
*
|
||||
* The behavior of asserts can be overwritten by redefining them here.
|
||||
* E.g. #define LV_ASSERT_MEM(p) <my_assert_code>
|
||||
*/
|
||||
#define LV_USE_DEBUG 0
|
||||
#if LV_USE_DEBUG
|
||||
|
||||
/*Check if the parameter is NULL. (Quite fast) */
|
||||
#define LV_USE_ASSERT_NULL 1
|
||||
|
||||
/*Checks is the memory is successfully allocated or no. (Quite fast)*/
|
||||
#define LV_USE_ASSERT_MEM 1
|
||||
|
||||
/*Check the integrity of `lv_mem` after critical operations. (Slow)*/
|
||||
#define LV_USE_ASSERT_MEM_INTEGRITY 0
|
||||
|
||||
/* Check the strings.
|
||||
* Search for NULL, very long strings, invalid characters, and unnatural repetitions. (Slow)
|
||||
* If disabled `LV_USE_ASSERT_NULL` will be performed instead (if it's enabled) */
|
||||
#define LV_USE_ASSERT_STR 0
|
||||
|
||||
/* Check NULL, the object's type and existence (e.g. not deleted). (Quite slow)
|
||||
* If disabled `LV_USE_ASSERT_NULL` will be performed instead (if it's enabled) */
|
||||
#define LV_USE_ASSERT_OBJ 0
|
||||
|
||||
/*Check if the styles are properly initialized. (Fast)*/
|
||||
#define LV_USE_ASSERT_STYLE 0
|
||||
|
||||
#endif /*LV_USE_DEBUG*/
|
||||
|
||||
/*==================
|
||||
* FONT USAGE
|
||||
*===================*/
|
||||
|
||||
/* The built-in fonts contains the ASCII range and some Symbols with 4 bit-per-pixel.
|
||||
* The symbols are available via `LV_SYMBOL_...` defines
|
||||
* More info about fonts: https://docs.lvgl.io/v7/en/html/overview/font.html
|
||||
* To create a new font go to: https://lvgl.com/ttf-font-to-c-array
|
||||
*/
|
||||
|
||||
/* Montserrat fonts with bpp = 4
|
||||
* https://fonts.google.com/specimen/Montserrat */
|
||||
#define LV_FONT_MONTSERRAT_8 0
|
||||
#define LV_FONT_MONTSERRAT_10 0
|
||||
#define LV_FONT_MONTSERRAT_12 1
|
||||
#define LV_FONT_MONTSERRAT_14 1
|
||||
#define LV_FONT_MONTSERRAT_16 1
|
||||
#define LV_FONT_MONTSERRAT_18 0
|
||||
#define LV_FONT_MONTSERRAT_20 0
|
||||
#define LV_FONT_MONTSERRAT_22 0
|
||||
#define LV_FONT_MONTSERRAT_24 0
|
||||
#define LV_FONT_MONTSERRAT_26 0
|
||||
#define LV_FONT_MONTSERRAT_28 0
|
||||
#define LV_FONT_MONTSERRAT_30 0
|
||||
#define LV_FONT_MONTSERRAT_32 0
|
||||
#define LV_FONT_MONTSERRAT_34 0
|
||||
#define LV_FONT_MONTSERRAT_36 0
|
||||
#define LV_FONT_MONTSERRAT_38 0
|
||||
#define LV_FONT_MONTSERRAT_40 0
|
||||
#define LV_FONT_MONTSERRAT_42 0
|
||||
#define LV_FONT_MONTSERRAT_44 0
|
||||
#define LV_FONT_MONTSERRAT_46 0
|
||||
#define LV_FONT_MONTSERRAT_48 1
|
||||
|
||||
/* Demonstrate special features */
|
||||
#define LV_FONT_MONTSERRAT_12_SUBPX 0
|
||||
#define LV_FONT_MONTSERRAT_28_COMPRESSED 0 /*bpp = 3*/
|
||||
#define LV_FONT_DEJAVU_16_PERSIAN_HEBREW 0 /*Hebrew, Arabic, PErisan letters and all their forms*/
|
||||
#define LV_FONT_SIMSUN_16_CJK 0 /*1000 most common CJK radicals*/
|
||||
|
||||
/*Pixel perfect monospace font
|
||||
* http://pelulamu.net/unscii/ */
|
||||
#define LV_FONT_UNSCII_8 0
|
||||
#define LV_FONT_UNSCII_16 0
|
||||
|
||||
/* Optionally declare your custom fonts here.
|
||||
* You can use these fonts as default font too
|
||||
* and they will be available globally. E.g.
|
||||
* #define LV_FONT_CUSTOM_DECLARE LV_FONT_DECLARE(my_font_1) \
|
||||
* LV_FONT_DECLARE(my_font_2)
|
||||
*/
|
||||
#define LV_FONT_CUSTOM_DECLARE
|
||||
|
||||
/* Enable it if you have fonts with a lot of characters.
|
||||
* The limit depends on the font size, font face and bpp
|
||||
* but with > 10,000 characters if you see issues probably you need to enable it.*/
|
||||
#define LV_FONT_FMT_TXT_LARGE 0
|
||||
|
||||
/* Enables/disables support for compressed fonts. If it's disabled, compressed
|
||||
* glyphs cannot be processed by the library and won't be rendered.
|
||||
*/
|
||||
#define LV_USE_FONT_COMPRESSED 1
|
||||
|
||||
/* Enable subpixel rendering */
|
||||
#define LV_USE_FONT_SUBPX 1
|
||||
#if LV_USE_FONT_SUBPX
|
||||
/* Set the pixel order of the display.
|
||||
* Important only if "subpx fonts" are used.
|
||||
* With "normal" font it doesn't matter.
|
||||
*/
|
||||
#define LV_FONT_SUBPX_BGR 0
|
||||
#endif
|
||||
|
||||
/*Declare the type of the user data of fonts (can be e.g. `void *`, `int`, `struct`)*/
|
||||
typedef void * lv_font_user_data_t;
|
||||
|
||||
/*================
|
||||
* THEME USAGE
|
||||
*================*/
|
||||
|
||||
/*Always enable at least on theme*/
|
||||
|
||||
/* No theme, you can apply your styles as you need
|
||||
* No flags. Set LV_THEME_DEFAULT_FLAG 0 */
|
||||
#define LV_USE_THEME_EMPTY 0
|
||||
|
||||
/*Simple to the create your theme based on it
|
||||
* No flags. Set LV_THEME_DEFAULT_FLAG 0 */
|
||||
#define LV_USE_THEME_TEMPLATE 0
|
||||
|
||||
/* A fast and impressive theme.
|
||||
* Flags:
|
||||
* LV_THEME_MATERIAL_FLAG_LIGHT: light theme
|
||||
* LV_THEME_MATERIAL_FLAG_DARK: dark theme
|
||||
* LV_THEME_MATERIAL_FLAG_NO_TRANSITION: disable transitions (state change animations)
|
||||
* LV_THEME_MATERIAL_FLAG_NO_FOCUS: disable indication of focused state)
|
||||
* */
|
||||
#define LV_USE_THEME_MATERIAL 1
|
||||
|
||||
/* Mono-color theme for monochrome displays.
|
||||
* If LV_THEME_DEFAULT_COLOR_PRIMARY is LV_COLOR_BLACK the
|
||||
* texts and borders will be black and the background will be
|
||||
* white. Else the colors are inverted.
|
||||
* No flags. Set LV_THEME_DEFAULT_FLAG 0 */
|
||||
#define LV_USE_THEME_MONO 0
|
||||
|
||||
#define LV_THEME_DEFAULT_INCLUDE <stdint.h> /*Include a header for the init. function*/
|
||||
#define LV_THEME_DEFAULT_INIT lv_theme_material_init
|
||||
#define LV_THEME_DEFAULT_COLOR_PRIMARY lv_color_hex(0x01a2b1)
|
||||
#define LV_THEME_DEFAULT_COLOR_SECONDARY lv_color_hex(0x44d1b6)
|
||||
#define LV_THEME_DEFAULT_FLAG LV_THEME_MATERIAL_FLAG_DARK
|
||||
#define LV_THEME_DEFAULT_FONT_SMALL &lv_font_montserrat_12
|
||||
#define LV_THEME_DEFAULT_FONT_NORMAL &lv_font_montserrat_12
|
||||
#define LV_THEME_DEFAULT_FONT_SUBTITLE &lv_font_montserrat_14
|
||||
#define LV_THEME_DEFAULT_FONT_TITLE &lv_font_montserrat_14
|
||||
|
||||
/*=================
|
||||
* Text settings
|
||||
*=================*/
|
||||
|
||||
/* Select a character encoding for strings.
|
||||
* Your IDE or editor should have the same character encoding
|
||||
* - LV_TXT_ENC_UTF8
|
||||
* - LV_TXT_ENC_ASCII
|
||||
* */
|
||||
#define LV_TXT_ENC LV_TXT_ENC_UTF8
|
||||
|
||||
/*Can break (wrap) texts on these chars*/
|
||||
#define LV_TXT_BREAK_CHARS " ,.;:-_"
|
||||
|
||||
/* If a word is at least this long, will break wherever "prettiest"
|
||||
* To disable, set to a value <= 0 */
|
||||
#define LV_TXT_LINE_BREAK_LONG_LEN 0
|
||||
|
||||
/* Minimum number of characters in a long word to put on a line before a break.
|
||||
* Depends on LV_TXT_LINE_BREAK_LONG_LEN. */
|
||||
#define LV_TXT_LINE_BREAK_LONG_PRE_MIN_LEN 3
|
||||
|
||||
/* Minimum number of characters in a long word to put on a line after a break.
|
||||
* Depends on LV_TXT_LINE_BREAK_LONG_LEN. */
|
||||
#define LV_TXT_LINE_BREAK_LONG_POST_MIN_LEN 3
|
||||
|
||||
/* The control character to use for signalling text recoloring. */
|
||||
#define LV_TXT_COLOR_CMD "#"
|
||||
|
||||
/* Support bidirectional texts.
|
||||
* Allows mixing Left-to-Right and Right-to-Left texts.
|
||||
* The direction will be processed according to the Unicode Bidirectional Algorithm:
|
||||
* https://www.w3.org/International/articles/inline-bidi-markup/uba-basics*/
|
||||
#define LV_USE_BIDI 0
|
||||
#if LV_USE_BIDI
|
||||
/* Set the default direction. Supported values:
|
||||
* `LV_BIDI_DIR_LTR` Left-to-Right
|
||||
* `LV_BIDI_DIR_RTL` Right-to-Left
|
||||
* `LV_BIDI_DIR_AUTO` detect texts base direction */
|
||||
#define LV_BIDI_BASE_DIR_DEF LV_BIDI_DIR_AUTO
|
||||
#endif
|
||||
|
||||
/* Enable Arabic/Persian processing
|
||||
* In these languages characters should be replaced with
|
||||
* an other form based on their position in the text */
|
||||
#define LV_USE_ARABIC_PERSIAN_CHARS 0
|
||||
|
||||
/*Change the built in (v)snprintf functions*/
|
||||
#define LV_SPRINTF_CUSTOM 0
|
||||
#if LV_SPRINTF_CUSTOM
|
||||
# define LV_SPRINTF_INCLUDE <stdio.h>
|
||||
# define lv_snprintf snprintf
|
||||
# define lv_vsnprintf vsnprintf
|
||||
#else /*!LV_SPRINTF_CUSTOM*/
|
||||
# define LV_SPRINTF_DISABLE_FLOAT 1
|
||||
#endif /*LV_SPRINTF_CUSTOM*/
|
||||
|
||||
/*===================
|
||||
* LV_OBJ SETTINGS
|
||||
*==================*/
|
||||
|
||||
#if LV_USE_USER_DATA
|
||||
/*Declare the type of the user data of object (can be e.g. `void *`, `int`, `struct`)*/
|
||||
typedef void * lv_obj_user_data_t;
|
||||
/*Provide a function to free user data*/
|
||||
#define LV_USE_USER_DATA_FREE 0
|
||||
#if LV_USE_USER_DATA_FREE
|
||||
# define LV_USER_DATA_FREE_INCLUDE "something.h" /*Header for user data free function*/
|
||||
/* Function prototype : void user_data_free(lv_obj_t * obj); */
|
||||
# define LV_USER_DATA_FREE (user_data_free) /*Invoking for user data free function*/
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/*1: enable `lv_obj_realign()` based on `lv_obj_align()` parameters*/
|
||||
#define LV_USE_OBJ_REALIGN 1
|
||||
|
||||
/* Enable to make the object clickable on a larger area.
|
||||
* LV_EXT_CLICK_AREA_OFF or 0: Disable this feature
|
||||
* LV_EXT_CLICK_AREA_TINY: The extra area can be adjusted horizontally and vertically (0..255 px)
|
||||
* LV_EXT_CLICK_AREA_FULL: The extra area can be adjusted in all 4 directions (-32k..+32k px)
|
||||
*/
|
||||
#define LV_USE_EXT_CLICK_AREA LV_EXT_CLICK_AREA_TINY
|
||||
|
||||
/*==================
|
||||
* LV OBJ X USAGE
|
||||
*================*/
|
||||
/*
|
||||
* Documentation of the object types: https://docs.lvgl.com/#Object-types
|
||||
*/
|
||||
|
||||
/*Arc (dependencies: -)*/
|
||||
#define LV_USE_ARC 1
|
||||
|
||||
/*Bar (dependencies: -)*/
|
||||
#define LV_USE_BAR 1
|
||||
|
||||
/*Button (dependencies: lv_cont*/
|
||||
#define LV_USE_BTN 1
|
||||
|
||||
/*Button matrix (dependencies: -)*/
|
||||
#define LV_USE_BTNMATRIX 1
|
||||
|
||||
/*Calendar (dependencies: -)*/
|
||||
#define LV_USE_CALENDAR 1
|
||||
#if LV_USE_CALENDAR
|
||||
# define LV_CALENDAR_WEEK_STARTS_MONDAY 0
|
||||
#endif
|
||||
|
||||
/*Canvas (dependencies: lv_img)*/
|
||||
#define LV_USE_CANVAS 1
|
||||
|
||||
/*Check box (dependencies: lv_btn, lv_label)*/
|
||||
#define LV_USE_CHECKBOX 1
|
||||
|
||||
/*Chart (dependencies: -)*/
|
||||
#define LV_USE_CHART 1
|
||||
#if LV_USE_CHART
|
||||
# define LV_CHART_AXIS_TICK_LABEL_MAX_LEN 256
|
||||
#endif
|
||||
|
||||
/*Container (dependencies: -*/
|
||||
#define LV_USE_CONT 1
|
||||
|
||||
/*Color picker (dependencies: -*/
|
||||
#define LV_USE_CPICKER 1
|
||||
|
||||
/*Drop down list (dependencies: lv_page, lv_label, lv_symbol_def.h)*/
|
||||
#define LV_USE_DROPDOWN 1
|
||||
#if LV_USE_DROPDOWN != 0
|
||||
/*Open and close default animation time [ms] (0: no animation)*/
|
||||
# define LV_DROPDOWN_DEF_ANIM_TIME 200
|
||||
#endif
|
||||
|
||||
/*Gauge (dependencies:lv_bar, lv_linemeter)*/
|
||||
#define LV_USE_GAUGE 1
|
||||
|
||||
/*Image (dependencies: lv_label*/
|
||||
#define LV_USE_IMG 1
|
||||
|
||||
/*Image Button (dependencies: lv_btn*/
|
||||
#define LV_USE_IMGBTN 1
|
||||
#if LV_USE_IMGBTN
|
||||
/*1: The imgbtn requires left, mid and right parts and the width can be set freely*/
|
||||
# define LV_IMGBTN_TILED 0
|
||||
#endif
|
||||
|
||||
/*Keyboard (dependencies: lv_btnm)*/
|
||||
#define LV_USE_KEYBOARD 1
|
||||
|
||||
/*Label (dependencies: -*/
|
||||
#define LV_USE_LABEL 1
|
||||
#if LV_USE_LABEL != 0
|
||||
/*Hor, or ver. scroll speed [px/sec] in 'LV_LABEL_LONG_ROLL/ROLL_CIRC' mode*/
|
||||
# define LV_LABEL_DEF_SCROLL_SPEED 25
|
||||
|
||||
/* Waiting period at beginning/end of animation cycle */
|
||||
# define LV_LABEL_WAIT_CHAR_COUNT 3
|
||||
|
||||
/*Enable selecting text of the label */
|
||||
# define LV_LABEL_TEXT_SEL 0
|
||||
|
||||
/*Store extra some info in labels (12 bytes) to speed up drawing of very long texts*/
|
||||
# define LV_LABEL_LONG_TXT_HINT 0
|
||||
#endif
|
||||
|
||||
/*LED (dependencies: -)*/
|
||||
#define LV_USE_LED 1
|
||||
#if LV_USE_LED
|
||||
# define LV_LED_BRIGHT_MIN 120 /*Minimal brightness*/
|
||||
# define LV_LED_BRIGHT_MAX 255 /*Maximal brightness*/
|
||||
#endif
|
||||
|
||||
/*Line (dependencies: -*/
|
||||
#define LV_USE_LINE 1
|
||||
|
||||
/*List (dependencies: lv_page, lv_btn, lv_label, (lv_img optionally for icons ))*/
|
||||
#define LV_USE_LIST 1
|
||||
#if LV_USE_LIST != 0
|
||||
/*Default animation time of focusing to a list element [ms] (0: no animation) */
|
||||
# define LV_LIST_DEF_ANIM_TIME 100
|
||||
#endif
|
||||
|
||||
/*Line meter (dependencies: *;)*/
|
||||
#define LV_USE_LINEMETER 1
|
||||
#if LV_USE_LINEMETER
|
||||
/* Draw line more precisely at cost of performance.
|
||||
* Useful if there are lot of lines any minor are visible
|
||||
* 0: No extra precision
|
||||
* 1: Some extra precision
|
||||
* 2: Best precision
|
||||
*/
|
||||
# define LV_LINEMETER_PRECISE 1
|
||||
#endif
|
||||
|
||||
/*Mask (dependencies: -)*/
|
||||
#define LV_USE_OBJMASK 1
|
||||
|
||||
/*Message box (dependencies: lv_rect, lv_btnm, lv_label)*/
|
||||
#define LV_USE_MSGBOX 1
|
||||
|
||||
/*Page (dependencies: lv_cont)*/
|
||||
#define LV_USE_PAGE 1
|
||||
#if LV_USE_PAGE != 0
|
||||
/*Focus default animation time [ms] (0: no animation)*/
|
||||
# define LV_PAGE_DEF_ANIM_TIME 400
|
||||
#endif
|
||||
|
||||
/*Preload (dependencies: lv_arc, lv_anim)*/
|
||||
#define LV_USE_SPINNER 1
|
||||
#if LV_USE_SPINNER != 0
|
||||
# define LV_SPINNER_DEF_ARC_LENGTH 60 /*[deg]*/
|
||||
# define LV_SPINNER_DEF_SPIN_TIME 1000 /*[ms]*/
|
||||
# define LV_SPINNER_DEF_ANIM LV_SPINNER_TYPE_SPINNING_ARC
|
||||
#endif
|
||||
|
||||
/*Roller (dependencies: lv_ddlist)*/
|
||||
#define LV_USE_ROLLER 1
|
||||
#if LV_USE_ROLLER != 0
|
||||
/*Focus animation time [ms] (0: no animation)*/
|
||||
# define LV_ROLLER_DEF_ANIM_TIME 200
|
||||
|
||||
/*Number of extra "pages" when the roller is infinite*/
|
||||
# define LV_ROLLER_INF_PAGES 7
|
||||
#endif
|
||||
|
||||
/*Slider (dependencies: lv_bar)*/
|
||||
#define LV_USE_SLIDER 1
|
||||
|
||||
/*Spinbox (dependencies: lv_ta)*/
|
||||
#define LV_USE_SPINBOX 1
|
||||
|
||||
/*Switch (dependencies: lv_slider)*/
|
||||
#define LV_USE_SWITCH 1
|
||||
|
||||
/*Text area (dependencies: lv_label, lv_page)*/
|
||||
#define LV_USE_TEXTAREA 1
|
||||
#if LV_USE_TEXTAREA != 0
|
||||
# define LV_TEXTAREA_DEF_CURSOR_BLINK_TIME 400 /*ms*/
|
||||
# define LV_TEXTAREA_DEF_PWD_SHOW_TIME 1500 /*ms*/
|
||||
#endif
|
||||
|
||||
/*Table (dependencies: lv_label)*/
|
||||
#define LV_USE_TABLE 1
|
||||
#if LV_USE_TABLE
|
||||
# define LV_TABLE_COL_MAX 12
|
||||
# define LV_TABLE_CELL_STYLE_CNT 4
|
||||
#endif
|
||||
|
||||
|
||||
/*Tab (dependencies: lv_page, lv_btnm)*/
|
||||
#define LV_USE_TABVIEW 1
|
||||
# if LV_USE_TABVIEW != 0
|
||||
/*Time of slide animation [ms] (0: no animation)*/
|
||||
# define LV_TABVIEW_DEF_ANIM_TIME 300
|
||||
#endif
|
||||
|
||||
/*Tileview (dependencies: lv_page) */
|
||||
#define LV_USE_TILEVIEW 1
|
||||
#if LV_USE_TILEVIEW
|
||||
/*Time of slide animation [ms] (0: no animation)*/
|
||||
# define LV_TILEVIEW_DEF_ANIM_TIME 300
|
||||
#endif
|
||||
|
||||
/*Window (dependencies: lv_cont, lv_btn, lv_label, lv_img, lv_page)*/
|
||||
#define LV_USE_WIN 1
|
||||
|
||||
/*==================
|
||||
* Non-user section
|
||||
*==================*/
|
||||
|
||||
#if defined(_MSC_VER) && !defined(_CRT_SECURE_NO_WARNINGS) /* Disable warnings for Visual Studio*/
|
||||
# define _CRT_SECURE_NO_WARNINGS
|
||||
#endif
|
||||
|
||||
/*--END OF LV_CONF_H--*/
|
||||
|
||||
#endif /*LV_CONF_H*/
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,119 @@
|
||||
/**
|
||||
* @file lv_settings.h
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef LV_SETTINGS_H
|
||||
#define LV_SETTINGS_H
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
/*********************
|
||||
* INCLUDES
|
||||
*********************/
|
||||
|
||||
#ifdef LV_LVGL_H_INCLUDE_SIMPLE
|
||||
#include "lvgl.h"
|
||||
#else
|
||||
#include "lvgl/lvgl.h"
|
||||
#endif
|
||||
|
||||
/*********************
|
||||
* DEFINES
|
||||
*********************/
|
||||
|
||||
/**********************
|
||||
* TYPEDEFS
|
||||
**********************/
|
||||
|
||||
typedef enum {
|
||||
LV_SETTINGS_TYPE_LIST_BTN,
|
||||
LV_SETTINGS_TYPE_BTN,
|
||||
LV_SETTINGS_TYPE_SW,
|
||||
LV_SETTINGS_TYPE_DDLIST,
|
||||
LV_SETTINGS_TYPE_NUMSET,
|
||||
LV_SETTINGS_TYPE_SLIDER,
|
||||
LV_SETTINGS_TYPE_EDIT,
|
||||
|
||||
LV_SETTINGS_TYPE_INV = 0xff,
|
||||
}lv_settings_type_t;
|
||||
|
||||
|
||||
#define LV_SETTINGS_EDIT_PWD 1
|
||||
|
||||
typedef struct {
|
||||
uint8_t id;
|
||||
lv_settings_type_t type;
|
||||
char * name; /*Name or title of the item*/
|
||||
char * value; /*The current value as string*/
|
||||
int32_t state; /*The current state, e.g. slider's value, switch state as a number */
|
||||
int32_t param1;
|
||||
int32_t param2;
|
||||
lv_obj_t * cont;
|
||||
union {
|
||||
void * ptr;
|
||||
int32_t int32;
|
||||
}user_data;
|
||||
}lv_settings_item_t;
|
||||
|
||||
|
||||
/**********************
|
||||
* GLOBAL PROTOTYPES
|
||||
**********************/
|
||||
|
||||
/**
|
||||
* Create a settings application
|
||||
* @param root_item descriptor of the settings button. For example:
|
||||
* `lv_settings_menu_item_t root_item = {.name = "Settings", .event_cb = root_event_cb};`
|
||||
* @return the created settings button
|
||||
*/
|
||||
lv_obj_t * lv_settings_create(lv_settings_item_t * root_item, lv_event_cb_t event_cb);
|
||||
|
||||
/**
|
||||
* Automatically add the item to a group to allow navigation with keypad or encoder.
|
||||
* Should be called before `lv_settings_create`
|
||||
* The group can be change at any time.
|
||||
* @param g the group to use. `NULL` to not use this feature.
|
||||
*/
|
||||
void lv_settings_set_group(lv_group_t * g);
|
||||
void lv_settings_set_parent(lv_obj_t* p);
|
||||
void lv_settings_back();
|
||||
bool lv_settings_reopen_last();
|
||||
|
||||
/**
|
||||
* Change the maximum width of settings dialog object
|
||||
* @param max_width maximum width of the settings container page
|
||||
*/
|
||||
void lv_settings_set_max_width(lv_coord_t max_width);
|
||||
|
||||
/**
|
||||
* Create a new page ask `event_cb` to add the item with `LV_EVENT_REFRESH`
|
||||
* @param parent_item pointer to an item which open the the new page. Its `name` will be the title
|
||||
* @param event_cb event handler of the menu page
|
||||
*/
|
||||
void lv_settings_open_page(lv_settings_item_t * parent_item, lv_event_cb_t event_cb);
|
||||
|
||||
/**
|
||||
* Add a list element to the page. With `item->name` and `item->value` texts.
|
||||
* @param page pointer to a menu page created by `lv_settings_create_page`
|
||||
*/
|
||||
void lv_settings_add(lv_settings_item_t * item);
|
||||
|
||||
|
||||
/**
|
||||
* Refresh an item's name value and state.
|
||||
* @param item pointer to a an `lv_settings_item _t` item.
|
||||
*/
|
||||
void lv_settings_refr(lv_settings_item_t * item);
|
||||
|
||||
/**********************
|
||||
* MACROS
|
||||
**********************/
|
||||
|
||||
#ifdef __cplusplus
|
||||
} /* extern "C" */
|
||||
#endif
|
||||
|
||||
#endif /*LV_TEMPL_H*/
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,11 @@
|
||||
#pragma once
|
||||
#include <stdlib.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif
|
||||
void *lvgl_malloc(size_t size);
|
||||
void lvgl_free(void *ptr);
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif
|
||||
@@ -0,0 +1,85 @@
|
||||
#include <Arduino.h>
|
||||
#include <freertos/FreeRTOS.h>
|
||||
#include <Wire.h>
|
||||
#include <SPI.h>
|
||||
#include <SPIFFS.h>
|
||||
#include <FS.h>
|
||||
#include "config.h"
|
||||
#include "global.h"
|
||||
#include "esp_task_wdt.h"
|
||||
#include <OneButton.h>
|
||||
#include "power.h"
|
||||
#include "atracker.h"
|
||||
#include <esp_spiffs.h>
|
||||
#include <EEPROM.h>
|
||||
|
||||
cAirsoftTracker *global = NULL;
|
||||
SemaphoreHandle_t debugLock = xSemaphoreCreateMutex();
|
||||
void setup() {
|
||||
//Hardware init
|
||||
esp_bt_controller_mem_release(ESP_BT_MODE_CLASSIC_BT);
|
||||
#ifdef AD_NOSCREEN
|
||||
// esp_bt_controller_mem_release(ESP_BT_MODE_BTDM);
|
||||
#endif
|
||||
Serial.setRxBufferSize(2048);
|
||||
Serial.begin(115200);
|
||||
Serial.setDebugOutput(false);
|
||||
EEPROM.begin(256);
|
||||
|
||||
FS.begin(true);
|
||||
|
||||
uint32_t seed = esp_random();
|
||||
DEBUG_MSG("Setting random seed %u\n", seed);
|
||||
randomSeed(seed); // ESP docs say this is fairly random
|
||||
|
||||
DEBUG_MSG("CPU Clock: %d\n", getCpuFrequencyMhz());
|
||||
DEBUG_MSG("Total heap: %d\n", ESP.getHeapSize());
|
||||
DEBUG_MSG("Free heap: %d\n", ESP.getFreeHeap());
|
||||
DEBUG_MSG("Total PSRAM: %d\n", ESP.getPsramSize());
|
||||
DEBUG_MSG("Free PSRAM: %d\n", ESP.getFreePsram());
|
||||
|
||||
|
||||
auto res = esp_task_wdt_init(APP_WATCHDOG_SECS_INIT, true);
|
||||
assert(res == ESP_OK);
|
||||
|
||||
res = esp_task_wdt_add(NULL);
|
||||
assert(res == ESP_OK);
|
||||
|
||||
esp_vfs_spiffs_conf_t spiffs_1 = {
|
||||
.base_path = "/spiffs",
|
||||
.partition_label = "spiffs",
|
||||
.max_files = 32,
|
||||
.format_if_mount_failed = true
|
||||
};
|
||||
|
||||
esp_vfs_spiffs_conf_t spiffs_2 = {
|
||||
.base_path = "/config",
|
||||
.partition_label = "config",
|
||||
.max_files = 32,
|
||||
.format_if_mount_failed = true
|
||||
};
|
||||
SPIFFS.end();
|
||||
if(esp_vfs_spiffs_register(&spiffs_1)!=ESP_OK || esp_vfs_spiffs_register(&spiffs_2)!=ESP_OK){
|
||||
DEBUG_MSG("An Error has occurred while mounting SPIFFS\n");
|
||||
}
|
||||
|
||||
size_t total = 0;
|
||||
size_t used = 0;
|
||||
if(esp_spiffs_info(spiffs_1.partition_label, &total, &used) == ESP_OK)
|
||||
{
|
||||
DEBUG_MSG("SPIFFS %s used %d, total %d\n",spiffs_1.partition_label, used, total);
|
||||
}
|
||||
if(esp_spiffs_info(spiffs_2.partition_label, &total, &used) == ESP_OK)
|
||||
{
|
||||
DEBUG_MSG("SPIFFS %s used %d, total %d\n",spiffs_2.partition_label, used, total);
|
||||
}
|
||||
|
||||
global = new cAirsoftTracker();
|
||||
global->init();
|
||||
esp_task_wdt_delete(NULL);
|
||||
}
|
||||
|
||||
void loop()
|
||||
{
|
||||
delay(1000);
|
||||
}
|
||||
@@ -0,0 +1,400 @@
|
||||
#include "mapTiles.h"
|
||||
#include <fstream>
|
||||
#include <algorithm>
|
||||
#include "config.h"
|
||||
#include "atcommon.h"
|
||||
|
||||
void sMapTile::clear()
|
||||
{
|
||||
memset(this,0,sizeof(sMapTile));
|
||||
}
|
||||
|
||||
bool sMapTile::loadFromJson(const JsonVariant &json, uint32_t baseAddress)
|
||||
{
|
||||
if(json.containsKey("size")&&json.containsKey("bounds")&&json.containsKey("x")&&json.containsKey("y")
|
||||
&&json.containsKey("address")&&json.containsKey("image_stride")&&json.containsKey("image_size")&&json.containsKey("image_fmt"))
|
||||
{
|
||||
sizeX = json["size"][0];
|
||||
sizeY = json["size"][1];
|
||||
address = json["address"];
|
||||
address += baseAddress;
|
||||
size = json["image_size"];
|
||||
stride = json["image_stride"];
|
||||
fmt = json["image_fmt"];
|
||||
tileX = json["x"];
|
||||
tileY = json["y"];
|
||||
bound.x0 = json["bounds"][0].as<double>();
|
||||
bound.y0 = json["bounds"][1].as<double>();
|
||||
bound.x1 = json["bounds"][2].as<double>();
|
||||
bound.y1 = json["bounds"][3].as<double>();
|
||||
mapSizeX = bound.x1-bound.x0;
|
||||
mapSizeY = bound.y0-bound.y1;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
cMapZoomLevel::cMapZoomLevel()
|
||||
{
|
||||
clear();
|
||||
}
|
||||
|
||||
void cMapZoomLevel::clear()
|
||||
{
|
||||
memset(&m_zlevel,0,sizeof(m_zlevel));
|
||||
m_images.clear();
|
||||
}
|
||||
|
||||
void cMapZoomLevel::loadFromBinary(std::ifstream &file)
|
||||
{
|
||||
uint16_t cnt = 0;
|
||||
file.read((char*)&m_zlevel,sizeof(m_zlevel));
|
||||
file.read((char*)&cnt,2);
|
||||
m_images.resize(cnt);
|
||||
file.read((char*)m_images.data(),cnt*sizeof(sMapTile));
|
||||
}
|
||||
|
||||
bool cMapZoomLevel::loadFromJson(const JsonVariant &json, uint32_t baseAddress)
|
||||
{
|
||||
if(json.containsKey("pixel_size")&&json.containsKey("bounds")&&json.containsKey("images")&&json.containsKey("tile_size")&&json.containsKey("id"))
|
||||
{
|
||||
clear();
|
||||
m_zlevel.id = json["id"];
|
||||
m_zlevel.pixelSizeX = json["pixel_size"][0].as<double>();
|
||||
m_zlevel.pixelSizeY = json["pixel_size"][1].as<double>();
|
||||
m_zlevel.tileSizeX = json["tile_size"][0].as<double>();
|
||||
m_zlevel.tileSizeY = json["tile_size"][1].as<double>();
|
||||
m_zlevel.bound.x0 = json["bounds"][0].as<double>();
|
||||
m_zlevel.bound.y0 = json["bounds"][1].as<double>();
|
||||
m_zlevel.bound.x1 = json["bounds"][2].as<double>();
|
||||
m_zlevel.bound.y1 = json["bounds"][3].as<double>();
|
||||
const JsonVariant &images = json.getMember("images");
|
||||
m_images.resize(images.size());
|
||||
for(int i=0; i<images.size(); i++)
|
||||
{
|
||||
const JsonVariant &imageJson = images.getElement(i);
|
||||
m_images[i].loadFromJson(imageJson,baseAddress);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
cMapTiles &cMapZoomLevel::getImages()
|
||||
{
|
||||
return m_images;
|
||||
}
|
||||
|
||||
cMap::cMap()
|
||||
{
|
||||
clear();
|
||||
}
|
||||
|
||||
void cMap::clear()
|
||||
{
|
||||
m_map.name[0] = 0;
|
||||
m_map.bound.x0 = m_map.bound.x1 = m_map.bound.y0 = m_map.bound.y1 = 0;
|
||||
m_map.baseAddress = 0;
|
||||
m_zoomLevels.clear();
|
||||
m_loaded = false;
|
||||
}
|
||||
|
||||
void cMap::loadHeader(std::ifstream &file,const char *fileName)
|
||||
{
|
||||
m_binaryFileName = fileName;
|
||||
m_loaded = false;
|
||||
file.read((char*)&m_map,sizeof(sMap));
|
||||
}
|
||||
|
||||
void cMap::loadMapData()
|
||||
{
|
||||
m_loaded = true;
|
||||
m_zoomLevels.clear();
|
||||
try{
|
||||
DEBUG_MSG("Load map data %s\n",m_map.name);
|
||||
std::ifstream file(m_binaryFileName,std::ifstream::binary);
|
||||
if(file.good())
|
||||
{
|
||||
file.seekg(m_map.filePosition);
|
||||
byte cnt = 0;
|
||||
file.read((char*)&cnt,1);
|
||||
m_zoomLevels.resize(cnt);
|
||||
for(int i=0; i<cnt; i++)
|
||||
{
|
||||
cMapZoomLevel &zl = m_zoomLevels[i];
|
||||
zl.loadFromBinary(file);
|
||||
}
|
||||
DEBUG_MSG("Loaded Zoom Levels %d\n",m_zoomLevels.size());
|
||||
}
|
||||
else
|
||||
DEBUG_MSG("Load map error\n");
|
||||
}catch(...)
|
||||
{
|
||||
DEBUG_MSG("Can't load map %s\n",m_map.name);
|
||||
}
|
||||
}
|
||||
|
||||
bool cMap::loadFromJson(const JsonVariant &json)
|
||||
{
|
||||
if(json.containsKey("name")&&json.containsKey("bounds")&&json.containsKey("tile_matrix")&&json.containsKey("base_address"))
|
||||
{
|
||||
clear();
|
||||
m_loaded = true;
|
||||
strncpy(m_map.name,(const char *) json["name"],sizeof(m_map.name)-1);
|
||||
m_map.baseAddress = json["base_address"].as<uint32_t>();
|
||||
m_map.bound.x0 = json["bounds"][0].as<double>();
|
||||
m_map.bound.y0 = json["bounds"][1].as<double>();
|
||||
m_map.bound.x1 = json["bounds"][2].as<double>();
|
||||
m_map.bound.y1 = json["bounds"][3].as<double>();
|
||||
const JsonVariant &zls = json.getMember("tile_matrix");
|
||||
for(int i=0; i<zls.size(); i++)
|
||||
{
|
||||
cMapZoomLevel zl;
|
||||
const JsonVariant &zlJson = zls.getElement(i);
|
||||
if(zl.loadFromJson(zlJson,m_map.baseAddress))
|
||||
{
|
||||
m_zoomLevels.push_back(zl);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
cMaps::cMaps()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
/*
|
||||
void cMaps::convertJsonToBinary(const char *filename)
|
||||
{
|
||||
char fileNameBuffer[32];
|
||||
snprintf(fileNameBuffer, sizeof(fileNameBuffer),"%ssflash.json",filename);
|
||||
std::ifstream file(fileNameBuffer,std::ifstream::binary);
|
||||
if(file)
|
||||
{
|
||||
DEBUG_MSG("Load maps from %s\n",fileNameBuffer);
|
||||
DynamicJsonDocument json(128*1024);
|
||||
std::ifstream file(fileNameBuffer,std::ifstream::binary);
|
||||
DeserializationError error = deserializeJson(json,file);
|
||||
if(!error)
|
||||
{
|
||||
char mapFile[32];
|
||||
for(int i=0; i<json.size(); i++)
|
||||
{
|
||||
const JsonVariant &mapJson = json.getElement(i);
|
||||
const JsonVariant &zls = mapJson.getMember("tile_matrix");
|
||||
snprintf(mapFile, sizeof(mapFile),"%smap%d.jbin",filename,i);
|
||||
std::ofstream file(mapFile,std::ofstream::binary);
|
||||
serializeMsgPack(zls,file);
|
||||
DEBUG_MSG("Save map %d to %s\n",i, mapFile);
|
||||
mapJson["tile_matrix"] = mapFile;
|
||||
}
|
||||
snprintf(mapFile, sizeof(mapFile),"%smaps.jbin",filename);
|
||||
std::ofstream file(mapFile,std::ofstream::binary);
|
||||
serializeMsgPack(json,file);
|
||||
remove(fileNameBuffer);
|
||||
}
|
||||
else
|
||||
DEBUG_MSG("Load maps error %s\n",error.c_str());
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
bool cMaps::loadFromBinary(const char *filename)
|
||||
{
|
||||
m_maps.clear();
|
||||
try{
|
||||
DEBUG_MSG("Load maps from %s\n",filename);
|
||||
std::ifstream file(filename,std::ifstream::binary);
|
||||
if(file.good())
|
||||
{
|
||||
byte cnt = 0;
|
||||
file.read((char*)&cnt,1);
|
||||
if(file)
|
||||
{
|
||||
m_maps.reserve(cnt);
|
||||
for(int i=0; i<cnt; i++)
|
||||
{
|
||||
cMap map;
|
||||
map.loadHeader(file,filename);
|
||||
m_maps.push_back(map);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else
|
||||
DEBUG_MSG("Load maps error\n");
|
||||
}catch(...)
|
||||
{
|
||||
DEBUG_MSG("Can't load maps from %s\n",filename);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool cMaps::loadFromJson(const char *filename)
|
||||
{
|
||||
m_maps.clear();
|
||||
try{
|
||||
DEBUG_MSG("Load maps from %s\n",filename);
|
||||
DynamicJsonDocument json(156*1024);
|
||||
std::ifstream file(filename,std::ifstream::binary);
|
||||
DeserializationError error = deserializeJson(json,file);
|
||||
if(!error)
|
||||
{
|
||||
for(int i=0; i<json.size(); i++)
|
||||
{
|
||||
cMap map;
|
||||
const JsonVariant &mapJson = json.getElement(i);
|
||||
if(map.loadFromJson(mapJson))
|
||||
{
|
||||
m_maps.push_back(map);
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
else
|
||||
DEBUG_MSG("Load maps error %s\n",error.c_str());
|
||||
}catch(...)
|
||||
{
|
||||
DEBUG_MSG("Can't load maps from %s\n",filename);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
cMapProcessor::cMapProcessor(cMapsList &maps,cMapProcessorCallbacks *callbacks):m_maps(maps),m_lastMap(NULL),m_callbacks(callbacks)
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
sBoundaries cMapProcessor::getScreenBoundaries(double x, double y, double radius) const
|
||||
{
|
||||
sBoundaries result;
|
||||
result.x0 = x-radius;
|
||||
result.y0 = y+radius;
|
||||
result.x1 = x+radius;
|
||||
result.y1 = y-radius;
|
||||
return result;
|
||||
}
|
||||
|
||||
double distance(double x1, double y1, double x2, double y2)
|
||||
{
|
||||
return sqrt(pow(x2 - x1, 2) +
|
||||
pow(y2 - y1, 2) * 1.0);
|
||||
}
|
||||
|
||||
void cMapProcessor::clearUnusedMaps(double x, double y)
|
||||
{
|
||||
for(auto itr=m_maps.begin(); itr!=m_maps.end();)
|
||||
{
|
||||
if(&(*itr)!=m_lastMap)
|
||||
{
|
||||
auto b = (*itr).getMap().bound;
|
||||
if(distance(x,y,b.x0,b.y0)>=0000)
|
||||
{
|
||||
itr = m_maps.erase(itr);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
itr++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
cMapTilesList cMapProcessor::getTilesToRender(double x, double y, double radius)
|
||||
{
|
||||
cMapTilesList result;
|
||||
cMap *map = NULL;
|
||||
if(m_lastMap)
|
||||
{
|
||||
if(checkMap(x,y,*m_lastMap))
|
||||
{
|
||||
map = m_lastMap;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_lastMap = NULL;
|
||||
}
|
||||
}
|
||||
if(!map)
|
||||
{
|
||||
for(auto itr=m_maps.begin(); itr!=m_maps.end(); itr++)
|
||||
{
|
||||
if(checkMap(x,y,*itr))
|
||||
{
|
||||
m_lastMap = &*itr;
|
||||
map = m_lastMap;
|
||||
}
|
||||
}
|
||||
// if(map)
|
||||
// clearUnusedMaps(x,y);
|
||||
}
|
||||
if(map)
|
||||
{
|
||||
if(!map->isLoaded())
|
||||
{
|
||||
map->loadMapData();
|
||||
if(m_callbacks)
|
||||
m_callbacks->onMapLoad(map);
|
||||
}
|
||||
double zoomDiff = 0;
|
||||
cMapZoomLevel *zoomLevel = NULL;
|
||||
cMapZoomLevels &zls = map->getMapZoomLevels();
|
||||
for(auto itr=zls.begin(); itr!=zls.end(); itr++)
|
||||
{
|
||||
sMapZoomLevel &zl = itr->getZoomLevel();
|
||||
double zoomSize = std::max(abs(zl.tileSizeX),abs(zl.tileSizeY))/2;
|
||||
if(!zoomLevel || abs(zoomSize-radius)<zoomDiff)
|
||||
{
|
||||
zoomDiff = abs(zoomSize-radius);
|
||||
zoomLevel = &*itr;
|
||||
}
|
||||
}
|
||||
if(zoomLevel)
|
||||
{
|
||||
cMapTiles &tiles = zoomLevel->getImages();
|
||||
sBoundaries sb = getScreenBoundaries(x,y,radius);
|
||||
for(auto itr=tiles.begin(); itr !=tiles.end(); itr++)
|
||||
{
|
||||
if(checkIntersection(sb,itr->bound))
|
||||
{
|
||||
result.push_back(*itr);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
bool cMapProcessor::checkMap(const double &x, const double &y,cMap &map)
|
||||
{
|
||||
return isPointInside(x,y,map.getMap().bound);
|
||||
}
|
||||
|
||||
bool cMapProcessor::checkIntersection(const sBoundaries &b1, const sBoundaries &b2) const
|
||||
{
|
||||
return isPointInside(b1.x0,b1.y0,b2) || isPointInside(b1.x1,b1.y0,b2) || isPointInside(b1.x0,b1.y1,b2) || isPointInside(b1.x1,b1.y1,b2) ||
|
||||
isPointInside(b2.x0,b2.y0,b1) || isPointInside(b2.x1,b2.y0,b1) || isPointInside(b2.x0,b2.y1,b1) || isPointInside(b2.x1,b2.y1,b1);
|
||||
}
|
||||
|
||||
void cMapProcessor::calcTile(double x, double y, const sMapTile &tile, double radius, double heading, double &scaleX, double &scaleY, double &tx, double &ty)
|
||||
{
|
||||
scaleX = (tile.mapSizeX)/(radius*2);
|
||||
scaleY = (tile.mapSizeY)/(radius*2);
|
||||
tx = (tile.bound.x0-x)*tile.sizeX/tile.mapSizeX;//*(240/(240+3/scaleX));
|
||||
ty = (y-tile.bound.y0)*tile.sizeY/tile.mapSizeY;//*(240/(240+3/scaleY));
|
||||
}
|
||||
|
||||
double cMapProcessor::calcDistanceC(double x, double y)
|
||||
{
|
||||
double lat1,lng1;
|
||||
double lat2,lng2;
|
||||
convertMeterToDegree(x,y,lng1,lat1);
|
||||
convertMeterToDegree(x,y+1000,lng2,lat2);
|
||||
double d = distanceBetween(lat1, lng1, lat2, lng2);
|
||||
if(d>0)
|
||||
return abs(1000/d);
|
||||
else
|
||||
return 1;
|
||||
}
|
||||
+150
@@ -0,0 +1,150 @@
|
||||
#pragma once
|
||||
#define ARDUINOJSON_USE_DOUBLE 1
|
||||
#include <Arduino.h>
|
||||
#include <ArduinoJson.h>
|
||||
#include <stdint.h>
|
||||
#include <list>
|
||||
#include <map>
|
||||
#include <vector>
|
||||
|
||||
#pragma pack(push, 1)
|
||||
//16
|
||||
struct sBoundaries
|
||||
{
|
||||
float x0;
|
||||
float y0;
|
||||
float x1;
|
||||
float y1;
|
||||
};
|
||||
|
||||
//44
|
||||
struct sMapTile
|
||||
{
|
||||
uint32_t address;
|
||||
uint32_t size;
|
||||
uint16_t sizeX;
|
||||
uint16_t sizeY;
|
||||
uint16_t stride;
|
||||
uint16_t fmt;
|
||||
uint16_t tileX;
|
||||
uint16_t tileY;
|
||||
float mapSizeX;
|
||||
float mapSizeY;
|
||||
sBoundaries bound;
|
||||
sMapTile() { clear(); }
|
||||
void clear();
|
||||
bool loadFromJson(const JsonVariant &json, uint32_t baseAddress);
|
||||
};
|
||||
|
||||
//34
|
||||
struct sMapZoomLevel
|
||||
{
|
||||
uint16_t id;
|
||||
sBoundaries bound;
|
||||
float pixelSizeX;
|
||||
float pixelSizeY;
|
||||
float tileSizeX;
|
||||
float tileSizeY;
|
||||
};
|
||||
|
||||
typedef std::vector<sMapTile> cMapTiles;
|
||||
typedef std::list<sMapTile> cMapTilesList;
|
||||
class cMapZoomLevel
|
||||
{
|
||||
protected:
|
||||
sMapZoomLevel m_zlevel;
|
||||
cMapTiles m_images;
|
||||
public:
|
||||
cMapZoomLevel();
|
||||
|
||||
sMapZoomLevel &getZoomLevel()
|
||||
{
|
||||
return m_zlevel;
|
||||
}
|
||||
void clear();
|
||||
bool loadFromJson(const JsonVariant &json, uint32_t baseAddress);
|
||||
void loadFromBinary(std::ifstream &file);
|
||||
cMapTiles &getImages();
|
||||
};
|
||||
|
||||
//60
|
||||
struct sMap
|
||||
{
|
||||
char name[32];
|
||||
sBoundaries bound;
|
||||
uint32_t baseAddress;
|
||||
uint32_t filePosition;
|
||||
uint32_t size;
|
||||
};
|
||||
|
||||
typedef std::vector<cMapZoomLevel> cMapZoomLevels;
|
||||
class cMap
|
||||
{
|
||||
protected:
|
||||
sMap m_map;
|
||||
cMapZoomLevels m_zoomLevels;
|
||||
bool m_loaded;
|
||||
std::string m_binaryFileName;
|
||||
public:
|
||||
cMap();
|
||||
sMap &getMap()
|
||||
{
|
||||
return m_map;
|
||||
}
|
||||
void clear();
|
||||
bool loadFromJson(const JsonVariant &json);
|
||||
cMapZoomLevels &getMapZoomLevels()
|
||||
{
|
||||
return m_zoomLevels;
|
||||
}
|
||||
void loadHeader(std::ifstream &file,const char *fileName);
|
||||
void loadMapData();
|
||||
bool isLoaded() { return m_loaded; }
|
||||
};
|
||||
|
||||
|
||||
typedef std::vector<cMap> cMapsList;
|
||||
class cMaps
|
||||
{
|
||||
protected:
|
||||
cMapsList m_maps;
|
||||
public:
|
||||
cMaps();
|
||||
cMapsList &getMaps()
|
||||
{
|
||||
return m_maps;
|
||||
}
|
||||
bool loadFromJson(const char *filename);
|
||||
bool loadFromBinary(const char *filename);
|
||||
bool loadMapData(sMap *m_map);
|
||||
};
|
||||
|
||||
class cMapProcessorCallbacks
|
||||
{
|
||||
public:
|
||||
virtual void onMapLoad(cMap *map)=0;
|
||||
};
|
||||
|
||||
class cMapProcessor
|
||||
{
|
||||
protected:
|
||||
cMapsList &m_maps;
|
||||
cMap *m_lastMap;
|
||||
cMapProcessorCallbacks *m_callbacks;
|
||||
bool checkMap(const double &x, const double &y,cMap &map);
|
||||
void clearUnusedMaps(double x, double y);
|
||||
public:
|
||||
cMapProcessor(cMapsList &maps,cMapProcessorCallbacks *callbacks);
|
||||
sBoundaries getScreenBoundaries(double x, double y, double radius) const;
|
||||
cMapTilesList getTilesToRender(double x, double y, double radius);
|
||||
bool checkIntersection(const sBoundaries &b1, const sBoundaries &b2) const;
|
||||
bool isPointInside(const double &x, const double &y,const sBoundaries &b) const
|
||||
{
|
||||
return (x>=b.x0 && x<=b.x1 && y>=b.y1 && y<=b.y0);
|
||||
}
|
||||
|
||||
double calcDistanceC(double x, double y);
|
||||
void calcTile(double x, double y, const sMapTile &tile, double radius, double heading, double &scaleX, double &scaleY, double &tx, double &ty);
|
||||
};
|
||||
|
||||
#pragma pack(pop)
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user