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

payne.pan

前段，抄袭国外论坛写了 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  
}

鹰隼001

帮顶……

纸鹰

sr04质量也参差不齐

1822757516

顶顶！

西柚西柚2014

帮佩恩老师顶

payne.pan

纸鹰 发表于 2018-1-21 13:36
sr04质量也参差不齐

能找到 检测 SR04 质量的方法就好了，  这可能要在线路上测信号，找到信号在哪里被影响了，衰减了。

自贡老赵

我也想装个  但是又觉得没什么用   降落伞最实用

牛牛多轴

发个制作方法啊，造福一下模友