STM32 Arduino BI Металлоискатель

folny82

Новичок
Сообщения
2
Репутация
0
Баллы
1
#1
Привет друзья

Я начал писать свой собственный код для металлоискателя, основанный на STM32 Arduino коде работает правильно, отображение VDI на последовательном порту это нормально, но кто нибудь может посоветовать, как добавить в код функцию отладки GEB (Ground Balanc) ?

Вот мой код, спасибо заранее за все советы.

#include <STM32ADC.h>
STM32ADC myADC(ADC1);
uint8 RxPin = PA6;
const int TxPin = PA4;
const int maxSamples = 1;
uint16_t dataPoints[maxSamples];

#define SerialMode Serial

uint8_t TxPhase = 0; //TxPhase adjust 0-16
int32_t frequency = 10000; // Frequency(Hz)
int prescaler = 4;
double freq;

const uint16_t numSamplesToAverage = 128;
const double radiansToDegrees = 180.0/3.1415927;

int32_t bins[16];
int32_t calib[16];
volatile int32_t averages[16];

int VDI = 0;
int Calibrate = 1;
uint16_t numSamples = 0;
volatile uint32_t ticks = 0;
int interruptCounter = 0;
volatile uint8_t lastctr;
volatile uint16_t misses = 0;
uint8_t ctr = 0;
int32_t input_average;
volatile bool sampleReady = false;

const int32_t RXsin[] = {0, 49, 91, 118, 128, 118, 91, 49, 0, -49, -91, -118, -128, -118, -91, -49};

const int32_t RXcos[] = {-128, -118, -91, -49, 0, 49, 91, 118, 128, 118, 91, 49, 0, -49, -91, -118};

const int32_t Tx[] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 4095, 4095, 4095, 4095, 4095, 4095, 4095};

void Tx_Rx_Isr(void) {

ticks++;
ctr++;
TxPhase++;

digitalWrite(TxPin, Tx[TxPhase-1]);

int32_t i = dataPoints[0] - 2048;
input_average -= (input_average >> 10);
input_average += i;
input_average = input_average + i;
bins[ctr-1] = i - (input_average >> 10);

if (ctr == 16)
{
++numSamples;
if (numSamples == numSamplesToAverage)
{
numSamples = 0;
if (!sampleReady)
{
memcpy((void*)averages, bins, sizeof(averages));
sampleReady = true;
}
memset(bins, 0, sizeof(bins));
}
ctr = 0;
}

if (TxPhase == 16)
{
TxPhase = 0;
}
}

void setup()
{
SerialMode.begin(57600);
delay(10);

pinMode(TxPin, OUTPUT);

Timer3.setPrescaleFactor(prescaler);
freq = (72000000/2/prescaler)/frequency/8;
Timer3.setOverflow(freq);
Timer3.setMasterModeTrGo(TIMER_CR2_MMS_UPDATE);

myADC.calibrate();
myADC.setSampleRate(ADC_SMPR_1_5);
myADC.setPins(&RxPin, 1);
myADC.setDMA(dataPoints, maxSamples, (DMA_MINC_MODE | DMA_CIRC_MODE | DMA_HALF_TRNS | DMA_TRNS_CMPLT), Tx_Rx_Isr);
myADC.setTrigger(ADC_EXT_EV_TIM3_TRGO);
myADC.startConversion();

while (!sampleReady) {}
misses = 0;
sampleReady = false;
}


void loop(){
while (!sampleReady) {}
uint32_t oldTicks = ticks;

if(1 == Calibrate){
for (int i = 0; i < 16; ++i){
calib = averages;
}

sampleReady = false;
Calibrate = 0;
}
else
{
for (int i = 0; i < 16; ++i){
averages -= calib;
}

int32_t tmp1 = 0, tmp2 = 0;
int32_t rxX, rxY;

for (int i = 0; i < 16; ++i){
tmp1 += (averages * RXsin)/512;
tmp2 += (averages * RXcos)/512;
}

tmp1 -= (rxX >> 9);
tmp2 -= (rxY >> 9);
rxX += tmp1;
rxY += tmp2;

int VDI = atan2(rxY, rxX) * radiansToDegrees;
int Amp = sqrt((rxY * rxY) + (rxX * rxX));

SerialMode.print("Amp: ");
SerialMode.println(Amp);
SerialMode.println();
if (Amp >= 5){
SerialMode.print("VDI: ");
SerialMode.println(VDI);
}

sampleReady = false;
}
while (ticks - oldTicks < 16000)
{
}
}
 

folny82

Новичок
Сообщения
2
Репутация
0
Баллы
1
#2
Код:
#include "libmaple/dac.h"
#include <STM32ADC.h>
STM32ADC myADC(ADC1);
uint8 RxPin = PA6;
const int Samples = 1;
uint16_t ADCdata[Samples];
int TxPin = PA4;

int Debug = 0;
#define SetSerial Serial

int32_t frequency = 10000; // Frequency(Hz)
int prescaler = 4;
double freq;

const uint16_t numSamplesToAverage = 64;
const double radiansToDegrees = 180.0/3.14;

int32_t bins[16];
int32_t calib[16];               
volatile int32_t averages[16];

int Calibrate = 1;
double VDI = 0;
double AMP = 0;
int SendSerial = 0;
float alpha = 0.10;
uint16_t numSamples = 0;     
volatile uint32_t ticks = 0;                   
volatile uint8_t lastctr;
volatile uint16_t misses = 0;
uint8_t  ctr = -1;
int32_t input_average;
volatile bool sampleReady = false;

const int32_t Tx[] = {0, 0, 0, 0, 0, 0, 0, 0, 4095, 4095, 4095, 4095, 4095, 4095, 4095, 4095};
      
void Tx_Rx_Isr(void) {
  ctr++;
  ticks++;
 
  digitalWrite(TxPin, Tx[ctr]);
 
  int16_t ADC = ADCdata[0] - 2047;
  input_average -= (input_average >> 10);
  input_average += ADC;
  input_average = input_average + ADC;
  bins[ctr] = ADC - (input_average >> 10);

  if (ctr == 15)
  {
    ++numSamples;
    if (numSamples == numSamplesToAverage)
    {
      numSamples = 0;
      if (!sampleReady)     
      {
       memcpy((void*)averages, bins, sizeof(averages));
       sampleReady = true;
      }
      memset(bins, 0, sizeof(bins));
    }
    ctr = -1;
  }
}

void setup()
{
  SetSerial.begin(57600);     
  delay(10); 
 
  pinMode(TxPin, OUTPUT); 
  dac_init(DAC,DAC_CH2);
 
  Timer3.setPrescaleFactor(prescaler);
  freq = (72000000/2/prescaler)/frequency/8;
  Timer3.setOverflow(freq);
  Timer3.setMasterModeTrGo(TIMER_CR2_MMS_UPDATE);

  myADC.calibrate();
  myADC.setSampleRate(ADC_SMPR_1_5); 
  myADC.setPins(&RxPin, 1);
  myADC.setDMA(ADCdata, Samples, (DMA_MINC_MODE | DMA_CIRC_MODE | DMA_HALF_TRNS | DMA_TRNS_CMPLT), Tx_Rx_Isr);
  myADC.setTrigger(ADC_EXT_EV_TIM3_TRGO);
  myADC.startConversion();

  while (!sampleReady) {}     
  misses = 0;
  sampleReady = false;
}

void loop(){
   while (!sampleReady){}
   uint32_t oldTicks = ticks;

   if(1 == Calibrate){
   for (int i = 0; i < 16; ++i){
   calib[i] = averages[i];
   }
   sampleReady = false;
   Calibrate = 0;
   }else{
    
   SendSerial++;
   int32_t adcX = 0, adcY = 0;
   double filterX , filterY;
  
   for (int i = 4; i < 12; ++i){
   adcX += (averages[i] - calib[i]);
   }

   for (int i = 0; i < 8; ++i){
   adcY += (averages[i] - calib[i]);
   }

   filterX = (alpha*adcX) + ((1-alpha) * filterX);
   filterY = (alpha*adcY) + ((1-alpha) * filterY);

   if(1 == Debug){
   SetSerial.print("X: ");
   SetSerial.println(filterX);
   SetSerial.print("Y: ");
   SetSerial.println(filterY);
   SetSerial.println();
   }

   VDI += atan2(filterX, filterY) * radiansToDegrees;
   AMP =  sqrt((filterX * filterX) + (filterY * filterY));
   sampleReady = false;
  
   filterX = 0;
   filterY = 0;

   if(0 == Debug){
   if(16 == SendSerial){
   SetSerial.print("AMP: ");
   SetSerial.println((int)AMP);
   SetSerial.println();
   if (AMP >= 2){
   SetSerial.print("VDI: ");
   SetSerial.println((int)VDI/16);
   }
   AMP = 0;
   VDI = 0;
   SendSerial = 0;       
   }
   }
   }
   if(0 == Debug){
   while (ticks - oldTicks < 1024){}
   }else{
   while (ticks - oldTicks < 16000){}
  }
}