Wsadziłem apm , ale też udało się kupić pix 2.4.8 jak dobrze pamiętam .Najlepszą opcja na razie jest pływanie wydając polecenia telefonem . Tover chyba tak się nazywa.
Największym problemem płynięcia w auto to to że płynie do 1 punktu puźniej 2 punktu.
Urodziła się myśl że można teoretycznie zrobić z apki , bardziej chodzi że w kupie siła .
I już tłumaczę wgrałem openTx do flysky fsi6x i nawet polecam jest w wersji polskojęzycznej . Aktualizuje go Janusz nasz rodak .
To co mnie zainteresowało to zmiana nazw czujników i organizacja ich oraz że przemawia w języku polskim.
Można różne alarmy nastawiać , pewnie jak ktoś się zna to wiele więcej .
- 20230607_205307_HDR~2.jpg (198.67 KiB) Przejrzano 1029 razy
- 20230607_205303_HDR~2.jpg (226.7 KiB) Przejrzano 1029 razy
Biblioteka IBus
https://github.com/adis1313/iBUSTelemet ... SSensors.h
Forum i BIBLOTEKA MAVLINK
https://discuss.ardupilot.org/t/mavlink ... step/25566
I teraz tak przepraszam za haos w kodzie nie jestem programista .
Kod: Zaznacz cały
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////TELEMETRIA MAVLINK/ARDUINO TO IBUS FLYSKU DO LUDKI ZANETOWEJ////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "mavlink.h" //BIBLOTEKA MAVLINK
#include <iBUSTelemetry.h> //BIBLOTEKA IBUS
#define UPDATE_INTERVAL 700 //INTERWAŁ WYSLANIA IBUS
iBUSTelemetry telemetry(8); //PIN IBUS
uint32_t prevMillis = 0; //CZAS MILIS
///////////////////////////////////////////////////////////////////REJESTRY MAVLINK DO WYSLANIA IBUS///////////////////////////////////////////////////////////////////
int PunktyTAB;
int Blokada;
int TAB;
float Latitude_Misson[6]; //SZEROKOSC GPS MISI
float Longitude_Misson[6];//WYSOKOSC GPS MISI
int Fix_type; //RODZAJ FIX SAT
float Latitude; //SZEROKOSC GPS
float 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
int Punkty; //ILOSC PUNKTOW W PAMIECI
int W_Punkt; //PLYN DO NR PUNKTU
int Ch4; //WYBOR PUNKTU MISJI
int Ch5; //WYSLANIE DO PUNKTU
int Ch6; //DEPPER KANAL
int Ch7; //ODSWIEZENIE MISJI
//////////////////////////////////////////////////////////////////////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
///////////////////////////////////////////////////////////////STEROWANIE WYBOREM PUNKTU///////////////////////////////////////////////////////////////////////////////
int BlokP = 0;
int BlokM = 2;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////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); //czujniki klap , wody , sterowanie go-to
telemetry.addSensor(0x87);
}
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////PROGRAM GLUWNY/////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
if (BlokM == 2) {
zapytanie(); //tu wogole porzazka do przemyslenia
for (TAB=Punkty; TAB>=0;TAB--){
zapytaniepunkt();
}
BlokM = 0;
}
MavLink_receive();
czujniki();
updateValues();
telemetry.run();
}
////////////////////////////////////////////////////// 1 )zapytanie o ilosc punktow///////////////////////////////////////////////////////////////////////////////////
void zapytanie() {
uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software
uint8_t _component_id = 2; // component id of sending station 2 works fine
uint8_t _target_system = 1; // Pixhawk id
uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
mavlink_msg_mission_request_list_pack(_system_id, _component_id, &msg, _target_system, _target_component);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message (.write sends as bytes)
Serial1.write(buf, len);
}
/////////////////////////////////////////////////////// 3) zapytanie punkty o kordynaty////////////////////////////////////////////////////////////////////////////
void zapytaniepunkt() {
uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software
uint8_t _component_id = 2; // component id of sending station 2 works fine
uint8_t _target_system = 1; // Pixhawk id
uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)
uint16_t seq = TAB;
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
mavlink_msg_mission_request_pack(_system_id, _component_id, &msg,_target_system, _target_component, seq);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message (.write sends as bytes)
Serial1.write(buf, len);
}
////////////////////////////////////////////////////////ODBIERA BICIE SERCA I GPS ZERZNIETE Z TUTKOW//////////////////////////////////////////////////////////////////
void MavLink_receive()
{ //ta petla odczytuje dane z apm /pix
mavlink_message_t msg; // to co apm wysyla sam dobrze działa
mavlink_status_t status; // gorzej z planem misji i pozycja domowa
//pruby jakies sa , ale malo skuteczne
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);
Fix_type = packet.fix_type;
Latitude = packet.lat;
Longitude = packet.lon;
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);
Flight_mode = hb.custom_mode;
}
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;
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.chan1_raw;
Ch5 = rc_channels_raw.chan5_raw;
Ch6 = rc_channels_raw.chan6_raw;
Ch7 = rc_channels_raw.chan7_raw;
}
break;
case MAVLINK_MSG_ID_MISSION_COUNT:
{
mavlink_mission_count_t mission_count;
mavlink_msg_mission_count_decode(&msg,&mission_count) ;
Punkty = mission_count.count;
}
break;
case MAVLINK_MSG_ID_MISSION_ITEM:
{
mavlink_mission_item_t mission_item;
mavlink_msg_mission_item_decode(&msg,&mission_item) ;
Latitude_Misson[mission_item.seq] = mission_item.x;
Longitude_Misson[mission_item.seq]= mission_item.y;
}
break;
}
}
}
}
//////////////////////////////////////////////////////////TU COS WYSYLA 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 //to nie wiem co to jest dowiedziec sie było w przykladzie
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()
{
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); //odczyt temperatury prze termistory ntc
otemp1 = (1 / ((ln / B) + (1 / T0))); //dziła nawet, wyciac polowe kodu
otemp1 = otemp1 - 273.15; //z powodu braku miejsca do emulacji czujnikow
////////////////czujnik temp2 //wysyła jedna wyzsza wartos z dwuch czujnikow silnikow
temp2 = (VCC / 1023.00) * temp2; //wysyła jedna wyzsza wartos z dwuch czujnikow regulatorow
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 { // czujnik uzywa pinu i/o atmegi jest zwarciowym czyjnikiem(moze byc potrzebny wzmacniacz sygnalu)
Woda = 1; //WODA W SKORUPIE // ten fragment zostanie bez zmian
}
//////////////////////////////////////////////////////////////////////KLAPY SPUSTOWE//////////////////////////////////////////////////////////////////////////////////
if ((digitalRead(PinKlapa1) == LOW) && (digitalRead(PinKlapa2) == LOW)) {
Klapy = 0; //KLAPY ZAMKNIETE
}
if ((digitalRead(PinKlapa1) == HIGH) && (digitalRead(PinKlapa2) == HIGH)) {
Klapy = 3; //KLAPY OTWARTE //klapu zanetowe uzywaja i/o atmegi jest zwarciowym czyjnikiem
} //podpiac mase atmegi pod prety mechanizmu zrzutu, a piny klap do zawiasow klap
if ((digitalRead(PinKlapa1) == HIGH) && (digitalRead(PinKlapa2) == LOW)) { //wazne zawiasy,klapy i bolce musza byc metalowe, inaczej uzyc krancowki
Klapy = 1; //KLAPA1 OTWARTA / KLAPA2 ZAMKNIETA // ten fragment zostanie bez zmian
}
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; // ten fragment do poprawy najlepiej pobrac home z APM/PIX
BlokG = 1; //ten fragment zapamietuje gps pod warunkiem fix3/6 sat z aktualnej pozycji
}
POS = (((LatZ - Latitude) * (LatZ - Latitude)) + ((LonZ - Longitude) * (LonZ - Longitude)) * 73);
GPSdist = sqrt(POS) / 1000;
/////////////////////////////////////////////////////////////////////////STEROWANIE DEPPER////////////////////////////////////////////////////////////////////////////
if (Ch6 > 1800) //DEPPER WLACZNIK
{
digitalWrite(PinDepper, HIGH); //sterowanie pin atmega echosondą
} else { // zostaje
digitalWrite(PinDepper, LOW);
}
///////////////////////////////////////////////////////////////////////////////ODCZYT MISJI//////////////////////////////////////////////////////////////////////////
if (Ch7 > 1800) {
BlokM = 1; // do odczytu misji kanalem 7 z aparatury
} // wrazie wu poprawic
if ((Ch7 < 1200)&&(BlokM == 1)) {
BlokM = 2;
}
//////////////////////////////////////////////////////////////////////////WYBOR PUNKTOW I MAV GOTO///////////////////////////////////////////////////////////////////
if ((Punkty > W_Punkt) && (Ch4 > 1800) && (BlokP == 0)) {
W_Punkt = W_Punkt + 1;
BlokP = 1;
}
if ((Ch4 >= 1200) && (Ch4 <= 1800)) { //wybur punktu misji kanalem 4 z aparatuty
BlokP = 0; //wyslanie do punktu kanalem 5 z aparatury
} //jecze nie działa trzeba uzyc go-to
if ((0 < W_Punkt) && (Ch4 < 1200) && (BlokP == 0)) {
W_Punkt = W_Punkt - 1;
BlokP = 1;
}
if (Ch5 > 1800) { //MAV GOTO TU ZROBIC
}
}
//////////////////////////////////////////////////////////////////////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); //nawet działa wiekszosc czujnikow
telemetry.setSensorValueFP(7, Current_battery); //podzielic fix i sat
telemetry.setSensorValueFP(8, tempS); //podzielic Punkty i W_Punkt
telemetry.setSensorValueFP(9, tempR); //nie wielkie zmiany bibloteka robi robote do 15 czujnikow
telemetry.setSensorValue(10, Heading);
telemetry.setSensorValue(11, HDOP);
telemetry.setSensorValue(12, GPSdist);
telemetry.setSensorValue(13, Klapy);
telemetry.setSensorValue(14, Woda);//
telemetry.setSensorValue(15, Punkty);
}
}
Ta wersja ma dodatkowo odpytac o misje w apm/pix
Zamysł był taki żeby wysyłać łudke do 1 punktu a nie przez 2 lub 3 . Bo to wprowadza chaos na 2 wędce .
Myślę że rozwiazanie dobre koszt około 20 zł za atmege na płytce i parę rezystorów i termistorów i kabelki wiadomo.
Piszę tu bo widzę że wątek jest wciąż aktywny.
Ten kod pobiera misje lepiej lub gorzej zamieszczam bo może ktoś zna angielski i dopytać się na forum APM może da się prościej lub znajdzie ktoś rozważanie.
Autora opentx może uda się poprosić o zmodyfikowanie telemetri typowo pod nas powiedzmy każdy da na flache ,
Rok nie pije a rozpijam innych .liczę na pomoc sugestie .każdy coś wie .
Ten kod pobiera tylko misje nie wysyła do aparatury,coś trza poprawić
Kod: Zaznacz cały
.
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////TELEMETRIA MAVLINK/ARDUINO TO IBUS FLYSKU DO LUDKI ZANETOWEJ////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#include "mavlink.h" //BIBLOTEKA MAVLINK
int punkty; //LICZ PUNKTY
int punktyTAB;
int blokada;
int B;
double Latitude_Misson[10]; //SZEROKOSC GPS MISI
double Longitude_Misson[10];//WYSOKOSC GPS MISI
void setup() {
Serial.begin(57600); //UART PC
Serial1.begin(57600);
}
void loop() {
if (blokada==0){
zapytanie();
blokada=1;
}
if (blokada==1){
odebranie();
Serial.print("punkty; ");Serial.println(punkty);
blokada=2;
}
if (blokada==2){
for (B=punkty; B>=0;B--){
punktyTAB = B;
zapytaniepunkt();
odeM();
Serial.print(punktyTAB);Serial.print(")la; ");Serial.print(Latitude_Misson[punktyTAB], 6 );Serial.print(" lo; ");Serial.println(Longitude_Misson[punktyTAB], 6 );
blokada=0;
}
}
}
////////////////////////////////////////////////////// 1 )zapytanie o ilosc punktow///////////////////////////////////////////////////////////////////////////////////
void zapytanie() {
//Step #1 of uploading a new waypoint
uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software
uint8_t _component_id = 2; // component id of sending station 2 works fine
uint8_t _target_system = 1; // Pixhawk id
uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
mavlink_msg_mission_request_list_pack(_system_id, _component_id, &msg, _target_system, _target_component);
//uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, uint16_t count
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message (.write sends as bytes)
Serial1.write(buf, len);
}
///////////////////////////////////////////////////// 2 )odsluchanie ile jest punktow///////////////////////////////////////////////////////////////////////////////
void odebranie() {
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)
{
// Step 2 uploading a new waypoint - Check for mission replies
case MAVLINK_MSG_ID_MISSION_COUNT:
{
mavlink_mission_count_t mission_count;
mavlink_msg_mission_count_decode(&msg,&mission_count) ;
punkty = mission_count.count;
}
break;
}
}
}
}
/////////////////////////////////////////////////////// 3) zapytanie punkty o kordynaty////////////////////////////////////////////////////////////////////////////
void zapytaniepunkt() {
uint8_t _system_id = 255; // system id of sending station. 255 is Ground control software
uint8_t _component_id = 2; // component id of sending station 2 works fine
uint8_t _target_system = 1; // Pixhawk id
uint8_t _target_component = 0; // Pixhawk component id, 0 = all (seems to work fine)
uint16_t seq = punktyTAB;
// Initialize the required buffers
mavlink_message_t msg;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
// Pack the message
mavlink_msg_mission_request_pack(_system_id, _component_id, &msg,_target_system, _target_component, seq);
// Copy the message to the send buffer
uint16_t len = mavlink_msg_to_send_buffer(buf, &msg);
// Send the message (.write sends as bytes)
Serial1.write(buf, len);
}
/////////////////////////////////////////////////////// 4) zapisanie kordynatow do tablicy ///////////////////////////////////////////////////////////////////////////
void odeM() {
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_MISSION_ITEM:
{
mavlink_mission_item_t mission_item;
mavlink_msg_mission_item_decode(&msg,&mission_item) ;
Latitude_Misson[mission_item.seq] = mission_item.x;
Longitude_Misson[mission_item.seq]= mission_item.y;
}
break;
}
}
}
}
