kopia lustrzana https://github.com/YohanHadji/R2Home
1280 wiersze
33 KiB
C++
1280 wiersze
33 KiB
C++
#include <TinyGPS++.h>
|
|
#include <SD.h>
|
|
#include "Adafruit_BMP280.h"
|
|
#include <SBUS.h>
|
|
#include <EasyBuzzer.h>
|
|
#include <Watchdog.h>
|
|
#include <PWMServo.h>
|
|
#include <movingAvg.h>
|
|
#include <Adafruit_NeoPixel.h>
|
|
#include <EEPROM.h>
|
|
|
|
// ----------------------------------- SETUP PANEL ----------------------------------- //
|
|
|
|
|
|
boolean drop = true;
|
|
|
|
boolean record_home = false;
|
|
|
|
int autopilot_mode = 0;
|
|
|
|
float dep_altitude = 100;
|
|
float vup = 3;
|
|
float vdown = -4;
|
|
|
|
int cog_count = 2;
|
|
|
|
float bcritical = 3.7;
|
|
float blow = 3.8;
|
|
float no_batt = 4;
|
|
|
|
int servo_man_min = 1100;
|
|
int servo_man_max = 1900;
|
|
|
|
int servo_auto_min = 1285;
|
|
int servo_auto_max = 1715;
|
|
|
|
float gliding_timer = 2500;
|
|
|
|
int time_up = 0;
|
|
int time_down = 1000;
|
|
|
|
int cmd_right = 2000;
|
|
int cmd_left = 1000;
|
|
|
|
float Kt = 2;
|
|
|
|
int rate_yaw = 1;
|
|
|
|
|
|
// NAV PIDs //
|
|
float NKp = 0.8;
|
|
float NKd = 0.15;
|
|
|
|
// ----------------------------------- GLOBAL VARIABLES ----------------------------------- //
|
|
|
|
// BARO //
|
|
Adafruit_BMP280 bmp;
|
|
float alt_baro = 0;
|
|
float prev_alt = 0;
|
|
float vspeed = 0;
|
|
float baro_set = 1000;
|
|
float baro_count = 0;
|
|
int vspeed_count = 0;
|
|
boolean new_baro = false;
|
|
float dt = 0;
|
|
movingAvg al(5);
|
|
movingAvg vs(5);
|
|
|
|
|
|
// BATTERY //
|
|
movingAvg voltage(50);
|
|
float vpin = A17;
|
|
float vbatt = 0;
|
|
boolean batt_critical = false;
|
|
boolean batt_low = false;
|
|
|
|
// GPS //
|
|
TinyGPSPlus gps;
|
|
TinyGPSCustom fix_type(gps, "GNGSA", 2);
|
|
movingAvg rs(2);
|
|
unsigned char serial2bufferRead[1000];
|
|
float prev_cog = 0;
|
|
int gps_count = 0;
|
|
int valid_count = 0;
|
|
float prev_gps = 0;
|
|
boolean new_gps = false;
|
|
boolean cog_ok = 0;
|
|
boolean new_cog = false;
|
|
|
|
|
|
// LED //
|
|
#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;
|
|
|
|
// MISC //
|
|
int buzzer = 2;
|
|
int cam = 23;
|
|
int sw = 22;
|
|
int crash_count = 0;
|
|
int cells = 0;
|
|
boolean armed = false;
|
|
|
|
// NAV //
|
|
movingAvg rc(2);
|
|
movingAvg mult(10);
|
|
float merged_alt = 0;
|
|
float setPoint_Home = 0;
|
|
float errHome = 0;
|
|
float raterror = 0;
|
|
float last_errHome = 0;
|
|
float baro_weight = 1;
|
|
float gps_weight = 1;
|
|
float lat_B = 0;
|
|
float lon_B = 0;
|
|
float cmd = 0;
|
|
boolean spiral = false;
|
|
float cmdHome = 1500;
|
|
float next_cog = 0;
|
|
float ratecog = 0;
|
|
float prev_cog_b = 0;
|
|
float cmd_mult = 0;
|
|
|
|
// RX //
|
|
SBUS rx(Serial1);
|
|
movingAvg s_steer(25);
|
|
uint16_t channels[16];
|
|
bool failSafe;
|
|
bool lostFrame;
|
|
|
|
// SD CARD //
|
|
File dataFile;
|
|
const int chipSelect = BUILTIN_SDCARD;
|
|
char namebuff[20];
|
|
unsigned int addr = 0;
|
|
String filename;
|
|
int datatest = 0;
|
|
int count = 0;
|
|
|
|
// SERVO /
|
|
movingAvg st(5);
|
|
PWMServo steer;
|
|
PWMServo deploy;
|
|
PWMServo left;
|
|
PWMServo right;
|
|
PWMServo esc;
|
|
|
|
float steer_man = 1500;
|
|
float steer_auto = 1500;
|
|
|
|
float servo_deploy = 2000;
|
|
float servo_esc = 1000;
|
|
|
|
float servo_left = 1500;
|
|
float servo_right = 1500;
|
|
|
|
// STATUS //
|
|
int flight_mode = 0;
|
|
int prev_mode = 0;
|
|
bool deployed = false;
|
|
bool baro_stab = false;
|
|
bool gps_stab = false;
|
|
bool initialised = false;
|
|
bool wing_opened = false;
|
|
|
|
bool sd_ok = false;
|
|
bool gps_ok = false;
|
|
|
|
int packet_count = 0;
|
|
int reboot_state = 0;
|
|
|
|
// STRING //
|
|
char sdnamebuff[20];
|
|
char lat_A_text[30];
|
|
char lon_A_text[30];
|
|
char alt_gps_text[30];
|
|
char cog_text[30];
|
|
char speed_text[30];
|
|
char sat_text[30];
|
|
char fix_type_text[10];
|
|
char hdop_text[10];
|
|
char pos_age_text[10];
|
|
char count_text[10];
|
|
char time_text[10];
|
|
|
|
char alt_baro_text[30];
|
|
char vspeed_text[30];
|
|
|
|
char merged_alt_text[30];
|
|
char baro_weight_text[30];
|
|
char gps_weight_text[30];
|
|
char setPoint_Home_text[30];
|
|
char lat_B_text[30];
|
|
char lon_B_text[30];
|
|
char errHome_text[30];
|
|
char raterror_text[30];
|
|
|
|
char a_text[10];
|
|
char b_text[10];
|
|
char c_text[10];
|
|
char d_text[10];
|
|
char e_text[10];
|
|
char f_text[10];
|
|
char g_text[10];
|
|
|
|
char deploy_text[10];
|
|
char left_text[10];
|
|
char right_text[10];
|
|
char esc_text[10];
|
|
|
|
char gps_ok_text[10];
|
|
char cog_ok_text[10];
|
|
char failSafe_text[10];
|
|
char baro_stab_text[10];
|
|
char gps_stab_text[10];
|
|
char deployed_text[10];
|
|
char flight_mode_text[10];
|
|
char vbatt_text[10];
|
|
char looptime_text[10];
|
|
char packet_count_text[10];
|
|
char initialised_text[10];
|
|
char wing_opened_text[10];
|
|
char next_cog_text[10];
|
|
char cmd_mult_text[10];
|
|
|
|
char mainSD[240];
|
|
char mainTLM[250];
|
|
|
|
char mintext[120];
|
|
char gps_text[120];
|
|
char baro_text[120];
|
|
char rc_text[120];
|
|
char servo_text[120];
|
|
char status_text[120];
|
|
char alt_text[120];
|
|
char nav_text[120];
|
|
|
|
char date_time[80];
|
|
|
|
char date_year[10];
|
|
char date_month[10];
|
|
char date_day[10];
|
|
char time_hour[10];
|
|
char time_minute[10];
|
|
char time_seconde[10];
|
|
|
|
int time_number = 0;
|
|
boolean flight_rebooted = false;
|
|
|
|
// TIMERS //
|
|
|
|
unsigned long baroA = 0;
|
|
unsigned long baroB = 0;
|
|
unsigned long tstab = 0;
|
|
unsigned long tlm = 0;
|
|
unsigned long sd = 0;
|
|
unsigned long baro_blk = 0;
|
|
unsigned long init_time = 0;
|
|
unsigned long sat_buzz = 0;
|
|
unsigned long tparallax = 0;
|
|
unsigned long tpwm = 0;
|
|
unsigned long batt_buzz = 0;
|
|
unsigned long long tloop = 0;
|
|
unsigned long long tgps = 0;
|
|
unsigned long long tup = 0;
|
|
unsigned long long tdown = 0;
|
|
|
|
unsigned long long reboot_time = 0;
|
|
|
|
|
|
int loop_time = 999;
|
|
|
|
int delaySD = 100; // Datalog
|
|
int delayTLM = 1000; // Tlm
|
|
|
|
|
|
// WATCHDOG //
|
|
Watchdog watchdog;
|
|
|
|
|
|
void setup() {
|
|
|
|
pinMode(A17, INPUT);
|
|
pinMode(buzzer, OUTPUT);
|
|
pinMode(cam, OUTPUT);
|
|
pinMode(sw, INPUT);
|
|
digitalWrite(cam, LOW);
|
|
|
|
//autopilot_mode = digitalRead(sw);
|
|
//Serial.println(autopilot_mode);
|
|
|
|
Serial.begin(115200);
|
|
Serial5.begin(57600);
|
|
|
|
gpset(57600, 5, 2, 1, 0); // baud, Hz, mode, nmea, cog filter (0 = Off, 1 = On)
|
|
|
|
rx.begin();
|
|
|
|
EasyBuzzer.setPin(buzzer);
|
|
|
|
voltage.begin();
|
|
vs.begin();
|
|
al.begin();
|
|
s_steer.begin();
|
|
rs.begin();
|
|
rc.begin();
|
|
mult.begin();
|
|
|
|
left.attach(6, 1000, 2000);
|
|
right.attach(7, 1000, 2000);
|
|
|
|
if (drop == true) { deploy.attach(8, 1000, 2000); }
|
|
else { esc.attach(8, 1000, 2000); }
|
|
|
|
|
|
bmp.begin(0X76);
|
|
|
|
bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,
|
|
Adafruit_BMP280::SAMPLING_X1,
|
|
Adafruit_BMP280::SAMPLING_X2,
|
|
Adafruit_BMP280::FILTER_X16,
|
|
Adafruit_BMP280::STANDBY_MS_1);
|
|
|
|
strip.begin();
|
|
strip.show();
|
|
strip.setBrightness(BRIGHTNESS);
|
|
|
|
tone(buzzer, 523);
|
|
delay(200);
|
|
tone(buzzer, 582);
|
|
delay(200);
|
|
tone(buzzer, 762);
|
|
delay(300);
|
|
noTone(buzzer);
|
|
|
|
EEPROM.get(0, reboot_state);
|
|
|
|
if (reboot_state == 1) {
|
|
EEPROM.get(10, dep_altitude);
|
|
EEPROM.get(30, lat_B);
|
|
EEPROM.get(50, lon_B);
|
|
EEPROM.get(70, baro_set);
|
|
EEPROM.get(90, time_number);
|
|
EEPROM.get(120, reboot_time);
|
|
|
|
flight_rebooted = true;
|
|
}
|
|
|
|
watchdog.enable(Watchdog::TIMEOUT_1S);
|
|
|
|
}
|
|
|
|
void loop() {
|
|
|
|
tloop = micros();
|
|
|
|
getdata();
|
|
datacmpt();
|
|
|
|
flight_state();
|
|
|
|
applycmd();
|
|
updatecmd(50);
|
|
|
|
userinter();
|
|
|
|
loop_time = (micros()-tloop);
|
|
if (loop_time<150000) {watchdog.reset(); crash_count = 0; }
|
|
if (loop_time>150000) {
|
|
crash_count = (crash_count + 1);
|
|
if (crash_count<=15) { watchdog.reset(); }
|
|
else {
|
|
reboot_state = 1;
|
|
EEPROM.put(0, reboot_state);
|
|
EEPROM.put(120, millis());
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
|
|
void getdata() {
|
|
|
|
// -------------------------- Get GPS -------------------------- //
|
|
|
|
while (Serial7.available()) { gps.encode(Serial7.read()); }
|
|
|
|
|
|
// -------------------------- Get BARO & COMPASS -------------------------- //
|
|
|
|
if ((millis()-baroA)>=10) {
|
|
baroA = millis();
|
|
unsigned long waita = millis();
|
|
alt_baro = al.reading(bmp.readAltitude(baro_set)*100);
|
|
//if (((millis() - waita) > 75) or (alt_baro > 40000)) { EasyBuzzer.singleBeep(3000,50); bmp.begin(0X76); 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();
|
|
}
|
|
}
|
|
|
|
// -------------------------- Get RC -------------------------- //
|
|
|
|
rx.read(&channels[0], &failSafe, &lostFrame);
|
|
|
|
if ((channels[3])>=1500 or (channels[3])==0) { failSafe = true; }
|
|
else { failSafe = false; }
|
|
|
|
steer_man = map(s_steer.reading(channels[0]), 50, 1950, 1000, 2000);
|
|
steer_man = constrain(steer_man, servo_man_min, servo_man_max);
|
|
|
|
// -------------------------- Get Vbatt -------------------------- //
|
|
|
|
vbatt = analogRead(A17);
|
|
vbatt = (voltage.reading(vbatt)*0.02579582875);
|
|
|
|
|
|
}
|
|
|
|
// ----------------------------------------------------- Data CMPT ----------------------------------------------------- //
|
|
|
|
void datacmpt() {
|
|
|
|
// -------------------------- NAV -------------------------- //
|
|
|
|
// -------------------- GPS Healt -------------------- //
|
|
|
|
if (((gps.location.age()) < 999) and (atof(fix_type.value()) == 3)) { gps_ok = true; }
|
|
else { gps_ok = false; }
|
|
|
|
// -------------------------- STUFF DEPENDING ON GPS COURSE OVER GROUND -------------------------- //
|
|
|
|
// -------------------- Autopilot -------------------- //
|
|
|
|
if (gps.course.isUpdated()) {
|
|
new_cog = true;
|
|
if (cog_valid(cog_count) == true) {
|
|
cog_ok = true;
|
|
setPoint_Home = TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),lat_B,lon_B);
|
|
errHome = getangle(gps.course.deg(), setPoint_Home);
|
|
}
|
|
else { cog_ok = false; last_errHome = errHome; }
|
|
prev_cog = gps.course.deg();
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
if ((gps_ok == true) and (cog_ok == true)) {
|
|
|
|
switch (autopilot_mode) {
|
|
|
|
case 0:
|
|
if (new_cog == true) {
|
|
|
|
raterror = getangle((last_errHome+180),(errHome+180))*5; raterror = rs.reading(raterror);
|
|
ratecog = getangle(prev_cog_b,gps.course.deg())*5; ratecog = rc.reading(ratecog);
|
|
next_cog = (int(gps.course.deg() + (ratecog/2))%360);
|
|
|
|
float PIDsum = ((NKp*errHome)+(NKd*raterror));
|
|
cmd_mult = (0.09999996 + (1 - 0.09999996)/(1 + pow(((abs(ratecog))/49.54231),24.26521))); cmd_mult = mult.reading(cmd_mult*1000); cmd_mult = (cmd_mult/1000);
|
|
|
|
cmdHome = PIDsum*cmd_mult ;
|
|
|
|
//if (TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),lat_B,lon_B)<5) { cmdHome = 180; }
|
|
if (vspeed<-4) { spiral = true; }
|
|
if (vspeed>-3) { spiral = false; }
|
|
if (spiral == true) { cmdHome = 0; }
|
|
|
|
new_cog = false;
|
|
last_errHome = errHome;
|
|
prev_cog_b = gps.course.deg();
|
|
|
|
steer_auto = map(cmdHome, -180, +180, 1050, 1950);
|
|
steer_auto = constrain(steer_auto, servo_auto_min, servo_auto_max);
|
|
|
|
}
|
|
break;
|
|
|
|
case 1:
|
|
time_up = map(abs(errHome), 0, 180, 200, 600);
|
|
if ((millis()-tup)>=time_down) {
|
|
tup = millis();
|
|
if (errHome > 0) { steer_auto = cmd_right; }
|
|
else { steer_auto = cmd_left; }
|
|
}
|
|
if ((millis()-tup)>=time_up) { steer_auto = 1500; }
|
|
if (abs(errHome)<5) { steer_auto = 1500; }
|
|
break;
|
|
|
|
case 2:
|
|
if (new_cog == true) {
|
|
raterror = getangle((last_errHome+180),(errHome+180))*5;
|
|
float setPoint_rate = (errHome*rate_yaw);
|
|
float errRate = (setPoint_rate - raterror);
|
|
cmdHome = cmdHome + (errRate/10) ;
|
|
if (cmdHome > 2000) { cmdHome = 2000; }
|
|
if (cmdHome < 1000) { cmdHome = 1000; }
|
|
last_errHome = errHome;
|
|
|
|
steer_auto = map(cmdHome, -180, +180, 1000, 2000);
|
|
steer_auto = constrain(steer_auto, servo_auto_min, servo_auto_max);
|
|
new_cog = false;
|
|
}
|
|
break;
|
|
|
|
}
|
|
}
|
|
|
|
else { steer_auto = 1500; }
|
|
|
|
|
|
// -------------------- 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;
|
|
|
|
}
|
|
|
|
// -------------------- Alt Fusion -------------------- //
|
|
|
|
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);
|
|
|
|
|
|
// -------------------- Stationary -------------------- //
|
|
|
|
if ((millis()-tstab) >= 1000) {
|
|
tstab = millis();
|
|
|
|
if (vspeed < 0.2 and vspeed > -0.2) { vspeed_count = (vspeed_count + 1); }
|
|
else { vspeed_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 (vspeed_count >= 10) { baro_stab = true; }
|
|
else { baro_stab = false; }
|
|
|
|
if (gps_count >= 10) { gps_stab = true; }
|
|
else { gps_stab = false; }
|
|
|
|
prev_gps = gps.altitude.meters();
|
|
|
|
|
|
}
|
|
|
|
// -------------------------- String -------------------------- //
|
|
|
|
dtostrf(gps.date.year(), 2, 0, date_year);
|
|
dtostrf(gps.date.month(), 1, 0, date_month);
|
|
dtostrf(gps.date.day(), 1, 0, date_day);
|
|
|
|
dtostrf(gps.time.hour(), 1, 0, time_hour);
|
|
dtostrf(gps.time.minute(), 1, 0, time_minute);
|
|
dtostrf(gps.time.second(), 1, 0, time_seconde);
|
|
|
|
|
|
snprintf(date_time, 100, "%s:%s:%s,%s:%s:%s", date_year, date_month, date_day, time_hour, time_minute, time_seconde);
|
|
|
|
if (flight_rebooted == false) { time_number = ((gps.date.day()*1000000) + (gps.time.hour()*10000) + (gps.time.minute()*100) + gps.time.second()); }
|
|
|
|
dtostrf(gps.location.lat(), 10, 10, lat_A_text);
|
|
dtostrf(gps.location.lng(), 10, 10, lon_A_text);
|
|
dtostrf(gps.altitude.meters(), 1, 1, alt_gps_text);
|
|
dtostrf(gps.course.deg(), 1, 0, cog_text);
|
|
dtostrf(gps.speed.mps(), 1, 1, speed_text);
|
|
dtostrf(gps.satellites.value(), 1, 0, sat_text);
|
|
dtostrf(atof(fix_type.value()), 1, 0, fix_type_text);
|
|
dtostrf(gps.hdop.value(), 1, 0, hdop_text);
|
|
|
|
if (gps.location.age()>10000) { dtostrf(9999, 1, 0, pos_age_text); }
|
|
else { dtostrf(gps.location.age(), 1, 0, pos_age_text); }
|
|
|
|
snprintf(gps_text, 120, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s", 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);
|
|
|
|
dtostrf(alt_baro, 1, 3, alt_baro_text);
|
|
dtostrf(vspeed, 1, 2, vspeed_text);
|
|
|
|
snprintf(baro_text, 120, "%s,%s", alt_baro_text, vspeed_text);
|
|
|
|
dtostrf(merged_alt, 2, 3, merged_alt_text);
|
|
dtostrf(gps_weight, 2, 2, gps_weight_text);
|
|
dtostrf(baro_weight, 2, 3, baro_weight_text);
|
|
|
|
dtostrf(setPoint_Home, 2, 2, setPoint_Home_text);
|
|
dtostrf(errHome, 1, 2, errHome_text);
|
|
dtostrf(raterror, 1, 0, raterror_text);
|
|
dtostrf(cmd_mult, 1, 3, cmd_mult_text);
|
|
|
|
dtostrf(lat_B, 1, 5, lat_B_text);
|
|
dtostrf(lon_B, 1, 5, lon_B_text);
|
|
|
|
dtostrf(next_cog, 1, 0, next_cog_text);
|
|
|
|
snprintf(nav_text, 140, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s", merged_alt_text, baro_weight_text, gps_weight_text, setPoint_Home_text, errHome_text, raterror_text, next_cog_text, cmd_mult_text, lat_B_text, lon_B_text);
|
|
|
|
dtostrf(channels[0], 2, 0, a_text);
|
|
dtostrf(channels[1], 2, 0, b_text);
|
|
dtostrf(channels[2], 2, 0, c_text);
|
|
dtostrf(channels[3], 2, 0, d_text);
|
|
dtostrf(channels[4], 2, 0, e_text);
|
|
dtostrf(channels[5], 2, 0, f_text);
|
|
dtostrf(channels[6], 2, 0, g_text);
|
|
|
|
snprintf(rc_text, 120, "%s,%s,%s,%s,%s,%s,%s", a_text, b_text, c_text, d_text, e_text, f_text, g_text);
|
|
|
|
if (drop == true) {
|
|
dtostrf(servo_left, 2, 0, left_text);
|
|
dtostrf(servo_right, 2, 0, right_text);
|
|
dtostrf(servo_deploy, 2, 0, deploy_text);
|
|
snprintf(servo_text, 40, "%s,%s,%s", left_text, right_text, deploy_text);
|
|
}
|
|
|
|
else {
|
|
dtostrf(servo_left, 2, 0, left_text);
|
|
dtostrf(servo_right, 2, 0, right_text);
|
|
dtostrf(servo_esc, 2, 0, esc_text);
|
|
snprintf(servo_text, 40, "%s,%s,%s", left_text, right_text, esc_text);
|
|
}
|
|
|
|
unsigned long long actual_time = (millis()+reboot_time);
|
|
dtostrf(actual_time, 1, 0, time_text);
|
|
dtostrf(flight_mode, 1, 0, flight_mode_text);
|
|
dtostrf(gps_ok, 1, 0, gps_ok_text);
|
|
dtostrf(cog_ok, 1, 0, cog_ok_text);
|
|
dtostrf(failSafe, 1, 0, failSafe_text);
|
|
dtostrf(gps_stab, 1, 0, gps_stab_text);
|
|
dtostrf(baro_stab, 1, 0, baro_stab_text);
|
|
dtostrf(deployed, 1, 0, deployed_text);
|
|
dtostrf(wing_opened, 1, 0, wing_opened_text);
|
|
dtostrf(vbatt, 1, 3, vbatt_text);
|
|
dtostrf(loop_time, 1, 0, looptime_text);
|
|
dtostrf(packet_count, 1, 0, packet_count_text);
|
|
dtostrf(initialised, 1, 0, initialised_text);
|
|
|
|
snprintf(status_text, 100, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s", time_text, packet_count_text, flight_mode_text, gps_ok_text, cog_ok_text, failSafe_text, gps_stab_text, baro_stab_text, deployed_text, wing_opened_text, initialised_text, vbatt_text, looptime_text);
|
|
|
|
snprintf(mainSD, 340, "%s,%s,%s,%s,%s,%s", status_text, gps_text, baro_text, nav_text, rc_text, servo_text);
|
|
|
|
snprintf(mainTLM, 250, "/*%s,%s,%s,%s*/", status_text, gps_text, baro_text, servo_text);
|
|
|
|
// -------------------------- TLM -------------------------- //
|
|
|
|
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:
|
|
delaySD = 50;
|
|
delayTLM = 1000;
|
|
break;
|
|
|
|
}
|
|
|
|
if (millis()-tlm>=delayTLM) {
|
|
tlm = millis();
|
|
packet_count = (packet_count +1);
|
|
Serial5.println(mainTLM);
|
|
Serial.println(mainTLM);
|
|
}
|
|
|
|
// -------------------------- SD DATALOG -------------------------- //
|
|
|
|
if (record_home == false) {
|
|
|
|
if (initialised == true) {
|
|
|
|
if ((millis()-sd)>=delaySD) {
|
|
sd = millis();
|
|
|
|
if (!SD.begin(chipSelect)) { sd_ok = false; }
|
|
else {
|
|
sd_ok = true;
|
|
dataFile = SD.open(namebuff, FILE_WRITE);
|
|
|
|
if (dataFile) { dataFile.println(mainSD); dataFile.close(); }
|
|
else { sd_ok = false; }
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
else {
|
|
|
|
if ((flight_mode == 9) or (flight_mode == 10)) {
|
|
|
|
if ((millis()-sd)>=delaySD) {
|
|
sd = millis();
|
|
|
|
if (!SD.begin(chipSelect)) { sd_ok = false; }
|
|
else {
|
|
sd_ok = true;
|
|
dataFile = SD.open(namebuff, FILE_WRITE);
|
|
|
|
if (dataFile) { dataFile.println(mainSD); dataFile.close(); }
|
|
else { sd_ok = false; }
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// ----------------------------------------------------- State machine ----------------------------------------------------- //
|
|
|
|
|
|
void flight_state() {
|
|
|
|
if (flight_mode != prev_mode) {
|
|
|
|
packet_count = (packet_count +1);
|
|
Serial5.println(mainTLM);
|
|
Serial.println(mainTLM);
|
|
|
|
if (!SD.begin(chipSelect)) {}
|
|
else {
|
|
dataFile = SD.open(namebuff, FILE_WRITE);
|
|
if (dataFile) { dataFile.println(mainSD); dataFile.close(); }
|
|
}
|
|
prev_mode = flight_mode;
|
|
}
|
|
|
|
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;
|
|
|
|
}
|
|
}
|
|
//------------------- 0 -------------------//
|
|
|
|
void flight_init() {
|
|
|
|
if (reboot_state == 0) {
|
|
|
|
if ((gps.satellites.value() >= 6) and (gps_ok == true) and (gps_stab == true) and (millis()>5000)) {
|
|
|
|
EasyBuzzer.beep(3000,100,50,10,500,1);
|
|
lat_B = gps.location.lat();
|
|
lon_B = gps.location.lng();
|
|
|
|
initialised = true;
|
|
|
|
EEPROM.put(10, dep_altitude);
|
|
EEPROM.put(30, lat_B);
|
|
EEPROM.put(50, lon_B);
|
|
|
|
while (abs(alt_baro-gps.altitude.meters())>0.01) {
|
|
if ((micros()-baro_blk)>10) {
|
|
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();
|
|
}
|
|
}
|
|
|
|
EEPROM.put(70, baro_set);
|
|
|
|
vs.reset();
|
|
al.reset();
|
|
|
|
dep_altitude = (dep_altitude+gps.altitude.meters());
|
|
|
|
strip.setBrightness(50);
|
|
setcam(1);
|
|
|
|
if (record_home == false) { newfile(); }
|
|
|
|
if (drop == true) { flight_mode = 1;}
|
|
else { flight_mode = 8; }
|
|
|
|
init_time = millis();
|
|
|
|
}
|
|
}
|
|
|
|
else {
|
|
|
|
if ((gps.satellites.value() >= 6) and (gps_ok == true) and (millis()>5000)) {
|
|
|
|
EasyBuzzer.beep(3000,100,50,10,500,1);
|
|
initialised = true;
|
|
|
|
vs.reset();
|
|
al.reset();
|
|
|
|
strip.setBrightness(50);
|
|
setcam(1);
|
|
|
|
if (drop == true) { flight_mode = 1;}
|
|
else { flight_mode = 8; }
|
|
|
|
init_time = millis();
|
|
|
|
dtostrf(time_number, 1, 0, sdnamebuff);
|
|
sprintf(namebuff, "%s.txt", sdnamebuff);
|
|
|
|
reboot_state = 0;
|
|
EEPROM.write(0, 0);
|
|
|
|
}
|
|
}
|
|
|
|
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>=1000) {
|
|
|
|
if (vspeed>vup) { flight_mode = 2; EasyBuzzer.beep(1000,100,50,2,500,1); strip.setBrightness(75); }
|
|
if (vspeed<vdown) { flight_mode = 3; EasyBuzzer.beep(3000,100,50,3,500,1); strip.setBrightness(100); }
|
|
if (atof(fix_type.value()) < 2) { flight_mode = 0; EasyBuzzer.beep(700,100,50,3,500,1); strip.setBrightness(50); }
|
|
|
|
}
|
|
}
|
|
|
|
//------------------- 2 -------------------//
|
|
|
|
void flight_ascent() { if (vspeed<0.5) {flight_mode = 1; strip.setBrightness(50);} }
|
|
|
|
//------------------- 3 -------------------//
|
|
|
|
void flight_descent() {
|
|
|
|
if (vspeed>-0.5) {flight_mode = 1; strip.setBrightness(50);}
|
|
if (merged_alt < dep_altitude) { flight_mode = 4; EasyBuzzer.beep(3000,100,50,5,500,1); strip.setBrightness(255); deployed = true; init_time = millis(); }
|
|
|
|
}
|
|
|
|
//------------------- 4 -------------------//
|
|
|
|
void flight_gliding() {
|
|
|
|
if (((millis()-init_time) >= gliding_timer) or (vspeed > (vdown+1))) {
|
|
wing_opened = true;
|
|
last_errHome = errHome;
|
|
if (failSafe == true) {flight_mode = 5; strip.setBrightness(100);}
|
|
else {flight_mode = 6; strip.setBrightness(100);}
|
|
}
|
|
}
|
|
|
|
//------------------- 5 -------------------//
|
|
|
|
void flight_gliding_auto() {
|
|
|
|
if (failSafe == false) {flight_mode = 6;}
|
|
if (vspeed < -6) { spiral = true; }
|
|
if (vspeed > -5) { spiral = false; }
|
|
}
|
|
|
|
//------------------- 6 -------------------//
|
|
|
|
void flight_gliding_manual() {
|
|
if (failSafe == true) {flight_mode = 5;}
|
|
}
|
|
|
|
//------------------- 7 -------------------//
|
|
|
|
void landed() { if ((baro_stab == false) or (gps_stab == false)) {flight_mode = 1; strip.setBrightness(50);} }
|
|
|
|
//------------------- 8 -------------------//
|
|
|
|
void motorised_man() {
|
|
|
|
if (channels[6] > 1000) { flight_mode = 9; if (record_home == true) {newfile();} }
|
|
if (failSafe == true) { flight_mode = 10; if (record_home == true) {newfile();} }
|
|
}
|
|
|
|
//------------------- 9 -------------------//
|
|
|
|
void motorised_auto() {
|
|
if (channels[6] < 1000) { flight_mode = 8; }
|
|
}
|
|
|
|
void motorised_failSafe() {
|
|
if (failSafe == false) { flight_mode = 8; }
|
|
}
|
|
// ----------------------------------------------------- Apply command ----------------------------------------------------- //
|
|
|
|
void applycmd() {
|
|
|
|
// -------------------------- Steering Servo -------------------------- //
|
|
|
|
switch(flight_mode) {
|
|
|
|
case 0:
|
|
case 1:
|
|
case 6:
|
|
if (failSafe == false) { servo_left = steer_man; servo_right = steer_man; }
|
|
else { servo_left = 1500; servo_right = 1500; }
|
|
break;
|
|
|
|
case 8:
|
|
servo_left = steer_man;
|
|
servo_right = steer_man;
|
|
break;
|
|
|
|
case 9:
|
|
case 10:
|
|
case 5:
|
|
servo_left = steer_auto;
|
|
servo_right = steer_auto;
|
|
break;
|
|
|
|
case 2:
|
|
case 3:
|
|
case 4:
|
|
servo_left = 1500;
|
|
servo_right = 1500;
|
|
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) { servo_deploy = 1500; }
|
|
else {
|
|
if (failSafe == true) { servo_deploy = 2000; }
|
|
else { servo_deploy = map(channels[6], 50, 1950, 1000, 2000);}
|
|
}
|
|
break;
|
|
|
|
case 8:
|
|
if ((channels[4]>1000) and (channels[2]<400)) { armed = true; }
|
|
if (channels[4]<1000) { armed = false; }
|
|
|
|
if (armed == true) { servo_esc = map(channels[2], 50, 1950, 1000, 2000); }
|
|
else { servo_esc = 900; }
|
|
|
|
if ((channels[4]>1000) and (channels[2]>400) and (armed == false)) { EasyBuzzer.singleBeep(3000,25); }
|
|
|
|
break;
|
|
|
|
case 9:
|
|
case 10:
|
|
servo_esc = 1000;
|
|
break;
|
|
|
|
}
|
|
}
|
|
|
|
// ----------------------------------------------------- Update command ----------------------------------------------------- //
|
|
|
|
void updatecmd(float a) {
|
|
|
|
if (drop == true) {
|
|
|
|
if ((millis()-tpwm)>=(1000/a)) {
|
|
|
|
tpwm = millis();
|
|
left.write(map(servo_left,1000,2000,0,180));
|
|
right.write(map(servo_right,1000,2000,0,180));
|
|
deploy.write(map(servo_deploy,1000,2000,0,180));
|
|
|
|
}
|
|
}
|
|
|
|
else {
|
|
|
|
if ((millis()-tpwm)>=(1000/a)) {
|
|
|
|
tpwm = millis();
|
|
left.write(map(servo_left,1000,2000,0,180));
|
|
right.write(map(servo_right,1000,2000,0,180));
|
|
esc.write(map(servo_esc,1000,2000,0,180));
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
// ----------------------------------------------------- User Interface ----------------------------------------------------- //
|
|
|
|
void userinter() {
|
|
|
|
// -------------------------- Buzzer -------------------------- //
|
|
|
|
EasyBuzzer.update();
|
|
|
|
// -------------------------- Voltage monitoring -------------------------- //
|
|
|
|
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; }
|
|
|
|
|
|
// -------------------------- RGB LED -------------------------- //
|
|
|
|
updateled();
|
|
|
|
}
|
|
|
|
// ----------------------------------------------------- Misc Functions ----------------------------------------------------- //
|
|
|
|
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();
|
|
colorWipe(strip.Color(b,a,c), 0);
|
|
duration = d;
|
|
timeled = millis();
|
|
}
|
|
}
|
|
|
|
void updateled() {
|
|
if ((millis()-timeled)>=duration) {
|
|
colorWipe(strip.Color(0,0,0), 0);
|
|
}
|
|
}
|
|
|
|
void colorWipe(uint32_t color, int wait) {
|
|
for(int i=0; i<strip.numPixels(); i++) {
|
|
strip.setPixelColor(i, color);
|
|
strip.show();
|
|
}
|
|
}
|
|
|
|
boolean cog_valid(int a) {
|
|
if (abs(gps.course.deg()-prev_cog)<0.1) { valid_count++; }
|
|
else { valid_count = 0; }
|
|
if (valid_count >= a) { return false; }
|
|
else { return true; }
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
void setcam(int a) {
|
|
switch(a) {
|
|
case 1 :
|
|
digitalWrite(cam, HIGH);
|
|
break;
|
|
case 0 :
|
|
digitalWrite(cam, LOW);
|
|
break;
|
|
}
|
|
}
|
|
|
|
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());
|
|
}
|
|
|
|
void gpset(int a, int b, int c, int d, int e){
|
|
|
|
if (a == 9600) {
|
|
Serial7.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) {
|
|
Serial7.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));
|
|
Serial7.end();
|
|
Serial7.begin(57600);
|
|
delay(100);
|
|
}
|
|
|
|
if (a == 115200) {
|
|
Serial7.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));
|
|
Serial7.end();
|
|
Serial7.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));
|
|
}
|
|
|
|
Serial7.addMemoryForRead(serial2bufferRead, sizeof(serial2bufferRead));
|
|
|
|
}
|
|
|
|
void sendPacket(byte *packet, byte len){
|
|
for (byte i = 0; i < len; i++) { Serial7.write(packet[i]); }
|
|
}
|
|
|
|
void newfile() {
|
|
dtostrf(time_number, 1, 0, sdnamebuff);
|
|
sprintf(namebuff, "%s.txt", sdnamebuff);
|
|
|
|
if (!SD.begin(chipSelect)) {}
|
|
else {
|
|
SdFile::dateTimeCallback(dateTime);
|
|
dataFile = SD.open(namebuff, FILE_WRITE);
|
|
if (dataFile) {
|
|
dataFile.println("time (ms), Packet_Count (text), Mode (text), GPS_Ok (text), COG_Ok (text), FailSafe (text), GPS_Stab (text), Baro_Stab (text), Deployed (text), Wing_Opened (text), Initialised (tex), Vbatt (V), Looptime (µS), 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), Vertical_Speed (m/s), Altitude (m), Baro_Weight (text), GPS_Weight (text), SetPoint_Home (deg), Err_Home (deg), Rate_Error (deg), Next_Cog (deg), Cmd_mult (text), LatB (deg), LonB (deg), Ch 0 (µs), Ch 1 (µs), Ch 2 (µs), Ch 3 (µs), Ch 4 (µs), Ch 5 (µs), Ch 6 (µs), PWM_L (µs), PWM_R (µs), PWM_D (µs)");
|
|
dataFile.close();
|
|
EEPROM.put(90, time_number);
|
|
}
|
|
}
|
|
}
|