Luca Antolini
1st Fork
Dependencies: mbed QEI DmTftLibrary
Diff: Demo/DemoApplication.cpp
- Revision:
- 8:c0a5f53fe196
- Parent:
- 7:bc2f0fe8056f
- Child:
- 9:0efc2c99078f
--- a/Demo/DemoApplication.cpp Wed Apr 19 14:21:05 2017 +0000
+++ b/Demo/DemoApplication.cpp Thu May 11 15:42:05 2017 +0000
@@ -13,12 +13,23 @@
*/
#include "mbed.h"
+#include <math.h>
#include "radio.h"
#include "sx1280-hal.h"
#include "Eeprom.h"
#include "DemoApplication.h"
#include "FreqLUT.h"
+double t0 = -0.016432807883697; // X0
+double t1 = 0.323147003165358; // X1
+double t2 = 0.014922061351196; // X1^2
+double t3 = 0.000137832006285; // X1^3
+double t4 = 0.536873856625399; // X2
+double t5 = 0.040890089178579; // X2^2
+double t6 = -0.001074801048732; // X2^3
+double t7 = 0.000009240142234; // X2^4
+
+
/*!
* \brief Defines the local payload buffer size
@@ -74,9 +85,11 @@
uint8_t Buffer[BUFFER_SIZE];
static uint8_t CurrentChannel;
-static uint8_t MeasuredChannels;
+static uint16_t MeasuredChannels;
int RngResultIndex;
double RawRngResults[DEMO_RNG_CHANNELS_COUNT_MAX];
+double RssiRng[DEMO_RNG_CHANNELS_COUNT_MAX];
+
/*!
* \brief Function to be executed on Radio Tx Done event
@@ -969,8 +982,8 @@
if( Eeprom.EepromData.DemoSettings.Entity == MASTER )
{
Eeprom.EepromData.DemoSettings.InterPacketDelay = RefreshScreenDelayMs;
- Radio.SetDioIrqParams( IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_RANGING_MASTER_RESULT_VALID | IRQ_RANGING_MASTER_RESULT_TIMEOUT,
- IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_RANGING_MASTER_RESULT_VALID | IRQ_RANGING_MASTER_RESULT_TIMEOUT,
+ Radio.SetDioIrqParams( IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_RANGING_MASTER_RESULT_VALID | IRQ_RANGING_MASTER_TIMEOUT,
+ IRQ_RX_DONE | IRQ_TX_DONE | IRQ_RX_TX_TIMEOUT | IRQ_RANGING_MASTER_RESULT_VALID | IRQ_RANGING_MASTER_TIMEOUT,
IRQ_RADIO_NONE, IRQ_RADIO_NONE );
Eeprom.EepromData.DemoSettings.RngDistance = 0.0;
DemoInternalState = APP_RANGING_CONFIG;
@@ -999,7 +1012,7 @@
memcpy( &( PacketParams.Params.LoRa.PreambleLength ), Eeprom.Buffer + PAK_RNG_PREAMBLE_LEN_EEPROM_ADDR, 1 );
memcpy( &( PacketParams.Params.LoRa.HeaderType ), Eeprom.Buffer + PAK_RNG_HEADERTYPE_EEPROM_ADDR, 1 );
PacketParams.Params.LoRa.PayloadLength = 7;
- memcpy( &( PacketParams.Params.LoRa.CrcMode ), Eeprom.Buffer + PAK_RNG_CRC_MODE_EEPROM_ADDR, 1 );
+ memcpy( &( PacketParams.Params.LoRa.Crc ), Eeprom.Buffer + PAK_RNG_CRC_MODE_EEPROM_ADDR, 1 );
memcpy( &( PacketParams.Params.LoRa.InvertIQ ), Eeprom.Buffer + PAK_RNG_IQ_INV_EEPROM_ADDR, 1 );
Radio.SetPacketType( ModulationParams.PacketType );
Radio.SetModulationParams( &ModulationParams );
@@ -1122,7 +1135,7 @@
memcpy( &( PacketParams.Params.LoRa.PreambleLength ), Eeprom.Buffer + PAK_RNG_PREAMBLE_LEN_EEPROM_ADDR, 1 );
memcpy( &( PacketParams.Params.LoRa.HeaderType ), Eeprom.Buffer + PAK_RNG_HEADERTYPE_EEPROM_ADDR, 1 );
PacketParams.Params.LoRa.PayloadLength = 10;
- memcpy( &( PacketParams.Params.LoRa.CrcMode ), Eeprom.Buffer + PAK_RNG_CRC_MODE_EEPROM_ADDR, 1 );
+ memcpy( &( PacketParams.Params.LoRa.Crc ), Eeprom.Buffer + PAK_RNG_CRC_MODE_EEPROM_ADDR, 1 );
memcpy( &( PacketParams.Params.LoRa.InvertIQ ), Eeprom.Buffer + PAK_RNG_IQ_INV_EEPROM_ADDR, 1 );
Radio.SetPacketType( ModulationParams.PacketType );
@@ -1174,12 +1187,14 @@
RX_LED = 0;
DemoInternalState = APP_RANGING_CONFIG;
Eeprom.EepromData.DemoSettings.HoldDemo = true;
+ refreshDisplay = 1; // display error on token color (RNG_TIMEOUT)
break;
case APP_TX_TIMEOUT:
TX_LED = 0;
DemoInternalState = APP_RANGING_CONFIG;
Eeprom.EepromData.DemoSettings.HoldDemo = true;
+ refreshDisplay = 1; // display error on token color (RNG_TIMEOUT)
break;
case APP_IDLE: // do nothing
@@ -1205,7 +1220,7 @@
memcpy( &( PacketParams.Params.LoRa.PreambleLength ), Eeprom.Buffer + PAK_RNG_PREAMBLE_LEN_EEPROM_ADDR, 1 );
memcpy( &( PacketParams.Params.LoRa.HeaderType ), Eeprom.Buffer + PAK_RNG_HEADERTYPE_EEPROM_ADDR, 1 );
PacketParams.Params.LoRa.PayloadLength = 9;
- memcpy( &( PacketParams.Params.LoRa.CrcMode ), Eeprom.Buffer + PAK_RNG_CRC_MODE_EEPROM_ADDR, 1 );
+ memcpy( &( PacketParams.Params.LoRa.Crc ), Eeprom.Buffer + PAK_RNG_CRC_MODE_EEPROM_ADDR, 1 );
memcpy( &( PacketParams.Params.LoRa.InvertIQ ), Eeprom.Buffer + PAK_RNG_IQ_INV_EEPROM_ADDR, 1 );
Radio.SetPacketType( ModulationParams.PacketType );
Radio.SetModulationParams( &ModulationParams );
@@ -1229,7 +1244,8 @@
switch( Eeprom.EepromData.DemoSettings.RngAntenna )
{
case DEMO_RNG_ANT_1:
- ANT_SW = 1; // ANT1
+ //ANT_SW = 1; // ANT1
+ Eeprom.EepromData.DemoSettings.AntennaSwitch = 0;
CurrentChannel++;
if( CurrentChannel >= CHANNELS )
{
@@ -1238,7 +1254,8 @@
break;
case DEMO_RNG_ANT_2:
- ANT_SW = 0; // ANT2
+ //ANT_SW = 0; // ANT2
+ Eeprom.EepromData.DemoSettings.AntennaSwitch = 1;
CurrentChannel++;
if( CurrentChannel >= CHANNELS )
{
@@ -1249,11 +1266,13 @@
case DEMO_RNG_ANT_BOTH:
if( ANT_SW == 1 )
{
- ANT_SW = 0;
+ //ANT_SW = 0;
+ Eeprom.EepromData.DemoSettings.AntennaSwitch = 1;
}
else
{
- ANT_SW = 1;
+ //ANT_SW = 1;
+ Eeprom.EepromData.DemoSettings.AntennaSwitch = 0;
CurrentChannel++;
if( CurrentChannel >= CHANNELS )
{
@@ -1337,7 +1356,7 @@
memcpy( &( PacketParams.Params.LoRa.PreambleLength ), Eeprom.Buffer + PAK_RNG_PREAMBLE_LEN_EEPROM_ADDR, 1 );
memcpy( &( PacketParams.Params.LoRa.HeaderType ), Eeprom.Buffer + PAK_RNG_HEADERTYPE_EEPROM_ADDR, 1 );
PacketParams.Params.LoRa.PayloadLength = 10;
- memcpy( &( PacketParams.Params.LoRa.CrcMode ), Eeprom.Buffer + PAK_RNG_CRC_MODE_EEPROM_ADDR, 1 );
+ memcpy( &( PacketParams.Params.LoRa.Crc ), Eeprom.Buffer + PAK_RNG_CRC_MODE_EEPROM_ADDR, 1 );
memcpy( &( PacketParams.Params.LoRa.InvertIQ ), Eeprom.Buffer + PAK_RNG_IQ_INV_EEPROM_ADDR, 1 );
Radio.SetPacketType( ModulationParams.PacketType );
@@ -1501,7 +1520,7 @@
double tPreamble = ( Eeprom.EepromData.PacketParams.Params.LoRa.PreambleLength + 4.25 ) * ts; // time of preamble
uint8_t de = 1; // always 1 on SX1280. "low data rate optimization" condition.
double tmp = ceil( ( 8 * Eeprom.EepromData.PacketParams.Params.LoRa.PayloadLength - 4 * ( Eeprom.EepromData.ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) +
- 28 + 16 * ( ( Eeprom.EepromData.PacketParams.Params.LoRa.CrcMode == 0x00 ) ? 0 : 1 ) - \
+ 28 + 16 * ( ( Eeprom.EepromData.PacketParams.Params.LoRa.Crc == 0x00 ) ? 0 : 1 ) - \
( ( Eeprom.EepromData.PacketParams.Params.LoRa.HeaderType >> 7 ) ? 20 : 0 ) ) / \
( double )( 4 * ( ( Eeprom.EepromData.ModulationParams.Params.LoRa.SpreadingFactor >> 4 ) - ( de * 2 ) ) ) * \
( ( Eeprom.EepromData.ModulationParams.Params.LoRa.CodingRate % 4 ) + 4 ) ); // Symbol length of payload and time
@@ -1724,7 +1743,7 @@
PacketParams.Params.LoRa.PreambleLength = Eeprom.EepromData.DemoSettings.PacketParam1;
PacketParams.Params.LoRa.HeaderType = ( RadioLoRaPacketLengthsModes_t )Eeprom.EepromData.DemoSettings.PacketParam2;
PacketParams.Params.LoRa.PayloadLength = Eeprom.EepromData.DemoSettings.PacketParam3;
- PacketParams.Params.LoRa.CrcMode = ( RadioLoRaCrcModes_t ) Eeprom.EepromData.DemoSettings.PacketParam4;
+ PacketParams.Params.LoRa.Crc = ( RadioLoRaCrcModes_t ) Eeprom.EepromData.DemoSettings.PacketParam4;
PacketParams.Params.LoRa.InvertIQ = ( RadioLoRaIQModes_t ) Eeprom.EepromData.DemoSettings.PacketParam5;
Eeprom.EepromData.DemoSettings.PayloadLength = PacketParams.Params.LoRa.PayloadLength;
@@ -1805,7 +1824,8 @@
// only used in GFSK, FLRC (4 bytes max) and BLE mode
Radio.SetSyncWord( 1, ( uint8_t[] ){ 0xDD, 0xA0, 0x96, 0x69, 0xDD } );
// only used in GFSK, FLRC
- Radio.SetCrcSeed( 0x4567 );
+ uint8_t crcSeedLocal[3] = { 0x00, 0x45, 0x67 };
+ Radio.SetCrcSeed( crcSeedLocal );
Radio.SetCrcPolynomial( 0x0123 );
Radio.SetTxParams( Eeprom.EepromData.DemoSettings.TxPower, RADIO_RAMP_20_US );
}
@@ -1826,78 +1846,46 @@
uint8_t CheckDistance( void )
{
double mean = 0.0;
- double meanRaw = 0.0;
double total = 0.0;
- double squareDeviation = 0.0;
- double squareDeviationRaw = 0.0;
- uint8_t j = 0;
- uint8_t i;
+ double rssi = Eeprom.EepromData.DemoSettings.RssiValue;
+
+ uint16_t j = 0;
+ uint16_t i;
printf( "#id: %d", Eeprom.EepromData.DemoSettings.CntPacketTx );
if( RngResultIndex > 0 )
{
- // compute average and std deviation w/o frequency correction
- for( i = 0; i < RngResultIndex; ++i )
- {
- total += RawRngResults[i];
- squareDeviationRaw += RawRngResults[i] * RawRngResults[i];
- }
- meanRaw = total / RngResultIndex;
- squareDeviationRaw = sqrt( ( squareDeviationRaw / RngResultIndex ) - ( meanRaw * meanRaw ) );
-
// compute average and std deviation
total = 0.0;
for( i = 0; i < RngResultIndex; ++i )
{
- RawRngResults[i] -= ( Eeprom.EepromData.DemoSettings.RngFeiFactor * Eeprom.EepromData.DemoSettings.RngFei / 1000 );
- total += RawRngResults[i];
- squareDeviation += RawRngResults[i] * RawRngResults[i];
+ RawRngResults[i] = RawRngResults[i] - ( Eeprom.EepromData.DemoSettings.RngFeiFactor * Eeprom.EepromData.DemoSettings.RngFei / 1000 );
}
- mean = total / RngResultIndex;
- squareDeviation = sqrt( ( squareDeviation / RngResultIndex ) - ( mean * mean ) );
- printf( ", Moy: %5.1f, Std: %6.3f", mean, squareDeviation );
-/*
- * also use RSSI since chip V3 for filter ranging values
- */
- total = 0.0;
- // select measure with correct Z Score
- for( i = 0; i < RngResultIndex; ++i )
+ for (int i = RngResultIndex - 1; i > 0; --i)
{
- if( ( fabs( RawRngResults[i] - meanRaw ) / squareDeviationRaw ) <= ( double )Eeprom.EepromData.DemoSettings.RngZscoreMax )
+ for (int j = 0; j < i; ++j)
{
- total += RawRngResults[i];
- j++;
+ if (RawRngResults[j] > RawRngResults[j+1])
+ {
+ int temp = RawRngResults[j];
+ RawRngResults[j] = RawRngResults[j+1];
+ RawRngResults[j+1] = temp;
+ }
}
}
- if( j > 0 )
- {
- meanRaw = total / j;
- }
- else
- {
- meanRaw = 0.0;
- }
- Eeprom.EepromData.DemoSettings.RngRawDistance = meanRaw;
- total = 0.0;
- j = 0;
- // select measure with correct Z Score
- for( i = 0; i < RngResultIndex; ++i )
+ double median;
+ if ((RngResultIndex % 2) == 0)
{
- if( ( fabs( RawRngResults[i] - mean ) / squareDeviation ) <= ( double )Eeprom.EepromData.DemoSettings.RngZscoreMax )
- {
- total += RawRngResults[i];
- j++;
- }
+ median = (RawRngResults[RngResultIndex/2] + RawRngResults[(RngResultIndex/2) - 1])/2.0;
+ }
+ else
+ {
+ median = RawRngResults[RngResultIndex/2];
}
- if( j > 0 )
- {
- mean = total / j;
- }
- else
- {
- mean = 0.0;
- }
+
+ mean = t0 + t1 * rssi + t2 * pow(rssi,2) + t3 * pow(rssi, 3) +t4 * median + t5 * pow(median,2) + t6 * pow(median, 3) + t7 * pow(median, 4) ;
+
if( j < DEMO_RNG_CHANNELS_COUNT_MIN )
{
Eeprom.EepromData.DemoSettings.RngStatus = RNG_PER_ERROR;
@@ -1906,10 +1894,7 @@
{
Eeprom.EepromData.DemoSettings.RngStatus = RNG_VALID;
}
- if( mean <= 18.5 )
- {
- mean = exp( ( mean + 2.4917 ) / 7.2262 );
- }
+
if( mean < 0 )
{
Eeprom.EepromData.DemoSettings.RngDistance = 0.0;