Arduino.ru

Господа, добрый день.
Есть Arduino mega. Она должна будет управлять небольшим экспериментальным стратостатом)
Код протестирован мною, но боюсь, что мог что либо упустить.
Если у вас есть возможность, то можете провести так называемое код ревью.
Особенно опасаюсь, что если какой либо датчик отключится во время работы платы, то будет зависание всей ардуины.

Заранее спасибо!

#include "settings.h"
#include "pin_map.h"
#include "sensors.h"
// библиотека для работы I²C
#include <Wire.h>
#include <SPI.h>
#include <SD.h>

// Библиотека для барометра
#include <MS5611.h>

// Создаем объект для работы с барометром
MS5611 ms5611;

// для диода
boolean InitSensor;
boolean InitSD;

typedef enum
{
    STATE_CLIMB_TO_15KM,
    STATE_MAINTAINING_HEIGHT,
    STATE_PLANNED_LANDING
}state_t;

unsigned long mainTimestamp = 0; 

state_t state = STATE_CLIMB_TO_15KM;

float speed[SPEED_COUNT_FOR_FORMULA];

float finalSpeed = 0;

float deltaAltitude = 1.0f;

float getSensorValue(sensor_id_t id);

void writeSD(String str)
{
    File logFile = SD.open("log2.txt", FILE_WRITE);

    // if the file is available, write to it:
    if (logFile) 
    {
        logFile.println(String(millis()) + " ::: " + " "+ str);
        logFile.close();
        //Serial.println("Write to log2.txt---OK");
        InitSD = true;
    }
    // if the file isn't open, pop up an error:
    else 
    {
        //Serial.println("error opening log2.txt");
        InitSD = false;
    }
}

void telemetryWriter()
{
    String result = "";
    
    result += String(millis()/60000);
    result += ":::min";
    //result += String(millis()/60000);
    result += ":::";
    result += " Altitude = " + String(getSensorValue(SENSOR_ALTITUDE), 2) + " ";
    result += " CalcSpeed =" + String(getSpeedZ(), 3) + " ";
    result += " Pressure =" + String(ms5611.readPressure()) + " ";
    result += " ExternalTemperature =" + String(getSensorValue(SENSOR_TEMPERATURE), 2) + " ";
    result += "\n";
    
    //Serial.println(result);
    
    File dataFile = SD.open("datalog2.txt", FILE_WRITE);

    // if the file is available, write to it:
    if (dataFile) 
    {
        dataFile.println(result);
        dataFile.close();
        //Serial.println("Write to datalog2.txt---OK");
        writeSD("Write to datalog2.txt---OK");
    }
    // if the file isn't open, pop up an error:
    else 
    {
        //Serial.println("error opening datalog2.txt");
        writeSD("error opening datalog2.txt");
    }
}

void initCommon()
{
    for(int i = 0; i < SPEED_COUNT_FOR_FORMULA; i++)
    {
        speed[i] = 0;
    }
}

void initSensors()
{
    initCommon();

    if(!ms5611.begin())
    {
        //Serial.println("Could not find a valid MS5611 sensor, check wiring!");
        writeSD("Could not find a valid MS5611 sensor, check wiring!");
    }
    
    deltaAltitude = REAL_START_ALTITUDE / getSensorValue(SENSOR_ALTITUDE);
}

float getSensorValue(sensor_id_t id)
{
    float result = 0.0f;
    long int realPressure = 0;
    switch (id)
    {
        case SENSOR_TEMPERATURE:
            result = ms5611.readTemperature();
            InitSensor = true;
            break;
        
        case SENSOR_PRESSURE_FOR_ALTITUDE:
            result = ms5611.readPressure();
            InitSensor = true;
            break;

        case SENSOR_ALTITUDE:
            realPressure = ms5611.readPressure();
            // Calculate altitude
            result = ms5611.getAltitude(realPressure) * deltaAltitude;
            //Serial.println("Pressure for alt:" + String(realPressure) +" result alt:" + String(result));
            writeSD("Pressure for alt:" + String(realPressure) +" result alt:" + String(result));
             InitSensor = true;
            break;

        default:
            //Serial.println("Read SENSOR ERROR");
            InitSensor = false;
            break;
    }

    return result;
}

float getSpeed()
{
    float result = 0;
    static int item = 0;
    static float lastAltitude = 0; 
    static unsigned long timestampForSpeed = 0;
    float altitude = getSensorValue(SENSOR_ALTITUDE);

    speed[item] = (altitude - lastAltitude) / (((float)(millis() - timestampForSpeed)) / 1000.0f);
    //Serial.println("Time for current speed: " +  String((((float)(millis() - timestampForSpeed)) / 1000.0f)));
    //Serial.println("Current speed:" + String(speed[item]));
    timestampForSpeed = millis();

    item++;

    if (item == SPEED_COUNT_FOR_FORMULA)
    {
        item = 0;
    }

    for (int i = 0; i < SPEED_COUNT_FOR_FORMULA; i++)
    {
        result += speed[i]; 
    }

    result /= SPEED_COUNT_FOR_FORMULA;
    lastAltitude = altitude;
    //Serial.print("Speed after filter: "); Serial.println(result);
    timestampForSpeed = millis();
    return result;
}

void updateSpeed()
{
    finalSpeed = getSpeed();
}

float getSpeedZ()
{
    return finalSpeed;
}

void balloonGateControl(bool value)
{
    if(value)
    {
        digitalWrite(BALLON_GATE_1_PIN, ON);
//        digitalWrite(BALLON_GATE_2_PIN, ON);
//        digitalWrite(BALLON_GATE_3_PIN, ON);
        Serial.println("Ballon gate write " + String(ON));
        writeSD("Ballon gate ON write " + String(ON));
    }
    else
    {
        digitalWrite(BALLON_GATE_1_PIN, OFF);
//        digitalWrite(BALLON_GATE_2_PIN, OFF);
//        digitalWrite(BALLON_GATE_3_PIN, OFF);
        Serial.println("Ballon gate write " + String(OFF));
        writeSD("Ballon gate OFF write " + String(OFF));
    }
    
}

void speedControl(float targetAccel)
{
    float speed = 0.0f;

    speed = getSpeedZ();
    
    writeSD("Target speed " + String(targetAccel) + " Current speed " + String(speed));

    // Если ускорение больше требуемого,
    if (speed > targetAccel)
    {
        // то открываем клапан
        balloonGateControl(true);
        //Serial.println("Ballon gate open");
        writeSD("Ballon gate open");
    }
    // Если ускорение меньше требуемого,
    else if (speed <= targetAccel)
    {
        // то закрываем клапан
        balloonGateControl(false);
        //Serial.println("Ballon gate close");
        writeSD("Ballon gate close");
    }
    else
    {
        //Serial.println("Ballon gate ERROR");
        writeSD("Ballon gate ERROR");
    }        
}

void setup() 
{
    // Инициализация логгера в порт
    Serial.begin(115200);

    if (!SD.begin(SD_CARD_ENABLE_PIN)) 
    {
        Serial.println("Card failed, or not present");
        // don't do anything more:
    }
    Serial.println("card initialized.");

    // Инициализация сенсоров
    initSensors();

    // Настройка пина лопателя шаров
    pinMode(BALLOON_POKE_1_PIN, OUTPUT);
//    pinMode(BALLOON_POKE_2_PIN, OUTPUT);

    // Настройка пинов управления клапанами
    pinMode(BALLON_GATE_1_PIN, OUTPUT);
//    pinMode(BALLON_GATE_2_PIN, OUTPUT);
//    pinMode(BALLON_GATE_3_PIN, OUTPUT);

    // Настройка пина для актуатора
    pinMode(ACTUATOR_PIN, OUTPUT);
    digitalWrite(BALLOON_POKE_1_PIN, LOW);
 //   digitalWrite(BALLOON_POKE_2_PIN, LOW);
    balloonGateControl(false);
    digitalWrite(ACTUATOR_PIN, LOW);

    // Тестово отправим сообщение с координатами
    Serial.println("Programm Started");
    writeSD("Programm Started");

    //пин диода
    pinMode(LED_PIN, OUTPUT);
}

void loop() 
{
    static unsigned long timestampWriteTelemetry = 0;
    float altitude = getSensorValue(SENSOR_ALTITUDE);
    static bool flagMaxAltitude = false;
    updateSpeed();
    
    switch(state)
    {
        case STATE_CLIMB_TO_15KM:
            // Если достигли нужной высоты - переходим в удержание высоты
            if (altitude >= FIRST_ALTITUDE_TARGET_MIN)
            {
                state = STATE_MAINTAINING_HEIGHT;
                //Serial.println("Go to STATE_MAINTAINING_HEIGHT");
                writeSD("Go to 24 KM - CONTROL SPEED ");
                // фиксируем время достижения высоты
                mainTimestamp = millis(); 
            }
            // Если за отведенное время не достигли высоты то переходим в режим посадки.
            if (millis() - mainTimestamp > TIME_TO_15000M)
            {
                state = STATE_PLANNED_LANDING;
                //Serial.println("Go to STATE_PLANNED_LANDING");
                writeSD("FAIL - GO BACK");
                // фиксируем время достижения высоты
                mainTimestamp = millis();
                // Открываем клапана
                balloonGateControl(true);
                // Включаем лопатель шаров
                digitalWrite(BALLOON_POKE_1_PIN, HIGH);
//                digitalWrite(BALLOON_POKE_2_PIN, HIGH);
                //Serial.println("Ballon Poke is ON");
                writeSD("Ballon Poke is ON");
            }
            
            break;

        case STATE_MAINTAINING_HEIGHT:
            // следим за временем полёта
            if (millis() - mainTimestamp > TIME_MAINTAINING)
            {
                state = STATE_PLANNED_LANDING;
                //Serial.println("Go to STATE_PLANNED_LANDING");
                writeSD("ALL GOOD - GO HOME");
                // фиксируем время достижения высоты
                mainTimestamp = millis();
                // Открываем клапана
                balloonGateControl(true);
                // Включаем лопатель шаров
                digitalWrite(BALLOON_POKE_1_PIN, HIGH);
//                digitalWrite(BALLOON_POKE_2_PIN, HIGH);
                //Serial.println("Ballon Poke is ON");
                writeSD("Ballon Poke is ON");
            }

            // Если высота больше минимальной и меньше максимальной
            if ((altitude >= ALTITUDE_TARGET_MIN) && (altitude < ALTITUDE_TARGET_MAX) && !flagMaxAltitude)
            {
                // контроль скорости
                speedControl(ACCEL_MAX_IN_MAINTAINING_HEIGHT);
            }
            else if ((altitude >= ALTITUDE_TARGET_MAX) || flagMaxAltitude)
            {
                speedControl(ACCEL_TO_DOWN);
                flagMaxAltitude = true;
            }
            if ((altitude < (ALTITUDE_TARGET_MIN*0.85)) || (altitude > ALTITUDE_CRITICAL))
            {
                state = STATE_PLANNED_LANDING;
                //Serial.println("Go to STATE_PLANNED_LANDING");
                writeSD("Go to STATE_PLANNED_LANDING");
                // фиксируем время достижения высоты
                mainTimestamp = millis();
                // Открываем клапана
                balloonGateControl(true);
                //Serial.println("Ballon Gate is ON");
                writeSD("Ballon Gate is ON");
                // Включаем лопатель шаров
                digitalWrite(BALLOON_POKE_1_PIN, HIGH);
//                digitalWrite(BALLOON_POKE_2_PIN, HIGH);
                //Serial.println("Ballon Poke is ON");
                writeSD("Ballon Poke is ON");
            }
            
            break;

        case STATE_PLANNED_LANDING:
            balloonGateControl(true);
            // Ждём 60 секунд перед включением актуатора
            if (millis() - mainTimestamp > 60000)
            {
                // Выключаем лопатели
                digitalWrite(BALLOON_POKE_1_PIN, LOW);
//                digitalWrite(BALLOON_POKE_2_PIN, LOW);
               // Serial.println("Ballon Poke is OFF");
                writeSD("Ballon Poke is OFF");
                // Включаем актуатор
                digitalWrite(ACTUATOR_PIN, HIGH);
               // Serial.println("Actuator is ON");
                writeSD("Actuator is ON");
            }
            break;

        default:

            break;
    }

    if (millis() - timestampWriteTelemetry > TELEMETRY_DELAY_MS)
    {
        //Serial.println("Start telemetry write");
        telemetryWriter();
        timestampWriteTelemetry = millis();
        //Serial.println("End telemetry write");
        writeSD("End telemetry write");
    }

    delay(1000);

 if (InitSD & InitSensor)
    {
      digitalWrite(LED_PIN, HIGH);
    }
    else
    {
      digitalWrite(LED_PIN, LOW);
    }
    
}