RE: Mavlink biblioteka jak używać? - Dzimi87 - 08-06-2023
Dobra już częściowo działające i współpracujące z open tx i flyplus. Tu sobie taki warsztacik zrobimy .
[attachment=1347]
[attachment=1348]
[attachment=1349]
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-waypoint-and-interfacing-the-pixhawk-to-an-arduino-using-mavlink/
A na tej jest metodą pobrania misji i w bibliotece są te pliki.
https://mavlink.io/en/services/mission.html#mission_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 .
RE: Mavlink biblioteka jak używać? - Dzimi87 - 23-06-2023
Ż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.
RE: Mavlink biblioteka jak używać? - MarekT - 29-06-2023
(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-flyski-telemetria-i-bus-czy-mozna-uzyc-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
RE: Mavlink biblioteka jak używać? - MarekT - 29-06-2023
(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-flyski-telemetria-i-bus-czy-mozna-uzyc-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 ...
RE: Mavlink biblioteka jak używać? - Dzimi87 - 02-07-2023
Regulatory są git nawet dokumentację można znaleźć w internecie . A jak coś da się naprawić to +100 % dla mnie robi się lepsze.
|