自制 apm/pixhawk 声纳，有问题，请教

payne_pan · 发表于 2018-1-21 11:29

前段，抄袭国外论坛写了 apm/pixhawk 的 I2C 声纳，
用arduino + SR-04 做的，滤波也做了，但定位效果不理想
（比如：地板飞向茶几，高度能拉起来，但再飞回地板，高度就没降下来）
特把源代码贴出来，请讨论指教

//
//  I2C interface for SR04 sonar
//  [ HC-SR04 5V ] <=> [ Trig(8), Echo(9) ArduinoNano 5V SDA(A4), SCL(A5) ] <=> [Bi-Directional Logic Level Converter 5V 3.3V] <=> [Rixracer/Pixhawk I2C]
//  Mission Planner | Optional Hardware | Sonar = MaxbotixI2C
//

#include <Wire.h>
//#define DEBUG

#define MIN_RANGE 10                         // Based on specs from MB1200 data sheet
#define MAX_RANGE 765                      // Based on specs from MB1200 data sheet
#define ARRAY_SIZE 11                         // size of median and mode sampling arrays. Needs to be an odd number

#define  SLAVE_ADDRESS       0x70
#define  MAX_PACKET_LENGHT 3
#define  DATA_LENGHT       2

const int sonar_Trig = 8;
const int sonar_Echo = 9;

byte command[MAX_PACKET_LENGHT];
byte buffer[DATA_LENGHT];

// For Mode / median filter variables needed to store values
// array to store the raw samples
int rawValues[ARRAY_SIZE];
// array to store the sorted samples
int srtValues[ARRAY_SIZE];
// Index for latest position in array
int idx = 0;

int val = 0;
byte* valPtr;
boolean cmdReady = false;

void setup() {
#ifdef DEBUG
  Serial.begin(57600);
#endif

  valPtr = (byte*) &val;
  pinMode(sonar_Trig, OUTPUT);
  pinMode(sonar_Echo, INPUT);
  Wire.begin(SLAVE_ADDRESS);
  Wire.onRequest(requestEvent);
  Wire.onReceive(receiveEvent);
  pinMode(13, OUTPUT);
  digitalWrite(13, HIGH);

   for(int i=0; i < ARRAY_SIZE; i++)    // Set all array value to Zeros
   {
      rawValues = 0;
      srtValues = 0;
   }
   idx = 0;                               // Set index to first place in array
}

void isort(int *a, int n){
for (int i = 1; i < n; ++i)
  {
int j = a;
int k;
for (k = i - 1; (k >= 0) && (j < a[k]); k--)
{
   a[k + 1] = a[k];
}
a[k + 1] = j;
  }
}

int median(int *x,int n){
return (x[(n/2)]);
}

//Mode function, returning the modal value (or median value if mode does not exist).
int mode(int *x,int n){

  int i = 0;
  int count = 0;
  int maxCount = 0;
  int mode = 0;
  int bimodal;
  int prevCount = 0;

  while(i<(n-1)){

prevCount=count;
count=0;

while(x==x[i+1]){

   count++;
   i++;
}

if(count>prevCount&count>maxCount){
   mode=x;
   maxCount=count;
   bimodal=0;
}
if(count==0){
   i++;
}
if(count==maxCount){                   //If the dataset has 2 or more modes.
   bimodal=1;
}
if(mode==0||bimodal==1){                //Return the median if there is no mode.
   mode=x[(n/2)];
}
return mode;
  }
}

void loop() {
  digitalWrite(13, LOW);
  digitalWrite(sonar_Trig, HIGH);
  delayMicroseconds(10);
  digitalWrite(sonar_Trig, LOW);
  unsigned int  tmp =pulseIn(sonar_Echo, HIGH)/58;
  if (tmp> MAX_RANGE) tmp=MAX_RANGE;
  if (tmp< MIN_RANGE) tmp=0;

rawValues[idx] = tmp;
idx++;
if (idx>=ARRAY_SIZE)
   idx=0;
for(int i=0; i < ARRAY_SIZE; i++)       // copy raw data to sorted data array
   srtValues=rawValues;
isort(srtValues,ARRAY_SIZE);             // sort the sorted data array
//val = median(srtValues,ARRAY_SIZE);    // find the median value and use it
val = mode(srtValues,ARRAY_SIZE);
  delay(25);
}

void requestEvent()
{
if ( cmdReady ) {
digitalWrite(13, HIGH);

char* ptr = (char*) &val;
buffer[1] = *ptr++;
buffer[0] = *ptr++;
Wire.write(buffer, 2);
cmdReady = false;
}
}

void receiveEvent(int bytesReceived)
{
  for(int i = 0; i < bytesReceived; i++)
  {
if( i < MAX_PACKET_LENGHT){
   command = Wire.read();
} else{
      Wire.read();
   }
  }

  switch(command[0]){
case 0x51:
   cmdReady = true;
break;
  }
#ifdef DEBUG
  Serial.print("Command: " );
  Serial.println(command[0]);
  Serial.println(val);
  /*
  Serial.print(", ");
  Serial.print(command[1]);
  Serial.print(", ");
  Serial.println(command[2]);
  */
#endif
}