• Witaj na Forum Arduino Polska! Zapraszamy do rejestracji!
  • Znajdziesz tutaj wiele informacji na temat hardware / software.
Witaj! Logowanie Rejestracja


Ocena wątku:
  • 0 głosów - średnia: 0
  • 1
  • 2
  • 3
  • 4
  • 5
Mavlink biblioteka jak używać?
#11
Dobra już częściowo działające i współpracujące z open tx i flyplus. Tu sobie taki warsztacik zrobimy .
   
   
   
Kod:
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////TELEMETRIA MAVLINK/ARDUINO TO IBUS FLYSKU DO LUDKI ZANETOWEJ////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include  "mavlink.h"       //BIBLOTEKA MAVLINK  
#include <iBUSTelemetry.h>  //BIBLOTEKA IBUS
#define UPDATE_INTERVAL 500 //INTERWAŁ WYSLANIA IBUS
iBUSTelemetry telemetry(8); //PIN IBUS
uint32_t prevMillis = 0;    //CZAS MILIS
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////REJESTRY MAVLINK DO WYSLANIA IBUS///////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int    Fix_type;          //RODZAJ FIX SAT
double Latitude;          //SZEROKOSC GPS
double Longitude;         //WYSOKOSC GPS
float  Speed;             //PREDKOSC M/S
int    Satellites_visible;//ILOSC SATELIT
int    Flight_mode;       //Flight_mode
float  Voltage_battery;   //NAPIECIE AKU
float  Current_battery;   //PRAD AKU
int    Heading;           //COMPASS
int    HDOP;              //HDOP
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////TEMPERATURA TO IBUS/////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////CZUJNIKI ZEWNETRZNE TEMPERATURY I REJESTRY////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int PinTemp0 = A0 ;   //PIN TEMPERATURY1
int PinTemp1 = A1 ;   //PIN TEMPERATURY2
int PinTemp2 = A2 ;   //PIN TEMPERATURY3
int PinTemp3 = A3 ;   //PIN TEMPERATURY4
float temp0;          //
float temp1;          //ODCZYTANE TEMPERATURY
float temp2;          //
float temp3;          //WYLICZENIA DO NTC 10K WRAZIE POTRZEBY MODYFIKOWAC
float otemp0;         //PRZELICZONE NA NASZE
float otemp1;         //
float otemp2;         //
float otemp3;         //
float tempS;          // TEMPERATURA 2 SILNIKOW POKAZANIE WYZSZEJ
float tempR;          // TEMPERATURA 2 REGULATOROW POKAZANIE WYZSZEJ
//PARAMETRY NTC POTRZEBNE DO WYLICZENIA TEMPERATURY
#define RT0 10000   // Ω
#define B 3977      // K
#define VCC 5    //Supply voltage
#define R 10000  //R=10KΩ
float RT, VR, ln ;
float T0 = 298.15; //25 stopni w kelwinach
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////POMOCNICZE GPS DO DYSTANSU I PIAGORAS///////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double  LatZ;         //SZEROKOSC GPS ZAPAMIETANA
double  LonZ;         //WYSOKOSC GPS ZAPAMIETANA                                            ////TU POMYSLEC JAK POBRAC Z APM//////
int     BlokG = 0;    //BLOKADA ZAPAMIETANIA GPS 0 PRZYJMUJE 1 ZAPISANA I ZABLOKOWANA
double  POS;          //WYLICZENIE ODLEGLOSCI
double  GPSdist;      //WYLICZONA ODLEGLOSC
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////WODA W LUDCE TO IBUS///////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int PinWoda = 2 ;   //PIN ZALANIA WODA
int Woda;           //ZMIENNA WODA W LUDCE
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////KLAPY SPUSTOWE TO IBUS/////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int PinKlapa1 = 3 ;   //PIN klapa1
int PinKlapa2 = 4 ;   //PIN klapa2
int Klapy;            //ZMIENNA KLAP
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////DEPPER ECHOSONDA/////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int PinDepper = 5 ;   //PIN DEPPER STEROWANIE
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////KONFIGURACJA////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void setup() {
pinMode(PinWoda,  INPUT_PULLUP); //PIN ZALANIA WODA PODCIAGNIETY DO PLUSA
pinMode(PinKlapa1,INPUT_PULLUP); //PIN KLAPY1 PODCIAGNIETY DO PLUSA         CZUJMIKO I/0
pinMode(PinKlapa2,INPUT_PULLUP); //PIN KLAPY2 PODCIAGNIETY DO PLUSA
pinMode(PinDepper,OUTPUT);       //PIN STEROWANIE DEPPER            
Serial.begin(57600);             //UART PC
Serial1.begin(57600);            //UART MAVLINK
request_datastream();            //NIE WIEM COS WYSYLA RAZ
telemetry.begin();               //URUCHOMIE IBUS
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////CZUJNIKI IBUS////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
telemetry.addSensor(IBUS_MEAS_TYPE_GPS_STATUS);   // STATUS GPS I ILOSC SATELIT
telemetry.addSensor(IBUS_MEAS_TYPE_GPS_LAT);      // SZEROKOSC GEO
telemetry.addSensor(IBUS_MEAS_TYPE_GPS_LON);      // DLUGOSC GEO
telemetry.addSensor(IBUS_MEAS_TYPE_GROUND_SPEED); // PREDKOSC M/S GPS
telemetry.addSensor(IBUS_MEAS_TYPE_FLIGHT_MODE);  // FLIGHT MODE
telemetry.addSensor(IBUS_MEAS_TYPE_EXTV);         // NAPIECIE AKUMULATORA
telemetry.addSensor(IBUS_MEAS_TYPE_BAT_CURR);     // PRĄD AKUMULATORA
telemetry.addSensor(IBUS_MEAS_TYPE_TEM);          // TEMPERATURA SILNIKA
telemetry.addSensor(IBUS_MEAS_TYPE_TEM);          // TEMPERATURA MOSTKA H
telemetry.addSensor(IBUS_MEAS_TYPE_CMP_HEAD);     // KOMPAS 0 TO POLNOC
telemetry.addSensor(IBUS_MEAS_TYPE_COG);          // HDOP
telemetry.addSensor(IBUS_MEAS_TYPE_GPS_DIST);     // DYSTANS NIE WIEM JAK BOBRAC Z APM NARZAZIE WYLICZONY
telemetry.addSensor(0x85);
telemetry.addSensor(0x86);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////PROGRAM GLUWNY/////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {        
MavLink_receive();
czujniki(); 
updateValues();
telemetry.run();
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////ODBIERA BICIE SERCA I GPS ZERZNIETE Z TUTKOW//////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void MavLink_receive()
  {
  mavlink_message_t msg;
  mavlink_status_t status;

  while(Serial1.available())
  {
    uint8_t c= Serial1.read();

    //Get new message
    if(mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
    {

    //Handle new message from autopilot
      switch(msg.msgid)
      {

        case MAVLINK_MSG_ID_GPS_RAW_INT:
      {
        mavlink_gps_raw_int_t packet;
        mavlink_msg_gps_raw_int_decode(&msg, &packet);
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////ODEBRANIE/WYSLANIE DO REJESTRU///////////////////////////////////////////////////////////////////////   
          Fix_type = packet.fix_type;
          Latitude = packet.lat;
          Longitude = packet.lon;
          //Alt = packet.alt;
          Speed = packet.vel;
          Satellites_visible = packet.satellites_visible;
          HDOP = packet.eph;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////       
      }
      break;
       case MAVLINK_MSG_ID_HEARTBEAT:
       {
       mavlink_heartbeat_t hb;
       mavlink_msg_heartbeat_decode(&msg,&hb);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////ODEBRANIE/WYSLANIE DO REJESTRU////////////////////////////////////////////////////////////////////////      
          Flight_mode = hb.custom_mode;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////     
      }
      break;
      case MAVLINK_MSG_ID_SYS_STATUS:  // #1: SYS_STATUS
      {
        mavlink_sys_status_t sys_status;
        mavlink_msg_sys_status_decode(&msg, &sys_status);
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////ODEBRANIE/WYSLANIE DO REJESTRU///////////////////////////////////////////////////////////////////////////// 
        Voltage_battery = sys_status.voltage_battery;
        Current_battery = sys_status.current_battery;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
}
break;
case MAVLINK_MSG_ID_VFR_HUD:
    {
      mavlink_vfr_hud_t packet;
      mavlink_msg_vfr_hud_decode(&msg, &packet);
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////ODEBRANIE/WYSLANIE DO REJESTRU////////////////////////////////////////////////////////////////////////////////     
      Heading = packet.heading;
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////           
    }
    break;          
      } 
    }
  }
}
//////////////////////////////////////////////////////////TU COS WYSYLA MAWLINK RAZ////////////////////////////////////////////////////////////////////////////////
void request_datastream() {
//Request Data from Pixhawk
  uint8_t _system_id = 255; // id of computer which is sending the command (ground control software has id of 255)
  uint8_t _component_id = 2; // seems like it can be any # except the number of what Pixhawk sys_id is
  uint8_t _target_system = 1; // Id # of Pixhawk (should be 1)
  uint8_t _target_component = 0; // Target component, 0 = all (seems to work with 0 or 1
  uint8_t _req_stream_id = MAV_DATA_STREAM_ALL;
  uint16_t _req_message_rate = 0x01; //number of times per second to request the data in hex
  uint8_t _start_stop = 1; //1 = start, 0 = stop

  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  mavlink_msg_request_data_stream_pack(_system_id, _component_id, &msg, _target_system, _target_component, _req_stream_id, _req_message_rate, _start_stop);
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);  // Send the message (.write sends as bytes)

  Serial1.write(buf, len);//Write data to serial port
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////MOJE CZUJNIKI///////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void czujniki()
{
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 
/////////////////////////////////////////////////////////////////ODCZYT PIN ANALOGOWYCH TEMP///////////////////////////////////////////////////////////////////////////
temp0 = analogRead(PinTemp0);             
temp1 = analogRead(PinTemp1);
temp2 = analogRead(PinTemp2);
temp3 = analogRead(PinTemp3); 
/////////////////////////////////////////////////////////PRZELICZENIE WARTOSCI ANALOG NA STOPNIE///////////////////////////////////////////////////////////////////////   
///////////////////czujnik temp0
  temp0 = (VCC / 1023.00) * temp0;     
  VR = VCC - temp0;
  RT = temp0 / (VR / R);              
  ln = log(RT / RT0);
  otemp0 = (1 / ((ln / B) + (1 / T0)));
  otemp0 = otemp0 - 273.15;                
//////////////czujnik temp1          
  temp1 = (VCC / 1023.00) * temp1;    
  VR = VCC - temp1;
  RT = temp1 / (VR / R);              
  ln = log(RT / RT0);
  otemp1 = (1 / ((ln / B) + (1 / T0)));
  otemp1 = otemp1 - 273.15;                
////////////////czujnik temp2       
  temp2 = (VCC / 1023.00) * temp2;    
  VR = VCC - temp2;
  RT = temp2 / (VR / R);             
  ln = log(RT / RT0);
  otemp2 = (1 / ((ln / B) + (1 / T0)));
  otemp2 = otemp2 - 273.15;               
/////////////////czujnik temp3             
  temp3 = (VCC / 1023.00) * temp3;     
  VR = VCC - temp3;
  RT = temp3 / (VR / R);              
  ln = log(RT / RT0);
  otemp3 = (1 / ((ln / B) + (1 / T0)));
  otemp3 = otemp3 - 273.15;
  ////////////////WYSLANIE WYZSZYCH WARTOSCI DO APARATURY
if (otemp0 > otemp1) {
    tempS = otemp0;
  }else{
    tempS = otemp1;
  }
if (otemp2 > otemp3) {
    tempR = otemp2;
  }else{
    tempR = otemp3;
  }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////WODA W LUDCE////////////////////////////////////////////////////////////////////////////////////
if (digitalRead(PinWoda) == HIGH) {
    Woda = 0; //SUCHO W SKORUPIE
  }else{
    Woda = 1; //WODA W SKORUPIE
  }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////KLAPY SPUSTOWE//////////////////////////////////////////////////////////////////////////////////
if ((digitalRead(PinKlapa1) == LOW)&&(digitalRead(PinKlapa2) == LOW)) {
    Klapy = 0; //KLAPY ZAMKNIETE
  }
if ((digitalRead(PinKlapa1) == HIGH)&&(digitalRead(PinKlapa2) == HIGH)) {
    Klapy = 3; //KLAPY OTWARTE
  }
if ((digitalRead(PinKlapa1) == HIGH)&&(digitalRead(PinKlapa2) == LOW)) {
    Klapy = 1; //KLAPA1 OTWARTA / KLAPA2 ZAMKNIETA
  }
if ((digitalRead(PinKlapa1) == LOW)&&(digitalRead(PinKlapa2) == HIGH)) {
    Klapy = 2; //KLAPA1 ZAMKNIETA / KLAPA2 OTWARTA
  }
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////    
//////////////////////////////////////////////////////////GPS ZAPAMIETAC I PITAGORAS DLA ODLEGLOSCI////////////////////////////////////////////////////////////////////
if ((Fix_type >= 3)&&(Satellites_visible >= 6)&&(BlokG == 0)) {
    LatZ = Latitude;
    LonZ = Longitude;
    BlokG = 1;
  }    
   POS =(((LatZ-Latitude)*(LatZ-Latitude))+((LonZ-Longitude)*(LonZ-Longitude))*73);
   GPSdist =sqrt(POS)/1000;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////STEROWANIE DEPPER////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////ILOSC PUNKTOW///////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////IBUS AKTUALIZACJA CZUJNIKOW/////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void updateValues()
{
    uint32_t currMillis = millis();

    if (currMillis - prevMillis >= UPDATE_INTERVAL) { // Code in the middle of these brackets will be performed every 500ms.
        prevMillis = currMillis;

        telemetry.setSensorValue(1,telemetry.gpsStateValues(Fix_type,Satellites_visible));
        telemetry.setSensorValue(2,Latitude);
        telemetry.setSensorValue(3,Longitude);
        telemetry.setSensorValue(4,Speed);
        telemetry.setSensorValue(5,Flight_mode);                                                            
        telemetry.setSensorValueFP(6,Voltage_battery);
        telemetry.setSensorValueFP(7,Current_battery);
        telemetry.setSensorValueFP(8,tempS);
        telemetry.setSensorValueFP(9,tempR);
        telemetry.setSensorValue(10,Heading);
        telemetry.setSensorValue(11,HDOP);
        telemetry.setSensorValue(12,GPSdist);
        telemetry.setSensorValue(13,Klapy);
        telemetry.setSensorValue(14,Woda);
    }
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


Na tej stronie znalazłem jak wysłać misje do APM
https://www.locarbftw.com/uploading-a-wa...g-mavlink/

A na tej jest metodą pobrania misji i w bibliotece są te pliki.
https://mavlink.io/en/services/mission.h...sion_types

Ten mavlink jest ciężki do zrozumienia , a to pomoże w odczycie ilości zapisanych punktów i poznamy pozycję domowa bo ona tam jest .
 
Odpowiedź
#12
Żeby działało flight mode ustawiamy 
1 manual
2 rtl
3 rtl
4 hold 
5 rtl
6 guide 
Wyłączamy zapis na Chanel 7 


Piny Arduino nano bez układu CH 340  bo potrzebne piny Tx i RX, lub nano z 328pb bez usuwania uart 
Tylko w kodzie trzeba dopisać 1 przy serial .
A0 - A3 czujnik temperatury
A4 - A5 rezerwacja ( osobne napięcie i prąd ) do zrobienia
RX i TX UART mavlink
D2 czujnik zalania wodą 
D3 i D4 czujnik otwarcia klap
D5 Buzer pyka 3 razy po złapaniu fix 
D6 i D9 PWM jeszcze nie używane
D8 iBus 
D7 włącznik deeper naprzyklad 

13,12,11,rst potrzebne do programowania bez uart 

Zapis punktów do eeprom wybór i płynięcie bezpośrednio do wybranego punktu.
+ Pobiera telemetrię z APM
Kod:
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////TELEMETRIA MAVLINK/ARDUINO TO IBUS FLYSKU DO LUDKI ZANETOWEJ////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include  "mavlink.h"       //BIBLOTEKA MAVLINK
#include <EEPROM.h>         //BIBLOTEKA EEPROM
#include <iBUSTelemetry.h>  //BIBLOTEKA IBUS
#define UPDATE_INTERVAL 200 //INTERWAŁ WYSLANIA IBUS
iBUSTelemetry telemetry(8); //PIN IBUS
uint32_t prevMillis = 0;    //CZAS MILIS
int Round = 0;              //USTALENIE KOLEJNOSCI ZADAN
/////////////////////////////////////////////////////////////////////  REJESTRY POBRANE Z MAVLINK  ////////////////////////////////////////////////////////////////////
int      Fix_type;          //RODZAJ FIX SAT
float    Latitude;          //SZEROKOSC GPS
float    Longitude;         //DLUFOSC GPS
float    Altitude;          //WYSOKOSC GPS
uint16_t Speed;             //PREDKOSC M/S
uint8_t  Satellites_visible;//ILOSC SATELIT
int      Flight_mode;       //Flight_mode arduino
uint32_t custom_mode_apm;   //Flight_mode apm
uint16_t Voltage_battery;   //NAPIECIE AKU
int16_t  Current_battery;   //PRAD AKU
uint16_t Heading;           //COMPASS
uint16_t HDOP;              //HDOP
uint16_t Ch4 = 1500;        //ODCZYT KANALU CH4
uint16_t Ch5;               //ODCZYT KANALU CH5
uint16_t Ch6;               //ODCZYT KANALU CH6
uint16_t Ch7;               //ODCZYT KANALU CH7
uint16_t Ch8;               //ODCZYT KANALU CH8
///////////////////////////////////////////////////////////////// CZUJNIKI TEMPERATURY I REJESTRY //////////////////////////////////////////////////////////////////////
int PinTemp0 = A0 ;   //PIN TEMPERATURY1
int PinTemp1 = A1 ;   //PIN TEMPERATURY2
int PinTemp2 = A2 ;   //PIN TEMPERATURY3
int PinTemp3 = A3 ;   //PIN TEMPERATURY4
int temp0;
int temp1;
int temp2;
int temp3;
float tempS;          // TEMPERATURA 2 SILNIKOW POKAZANIE WYZSZEJ
float tempR;          // TEMPERATURA 2 REGULATOROW POKAZANIE WYZSZEJ
float OtempS;         //WYLICZENIA DO NTC 10K WRAZIE POTRZEBY MODYFIKOWAC
float OtempR;         //PRZELICZONE NA NASZE
//PARAMETRY NTC POTRZEBNE DO WYLICZENIA TEMPERATURY
#define RT0 10000     // Ω
#define B 3977        // K
#define VCC 5         //Supply voltage
#define R 10000       //R=10KΩ
float RT, VR, ln ;
float T0 = 298.15;    //25 stopni w kelwinach
///////////////////////////////////////////////////////////////////// STEROWANIE MISJAMI //////////////////////////////////////////////////////////////////////////////
int     Point;
int     Block_channel_point = 0;
int     Block_auto = 0;
int     Block_WR_eeprom = 0;
///////////////////////////////////////////////////////////// POMOCNICZE GPS DO DYSTANSU I PIAGORAS ///////////////////////////////////////////////////////////////////
float  LatitudeTAB[11];         //SZEROKOSC GPS ZAPAMIETANA
float  LongitudeTAB[11];        //DLUGOSC GPS ZAPAMIETANA
float  AltitudeTAB[11];         // WYSOKOSC GPS ZAPAMIETANA
int    Block_Home_position = 0; //BLOKADA ZAPAMIETANIA GPS 0 PRZYJMUJE 1 ZAPISANA I ZABLOKOWANA
float  Dist_2;                  //WYLICZENA ODLEGLOSC DO KWADRATU
float  GPSdist;                 //WYLICZONA ODLEGLOSC
int    Slot_kor;                //SPRAWDZA CZY JEST ZAPISANA KORDYNATA XYZ W TABLICY
///////////////////////////////////////////////////////////////////// ZMIENNA CZUJNIKA ZALANIA ////////////////////////////////////////////////////////////////////////
int PinWater = 2 ;     //PIN ZALANIA WODA
int Water;             //ZMIENNA WODA W LUDCE
////////////////////////////////////////////////////////////////////// ZMIENNE OTWARCIA KLAP /////////////////////////////////////////////////////////////////////////
int PinLapels1 = 3 ;   //PIN KLAPA1
int PinLapels2 = 4 ;   //PIN KLAPA2
int Lapels;            //ZMIENNA KLAP
////////////////////////////////////////////////////////////////////// DEPPER ECHOSONDA ///////////////////////////////////////////////////////////////////////////////
int PinDepper = 7 ;    //PIN DEPPER
/////////////////////////////////////////////////////////////////////////// BUZER ///////////////////////////////////////////////////////////////////////////////////
int PinBuzer = 5 ;    //PIN BUZER
//////////////////////////////////////////////////////////////////////// EEPROM ///////////////////////////////////////////////////////////////////////////////////////
int adr_eeprom = 0;   // ZMIENNA ADRES EPROM
////////////////////////////////////////////////////////////////////// KONFIGURACJA ////////////////////////////////////////////////////////////////////////////////////

void setup() {
  pinMode(PinWater,  INPUT_PULLUP); //PIN CZUJNIKA ZALANIA
  pinMode(PinLapels1, INPUT_PULLUP); //PIN OTWARCIA KLAPY1
  pinMode(PinLapels2, INPUT_PULLUP); //PIN OTWARCIA KLAPY2
  pinMode(PinDepper, OUTPUT);       //PIN STEROWANIE DEPPER
  pinMode(PinBuzer, OUTPUT);        //PIN STEROWANIE BUZER
  Serial.begin(57600);              //UART MAVLINK
  telemetry.begin();                //URUCHOMIE IBUS
  Request_datastream;               //URUCHOMIE STRUMIENI MAVLINK
  ////////////////////////////////////////////////////////////////CZUJNIKI IBUS////////////////////////////////////////////////////////////////////////////////////////
  telemetry.addSensor(IBUS_MEAS_TYPE_GPS_STATUS);   // STATUS GPS I ILOSC SATELIT
  telemetry.addSensor(IBUS_MEAS_TYPE_GPS_LAT);      // SZEROKOSC GEO
  telemetry.addSensor(IBUS_MEAS_TYPE_GPS_LON);      // DLUGOSC GEO
  telemetry.addSensor(IBUS_MEAS_TYPE_GROUND_SPEED); // PREDKOSC M/S GPS
  telemetry.addSensor(IBUS_MEAS_TYPE_FLIGHT_MODE);  // FLIGHT MODE
  telemetry.addSensor(IBUS_MEAS_TYPE_EXTV);         // NAPIECIE AKUMULATORA
  telemetry.addSensor(IBUS_MEAS_TYPE_BAT_CURR);     // PRĄD AKUMULATORA
  telemetry.addSensor(IBUS_MEAS_TYPE_TEM);          // TEMPERATURA SILNIKA
  telemetry.addSensor(IBUS_MEAS_TYPE_TEM);          // TEMPERATURA MOSTKA H
  telemetry.addSensor(IBUS_MEAS_TYPE_CMP_HEAD);     // KOMPAS 0 TO POLNOC
  telemetry.addSensor(IBUS_MEAS_TYPE_COG);          // HDOP
  telemetry.addSensor(IBUS_MEAS_TYPE_GPS_DIST);     // DYSTANS
  telemetry.addSensor(0x85);                        // CZUJNIK OTWARCIA KLAP
  telemetry.addSensor(0x86);                        // CZUJNIK ZALANIA
  telemetry.addSensor(0x87);                        // STEROWANIE PO PUNKTACH
//////////////////////////////////////////////////////////// ODCZYT EEPROM  ///////////////////////////////////////////////////////////////////////////////////////////
EEPROM_READ();
}
////////////////////////////////////////////////////////////PROGRAM GLUWNY/////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
  Round_task();
  MavLink_channels();
  Control_channel();
  if (Round == 0)
  {
    MavLink_receive();
  }
  if (Round == 1)
  {
    Sensor_Temp();
    Sensor_Water();
    Sensor_Lapels();
    Home_position();
    Dist_GPS();
  }
  if (Round == 2)
  {
    Update_Values_Ibus();
    Round = 0;
  }
  telemetry.run();
 
}
//////////////////////////////////////////////////////////1)PROSBA O STRUMIENIE //////////////////////////////////////////////////////////////////////////////////////
void Request_datastream() {

  uint8_t _system_id = 255;
  uint8_t _component_id = 2;
  uint8_t _target_system = 1;
  uint8_t _target_component = 0;
  uint8_t _req_stream_id = MAV_DATA_STREAM_ALL;
  uint16_t _req_message_rate = 0x01;
  uint8_t _start_stop = 1;

  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  mavlink_msg_request_data_stream_pack(_system_id, _component_id, &msg, _target_system, _target_component, _req_stream_id, _req_message_rate, _start_stop);
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

  Serial.write(buf, len);
}
///////////////////////////////////////////////////////////// 2) KOLEJKOWANIE ZADAN //////////////////////////////////////////////////////////////////////////////////

void Round_task()
{
  uint32_t currMillis = millis();

  if (currMillis - prevMillis >= UPDATE_INTERVAL) {
    prevMillis = currMillis;
    Round = Round + 1;
  }
}
///////////////////////////////////////////////////// 3) ODCZYTANIE KANAŁÓW MAVLINK APM ///////////////////////////////////////////////////////////////////////////////
void MavLink_channels()
{
  mavlink_message_t msg;
  mavlink_status_t status;

  while (Serial.available())
  {
    uint8_t c = Serial.read();

    if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
    {
      switch (msg.msgid)
      {
        case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
          {
            mavlink_rc_channels_raw_t rc_channels_raw;
            mavlink_msg_rc_channels_raw_decode(&msg, &rc_channels_raw);
            Ch4 = rc_channels_raw.chan3_raw;
            Ch5 = rc_channels_raw.chan5_raw;
            Ch6 = rc_channels_raw.chan6_raw;
            Ch7 = rc_channels_raw.chan7_raw;
            Ch8 = rc_channels_raw.chan8_raw;
          }
          break;
          case MAVLINK_MSG_ID_HEARTBEAT:
           {
             mavlink_heartbeat_t hb;
             mavlink_msg_heartbeat_decode(&msg, &hb);
             custom_mode_apm = hb.custom_mode;                       
           }
           break;  
      }
    }
  }
}
///////////////////////////////////////////////////// 4) ODCZYTANIE TELEMETRI MAVLINK I WYSLANIE DO REJESTROW /////////////////////////////////////////////////////////
void MavLink_receive()
{
  mavlink_message_t msg;
  mavlink_status_t status;

  while (Serial.available())
  {
    uint8_t c = Serial.read();

    if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status))
    {
      switch (msg.msgid)
      {
        case MAVLINK_MSG_ID_GPS_RAW_INT:
          {
            mavlink_gps_raw_int_t packet;
            mavlink_msg_gps_raw_int_decode(&msg, &packet);
            Fix_type = packet.fix_type;
            Latitude = packet.lat;
            Longitude = packet.lon;
            Altitude =  packet.alt;
            Speed = packet.vel;
            Satellites_visible = packet.satellites_visible;
            HDOP = packet.eph;
          }
          break;         
        case MAVLINK_MSG_ID_SYS_STATUS:
          {
            mavlink_sys_status_t sys_status;
            mavlink_msg_sys_status_decode(&msg, &sys_status);
            Voltage_battery = sys_status.voltage_battery;
            Current_battery = sys_status.current_battery;
          }
          break;
        case MAVLINK_MSG_ID_VFR_HUD:
          {
            mavlink_vfr_hud_t packet;
            mavlink_msg_vfr_hud_decode(&msg, &packet);
            Heading = packet.heading;
          }
          break;
      }
    }
  }
}
//////////////////////////////////////////////////////////////5) ODCZYT CZUJNIKOW TEMP /////////////////////////////////////////////////////////////////////////////////
void Sensor_Temp()
{
  temp0 = analogRead(PinTemp0);
  temp1 = analogRead(PinTemp1);
  temp2 = analogRead(PinTemp2);
  temp3 = analogRead(PinTemp3);
  ////////////////WYSLANIE WYZSZYCH WARTOSCI DO APARATURY
  if (temp0 > temp1) {
    tempS = temp0;
  } else {
    tempS = temp1;
  }
  if (temp2 > temp3) {
    tempR = temp2;
  } else {
    tempR = temp3;
  }
  /////////////////////////////////////////////////////////PRZELICZENIE WARTOSCI ANALOG NA STOPNIE///////////////////////////////////////////////////////////////////////
  ///////////////////czujnik tempM
  tempS = (VCC / 1023.00) * tempS;
  VR = VCC - tempS;
  RT = tempS / (VR / R);
  ln = log(RT / RT0);
  OtempS = (1 / ((ln / B) + (1 / T0)));
  OtempS = OtempS - 273.15;
  //////////////czujnik tempR
  tempR = (VCC / 1023.00) * tempR;
  VR = VCC - tempR;
  RT = tempR / (VR / R);
  ln = log(RT / RT0);
  OtempR = (1 / ((ln / B) + (1 / T0)));
  OtempR = OtempR - 273.15;
}
//////////////////////////////////////////////////////////////////  6) WODA W LUDCE //////////////////////////////////////////////////////////////////////////////////
void Sensor_Water()
{
  if (digitalRead(PinWater) == HIGH) {
    Water = 0; //SUCHO W SKORUPIE
  } else {
    Water = 1; //WODA W SKORUPIE
  }
}
///////////////////////////////////////////////////////////////////// 7) KLAPY SPUSTOWE  ////////////////////////////////////////////////////////////////////////////
void Sensor_Lapels()
{
  if ((digitalRead(PinLapels1) == LOW) && (digitalRead(PinLapels2) == LOW)) {
    Lapels = 0; //KLAPY ZAMKNIETE
  }
  if ((digitalRead(PinLapels1) == HIGH) && (digitalRead(PinLapels2) == HIGH)) {
    Lapels = 3; //KLAPY OTWARTE
  }
  if ((digitalRead(PinLapels1) == HIGH) && (digitalRead(PinLapels2) == LOW)) {
    Lapels = 1; //KLAPA1 OTWARTA / KLAPA2 ZAMKNIETA
  }
  if ((digitalRead(PinLapels1) == LOW) && (digitalRead(PinLapels2) == HIGH)) {
    Lapels = 2; //KLAPA1 ZAMKNIETA / KLAPA2 OTWARTA
  }
}
//////////////////////////////////////////////////////////////// 8)ZAPIS POZYCJI DOMOWEJ //////////////////////////////////////////////////////////////////////////////
void Home_position()
{
  if ((Fix_type >= 3) && (Satellites_visible >= 6) && (Block_Home_position == 0)) {  //&& (HDOP >= 1,20)
    LatitudeTAB[0]  = Latitude;
    LongitudeTAB[0] = Longitude;
    AltitudeTAB[0]  = Altitude;
    digitalWrite(PinBuzer, HIGH);  //zrobic jak kupie buzer z generatorem
    delay(100);
    digitalWrite(PinBuzer, LOW);
    delay(100);
    digitalWrite(PinBuzer, HIGH);
    delay(100);
    digitalWrite(PinBuzer, LOW);
    delay(100);
    digitalWrite(PinBuzer, HIGH);
    delay(100);
    digitalWrite(PinBuzer, LOW);
    Block_Home_position = 1;
  }
}
////////////////////////////////////////////////////////////////// 9)DYSTANS GPS /////////////////////////////////////////////////////////////////////////////////////
void Dist_GPS()
{
  Dist_2 = (((LatitudeTAB[0] - Latitude) * (LatitudeTAB[0] - Latitude)) + ((LongitudeTAB[0] - Longitude) * (LongitudeTAB[0] - Longitude)) * 73);
  GPSdist = sqrt(Dist_2) / 1000;
}
/////////////////////////////////////////////////////////////////////10) STEROWANIE KANALAMI //////////////////////////////////////////////////////////////////////////
void Control_channel()
{
  if ((10 > Point) && (Ch4 > 1600) && (Block_channel_point == 0)) //WYBOR PUNKTOW
  {
    Point = Point + 1;
    Block_channel_point = 1;
  }
  if ((Ch4 >= 1300) && (Ch4 <= 1600)) {
    Block_channel_point = 0;
  }
  if ((0 < Point) && (Ch4 < 1300) && (Block_channel_point == 0)) {
    Point = Point - 1;
    Block_channel_point = 1;
  }
  if ((10 == Point) && (Ch4 > 1600) && (Block_channel_point == 0)) //WYBOR PUNKTOW
  {
    Point = 0;
    Block_channel_point = 1;
  }
  if ((0 == Point) && (Ch4 < 1300) && (Block_channel_point == 0)) {
    Point = 10;
    Block_channel_point = 1;
  }
  /////////////DEPPER WLACZNIK///////////////
  if (Ch6 > 1800)
  {
    digitalWrite(PinDepper, HIGH);
  } else {
    digitalWrite(PinDepper, LOW);
  }
  /////////////////ZAPIS PUNKTU///////////////
  if ((Ch7 > 1800) && (Flight_mode == 1)&&(Block_WR_eeprom == 0)) {
    LatitudeTAB[Point] =  Latitude ;
    LongitudeTAB[Point] = Longitude;
    AltitudeTAB[Point] =  Altitude ;
    EEPROM.put((Point*4), LatitudeTAB[Point]);
    EEPROM.put((Point*4)+44, LongitudeTAB[Point]);
    EEPROM.put((Point*4)+88, AltitudeTAB[Point]);
    Block_WR_eeprom = 1; 
  }
  /////////////KASOWANIE PUNKTU///////////////
  if ((Ch7 > 1800) && (Flight_mode == 0) && (Point > 0)&&(Block_WR_eeprom == 0)) {
    LatitudeTAB[Point] = 0;
    LongitudeTAB[Point] =0;
    AltitudeTAB[Point] = 0;
    EEPROM.put((Point*4), 0);
    EEPROM.put((Point*4)+44, 0);
    EEPROM.put((Point*4)+88, 0);
    Block_WR_eeprom = 1;  
  }
////////////// ODBLOKOWANIE ZAPISU I KASOWANIA EEPROM //////////////////
  if (Ch7 < 1500){
  Block_WR_eeprom = 0; 
  }
  //////////////////SPRAWDZENIE SLOTU ZAPISU//
  if ((LatitudeTAB[Point] == 0) && (LongitudeTAB[Point] == 0) && (AltitudeTAB[Point] == 0)) {
    Slot_kor = 0;
  } else {
    Slot_kor = 1;
  }
  //////////////////WYBOR TRYBU //////////////
  if ((Ch8 > 1800) )
  {
    Flight_mode = 2;//AUTO PUNKT
    if ((Flight_mode == 2) && (Slot_kor == 1) && (Block_auto == 0) && (custom_mode_apm == 15)) {     
      Mission_item();     
      Block_auto = 1; 
    }
  }
  
  if ((Ch8 >= 1300) && (Ch8 <= 1700)) {
    Flight_mode = 1;//UCZENIE
    Block_auto = 0;
  }
  if (Ch8 < 1200) {
    Flight_mode = 0;//RECZNY
    Block_auto = 0;
  }
}
//////////////////////////////////////////////////////////  11) AKTUALIZACJA CZUJNIKOW  ////////////////////////////////////////////////////////////////////////////////
void Update_Values_Ibus()
{
  telemetry.setSensorValue(1, telemetry.gpsStateValues(Fix_type, 0));
  telemetry.setSensorValue(2, Latitude);
  telemetry.setSensorValue(3, Longitude);
  telemetry.setSensorValue(4, Speed);
  telemetry.setSensorValue(5, Flight_mode); //UMIESCIC W 4 BITACH
  telemetry.setSensorValueFP(6, Voltage_battery);
  telemetry.setSensorValueFP(7, Current_battery);
  telemetry.setSensorValueFP(8, OtempS);
  telemetry.setSensorValueFP(9, OtempR);
  telemetry.setSensorValue(10, Heading);
  telemetry.setSensorValue(11, HDOP);
  telemetry.setSensorValue(12, GPSdist);
  telemetry.setSensorValue(13, Lapels);
  telemetry.setSensorValue(14, Water);
  telemetry.setSensorValue(15, Point); //TRYB:X ILOSC PUNKTOW:XX WYBOR PUNKTY:XX SLOT:X
}
//////////////////////////////////////////////////////////// 12) WYSLIJ DO PUNKTU/////////////////////////////////////////////////////////////////////////////////////
void Mission_item() {

  uint8_t _system_id = 255;
  uint8_t _component_id = 2;
  uint8_t _target_system = 1;
  uint8_t _target_component = 0;
 
  float param1 = 0;
  float param2 = 0;
  float param3 = 0;
  float param4 = 0;
  float x =  LatitudeTAB[Point]/10000000; 
  float y =  LongitudeTAB[Point]/10000000;
  float z =  AltitudeTAB[Point]/1000;   
  uint16_t seq = 0;
  uint16_t command = MAV_CMD_NAV_WAYPOINT;
  uint8_t frame = 3;
  uint8_t current = 2; // false:0, true:1
  uint8_t autocontinue = 1; // Always 0

  mavlink_message_t msg;
  uint8_t buf[MAVLINK_MAX_PACKET_LEN];

  // Pack the message
  mavlink_msg_mission_item_pack(_system_id, _component_id, &msg, _target_system, _target_component, seq, frame, command, current, autocontinue, param1, param2, param3, param4, x, y, z);

  // Copy the message to the send buffer
  uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);

  // Send the message (.write sends as bytes)
  Serial.write(buf, len);

}
//////////////////////////////////////////////////////////// 13) EEPROM READ /////////////////////////////////////////////////////////////////////////////////////////
void EEPROM_READ() {
for (int Latitude_x = 0; Latitude_x<=10;Latitude_x++){
    adr_eeprom = Latitude_x * 4;
    EEPROM.get(adr_eeprom,LatitudeTAB[Latitude_x]);
//    Serial.print(adr_eeprom); Serial.print(LatitudeTAB[Latitude_x]); Serial.println(Latitude_x);  
}
Serial.println("//////////////////////////////////////////////////");
  for (int Longitude_y = 0; Longitude_y<=10;Longitude_y++){
    adr_eeprom = ( Longitude_y * 4)+44;
    EEPROM.get(adr_eeprom,LongitudeTAB[Longitude_y]);
//   Serial.print(adr_eeprom);Serial.print(LongitudeTAB[Longitude_y]);Serial.println(Longitude_y);
}
// Serial.println("//////////////////////////////////////////////////");
for (int Altitude_z = 0; Altitude_z<=10;Altitude_z++){
    adr_eeprom = ( Altitude_z * 4)+88;
    EEPROM.get(adr_eeprom,AltitudeTAB[Altitude_z]);
//    Serial.print(adr_eeprom);Serial.print(AltitudeTAB[Altitude_z]);Serial.println(Altitude_z);
}
//Serial.println("//////////////////////////////////////////////////");
}

Powoli przychodzi czas na zaprojektowanie pcb bo na drucikach słabo wygląda.
 
Odpowiedź
#13
(30-04-2023, 21:55)Dzimi87 napisał(a): Może wytłumaczę o co chodzi. Mega na pewno będzie za duże i prądożerne .
https://forum.arduinopolska.pl/watek-fly...-na-attyni
To działa i ma mało pinów i wiadomo zegar wewnętrzny różnie pracuje pokazuje 2 temperatury i napięcie i prąd.
Zabrakło mi fiksuGps ale tu juz nic nie dołożę.
Słabo mi idzie poruszanie po tej bibliotece mavlink ponoć jak ja obsłużyć można dojść po plikach xxx.h tylko trochę tego nie rozumiem.
A idea ma być taka biblioteka mavlink ma odczytać z APM tak zakładam co by się przydało.
Prąd.        -
Napięcie. -
Pozycje gps +
Fixgps + dziwne troszke
Ilosc satelit
Flyght mode +
Fajna by była odległość od home do łudeczki-
Ilość zapisanych punktów-
Tam gdzie plusik juz odczytuje na pc
Do tego będzie uart sprzętowy 328p taki posiada
Resztę pinów z atmegi myślę o 4 ptc do pomiary temperatury i 2 czyjniki zalania wodom i cos tam jeszcze do tego uzyje biblioteki ibus i wysle do odbiornika a odbiornik do aparatury
A leonardo i mikro posiada 1 uart to czyli pozostaje dalej przy 328p

Zastosowanie do łódki zanętowej
 
Odpowiedź
#14
(30-04-2023, 21:55)Dzimi87 napisał(a): Może wytłumaczę o co chodzi. Mega na pewno będzie za duże i prądożerne .
https://forum.arduinopolska.pl/watek-fly...-na-attyni
To działa i ma mało pinów i wiadomo zegar wewnętrzny różnie pracuje pokazuje 2 temperatury i napięcie i prąd.
Zabrakło mi fiksuGps ale tu juz nic nie dołożę.
Słabo mi idzie poruszanie po tej bibliotece mavlink ponoć jak ja obsłużyć można dojść po plikach xxx.h tylko trochę tego nie rozumiem.
A idea ma być taka biblioteka mavlink ma odczytać z APM tak zakładam co by się przydało.
Prąd.        -
Napięcie. -
Pozycje gps +
Fixgps + dziwne troszke
Ilosc satelit
Flyght mode +
Fajna by była odległość od home do łudeczki-
Ilość zapisanych punktów-
Tam gdzie plusik juz odczytuje na pc
Do tego będzie uart sprzętowy 328p taki posiada
Resztę pinów z atmegi myślę o 4 ptc do pomiary temperatury i 2 czyjniki zalania wodom i cos tam jeszcze do tego uzyje biblioteki ibus i wysle do odbiornika a odbiornik do aparatury
A leonardo i mikro posiada 1 uart to czyli pozostaje dalej przy 328p

Zastosowanie do łódki zanętowej
Jakies znajome regulatory ...
 
Odpowiedź
#15
Regulatory są git nawet dokumentację można znaleźć w internecie . A jak coś da się naprawić to +100 % dla mnie robi się lepsze.
 
Odpowiedź
  


Skocz do:


Przeglądający: 1 gości