kopia lustrzana https://github.com/YohanHadji/R2Home
Mathis et Yohan
rodzic
ab37831e7f
commit
ce46aa47b3
Plik binarny nie jest wyświetlany.
Plik binarny nie jest wyświetlany.
|
|
@ -1,196 +0,0 @@
|
|||
#include "PWM.hpp"
|
||||
|
||||
const byte MAX_ISR_COUNT = 20;
|
||||
|
||||
byte isr_count = 0;
|
||||
byte isr_pin[MAX_ISR_COUNT];
|
||||
unsigned int isr_value[MAX_ISR_COUNT];
|
||||
bool isr_last_state[MAX_ISR_COUNT];
|
||||
bool isr_trigger_state[MAX_ISR_COUNT];
|
||||
unsigned long isr_timer[MAX_ISR_COUNT];
|
||||
unsigned long isr_age[MAX_ISR_COUNT];
|
||||
|
||||
void ISR_generic(byte isr) {
|
||||
unsigned long now = micros();
|
||||
bool state_now = digitalRead(isr_pin[isr]);
|
||||
if (state_now != isr_last_state[isr]) {
|
||||
if (state_now == isr_trigger_state[isr]) {
|
||||
isr_timer[isr] = now;
|
||||
} else {
|
||||
isr_value[isr] = (unsigned int)(now - isr_timer[isr]);
|
||||
isr_age[isr] = now;
|
||||
}
|
||||
isr_last_state[isr] = state_now;
|
||||
}
|
||||
}
|
||||
|
||||
void ISR_0() {
|
||||
ISR_generic(0);
|
||||
}
|
||||
|
||||
void ISR_1() {
|
||||
ISR_generic(1);
|
||||
}
|
||||
|
||||
void ISR_2() {
|
||||
ISR_generic(2);
|
||||
}
|
||||
|
||||
void ISR_3() {
|
||||
ISR_generic(3);
|
||||
}
|
||||
|
||||
void ISR_4() {
|
||||
ISR_generic(4);
|
||||
}
|
||||
|
||||
void ISR_5() {
|
||||
ISR_generic(5);
|
||||
}
|
||||
|
||||
void ISR_6() {
|
||||
ISR_generic(6);
|
||||
}
|
||||
|
||||
void ISR_7() {
|
||||
ISR_generic(7);
|
||||
}
|
||||
|
||||
void ISR_8() {
|
||||
ISR_generic(8);
|
||||
}
|
||||
|
||||
void ISR_9() {
|
||||
ISR_generic(9);
|
||||
}
|
||||
|
||||
void ISR_10() {
|
||||
ISR_generic(10);
|
||||
}
|
||||
|
||||
void ISR_11() {
|
||||
ISR_generic(11);
|
||||
}
|
||||
|
||||
void ISR_12() {
|
||||
ISR_generic(12);
|
||||
}
|
||||
|
||||
void ISR_13() {
|
||||
ISR_generic(13);
|
||||
}
|
||||
|
||||
void ISR_14() {
|
||||
ISR_generic(14);
|
||||
}
|
||||
|
||||
void ISR_15() {
|
||||
ISR_generic(15);
|
||||
}
|
||||
|
||||
void ISR_16() {
|
||||
ISR_generic(16);
|
||||
}
|
||||
|
||||
void ISR_17() {
|
||||
ISR_generic(17);
|
||||
}
|
||||
|
||||
void ISR_18() {
|
||||
ISR_generic(18);
|
||||
}
|
||||
|
||||
void ISR_19() {
|
||||
ISR_generic(19);
|
||||
}
|
||||
|
||||
PWM::PWM(byte pin) {
|
||||
my_isr = isr_count;
|
||||
isr_count++;
|
||||
|
||||
isr_pin[my_isr] = pin;
|
||||
pinMode(isr_pin[my_isr], INPUT);
|
||||
}
|
||||
|
||||
int PWM::begin(bool measure_pulse_high) {
|
||||
isr_last_state[my_isr] = digitalRead(isr_pin[my_isr]);
|
||||
isr_trigger_state[my_isr] = measure_pulse_high;
|
||||
|
||||
switch (my_isr) {
|
||||
case 0:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_0, CHANGE);
|
||||
break;
|
||||
case 1:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_1, CHANGE);
|
||||
break;
|
||||
case 2:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_2, CHANGE);
|
||||
break;
|
||||
case 3:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_3, CHANGE);
|
||||
break;
|
||||
case 4:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_4, CHANGE);
|
||||
break;
|
||||
case 5:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_5, CHANGE);
|
||||
break;
|
||||
case 6:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_6, CHANGE);
|
||||
break;
|
||||
case 7:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_7, CHANGE);
|
||||
break;
|
||||
case 8:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_8, CHANGE);
|
||||
break;
|
||||
case 9:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_9, CHANGE);
|
||||
break;
|
||||
case 10:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_10, CHANGE);
|
||||
break;
|
||||
case 11:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_11, CHANGE);
|
||||
break;
|
||||
case 12:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_12, CHANGE);
|
||||
break;
|
||||
case 13:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_13, CHANGE);
|
||||
break;
|
||||
case 14:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_14, CHANGE);
|
||||
break;
|
||||
case 15:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_15, CHANGE);
|
||||
break;
|
||||
case 16:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_16, CHANGE);
|
||||
break;
|
||||
case 17:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_17, CHANGE);
|
||||
break;
|
||||
case 18:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_18, CHANGE);
|
||||
break;
|
||||
case 19:
|
||||
attachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]), ISR_19, CHANGE);
|
||||
break;
|
||||
default:
|
||||
return -1; // Error.
|
||||
}
|
||||
return 0; // Success.
|
||||
}
|
||||
|
||||
unsigned int PWM::getValue() {
|
||||
return isr_value[my_isr];
|
||||
}
|
||||
|
||||
void PWM::end() {
|
||||
detachInterrupt(digitalPinToInterrupt(isr_pin[my_isr]));
|
||||
}
|
||||
|
||||
unsigned long PWM::getAge() {
|
||||
return (micros() - isr_age[my_isr]);
|
||||
}
|
||||
|
|
@ -1,37 +0,0 @@
|
|||
#include <Arduino.h>
|
||||
|
||||
class PWM {
|
||||
byte my_isr;
|
||||
|
||||
public:
|
||||
PWM(byte pin);
|
||||
|
||||
/**
|
||||
Enables interrupt.
|
||||
|
||||
@param measure_pulse_high true: High Pulse duration is measured (normal pwm), false: Low Pulse duration is measured (inverted pwm).
|
||||
|
||||
@return 0 Success.
|
||||
@return -1 Error.
|
||||
*/
|
||||
int begin(bool measure_pulse_high);
|
||||
|
||||
/**
|
||||
Returns the most recent PWM value received.
|
||||
|
||||
@return PWM duration in microseconds.
|
||||
*/
|
||||
unsigned int getValue();
|
||||
|
||||
/**
|
||||
Returns the age of recent PWM value.
|
||||
|
||||
@return Age in microseconds.
|
||||
*/
|
||||
unsigned long getAge();
|
||||
|
||||
/**
|
||||
Disables interrupt.
|
||||
*/
|
||||
void end();
|
||||
};
|
||||
|
|
@ -1,329 +0,0 @@
|
|||
|
||||
#include <QMC5883LCompass.h>
|
||||
#include "Ublox.h"
|
||||
#include <SD.h>
|
||||
#include <SPI.h>
|
||||
#include <EEPROM.h>
|
||||
#include <Servo.h>
|
||||
#include <LoRa.h>
|
||||
#include "PWM.hpp"
|
||||
|
||||
const int chipSelect = BUILTIN_SDCARD;
|
||||
|
||||
char text[70];
|
||||
|
||||
float t2 = 0;
|
||||
float t1 = 0;
|
||||
int count = 1;
|
||||
char namebuff[20];
|
||||
unsigned int addr = 0;
|
||||
String filename;
|
||||
int led = 13;
|
||||
int datatest = 0;
|
||||
|
||||
float ail1 = 0;
|
||||
float ail2 = 0;
|
||||
|
||||
float elv = 0;
|
||||
|
||||
|
||||
char magn_text[30];
|
||||
char lat_text[30];
|
||||
char lon_text[30];
|
||||
char alt_text[30];
|
||||
char cog_text[30];
|
||||
char spe_text[30];
|
||||
char sat_text[30];
|
||||
char pwm1_text[30];
|
||||
char pwm2_text[30];
|
||||
char pwm3_text[30];
|
||||
|
||||
|
||||
float B = 0;
|
||||
float cog = 0;
|
||||
float spe = 0;
|
||||
float magn = 0;
|
||||
float bearing = 0;
|
||||
float degr = 0;
|
||||
float offst = 0;
|
||||
float cmd = 0;
|
||||
float alt = 0;
|
||||
float lata = 0;
|
||||
float latb = 45.195889269053446;
|
||||
float lona = 0;
|
||||
float lonb = 5.68393349647522;
|
||||
float sat = 0;
|
||||
int toss = 0;
|
||||
|
||||
int counter = 0;
|
||||
void getgps();
|
||||
void getcompass();
|
||||
|
||||
PWM ch1(4);
|
||||
PWM ch2(5);
|
||||
PWM ch3(6);
|
||||
|
||||
int pwm1 = 0;
|
||||
int pwm2 = 0;
|
||||
int pwm3 = 0;
|
||||
|
||||
|
||||
Servo servo1;
|
||||
Servo servo2;
|
||||
Ublox M8_Gps;
|
||||
QMC5883LCompass compass;
|
||||
|
||||
void setup() {
|
||||
|
||||
ch1.begin(true); // ch1 on pin 2 reading PWM HIGH duration
|
||||
ch2.begin(true); // ch2 on pin 3 reading PWM HIGH duration
|
||||
ch3.begin(true); // ch3 on pin 18 reading PWM HIGH duration
|
||||
|
||||
pinMode(led, OUTPUT);
|
||||
|
||||
//eppclear()
|
||||
epprom();
|
||||
|
||||
|
||||
Serial.begin(57600);
|
||||
SD.begin(chipSelect);
|
||||
Serial7.begin(57600);
|
||||
compass.init();
|
||||
compass.setCalibration(-1602, 1045, -1826, 647, -2963, 0);
|
||||
LoRa.begin(433E6);
|
||||
servo1.attach(19);
|
||||
servo2.attach(22);
|
||||
|
||||
|
||||
}
|
||||
|
||||
void loop() {
|
||||
|
||||
t1 = millis();
|
||||
|
||||
if (t1-t2>500) {
|
||||
|
||||
t2 = millis();
|
||||
|
||||
|
||||
datalog();
|
||||
|
||||
if (!LoRa.begin(433E6)) {
|
||||
|
||||
}
|
||||
|
||||
else {
|
||||
|
||||
sendtelem();
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
getgps();
|
||||
getcompass();
|
||||
|
||||
//B = magn;
|
||||
|
||||
//comptobj();
|
||||
rcread();
|
||||
servocmd();
|
||||
|
||||
|
||||
|
||||
}
|
||||
//-----------------------------------------------//
|
||||
void datalog() {
|
||||
|
||||
File dataFile = SD.open(namebuff, FILE_WRITE);
|
||||
|
||||
if (dataFile) {
|
||||
|
||||
dtostrf(magn, 4, 1, magn_text);
|
||||
dtostrf(lata, 10, 10, lat_text);
|
||||
dtostrf(lona, 10, 10, lon_text);
|
||||
dtostrf(alt, 4, 1, alt_text);
|
||||
dtostrf(cog, 4, 1, cog_text);
|
||||
dtostrf(spe, 5, 1, spe_text);
|
||||
dtostrf(sat, 3, 1, sat_text);
|
||||
dtostrf(pwm1, 4, 1, pwm1_text);
|
||||
dtostrf(pwm2, 4, 1, pwm2_text);
|
||||
dtostrf(pwm3, 4, 1, pwm3_text);
|
||||
|
||||
snprintf(text, 80, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s", magn_text, lat_text, lon_text, alt_text, cog_text, spe_text, sat_text, pwm1_text, pwm2_text, pwm3_text);
|
||||
|
||||
dataFile.println(text);
|
||||
dataFile.close();
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
//-----------------------------------------------//
|
||||
void epprom() {
|
||||
|
||||
EEPROM.get(addr, count);
|
||||
sprintf(namebuff, "Data%0d.txt", count);
|
||||
EEPROM.write(addr, (count+1));
|
||||
}
|
||||
//-----------------------------------------------//
|
||||
void eppclear() {
|
||||
for ( unsigned int i = 0 ; i < EEPROM.length() ; i++ )
|
||||
EEPROM.write(i, 0);
|
||||
}
|
||||
//-----------------------------------------------//
|
||||
void getgps() {
|
||||
if(!Serial7.available())
|
||||
return;
|
||||
|
||||
while(Serial7.available()){
|
||||
|
||||
char c = Serial7.read();
|
||||
if (M8_Gps.encode(c)) {
|
||||
|
||||
cog = ((M8_Gps.course)/100);
|
||||
spe = (M8_Gps.speed)/100;
|
||||
lata = M8_Gps.latitude;
|
||||
lona = M8_Gps.longitude;
|
||||
alt = M8_Gps.altitude;
|
||||
sat = M8_Gps.sats_in_use;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
//-----------------------------------------------//
|
||||
void getcompass() {
|
||||
compass.read();
|
||||
magn = 360 - ( ((compass.getAzimuth())+90) % 360 ) ;
|
||||
}
|
||||
//-----------------------------------------------//
|
||||
void averagedir() {
|
||||
if ((sqrt((magn-cog)*(magn-cog))) < 180) {
|
||||
|
||||
B = (magn + cog) / 2 ;
|
||||
|
||||
}
|
||||
|
||||
else {
|
||||
|
||||
B = fmod((((magn + cog)/2)+180), 360);
|
||||
|
||||
}
|
||||
}
|
||||
//-----------------------------------------------//
|
||||
void comptobj() {
|
||||
|
||||
float DX = latb - lata;
|
||||
float DY = lonb - lona;
|
||||
float rad = atan2(DY, DX);
|
||||
degr = rad * (180 / PI);
|
||||
bearing = fmod(degr + 360, 360);
|
||||
float A = bearing ;
|
||||
// float B = magn ;
|
||||
|
||||
if (sqrt((A-B)*(A-B)) < 180) {
|
||||
offst = (A-B);
|
||||
}
|
||||
|
||||
else {
|
||||
|
||||
if ((A-B) < 0) {
|
||||
offst = 360 +(A-B);
|
||||
}
|
||||
|
||||
else {
|
||||
offst = (-360) + (A-B) ;
|
||||
}
|
||||
}
|
||||
}
|
||||
//-----------------------------------------------//
|
||||
void servocmd() {
|
||||
//cmd = map(offst, -180,180, 1000, 2000);
|
||||
//servo.write(cmd);
|
||||
|
||||
|
||||
|
||||
float diff1 = sqrt(pow(1500-pwm1, 2)) ;
|
||||
float diff2 = sqrt(pow(1500-pwm2, 2)) ;
|
||||
|
||||
if (1500-pwm1 > 0) {
|
||||
|
||||
ail1 = 1500 - diff1 ;
|
||||
ail2 = 1500 + diff1 ;
|
||||
|
||||
}
|
||||
else {
|
||||
|
||||
ail1 = 1500 + diff1 ;
|
||||
ail2 = 1500 - diff1 ;
|
||||
|
||||
}
|
||||
|
||||
if (1500-pwm2 > 0) {
|
||||
|
||||
elv = 1500 + diff2 ;
|
||||
|
||||
}
|
||||
else {
|
||||
|
||||
elv = 1500 - diff2 ;
|
||||
|
||||
}
|
||||
|
||||
|
||||
//Serial.print(ail1); Serial.print(" ");
|
||||
//Serial.println(ail2);
|
||||
|
||||
float cmd1 = (0.5 * ail1) + (0.5* elv);
|
||||
float cmd2 = (0.5 * ail2) + (0.5* elv);
|
||||
|
||||
if (cmd1 > 2000 or cmd2 > 2000 or cmd1 < 1000 or cmd2 < 1000) {
|
||||
cmd1 = 1500;
|
||||
cmd2 = 1500;
|
||||
}
|
||||
|
||||
Serial.print(cmd1); Serial.print(" ");
|
||||
Serial.println(cmd2);
|
||||
|
||||
servo1.write(cmd1);
|
||||
servo2.write(cmd2);
|
||||
|
||||
|
||||
}
|
||||
//-----------------------------------------------//
|
||||
void sendtelem() {
|
||||
|
||||
//dtostrf(magn, 4, 1, magn_text);
|
||||
dtostrf(lata, 10, 10, lat_text);
|
||||
dtostrf(lona, 10, 10, lon_text);
|
||||
dtostrf(alt, 3, 1, alt_text);
|
||||
//dtostrf(cog, 4, 1, cog_text);
|
||||
//dtostrf(spe, 5, 1, spe_text);
|
||||
dtostrf(sat, 3, 1, sat_text);
|
||||
|
||||
snprintf(text, 45, "%s,%s,%s,%s", lat_text, lon_text, alt_text, sat_text);
|
||||
//Serial.println(text);
|
||||
|
||||
|
||||
|
||||
LoRa.beginPacket();
|
||||
LoRa.println(text);
|
||||
LoRa.endPacket();
|
||||
|
||||
}
|
||||
//-----------------------------------------------//
|
||||
void rcread() {
|
||||
|
||||
pwm1 = ch1.getValue();
|
||||
pwm2 = ch2.getValue();
|
||||
pwm3 = ch3.getValue();
|
||||
//Serial.print("CH1 : ");
|
||||
//Serial.print(pwm1); Serial.print(" ");
|
||||
//Serial.print("CH2 : ");
|
||||
//Serial.print(pwm2); Serial.print(" ");
|
||||
//Serial.print("CH3 : ");
|
||||
//Serial.println(pwm3);
|
||||
|
||||
}
|
||||
Plik binarny nie jest wyświetlany.
|
|
@ -0,0 +1,16 @@
|
|||
Name ,Manufacture Part Number,Order Qty.
|
||||
JST-GH PCB Connector,BM02B-GHS-TBT,10
|
||||
JST-GH PCB Connector,SM02B-GHS-TB,10
|
||||
JST-GH PCB Connector,BM03B-GHS-TBT,10
|
||||
JST-GH PCB Connector,SM04B-GHS-TB,10
|
||||
JST-GH PCB Connector,BM04B-GHS-TBT,10
|
||||
JST-GH PCB Connector,SM05B-GHS-TB,10
|
||||
JST-GH PCB Connector,SM06B-GHS-TB,10
|
||||
JST-GH PCB Connector,BM06B-GHS-TBT,10
|
||||
JST-HX PCB Connector,S2B-XH-A,10
|
||||
5V Linear Voltage Regulator ,L7805ABV ,2
|
||||
5-35V to 7-9V Buck Converter Breakout,,2
|
||||
5-35V to 5-6V Buck Converter Breakout,,2
|
||||
Capacitor 10µF 16V 5*12mm,,5
|
||||
Female Standart 2.54mm Header >24*1,,2
|
||||
Male Standart 2.54mm Header > 3*1,,3
|
||||
|
Ładowanie…
Reference in New Issue