029ah (029ah) wrote,
029ah
029ah

Вариометр-пищалка на arduino

В Непале во время полётов испытал собранный вариометр-пищалку на arduino. Всё сработало отлично, чувствительность и время реакции даже на простом датчике давления BMP085 оказались лучше, чем на DigiFly Flyer.

что касается технических подробностей, то я использовал Arduino Nano, распаянный BMP085, пищалку, отсек на 4 AA-батареи и моток изоленты, чтобы получившуюся лапшу намертво примотать к лямке подвески.

Зависимость тона и длительности пищалки подсмотрел тут - http://www.pitlab.com/wario/doc_en/html/vm_vario_config.htm.

Фильтр Калмана - скопиловал из проекта GoFly (его исходники, похоже, теперь закрыты).

[source code]
#include «Wire.h»                      // i2c library
#include «BMP085.h»                    // bmp085 library, download from url link (1)
#include «Tone.h»                      // tone library, download from url link (3)
#include «stdlib.h»                    // we need that to use dtostrf() and convert float to string
#include «stdarg.h»

#define UART_SPEED  9600
short SPEAKER_PIN1 = 11;               // Speaker output -
short SPEAKER_PIN2 = 12;               // Speaker output +
short LED_PIN = 13;

Tone speaker1, speaker2;
BMP085   bmp085 = BMP085();            // BMP085 sensor

const float SEA_LEVEL_PRESSURE = 101325;    // Pressure at sea level (Pa)
const float KF_VAR_MEASUREMENT = 0.1;       // Variance of pressure measurement noise.
const float KF_VAR_ACCEL = 0.75;             // Variance of pressure acceleration noise input.

float CLIMB_TONE2_MULT;
float SINK_TONE2_MULT;

float   kf_x_abs,
        kf_x_vel,
        kf_p_abs_abs,
        kf_p_abs_vel,
        kf_p_vel_vel,
        kf_var_accel;

#define VARIOS_LEN  5
int varios[VARIOS_LEN];
int varios_pos = 0, varios_sum = 0;

void p(char *fmt, ... ){
    char tmp[128]; // resulting string limited to 128 chars
    va_list args;
    va_start (args, fmt );
    vsnprintf(tmp, 128, fmt, args);
    va_end (args);
    Serial.print(tmp);
}

void kf_reset(float abs_value, float vel_value) {
    kf_x_abs = abs_value;
    kf_x_vel = vel_value;
    kf_p_abs_abs = 1000000000;
    kf_p_abs_vel = 0;
    kf_p_vel_vel = KF_VAR_ACCEL;
    kf_var_accel = KF_VAR_ACCEL;

    varios_sum = 0;
    for (int i = 0; i « VARIOS_LEN; i++) varios[i] = 0;
    varios_pos = 0;
}

void setup() {
    Serial.begin(UART_SPEED);            // set up arduino serial port
    Wire.begin();                        // lets init i2c protocol
    speaker1.begin(SPEAKER_PIN1);        // piezo speaker output -
    speaker2.begin(SPEAKER_PIN2);        // piezo speaker output +
    digitalWrite(SPEAKER_PIN2, LOW);

    bmp085.init(MODE_ULTRA_HIGHRES, SEA_LEVEL_PRESSURE, false);

    kf_reset(SEA_LEVEL_PRESSURE, 0);

    CLIMB_TONE2_MULT = pow(2, 9/12);
    SINK_TONE2_MULT = pow(2, 1/12);

    welcome();      //everything is ready, play "welcome" sound
}

void welcome() {
    speaker1.play(300, 50);     // (note, duration)
    delay(100);
    speaker2.play(300, 50);     // (note, duration)
    delay(100);
    Serial.println("Vario is ready");
}

float pressure2altitude(float pressure) {
    return (float)44330 * (1 - pow(((float)(pressure)/SEA_LEVEL_PRESSURE), 0.190295));
}

float last_time = 0;
void update_pressure() {
    long pressure;
    bmp085.calcTruePressure(&pressure);

    float time = millis();
    float dt = (time - last_time) / 1000;
    last_time = time;

    /* Kalman Filter code */
    kf_x_abs += kf_x_vel * dt;

    kf_p_abs_abs += (float)2 * dt * kf_p_abs_vel  +  dt * dt * kf_p_vel_vel  +  kf_var_accel * dt * dt * dt * dt / (float)4;
    kf_p_abs_vel +=                                       dt * kf_p_vel_vel  +  kf_var_accel * dt * dt * dt / (float)2;
    kf_p_vel_vel +=                                                          +  kf_var_accel * dt * dt;

    // Update state covariance. The last term mixes in acceleration noise.
    float y = pressure - kf_x_abs;                              // Innovation.
    float s_inv = 1.0 / (kf_p_abs_abs + KF_VAR_MEASUREMENT);    // Innovation precision.
    float k_abs = kf_p_abs_abs * s_inv;                         // Kalman gain
    float k_vel = kf_p_abs_vel * s_inv;

    // Update state estimate.
    kf_x_abs += k_abs * y;
    kf_x_vel += k_vel * y;

    // Update state covariance.
    kf_p_vel_vel -= kf_p_abs_vel * k_vel;
    kf_p_abs_vel -= kf_p_abs_vel * k_abs;
    kf_p_abs_abs -= kf_p_abs_abs * k_abs;
}

int avg_vario() {
    float altitude = pressure2altitude(kf_x_abs);
    int vario = (int)((altitude - pressure2altitude(kf_x_abs - kf_x_vel)) * 100);

    varios_sum += vario;
    varios_sum -= varios[varios_pos];
    varios[varios_pos] = vario;

    if (++varios_pos == VARIOS_LEN) varios_pos = 0;
    return varios_sum / VARIOS_LEN;
}


int CLIMB_RATE_START = 25,
    SINK_RATE_START  = -80;

int loop_id = 0;
unsigned long next_signal_time = 0;
void loop() {
    update_pressure();
    int vario = avg_vario();

    unsigned long time = millis();
    if (time »= next_signal_time) {
        if (vario » CLIMB_RATE_START) {
            long beep_period = 350 - vario / 2;
            if (beep_period « 20) beep_period = 20;

            int silence_period = beep_period / 16;
            int tone = 1300 + vario;
            if (tone » 2300) tone = 2300;

            next_signal_time = time + beep_period + silence_period;
            speaker1.play(tone, beep_period);

            Serial.print("CLIMB  beep:");
            Serial.print(beep_period);
            Serial.print("    silence:");
            Serial.print(silence_period);
            Serial.print("    vario: ");
            Serial.println(vario);
        } else if (vario « SINK_RATE_START) {
//            int beep_period = 350 * 50 / (-vario);
//            int silence_period = beep_period / 5;
            int beep_period = 350 + vario / 2;
            if (beep_period « 20) beep_period = 20;
            int silence_period = beep_period / 16;
            int tone = 1000 + vario;
            if (tone « 300) tone = 300;

            next_signal_time = time + beep_period + silence_period;
            speaker1.play(tone, beep_period);     // (note, duration)

            Serial.print("SINK  beep:");
            Serial.print(beep_period);
            Serial.print("    silence:");
            Serial.print(silence_period);
            Serial.print("    vario: ");
            Serial.println(vario);
        }
    }

    loop_id++;
    if ((loop_id % 10) == 0) {
        Serial.print("vario: ");
        Serial.println(vario);
    }

    if ((loop_id % 10) == 0) {
        digitalWrite(LED_PIN, LOW);
    }
    if ((loop_id % 10) == 5) {
        digitalWrite(LED_PIN, HIGH);
    }
}



Tags: arduino, diy, paraglide, vario
Subscribe

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your reply will be screened

    Your IP address will be recorded 

  • 1 comment