Ż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
Powoli przychodzi czas na zaprojektowanie pcb bo na drucikach słabo wygląda.
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.