mirror
/
R2Home
kopia lustrzana https://github.com/YohanHadji/R2Home
1
0
Forkuj 0
Kod Zgłoszenia Projekty Wydania Wiki Aktywność

Adding new code!

pull/1/head
YohanHadji 2022-03-25 19:03:31 +01:00
rodzic e8d67be65f
commit 2a03120871
16 zmienionych plików z 1771 dodań i 193 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

335
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.02/R2Home_OBC_V1.02.ino → RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.03/R2Home_OBC_V1.03.ino
Wyświetl plik

@ -23,8 +23,8 @@
#define drop true // R2Home's version, drop or motorised
#define record_home false // only record autopilot
#define dep_alt 100 // m above ground
#define vup 3 // m/s
#define vdown -3 // m/s
#define vup 1 // m/s
#define vdown -1 // m/s
#define gps_freq 5 // Hz
#define nav_waypoint false // Doing the waypoint sequence before reaching home?
#define nav_home true // Should it go home after the waypoint sequence?
@ -35,7 +35,8 @@
#define safe_trigger false // For HAB flight, will use a safer, but slower methode to detect apogee and transision to descent mode
#define cog_brake false // Will reduce command if CoG is turning faster than a threshold
#define descent_timer 5 // Code will check for x times every second that the vertical speed is lower than threshold to trigger descent mode
//#define descent_timer 5 Code will check for x times every second that the vertical speed is lower than threshold to trigger descent mode
#define gliding_timer 3000
#define time_out 300
@ -52,7 +53,6 @@
#define left_offset 100
#define right_offset 100
#define gliding_timer 3000
#define waypoint_threshold 10 // Distance to waypoint before going to the next waypoint
int dep_altitude = dep_alt;
@ -60,36 +60,29 @@ int cog_count = 2;
double Setpoint, Input, Output;
// NAV PIDs //
float NKp = 1;
float NKi = 0.05;
float NKd = 0.1;
PID myPID(&Input, &Output, &Setpoint,NKp, NKi, NKd, DIRECT);
double long sim_cmd_time = 0;
float sim_cmd = 0;
// ----------------------------------- GLOBAL VARIABLES ----------------------------------- //
// BARO //
Adafruit_BMP280 bmp(&Wire);
int baro_adress = 0x00;
int baro_adress = 0x00;
movingAvg baro_avg(20);
float alt_baro = 0;
float prev_alt = 0;
float vspeed = 0;
float baro_set = 1000;
float baro_count = 0;
int vspeed_count = 0;
bool new_baro = false;
float dt = 0;
bool stable_descent = false;
int descent_count = 0;
movingAvg al(5);
movingAvg vs(5);
bool new_baro_alt = false;
float baro_vspeed = 0;
float prev_alt_baro = 0;
unsigned long prev_time_baro = 0;
unsigned long stable_baro_timer = 0;
int stable_baro_alt_since = 0;
int baro_count = 0;
// BATTERY //
movingAvg voltage(50);
@ -102,15 +95,26 @@ boolean batt_low = false;
TinyGPSPlus gps;
TinyGPSCustom fix_type(gps, "GNGSA", 2);
movingAvg rs(1);
movingAvg gps_vs_time(10);
unsigned char serial2bufferRead[1000];
float prev_cog = 0;
int gps_count = 0;
int valid_count = 0;
float prev_gps = 0;
float prev_cog = 0;
boolean new_gps = false;
boolean cog_ok = 0;
boolean new_cog = false;
float ground_altitude = 0;
int valid_count = 0;
bool new_gps_alt = false;
float gps_vspeed = 0;
float prev_alt_gps = 0;
unsigned long prev_time_gps = 0;
unsigned long stable_gps_timer = 0;
int stable_gps_alt_since = 0;
bool new_merged_alt = false;
float merged_vspeed = 0;
float prev_alt_merged = 0;
unsigned long prev_time_merged = 0;
// LED //
#define LED_PIN 3
@ -143,7 +147,6 @@ int cells = 0;
bool armed = false;
bool camIsOn = false;
bool camReady = false;
bool flight_started = false;
int onDuration = 0;
int offDuration = 0;
@ -161,6 +164,7 @@ float lat_B = 0;
float lon_B = 0;
float cmd = 0;
boolean spiral = false;
float sim_cmd = 0;
float cmdHome = 1500;
float next_cog = 0;
float ratecog = 0;
@ -272,7 +276,6 @@ int loop_time_max_glob = 999;
double loop_time_mean = 999;
int loop_time_count = 0;
int delaySD = 100; // Datalog
int delayTLM = 1000; // Tlm
@ -302,9 +305,8 @@ void setup() {
EasyBuzzer.setPin(buzzer);
voltage.begin();
vs.begin();
al.begin();
baro_avg.begin();
voltage.begin();
roll_steer.begin();
pitch_steer.begin();
rs.begin();
@ -328,12 +330,8 @@ void setup() {
watchdog.enable(Watchdog::TIMEOUT_1S);
getconfig();
bmp.begin(baro_adress);
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
Adafruit_BMP280::SAMPLING_X1,
Adafruit_BMP280::SAMPLING_X2,
Adafruit_BMP280::FILTER_X16,
Adafruit_BMP280::STANDBY_MS_1);
baro_begin();
strip.begin();
strip.show();
@ -368,7 +366,6 @@ void setup() {
void loop() {
tloop = micros();
sim();
getdata();
@ -382,7 +379,6 @@ void loop() {
userinter();
loop_time = micros()-tloop;
loop_time_cmpt();
}
@ -391,34 +387,32 @@ void getdata() {
// -------------------------- Get GPS -------------------------- //
while (Serial7.available()) { gps.encode(Serial7.read()); }
while (Serial7.available()) {
gps.encode(Serial7.read());
if (gps.altitude.isUpdated()) {
new_gps_alt = true;
}
}
// -------------------------- Get BARO & COMPASS -------------------------- //
if ((millis()-baroA)>=10) {
if ((millis()-baroA)>=5) {
baroA = millis();
unsigned waitd = millis();
alt_baro = al.reading(bmp.readAltitude(baro_set)*100);
alt_baro = bmp.readAltitude(baro_set);
alt_baro = baro_avg.reading(alt_baro*100.0);
alt_baro = alt_baro/100.0;
if (((millis() - waitd) > 100)) {
EasyBuzzer.singleBeep(3000,50);
bmp.begin(baro_adress);
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
Adafruit_BMP280::SAMPLING_X1,
Adafruit_BMP280::SAMPLING_X2,
Adafruit_BMP280::FILTER_X16,
Adafruit_BMP280::STANDBY_MS_1);
}
alt_baro = (alt_baro/100);
baro_count = (baro_count + 1);;
if (baro_count >= 10) {
baro_count = 0;
new_baro = true;
dt = baroA-baroB;
baroB = millis();
}
baro_begin();
}
}
// -------------------------- Get RC -------------------------- //
rx.read(&channels[0], &failSafe, &lostFrame);
@ -498,12 +492,9 @@ void datacmpt() {
//cmdHome = cmd_mult*PIDsum
cmdHome = cmd_mult*Output;
if (vspeed<vdown-5) { spiral = true; }
if (vspeed>vdown-3) { spiral = false; }
if (baro_vspeed<vdown-5) { spiral = true; }
if (baro_vspeed>vdown-3) { spiral = false; }
if (spiral == true) { cmdHome = 0; }
if ((TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),lat_B,lon_B))<10) {
@ -526,70 +517,54 @@ void datacmpt() {
steer_auto = map(sim_cmd, -180, +180, 1000, 2000);
}
// -------------------- Alt Fusion -------------------- //
baro_weight = (1+(abs(baro_vspeed)/10));
float hdo = gps.hdop.value();
float gpsw = ((50/hdo)+(gps.altitude.meters()/10000));
gps_weight = (gpsw*((abs(alt_baro-gps.altitude.meters())/10)));
float total_weight = gps_weight+baro_weight;
merged_alt = ((alt_baro*baro_weight)+(gps.altitude.meters()*gps_weight))/(total_weight);
// -------------------- Vertical Speed -------------------- //
if (new_baro == true) {
new_baro = false;
if ((initialised == false) and (reboot_state != 1)) { baro_set = (baro_set + ((0-alt_baro)/100)); }
if (millis()<=5000) { alt_baro = 0; }
float da = (alt_baro - prev_alt);
if (abs(da) < 50) {
float vps = (da/(dt/1000));
vspeed = vs.reading(vps*100);
vspeed = (vspeed/100);
}
prev_alt = alt_baro;
if ((initialised == false) and (reboot_state != 1)) {
baro_set = (baro_set + ((0-alt_baro)/100));
}
if (millis()<=7500) { alt_baro = 0; }
if (millis()-prev_time_baro>=200) {
new_baro_alt = false;
float baro_da = (alt_baro - prev_alt_baro);
float baro_dt = (millis() - prev_time_baro);
baro_vspeed = (baro_da/(200.0/1000.0));
prev_alt_baro = alt_baro;
prev_time_baro = millis();
}
// -------------------- Alt Fusion -------------------- //
if (new_gps_alt) {
new_gps_alt = false;
float gps_da = (gps.altitude.meters() - prev_alt_gps);
float gps_dt = (millis() - prev_time_gps);
gps_vspeed = (gps_da/(gps_dt/1000));
prev_alt_gps = gps.altitude.meters();
prev_time_gps = millis();
}
baro_weight = (1+(abs(vspeed)/10));
float hdo = gps.hdop.value();
float gpsw = ((50/hdo)+(gps.altitude.meters()/10000));
gps_weight = (gpsw*((abs(alt_baro-gps.altitude.meters())/10)));
merged_alt = ((alt_baro*baro_weight)+(gps.altitude.meters()*gps_weight))/(baro_weight+gps_weight);
if (abs(baro_vspeed) > 1) {
stable_baro_timer = millis();
}
if (abs(gps_vspeed) > 1) {
stable_gps_timer = millis();
}
// -------------------- Stationary -------------------- //
if ((millis()-tstab) >= 1000) {
tstab = millis();
if (abs(vspeed) < 0.2) { vspeed_count = (vspeed_count + 1); }
else { vspeed_count = 0; }
if (abs(vspeed)>(-vdown)) { descent_count = (descent_count + 1); }
else { descent_count = 0; }
if (abs(prev_gps-gps.altitude.meters())<1 and (gps.altitude.meters() != 0)) { gps_count = (gps_count + 1); }
else { gps_count = 0; }
if (gps_count >= 10) { gps_stab = true; }
else if(millis()<(time_out*1000)) { gps_stab = false; }
else { gps_stab = true; }
if (vspeed_count >= 10) { baro_stab = true; }
else { baro_stab = false; }
if (descent_count >= descent_timer) { stable_descent = true; }
else { stable_descent = false; }
if (gps_count >= 10) { gps_stab = true; }
else { gps_stab = false; }
prev_gps = gps.altitude.meters();
}
stable_gps_alt_since = millis()-stable_gps_timer;
stable_baro_alt_since = millis()-stable_baro_timer;
// -------------------------- String -------------------------- //
@ -621,7 +596,7 @@ void datacmpt() {
String gps_text = date_time+","+lat_A_text+","+lon_A_text+","+alt_gps_text+","+cog_text+","+speed_text+","+sat_text+","+hdop_text+","+pos_age_text+","+fix_type_text;
String alt_baro_text = String(alt_baro, 3);
String vspeed_text = String(vspeed, 3);
String vspeed_text = String(baro_vspeed, 3);
String baro_text = alt_baro_text+","+vspeed_text;
@ -674,8 +649,8 @@ void datacmpt() {
String gps_ok_text = String(gps_ok) ;
String cog_ok_text = String(cog_ok);
String failSafe_text = String(failSafe);
String gps_stab_text = String(gps_stab);
String baro_stab_text = String(baro_stab);
String gps_stab_text = String(stable_gps_alt_since);
String baro_stab_text = String(stable_baro_alt_since);
String deployed_text = String(deployed);
String wing_opened_text = String(wing_opened);
String vbatt_text = String(vbatt, 3);
@ -719,7 +694,7 @@ void datacmpt() {
tlm = millis();
packet_count = (packet_count +1);
Serial5.println(mainTLM);
Serial.println(mainTLM);
//Serial.println(mainTLM);
if (sd_ok == true and sd_writing == true) {
dataFile = SD.open(namebuff, FILE_WRITE);
@ -837,11 +812,6 @@ void flight_state() {
motorised_failSafe();
setled(0, 255, 255, 25, 500);
break;
case 11:
motorised_cruise();
setled(0, 255, 255, 25, 500);
break;
}
}
@ -851,7 +821,7 @@ void flight_init() {
if (reboot_state !=1) {
if ((gps.satellites.value()>=6 and gps_ok and (gps_stab or (gps.satellites.value()>=6 and millis()>300000)) and millis()>5000) or no_init) {
if ((gps.satellites.value()>=6 and gps_ok and millis()>5000) or no_init) {
EasyBuzzer.beep(3000,100,50,10,500,1);
@ -876,9 +846,6 @@ void flight_init() {
EEPROM.put(50, lon_B);
baroset();
vs.reset();
al.reset();
if (no_init == false) {
dep_altitude = (dep_altitude+gps.altitude.meters());
@ -907,9 +874,6 @@ void flight_init() {
EasyBuzzer.beep(3000,100,50,10,500,1);
initialised = true;
vs.reset();
al.reset();
strip.setBrightness(50);
setcam(1, 60, 60);
@ -938,55 +902,48 @@ void flight_init() {
//------------------- 1 -------------------//
void ready_steady() {
if (millis()-init_time>=1000) {
if (vspeed>vup) {
if (baro_vspeed>vup) {
flight_mode = 2;
EasyBuzzer.beep(1000,100,50,2,500,1);
strip.setBrightness(255);
setcam(1, 20, 600);
}
if (safe_trigger and stable_descent) {
else if (baro_vspeed<vdown) {
flight_mode = 3;
EasyBuzzer.beep(3000,100,50,3,500,1);
strip.setBrightness(255);
setcam(1, 60, 600);
}
else if (vspeed<vdown) {
flight_mode = 3;
EasyBuzzer.beep(3000,100,50,3,500,1);
strip.setBrightness(255);
setcam(1, 60, 600);
}
if ((atof(fix_type.value()) < 2) and (no_init == false) and (flight_started == false)) {
flight_mode = 0;
EasyBuzzer.beep(700,100,50,3,500,1);
strip.setBrightness(255);
}
}
}
}
//------------------- 2 -------------------//
void flight_ascent() {
if (vspeed<0.5) {flight_mode = 1; strip.setBrightness(255);}
if ((gps.altitude.meters()-ground_altitude)>10) {
flight_started = true;
}
if (baro_vspeed<0.5) {
flight_mode = 1; strip.setBrightness(255);
}
}
//------------------- 3 -------------------//
void flight_descent() {
void flight_descent() {
if (baro_vspeed>1) {
flight_mode = 1;
strip.setBrightness(255);
}
if (vspeed>-0.5) {flight_mode = 1; strip.setBrightness(255);}
if (merged_alt < dep_altitude) { flight_mode = 4; EasyBuzzer.beep(3000,100,50,5,500,1); strip.setBrightness(255); deployed = true; init_time = millis(); setcam(1, 60, 600); }
}
if (merged_alt < dep_altitude) {
flight_mode = 4;
EasyBuzzer.beep(3000,100,50,5,500,1);
strip.setBrightness(255);
deployed = true;
init_time = millis();
setcam(1, 60, 600);
}
}
//------------------- 4 -------------------//
@ -1019,7 +976,10 @@ void flight_gliding_auto() {
myPID.SetMode(MANUAL);
}
if (i_want_to_fly == true) { spiral = false; flight_mode = 5; }
if (i_want_to_fly == true) {
spiral = false;
flight_mode = 5;
}
navigation();
}
@ -1051,12 +1011,6 @@ void motorised_man() {
myPID.SetMode(AUTOMATIC);
}
else if (channels[5] > 1000) {
flight_mode = 11;
cruise_cmd = gps.course.deg();
if (record_home == true) {newfile();}
}
if (failSafe == true) {
flight_mode = 10;
if (record_home == true) {newfile();}
@ -1068,8 +1022,11 @@ void motorised_man() {
void motorised_auto() {
navigation();
if (channels[6] < 1000) { flight_mode = 8; }
navigation();
if (channels[6] < 1000) {
flight_mode = 8;
}
}
//------------------- 10 -------------------//
@ -1084,23 +1041,6 @@ void motorised_failSafe() {
}
}
//------------------- 11 -------------------//
void motorised_cruise() {
if (channels[5] < 1000) {
flight_mode = 8;
myPID.SetMode(MANUAL);
}
float incr = map(channels[0], 67, 1982, -1000, 1000);
if (abs(incr)<50) { incr = 0; }
incr = incr/10000.0;
cruise_cmd = cruise_cmd + incr;
cruise_cmd = (int(cruise_cmd*1000.0)%360000)/1000.0;
Serial.println(cruise_cmd);
}
// ----------------------------------------------------- Apply command ----------------------------------------------------- //
void applycmd() {
@ -1248,6 +1188,7 @@ if (drop == true) {
}
}
else {
@ -1259,7 +1200,8 @@ else {
esc.write(map(servo_esc, 1000, 2000, 0, 180));
}
}
}
}
// ----------------------------------------------------- User Interface ----------------------------------------------------- //
@ -1487,11 +1429,10 @@ void baroset() {
baro_blk = millis();
alt_baro = (bmp.readAltitude(baro_set));
baro_set = (baro_set + ((gps.altitude.meters()-alt_baro)/8));
prev_alt = alt_baro;
watchdog.reset();
}
}
baro_avg.reset();
EEPROM.put(70, baro_set);
}
@ -1599,8 +1540,8 @@ void newfile() {
}
void sim() {
sim_cmd_time = (millis()*(PI/10000));
sim_cmd = sin(sim_cmd_time);
double long sim_cmd_time = (millis()*(PI/10000));
float sim_cmd = sin(sim_cmd_time);
sim_cmd = map(sim_cmd, -1, 1, -180, 180);
if (test_dir_rc) {
sim_cmd = map(roll_man, 1000, 2000, -180, 180);
@ -1730,3 +1671,11 @@ if (loop_time_count >= 100) {
}
}
}
void baro_begin() {
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
Adafruit_BMP280::SAMPLING_X1,
Adafruit_BMP280::SAMPLING_X16,
Adafruit_BMP280::FILTER_X16,
Adafruit_BMP280::STANDBY_MS_1);
}

121
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/barometer.hpp 100644
Wyświetl plik

@ -0,0 +1,121 @@
#include "config.h"
#include <Adafruit_BMP280.h>
#include <movingAvg.h>
Adafruit_BMP280 bmp(&Wire);
int baro_adress = 0x00;
double baro_set = 1000.0;
float alt_baro = 0;
float pressure_baro = 0;
float prev_alt_baro = 0;
float baro_vspeed = 0;
float baro_count = 0;
bool new_baro = false;
bool stable_descent = false;
movingAvg b_al(BARO_AL_AVG);
movingAvg b_vs(BARO_VS_AVG);
movingAvg ps(BARO_PS_AVG);
unsigned long baroA = 0;
unsigned long baroB = 0;
unsigned long baro_blk = 0;
unsigned long baro_clb = 0;
void baroset(float alt_set, int factor) {
do {
if ((micros()-baro_blk)>=10) {
baro_blk = millis();
alt_baro = bmp.readAltitude(baro_set);
baro_set = (baro_set + ((alt_set-alt_baro)/(8*factor)));
prev_alt_baro = alt_baro;
}
} while (abs(alt_baro-alt_set)>0.01);
}
void baro_adjust(float alt_set, int factor) {
if ((millis()-baro_clb)>1000) {
baro_clb = millis();
alt_baro = bmp.readAltitude(baro_set);
baro_set = (baro_set + ((alt_set-alt_baro)/(8*factor)));
}
}
void barometer_setup() {
b_vs.begin();
b_al.begin();
ps.begin();
bmp.begin(baro_adress);
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
Adafruit_BMP280::SAMPLING_X1,
Adafruit_BMP280::SAMPLING_X2,
Adafruit_BMP280::FILTER_X16,
Adafruit_BMP280::STANDBY_MS_1);
baroset(0, 1);
}
void cmpt_vertical_speed_baro(float da, int dt) {
baro_vspeed = (da/(dt/1000.0));
baro_vspeed = b_vs.reading(baro_vspeed*100);
baro_vspeed = (baro_vspeed/100);
}
void get_baro(int mode) {
if ((millis()-baroA)>=10 and mode == 0) {
baroA = millis();
unsigned waitd = millis();
alt_baro = bmp.readAltitude(baro_set);
pressure_baro = bmp.readPressure();
alt_baro = (b_al.reading(alt_baro*100.0)/100.0);
pressure_baro = (ps.reading(pressure_baro*100.0)/100.0);
if (((millis() - waitd) >= 100)) {
barometer_setup();
}
baro_count = (baro_count + 1);;
if (baro_count >= BARO_VS_SAMPLE) {
baro_count = 0;
cmpt_vertical_speed_baro(alt_baro-prev_alt_baro, baroA-baroB);
baroB = millis();
prev_alt_baro = alt_baro;
new_baro = true;
}
}
else {
baroA = millis();
unsigned waitd = millis();
alt_baro = bmp.readAltitude(baro_set);
pressure_baro = bmp.readPressure();
alt_baro = (b_al.reading(alt_baro*100.0)/100.0);
pressure_baro = (ps.reading(pressure_baro*100.0)/100.0);
if (((millis() - waitd) >= 100)) {
barometer_setup();
}
baro_count = (baro_count + 1);;
if (baro_count >= BARO_VS_SAMPLE) {
baro_count = 0;
cmpt_vertical_speed_baro(alt_baro-prev_alt_baro, baroA-baroB);
baroB = millis();
prev_alt_baro = alt_baro;
new_baro = true;
}
}
}
String baro_text() {
String alt_baro_text = String(alt_baro, 3);
String baro_vspeed_text = String(baro_vspeed, 3);
return alt_baro_text+","+baro_vspeed_text;
}

23
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/battery.hpp 100644
Wyświetl plik

@ -0,0 +1,23 @@
#include "config.h"
#include <movingAvg.h>
movingAvg voltage(50);
float vpin = A17;
float vbatt = 0;
boolean batt_critical = false;
boolean batt_low = false;
int cells = 0;
void battery_setup() {
pinMode(vpin, INPUT);
voltage.begin();
}
void get_vbatt() {
analogReadResolution(12);
vbatt = analogRead(A17);
vbatt = (voltage.reading(vbatt));
vbatt = vbatt/223.39;
}

52
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/camera.hpp 100644
Wyświetl plik

@ -0,0 +1,52 @@
#include "config.h"
int cam = 23;
bool camIsOn = false;
bool camReady = false;
int onDuration = 0;
int offDuration = 0;
unsigned long camOn = 0;
unsigned long camOff = 0;
void camera_setup() {
pinMode(cam, OUTPUT);
digitalWrite(cam, LOW);
}
void setcam(int a, int b, int c) {
switch(a) {
case 1 :
digitalWrite(cam, HIGH);
camOn = millis();
onDuration = (b+4)*1000;
offDuration = c*1000;
camIsOn = true;
break;
case 0 :
digitalWrite(cam, LOW);
camOff = millis();
onDuration = (b+4)*1000;
offDuration = c*1000;
camIsOn = false;
break;
}
camReady = true;
}
void updatecam() {
if (CAM_CYCLE and camReady) {
if(camIsOn) {
if ((millis()-camOn)>=onDuration) {
setcam(0, (onDuration/1000)-4, offDuration/1000);
}
else {
if ((millis()-camOff)>offDuration) {
setcam(1, (onDuration/1000)-4, offDuration/1000);
}
}
}
}
}

65
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/config.h 100644
Wyświetl plik

@ -0,0 +1,65 @@
//---------- CONFIG ----------//
#define I_WANT_TO_FLY false // Simulated servo movement to test the servo movement :))
#define TEST_DIR_RC false // Use channels 0 on the radio to test the direction of the autopilot and the servos, I_WANT_TO_FLY should be set true too.
#define BUZZER_TURN false // Buzzer sounds as function of the turn command
#define BUZZER_SWEEP false // Buzzer turn on steroïds, should be easier to understand his tricky language ^^
#define NO_INIT false // Skip init, for testing only purposes
#define NAV_WAYPOINT true // Doing the waypoint sequence before reaching home?
#define NAV_HOME false // Should it go home after the waypoint sequence?
#define SD_WRITING true // Should it write on the SD card or not?
#define LOW_RATE false // Dataloging at low HZ (if true) instead of 20Hz, for balloon flight
#define LED_MODEL 0 // Big led = 1, small led = 0.
#define CAM_CYCLE false // If true, camera will only be powered on for 20s every 10min, used to save battery on long HAB flight
#define SAFE_TRIGGER false // For HAB flight, will use a safer, but slower methode to detect apogee and transision to descent mode
#define COG_BRAKE false // Will reduce command if CoG is turning faster than a threshold
#define GPS_FREQ 5 // Hz
#define SERVO_RATE 50 // Hz
#define TLM_MONITOR true // Show telemetry on serial monitor
#define GPS_PORT Serial7
#define TLM_PORT Serial5
#define RX_PORT Serial1
#define TIME_OUT 300
//---------- FLIGHT SETTINGS ----------//
#define RC_MODE 1 // only roll (0), pitch and roll mixed (1), pitch and roll separated (2)
#define CONTROL_MODE 1 // neutral position at the center (0) or with "hands up" (1)
#define LINEAR_MODE 0 // command is linear (0), or linear but with a large deadband set using servo_start etc (1)
#define DROP true // R2Home's version, drop or motorised
#define DESCENT_TIMER 1000
#define GLIDING_TIMER 3000
#define DEP_ALT 100 // m above ground
#define VUP 1 // m/s
#define VDOWN -1.5 // m/s
#define NKP 1
#define NKI 0.05
#define NKD 0.1
#define BCRITICAL 3.4
#define BLOW 3.5
#define NO_BATT 4.0
#define SERVO_MAX_M 2000 // m for map
#define SERVO_MAX_C 2000 // c for constrain
#define TRIG 20
#define LEFT_OFFSET 100
#define RIGHT_OFFSET 100
#define HOME_WAYPOINT_THRESHOLD 10 // Distance to waypoint before going to the next waypoint
#define BARO_VS_AVG 1
#define BARO_VS_SAMPLE 50
#define BARO_AL_AVG 50
#define BARO_PS_AVG 1
#define GPS_VS_AVG 1
#define GPS_SAFE_AVG 50
#define BARO_SAFE_AVG 3
#define PRE_PE_AVG 50

18
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/control.hpp 100644
Wyświetl plik

@ -0,0 +1,18 @@
#include <PID_v1.h>
#include "config.h"
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint,NKP, NKI, NKD, DIRECT);
void navigation_setup() {
myPID.SetTunings(NKP, NKI, NKD);
myPID.SetOutputLimits(-180, 180);
myPID.SetMode(MANUAL);
}
float cmpt_cmd(float err) {
Input = -err;
Setpoint = 0;
myPID.Compute();
return map(Output, -180, 180, 1000, 2000);
}

163
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/data.hpp 100644
Wyświetl plik

@ -0,0 +1,163 @@
#include <SD.h>
#include <Watchdog.h>
#include "feedback.hpp"
Watchdog watchdog;
bool sd_ok = false;
int delaySD = 100; // Datalog
int delayTLM = 1000; // Tlm
char sdnamebuff[20];
String mainSD;
String mainTLM;
String minSD;
File dataFile;
File configFile;
const int chipSelect = BUILTIN_SDCARD;
char namebuff[20];
unsigned int addr = 0;
String filename;
int datatest = 0;
int count = 0;
int packet_count = 0;
unsigned long tlm = 0;
unsigned long sd = 0;
int time_number = 0;
void cmpt_data_rate(int flight_mode) {
switch(flight_mode) {
case 0:
delaySD = 200;
delayTLM = 5000;
break;
case 1:
case 2:
case 3:
case 4:
case 5:
case 6:
case 8:
case 9:
if (LOW_RATE) { delaySD = 200; }
else { delaySD = 50; }
delayTLM = 1000;
break;
}
}
void send_data() {
packet_count = (packet_count +1);
TLM_PORT.println(mainTLM);
if (TLM_MONITOR) {
Serial.println(mainTLM);
}
}
void save_data(bool initialised) {
if (initialised == true) {
if (sd_ok == true and SD_WRITING == true) {
dataFile = SD.open(namebuff, FILE_WRITE);
dataFile.println(mainSD);
dataFile.close();
}
}
}
void cmpt_string_data(int flight_mode, bool initialised, bool deployed, bool wing_opened) {
time_number = ((gps.date.day()*1000000) + (gps.time.hour()*10000) + (gps.time.minute()*100) + gps.time.second());
int actual_time = millis();
String time_text = String(actual_time);
String flight_mode_text = String(flight_mode);
String initialised_text = String(initialised);
String deployed_text = String(deployed);
String wing_opened_text = String(wing_opened);
String gps_ok_text = String(gps_ok) ;
String cog_ok_text = String(cog_ok);
String failsafe_text = String(failsafe);
String vbatt_text = String(vbatt,2);
String loop_rate_text = String(loop_rate);
String packet_count_text = String(packet_count);
String status_text = time_text+","+packet_count_text+","+flight_mode_text+","+initialised_text+","+deployed_text+","+wing_opened_text+","+gps_ok_text+","+cog_ok_text+","+failsafe_text+","+vbatt_text+","+loop_rate_text;
mainSD = status_text+","+gps_text()+","+baro_text()+","+nav_text()+","+rc_text()+","+servo_text();
mainTLM = "/*"+status_text+","+gps_text()+","+baro_text()+","+servo_text()+"/*";
}
void newfile() {
dtostrf(time_number, 1, 0, sdnamebuff);
sprintf(namebuff, "%s.txt", sdnamebuff);
if (!SD.begin(chipSelect)) { sd_ok = false; }
else {
sd_ok = true;
SdFile::dateTimeCallback(dateTime);
dataFile = SD.open(namebuff, FILE_WRITE);
delay(10);
if (dataFile) {
dataFile.println("time (ms), Packet_Count (text), Mode (text), Initialised (text), Deployed (text), Wing_Opened (text), GPS_Ok (text), COG_Ok (text), FailSafe (text), Vbatt (V), Loop_rate (Hz), GPS-date, GPS-time, lat (deg), lon (deg), alt (m), CoG (deg), Speed (m/s), Sat_in_use (text), HDOP (text), Position_Age (text), Fix_type (text), Baro_Alt (m), Baro_Vspeed (m/s), Altitude (m), Baro_Weight, GPS_Weight, Baro_Vspeed_AVG (m/s), GPS_Vspeed_AVG (m/s), VDOWN (m/s), SetPoint_Home (deg), Err_Home (deg), LatB (deg), LonB (deg), WaypointNumber (text), Distance (m), Ch 0 (us), Ch 1 (us), Ch 2 (us), Ch 3 (us), Ch 4 (us), Ch 5 (us), Ch 6 (us), PWM_L (us), PWM_R (us), PWM_D (us)");
dataFile.close();
}
}
}
void getconfig() {
watchdog.enable(Watchdog::TIMEOUT_1S);
if (!SD.begin(chipSelect)) { sd_ok = false; delay(1500); }
else {
sd_ok = true;
File configFile = SD.open("config.txt", FILE_READ);
if (configFile) {
// Reading the baro adress
String memory = "";
while (configFile.available()) {
char a = configFile.read();
if (a != 44) { memory = memory + a; }
else { break; }
}
baro_adress = memory.toFloat();
char a = '0';
unsigned int i(0);
// Reading the Waypoints
while (configFile.available() and (i<16)) {
String waypoint_str = "";
do {
a = configFile.read();
if (a != 44) { waypoint_str = waypoint_str + a; }
else { waypoint[i].latitude = (waypoint_str.toFloat()); break; }
} while (a != 44);
waypoint_str = "";
do {
a = configFile.read();
if (a != 44) { waypoint_str = waypoint_str + a; }
else { waypoint[i].longitude = (waypoint_str.toFloat()); break; }
} while (a != 44);
waypoint_str = "";
do {
a = configFile.read();
if (a != 44) { waypoint_str = waypoint_str + a; }
else { waypoint[i].radius = (waypoint_str.toFloat()); i++; break; }
} while (a != 44);
}
last_waypoint_number = i+1;
configFile.close();
}
else {
delay(1500) ;
}
}
}

13
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/debug.hpp 100644
Wyświetl plik

@ -0,0 +1,13 @@
#include "config.h"
double long sim_cmd_time = 0;
float sim_cmd = 0;
int loop_count = 0;
int loop_rate = 999;
unsigned long long tloop = 0;
float sim() {
sim_cmd_time = (millis()*(PI/10000));
sim_cmd = sin(sim_cmd_time);
return map(sim_cmd, -1, 1, 1000, 2000);
}

199
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/feedback.hpp 100644
Wyświetl plik

@ -0,0 +1,199 @@
#include <EasyBuzzer.h>
#include <Adafruit_NeoPixel.h>
#include "config.h"
#include "battery.hpp"
#include "camera.hpp"
#include "debug.hpp"
#include "navigation.hpp"
#include "rc.hpp"
#include "servo.hpp"
#define LED_PIN 3
#define LED_COUNT 1
#define BRIGHTNESS 15
Adafruit_NeoPixel strip(LED_COUNT, LED_PIN, NEO_GRBW + NEO_KHZ800);
int duration = 0;
int timeled = 0;
int lastled = 0;
int BUZZER_PIN = 2;
int mid_freq = 0;
float time_sweep = 0;
int direc = 0;
unsigned long sweep_start = 0;
unsigned long tbeep = 0;
bool new_sweep = false;
bool current_sweep = false;
int current_freq = 0;
float sweep_step = 1;
float time_step = 1;
unsigned long last_waypoint_time = 0;
unsigned long sat_buzz = 0;
unsigned long batt_buzz = 0;
unsigned long t_turn = 0;
int beep_start = 0;
int beep_stop = 0;
void buzzer_setup() {
pinMode(BUZZER_PIN, OUTPUT);
EasyBuzzer.setPin(BUZZER_PIN);
tone(BUZZER_PIN, 523);
delay(200);
tone(BUZZER_PIN, 582);
delay(200);
tone(BUZZER_PIN, 762);
delay(200);
noTone(BUZZER_PIN);
}
void buzzer_end_setup() {
tone(BUZZER_PIN, 582);
delay(200);
tone(BUZZER_PIN, 830);
delay(300);
noTone(BUZZER_PIN);
}
void sweep_beep_set(int freq, int dur, int dir) {
mid_freq = freq;
time_sweep = dur;
new_sweep = true;
direc = dir;
beep_start = millis();
}
void buzzer_turn(int flight_mode, int turn_cmd) {
if (BUZZER_TURN == true and (flight_mode == 5 or flight_mode == 9 or flight_mode == 10)) {
if (millis()-last_waypoint_time >= 2000) {
int tone_turn = map(turn_cmd, 1000, 2000, 1000, 2000);
if (BUZZER_SWEEP == true) {
double t_down = abs(map(turn_cmd, 1000, 2000, -3, 3))+1;
t_down = (1/t_down)*1500;
if ((millis()-t_turn)>=t_down) {
t_turn = millis();
int dir = 0;
if (tone_turn >= 1500) { dir = 0; }
else { dir = 1; }
float force = 0;
if (tone_turn >= 1500) { force = tone_turn - 1500; }
else { force = 1500 - tone_turn; }
force = constrain(force, 10, 200);
sweep_beep_set(tone_turn, force, dir);
}
}
else {
double t_down = abs(map(turn_cmd, 1000, 2000, -5, 5))+1;
t_down = (1/t_down)*500;
if ((millis()-t_turn)>=t_down) {
t_turn = millis();
EasyBuzzer.singleBeep(tone_turn, 20);
}
}
}
}
}
void sweep_beep_update() {
if (direc == 0) {
if (new_sweep == true) {
sweep_start = millis();
new_sweep = false;
current_sweep = true;
current_freq = mid_freq-((sweep_step*time_sweep)/(2*time_step));
EasyBuzzer.singleBeep((current_freq),time_step);
}
if ((current_sweep == true) and ((millis()-sweep_start)>=(time_step)) and (current_freq<mid_freq+((sweep_step*time_sweep)/(2*time_step)))) {
sweep_start = millis();
current_freq = current_freq + sweep_step;
EasyBuzzer.singleBeep((current_freq),time_step);
beep_stop = millis();
}
}
else {
if (new_sweep == true) {
sweep_start = millis();
new_sweep = false;
current_sweep = true;
current_freq = mid_freq+((sweep_step*time_sweep)/(2*time_step));
EasyBuzzer.singleBeep((current_freq),time_step);
}
if ((current_sweep == true) and ((millis()-sweep_start)>=(time_step)) and (current_freq>mid_freq-((sweep_step*time_sweep)/(2*time_step)))) {
sweep_start = millis();
current_freq = current_freq - sweep_step;
EasyBuzzer.singleBeep((current_freq),time_step);
beep_stop = millis();
}
}
}
void buzzer_batt() {
cells = int(vbatt/3.65);
if ((vbatt < (cells*BCRITICAL)) and (vbatt > NO_BATT)) {
batt_critical = true;
if (millis()-batt_buzz>=100) {
batt_buzz = millis();
EasyBuzzer.singleBeep(3000,25);
}
}
else { batt_critical = false; }
if ((vbatt < (cells*BLOW)) and (vbatt > (cells*BCRITICAL))) {
batt_low = true;
if (millis()-batt_buzz>=200) {
batt_buzz = millis();
EasyBuzzer.singleBeep(3000,50);
}
}
else { batt_low = false; }
}
void update_buzzer() {
sweep_beep_update();
EasyBuzzer.update();
}
void led_setup() {
strip.begin();
strip.show();
strip.setBrightness(255);
}
void colorWipe(uint32_t color, int wait) {
for(int i=0; i<strip.numPixels(); i++) {
strip.setPixelColor(i, color);
strip.show();
}
}
void setled(int a, int b, int c, int d, int e) {
if (batt_critical == true) { a = 255; b = 0; c = 0; d = 25; e = 50; }
if (batt_low == true) { a = 255; b = 0; c = 0; d = 25; e = 200; }
if ((millis()-lastled)>=e) {
lastled = millis();
if (LED_MODEL == 1) { colorWipe(strip.Color(b,a,c), 0); }
else { colorWipe(strip.Color(a,b,c), 0); }
duration = d;
timeled = millis();
}
}
void updateled() {
if ((millis()-timeled)>=duration) {
colorWipe(strip.Color(0,0,0), 0);
}
}

287
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/flight_state.hpp 100644
Wyświetl plik

@ -0,0 +1,287 @@
#include "config.h"
#include "data.hpp"
int flight_mode = 0;
int dep_altitude = 0;
bool initialised = false;
bool flight_started = false;
bool deployed = false;
bool wing_opened = false;
unsigned long init_time = 0;
//------------------- 0 -------------------//
void flight_init() {
if ((gps.satellites.value()>=6 and gps_ok and gps.hdop.value()<200) or NO_INIT) {
EasyBuzzer.beep(3000,100,50,10,500,1);
if ((NAV_WAYPOINT == true) and (NAV_HOME == true)) {
waypoint[last_waypoint_number].latitude = gps.location.lat();
waypoint[last_waypoint_number].longitude = gps.location.lng();
waypoint[last_waypoint_number].radius = HOME_WAYPOINT_THRESHOLD;
current_waypoint = waypoint[waypoint_number];
}
else if ((NAV_WAYPOINT == false) and (NAV_HOME == true)) {
current_waypoint.latitude = gps.location.lat();
current_waypoint.longitude = gps.location.lng();
current_waypoint.radius = HOME_WAYPOINT_THRESHOLD;
}
else if ((NAV_WAYPOINT == true) and (NAV_HOME == false)) {
current_waypoint = waypoint[waypoint_number];
}
baroset(gps.altitude.meters(), 1);
get_baro(1);
b_vs.reset();
b_al.reset();
g_vs.reset();
ground_altitude = gps.altitude.meters();
dep_altitude = (DEP_ALT+ground_altitude);
setcam(1, 60, 60);
newfile();
if (DROP == true) { flight_mode = 1;}
else { flight_mode = 8; }
init_time = millis();
initialised = true;
}
if (millis()-sat_buzz>5000) {
sat_buzz = millis();
EasyBuzzer.beep(2000,50,25,gps.satellites.value(),100,1);
}
}
//------------------- 1 -------------------//
void ready_steady() {
if (millis()-init_time>=5000) {
if (is_ascent(VUP, 0)) {
flight_mode = 2;
EasyBuzzer.beep(1000,100,50,2,500,1);
setcam(1, 20, 600);
}
if (is_descent(v_down(VDOWN), 0)) {
flight_mode = 3;
EasyBuzzer.beep(3000,100,50,3,500,1);
setcam(1, 60, 600);
init_time = millis();
}
if ((atof(fix_type.value()) < 2) and (NO_INIT == false) and (flight_started == false)) {
flight_mode = 0;
}
if (I_WANT_TO_FLY) {
flight_mode = 5;
}
}
}
//------------------- 2 -------------------//
void flight_ascent() {
if (baro_vspeed<0.5) {
flight_mode = 1;
}
if ((gps.altitude.meters()-ground_altitude)>10) {
flight_started = true;
}
}
//------------------- 3 -------------------//
void flight_descent() {
if (is_ascent(0, 0)) {
flight_mode = 1;
}
if (is_descent(v_down(VDOWN), 0) and (merged_alt < dep_altitude or (millis()-init_time>DESCENT_TIMER))) {
flight_mode = 4;
EasyBuzzer.beep(3000,100,50,5,500,1);
deployed = true;
init_time = millis();
setcam(1, 60, 600);
}
}
//------------------- 4 -------------------//
void flight_gliding() {
if ((millis()-init_time) >= GLIDING_TIMER) {
wing_opened = true;
if (failsafe == true) {
flight_mode = 5;
setcam(1, 20, 600);
myPID.SetMode(AUTOMATIC);
}
else {
flight_mode = 6;
myPID.SetMode(MANUAL);
}
}
}
//------------------- 5 -------------------//
void flight_gliding_auto() {
if (!gps_ok) {
flight_mode = 11;
myPID.SetMode(MANUAL);
}
if (failsafe == false) {
flight_mode = 6;
myPID.SetMode(MANUAL);
}
if (I_WANT_TO_FLY == true) {
flight_mode = 5;
}
navigation();
}
//------------------- 6 -------------------//
void flight_gliding_manual() {
if (failsafe == true) {
flight_mode = 5;
myPID.SetMode(AUTOMATIC);
}
}
//------------------- 7 -------------------//
void landed() {
}
//------------------- 8 -------------------//
void motorised_man() {
if (channels[6] > 1000) {
flight_mode = 9;
myPID.SetMode(AUTOMATIC);
}
if (failsafe == true) {
flight_mode = 10;
myPID.SetMode(AUTOMATIC);
}
}
//------------------- 9 -------------------//
void motorised_auto() {
navigation();
if (channels[6] < 1000) {
flight_mode = 8;
}
}
//------------------- 10 -------------------//
void motorised_failSafe() {
navigation();
if (failsafe == false) {
flight_mode = 8;
myPID.SetMode(MANUAL);
}
}
//------------------- 11 -------------------//
void flight_gliding_no_gps() {
if (gps_ok) {
flight_mode = 5;
myPID.SetMode(AUTOMATIC);
}
}
//------------------- STATE MACHINE -------------------//
void cmpt_flight_state() {
switch(flight_mode) {
case 0:
flight_init();
setled(255, 0, 0, 25, 2000);
break;
case 1:
ready_steady();
setled(0, 255, 0, 25, 500);
break;
case 2:
flight_ascent();
setled(0, 0, 255, 25, 2000);
break;
case 3:
flight_descent();
setled(255, 128, 0, 25, 1000);
break;
case 4:
flight_gliding();
break;
case 5:
flight_gliding_auto();
setled(255, 255, 0, 25, 1000);
break;
case 6:
flight_gliding_manual();
setled(0, 255, 255, 25, 1000);
break;
case 7:
landed();
setled(128, 0, 255, 25, 1000);
break;
case 8:
motorised_man();
setled(0, 0, 255, 25, 500);
break;
case 9:
motorised_auto();
setled(0, 255, 255, 25, 500);
break;
case 10:
motorised_failSafe();
setled(0, 255, 255, 25, 500);
break;
case 11:
flight_gliding_no_gps();
setled(255, 0, 0, 25, 200);
break;
}
}

175
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp 100644
Wyświetl plik

@ -0,0 +1,175 @@
#include "config.h"
#include <TinyGPS++.h>
#include <movingAvg.h>
TinyGPSPlus gps;
TinyGPSCustom fix_type(gps, "GNGSA", 2);
unsigned char serial2bufferRead[1000];
float prev_alt_gps = 0;
float gps_vspeed = 0;
unsigned long gpsB = 0;
float prev_cog = 0;
bool new_gps = false;
bool new_cog = false;
movingAvg g_vs(GPS_VS_AVG);
bool gps_ok = false;
bool cog_ok = false;
void cmpt_vertical_speed_gps(float da, int dt) {
gps_vspeed = (da/(dt/1000.0));
gps_vspeed = g_vs.reading(gps_vspeed*100);
gps_vspeed = (gps_vspeed/100);
}
void get_gps() {
while (GPS_PORT.available()) {
gps.encode(GPS_PORT.read());
}
if (((gps.location.age()) < ((1000/GPS_FREQ)*2)) and (atof(fix_type.value()) == 3)) {
gps_ok = true;
}
else { gps_ok = false; }
if (gps.course.isUpdated()) {
if (abs(prev_cog-gps.course.deg())>0.1 or gps.speed.mps()>3) {
cog_ok = true;
new_cog = true;
prev_cog = gps.course.deg();
}
else { cog_ok = false; }
}
if (gps.altitude.isUpdated()) {
cmpt_vertical_speed_gps(prev_alt_gps-gps.altitude.meters(), millis()-gpsB);
gpsB = millis();
prev_alt_gps = gps.altitude.meters();
new_gps = true;
}
}
void sendPacket(byte *packet, byte len){
for (byte i = 0; i < len; i++) { GPS_PORT.write(packet[i]); }
}
void gps_setup(int a, int b, int c, int d, int e){
g_vs.begin();
if (a == 9600) {
GPS_PORT.begin(9600);
delay(100);
byte packet1[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x80, 0x25, 0x00, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x92, 0xB5};
sendPacket(packet1, sizeof(packet1));
}
if (a == 57600) {
GPS_PORT.begin(9600);
delay(100);
byte packet2[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x00, 0xE1, 0x00, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xCE, 0xC9};
sendPacket(packet2, sizeof(packet2));
GPS_PORT.end();
GPS_PORT.begin(57600);
delay(100);
}
if (a == 115200) {
GPS_PORT.begin(9600);
delay(100);
byte packet3[] = {0xB5, 0x62, 0x06, 0x00, 0x14, 0x00, 0x01, 0x00, 0x00, 0x00, 0xC0, 0x08, 0x00, 0x00, 0x00, 0xC2, 0x01, 0x00, 0x07, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB0, 0x7E};
sendPacket(packet3, sizeof(packet3));
GPS_PORT.end();
GPS_PORT.begin(115200);
delay(100);
}
if (b == 1) {
byte packet4[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xE8, 0x03, 0x01, 0x00, 0x01, 0x00, 0x01, 0x39};
sendPacket(packet4, sizeof(packet4));
}
if (b == 5) {
byte packet5[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0xC8, 0x00, 0x01, 0x00, 0x01, 0x00, 0xDE, 0x6A};
sendPacket(packet5, sizeof(packet5));
}
if (b == 10) {
byte packet6[] = {0xB5, 0x62, 0x06, 0x08, 0x06, 0x00, 0x64, 0x00, 0x01, 0x00, 0x01, 0x00, 0x7A, 0x12};
sendPacket(packet6, sizeof(packet6));
}
if (c == 0) {
byte packet7[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x00, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7E, 0x3C};
sendPacket(packet7, sizeof(packet7));
}
if (c == 1) {
byte packet8[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x84, 0x08};
sendPacket(packet8, sizeof(packet8));
}
if (c == 2) {
byte packet8[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x07, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x85, 0x2A};
sendPacket(packet8, sizeof(packet8));
}
if (c == 4) {
byte packet8[] = {0xB5, 0x62, 0x06, 0x24, 0x24, 0x00, 0xFF, 0xFF, 0x08, 0x03, 0x00, 0x00, 0x00, 0x00, 0x10, 0x27, 0x00, 0x00, 0x05, 0x00, 0xFA, 0x00, 0xFA, 0x00, 0x64, 0x00, 0x5E, 0x01, 0x00, 0x3C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x86, 0x4C};
sendPacket(packet8, sizeof(packet8));
}
if (d == 1) {
byte packet9[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x01, 0x01, 0x00, 0x00, 0x01, 0x01, 0x00, 0x03, 0x35};
byte packet10[] = {0xB5, 0x62, 0x06, 0x01, 0x08, 0x00, 0xF0, 0x03, 0x01, 0x00, 0x00, 0x01, 0x01, 0x00, 0x05, 0x43};
sendPacket(packet9, sizeof(packet9));
sendPacket(packet10, sizeof(packet10));
}
if (e == 1) {
byte packet11[] = {0xB5, 0x62, 0x06, 0x1E, 0x14, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0A, 0x32, 0x00, 0x00, 0x99, 0x4C, 0x00, 0x00, 0x5B, 0x10};
sendPacket(packet11, sizeof(packet11));
}
GPS_PORT.addMemoryForRead(serial2bufferRead, sizeof(serial2bufferRead));
}
void dateTime(uint16_t* date, uint16_t* time){
*date = FAT_DATE(gps.date.year(), gps.date.month(), gps.date.day());
*time = FAT_TIME(gps.time.hour(), gps.time.minute(), gps.time.second());
}
String date_time() {
String date_year = String(gps.date.year());
String date_month = String(gps.date.month());
String date_day = String(gps.date.day());
String time_hour = String(gps.time.hour());
String time_minute = String(gps.time.minute());
String time_second = String(gps.time.second());
return date_year+":"+date_month+":"+date_day+","+time_hour+":"+time_minute+":"+time_second;
}
String gps_text() {
String lat_A_text = String(gps.location.lat(), 10);
String lon_A_text = String(gps.location.lng(), 10);
String alt_gps_text = String(gps.altitude.meters(), 1);
String cog_text = String(gps.course.deg());
String speed_text = String(gps.speed.mps(), 1);
String sat_text = String(gps.satellites.value());
String fix_type_text= String(atof(fix_type.value()));
String hdop_text = String(gps.hdop.value());
String pos_age_text;
if (gps.location.age()>10000) { pos_age_text = String(999); }
else { pos_age_text = String(gps.location.age()); }
return date_time()+","+lat_A_text+","+lon_A_text+","+alt_gps_text+","+cog_text+","+speed_text+","+sat_text+","+hdop_text+","+pos_age_text+","+fix_type_text;
}

97
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino 100644
Wyświetl plik

@ -0,0 +1,97 @@
#include <movingAvg.h>
#include "flight_state.hpp"
servo_cmd steering;
float setPoint_waypoint = 0;
float error_waypoint = 0;
float cmd_to_waypoint = 0;
void setup() {
Serial.begin(115200);
TLM_PORT.begin(57600);
battery_setup();
buzzer_setup();
camera_setup();
gps_setup(57600, GPS_FREQ, 2, 1, 0); // baud, Hz, mode, nmea, cog filter (0 = Off, 1 = On)
led_setup();
rc_setup();
servo_setup();
position_setup();
getconfig();
watchdog.reset();
barometer_setup();
navigation_setup();
buzzer_end_setup();
}
void loop() {
watchdog.reset();
getdata(); // Getting data from all the sensors
datacmpt(); // Using this data to do all what we have to do
loop_count++;
if ((millis()-tloop)>=1000) {
tloop = millis();
loop_rate = loop_count;
loop_count = 0;
}
}
void getdata() {
get_gps();
get_baro(0);
get_rc();
get_vbatt();
}
void datacmpt() {
if (new_cog) {
new_cog = false;
setPoint_waypoint = cmpt_setpoint(current_waypoint);
error_waypoint = cmpt_error(gps.course.deg(), setPoint_waypoint);
cmd_to_waypoint = cmpt_cmd(error_waypoint);
}
if (I_WANT_TO_FLY) {
cmd_to_waypoint = sim();
Serial.println(cmd_to_waypoint);
cog_ok = true;
}
steering = cmpt_servo(channels, cmd_to_waypoint, flight_mode, deployed, failsafe, cog_ok);
update_servo_cmd(steering, SERVO_RATE);
cmpt_flight_state();
cmpt_data_rate(flight_mode);
cmpt_fusion();
cmpt_vertical_state();
if ((millis()-sd)>=delaySD) {
sd = millis();
cmpt_string_data(flight_mode, initialised, deployed, wing_opened);
save_data(initialised);
}
if (millis()-tlm>=delayTLM) {
tlm = millis();
send_data();
}
buzzer_turn(flight_mode, setPoint_waypoint);
buzzer_batt();
update_buzzer();
updateled();
updatecam();
}

67
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/navigation.hpp 100644
Wyświetl plik

@ -0,0 +1,67 @@
#include "config.h"
#include "position.hpp"
#include "control.hpp"
struct gps_location {
double latitude = 0;
double longitude = 0;
double radius = 0;
};
gps_location waypoint[17];
gps_location current_waypoint;
int waypoint_number = 0;
int last_waypoint_number = 0;
float getangle(float a, float b) {
float angle = 0;
if (abs(a-b) < 180) { angle = (b-a);}
else {
if ((a-b) < 0) { angle = (-360) +(b-a);}
else { angle = (360) + (b-a);}
}
return angle;
}
float cmpt_setpoint(gps_location waypoint) {
return TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),waypoint.latitude,waypoint.longitude);
}
float cmpt_error(float cog, float setPoint) {
return getangle(cog, setPoint);
}
void navigation() {
if (NAV_WAYPOINT == true) {
float distance_to = TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),current_waypoint.latitude,current_waypoint.longitude);
if (distance_to < current_waypoint.radius) {
if (waypoint_number < 15) {
waypoint_number++;
}
if ((waypoint[waypoint_number].latitude !=0) and (waypoint[waypoint_number].longitude !=0)) {
current_waypoint = waypoint[waypoint_number];
}
}
}
}
String control_text() {
String setPoint_Home_text = String(cmpt_setpoint(current_waypoint),2);
String errHome_text = String(cmpt_error(gps.course.deg(), cmpt_setpoint(current_waypoint)),2);
String lat_B_text = String(current_waypoint.latitude,5);
String lon_B_text = String(current_waypoint.longitude,5);
String waypoint_text = String(waypoint_number);
String waypoint_distance = String(TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),current_waypoint.latitude,current_waypoint.longitude));
return setPoint_Home_text+","+errHome_text+","+lat_B_text+","+lon_B_text+","+waypoint_text+","+waypoint_distance;
}
String nav_text() {
return pos_text()+","+control_text();
}

130
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/position.hpp 100644
Wyświetl plik

@ -0,0 +1,130 @@
#include "config.h"
#include "barometer.hpp"
#include "gps.hpp"
#include <movingAvg.h>
float merged_alt = 0;
float merged_vspeed = 0;
float baro_alt_weight = 1;
float gps_alt_weight = 1;
float baro_vspeed_weight = 1;
float gps_vspeed_weight = 1;
float ground_altitude = 0;
unsigned long tstab = 0;
float gps_stab_factor = 0;
float baro_stab_factor = 0;
bool new_baro_fusion = false;
bool new_gps_fusion = false;
movingAvg gps_v(GPS_SAFE_AVG);
movingAvg baro_v(BARO_SAFE_AVG);
movingAvg ps_p(PRE_PE_AVG);
float pressure_percentage;
void position_setup() {
gps_v.begin();
baro_v.begin();
ps_p.begin();
}
void cmpt_fusion() {
if (new_baro_fusion == true and new_gps_fusion == true) {
new_baro_fusion = false;
new_gps_fusion = false;
pressure_percentage = (ps_p.reading((pressure_baro / (baro_set*100.0))*100.0)/100.0) ;
baro_alt_weight = pressure_percentage;
baro_vspeed_weight = pressure_percentage*pressure_percentage;
double h_dop = gps.hdop.value();
if (gps_ok) {
gps_alt_weight = 50.0/h_dop;
gps_vspeed_weight = 1;
}
else {
gps_alt_weight = 0;
gps_vspeed_weight = 0;
}
merged_alt = ((alt_baro*baro_alt_weight)+(gps.altitude.meters()*gps_alt_weight))/(baro_alt_weight+gps_alt_weight);
merged_vspeed = ((baro_vspeed*baro_vspeed_weight)+(gps_vspeed*gps_vspeed_weight))/(baro_vspeed_weight+gps_vspeed_weight);
}
}
bool cmpt_vertical_state() {
if (new_baro) {
new_baro = false;
new_baro_fusion = true;
baro_stab_factor = (baro_v.reading(baro_vspeed*100.0))/100.0;
}
if (new_gps) {
new_gps = false;
new_gps_fusion = true;
gps_stab_factor = (gps_v.reading(gps_vspeed*100))/100.0;
}
}
bool is_ascent(int v_trigger, bool mode) {
if (mode == 0) {
if (baro_stab_factor>v_trigger) {
return true;
}
else {
return false;
}
}
else {
if (gps_stab_factor>v_trigger and baro_stab_factor>v_trigger) {
return true;
}
else {
return false;
}
}
}
bool is_descent(int v_trigger, bool mode) {
if (mode == 0) {
if (baro_stab_factor<v_trigger) {
return true;
}
else {
return false;
}
}
else {
if (gps_stab_factor<v_trigger and baro_stab_factor<v_trigger) {
return true;
}
else {
return false;
}
}
}
float v_down(int vdown) {
return sqrt((baro_set*100.0)/pressure_baro)*vdown;
}
String pos_text() {
String merged_alt_text = String(merged_alt,3);
String gps_weight_text = String(gps_alt_weight,2);
String baro_weight_text = String(baro_alt_weight,3);
String baro_stab_text = String(baro_stab_factor, 2);
String gps_stab_text = String(gps_stab_factor, 2);
String v_down_text = String(v_down(VDOWN), 2);
return merged_alt_text+","+baro_weight_text+","+gps_weight_text+","+baro_stab_text+","+gps_stab_text+","+v_down_text;
}

31
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/rc.hpp 100644
Wyświetl plik

@ -0,0 +1,31 @@
#include <SBUS.h>
#include "config.h"
SBUS rx(RX_PORT);
uint16_t channels[16];
bool failsafe;
bool lostFrame;
void rc_setup() {
rx.begin();
}
void get_rc() {
rx.read(&channels[0], &failsafe, &lostFrame);
if ((channels[3])>=1500 or (channels[3])==0) { failsafe = true; }
else { failsafe = false; }
}
String rc_text() {
String a_text = String(channels[0]);
String b_text = String(channels[1]);
String c_text = String(channels[2]);
String d_text = String(channels[3]);
String e_text = String(channels[4]);
String f_text = String(channels[5]);
String g_text = String(channels[6]);
return a_text+","+b_text+","+c_text+","+d_text+","+e_text+","+f_text+","+g_text;
}

188
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/servo.hpp 100644
Wyświetl plik

@ -0,0 +1,188 @@
#include "config.h"
#include <PWMServo.h>
PWMServo steer;
PWMServo deploy;
PWMServo left;
PWMServo right;
PWMServo esc;
int servo_left = 0;
int servo_right = 0;
int servo_aux = 0;
unsigned long tpwm = 0;
struct servo_cmd {
float left = 1500;
float right = 1500;
float aux = 1500;
};
void servo_setup() {
left.attach(6, 1000, 2000);
right.attach(7, 1000, 2000);
if (DROP == true) { deploy.attach(8, 1000, 2000); }
else { esc.attach(8, 1000, 2000); }
}
servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool deployed, bool failsafe, bool cog_ok) {
float roll = map(channels[0], 67, 1982, 1000, 2000);
float pitch = map(channels[1], 67, 1982, 1000, 2000);
float aux = map(channels[2], 67, 1982, 1000, 2000);
float sw = map(channels[6], 67, 1982, 1000, 2000);
roll = constrain(roll, 1000, 2000);
pitch = constrain(pitch, 1000, 2000);
aux = constrain(aux, 1000, 2000);
sw = constrain(sw, 1000, 2000);
servo_cmd steering_cmpt ;
switch(flight_mode) {
case 0:
case 1:
case 6:
case 8:
// ---------- Stage 1 - RC mode ---------- //
switch (RC_MODE) {
case 0: // roll only
steering_cmpt.right = roll;
steering_cmpt.left = 3000-roll;
break;
case 1: // pitch and roll mixed
steering_cmpt.right = roll;
steering_cmpt.left = 3000-roll;
steering_cmpt.left = steering_cmpt.left+(pitch-1500);
steering_cmpt.right = steering_cmpt.right+(pitch-1500);
break;
case 2: // pitch and roll separated
steering_cmpt.left = roll;
steering_cmpt.right = pitch;
break;
}
if (failsafe) {
steering_cmpt.left = 1500;
steering_cmpt.right = 1500;
}
break;
case 9:
case 10:
case 5:
if (cog_ok) {
steering_cmpt.right = autopilot;
steering_cmpt.left = 3000-autopilot;
}
else {
steering_cmpt.right = 1500;
steering_cmpt.left = 1500;
}
break;
case 11:
steering_cmpt.right = 1500;
steering_cmpt.left = 1500;
break;
}
// ---------- Stage 2 - Linear mode ---------- //
switch (LINEAR_MODE) {
case 0: // control is fully linear
break;
case 1: // control start at servo_start with an offset
if (steering_cmpt.left>=(1500+TRIG)) {
steering_cmpt.left = map(steering_cmpt.left, 1500, 2000, 1500+LEFT_OFFSET, SERVO_MAX_M);
}
else if (steering_cmpt.left<=(1500-TRIG)) {
steering_cmpt.left = map(steering_cmpt.left, 1000, 1500, 1000, 1500-LEFT_OFFSET);
}
if (steering_cmpt.right>=(1500+TRIG)) {
steering_cmpt.right = map(steering_cmpt.right, 1500, 2000, 1500+RIGHT_OFFSET, SERVO_MAX_M);
}
else if (steering_cmpt.right<=(1500-TRIG)) {
steering_cmpt.right = map(steering_cmpt.right, 1000, 1500, 1000, 1500-RIGHT_OFFSET);
}
break;
}
// ---------- Stage 3 - Control mode ---------- //
switch (CONTROL_MODE) {
case 0: // neutral is center
break;
case 1: // neutral is hands up
steering_cmpt.right = constrain(map(steering_cmpt.right, 1500, 2000, 1000, SERVO_MAX_M), 1000, SERVO_MAX_C);
steering_cmpt.left = constrain(map(steering_cmpt.left, 1500, 2000, 1000, SERVO_MAX_M), 1000, SERVO_MAX_C);
break;
}
// -------------------------- Deployment Servo and ESC -------------------------- //
switch(flight_mode) {
case 0:
case 1:
case 2:
case 3:
case 4:
case 5:
case 6:
case 7:
if (deployed == true) {
steering_cmpt.aux = 1000;
}
else {
if (failsafe == true) {
steering_cmpt.aux = 2000;
}
else { steering_cmpt.aux = sw; }
}
break;
case 8:
steering_cmpt.aux = aux;
break;
case 9:
case 10:
steering_cmpt.aux = 1000;
break;
case 11:
steering_cmpt.aux = 1000;
break;
}
servo_left = steering_cmpt.left;
servo_right = steering_cmpt.right;
servo_aux = steering_cmpt.aux;
return steering_cmpt;
}
void update_servo_cmd(servo_cmd steering_apply, int a) {
if ((millis()-tpwm)>=(1000/a)) {
tpwm = millis();
left.write(map(steering_apply.left, 1000, 2000, 0, 180));
right.write(map(steering_apply.right, 1000, 2000, 0, 180));
deploy.write(map(steering_apply.aux, 1000, 2000, 0, 180));
}
}
String servo_text() {
String left_text = String(servo_left);
String right_text = String(servo_right);
String aux_text = String(servo_aux);
return left_text+","+right_text+","+aux_text;
}