kopia lustrzana https://github.com/YohanHadji/R2Home
184 wiersze
5.8 KiB
C++
184 wiersze
5.8 KiB
C++
#include <SD.h>
|
|
#include "feedback.hpp"
|
|
|
|
|
|
bool sd_ok = false;
|
|
unsigned int delaySD = 100; // Datalog
|
|
unsigned int delayTLM = 1000; // Tlm
|
|
|
|
char sdnamebuff[20];
|
|
char nameconfig[30];
|
|
String mainSD;
|
|
String mainTLM;
|
|
String minSD;
|
|
|
|
File dataFile;
|
|
File configFile;
|
|
File configSaveFile;
|
|
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 = 100; }
|
|
delayTLM = 1000;
|
|
break;
|
|
}
|
|
}
|
|
|
|
void send_data() {
|
|
packet_count = (packet_count +1);
|
|
TLM_PORT.println(mainTLM);
|
|
if (DEBUG) { Serial.println("Sending Data"); }
|
|
if (TLM_MONITOR) {
|
|
Serial.println(mainTLM);
|
|
}
|
|
}
|
|
|
|
void save_data() {
|
|
if (sd_ok == true and SD_WRITING == true) {
|
|
if (DEBUG) { Serial.println("Saving Data"); }
|
|
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, bool spiral) {
|
|
|
|
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 spiral_text = String(spiral);
|
|
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+","+spiral_text+","+failsafe_text+","+vbatt_text+","+loop_rate_text;
|
|
|
|
if (initialised) {
|
|
mainSD = status_text+","+gps_text()+","+baro_text()+","+nav_text()+","+rc_text()+","+servo_text()+","+servo_max_cmd_text();
|
|
}
|
|
mainTLM = "/*"+status_text+","+gps_text()+","+baro_text()+","+servo_text()+"/*";
|
|
}
|
|
|
|
void newfile() {
|
|
dtostrf(time_number, 1, 0, sdnamebuff);
|
|
sprintf(namebuff, "%s.txt", sdnamebuff);
|
|
sprintf(nameconfig, "config_%s.txt", sdnamebuff);
|
|
|
|
if (DEBUG) { Serial.println("Trying to create a new LOG file"); }
|
|
|
|
if (!SD.begin(chipSelect)) { sd_ok = false; if (DEBUG) { Serial.println("Failure"); } }
|
|
else {
|
|
if (DEBUG) { Serial.println("Success"); }
|
|
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), Spiral (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), Pressure (hpa), Baro_Vspeed (m/s), Altitude (m), Merged_Vspeed (m/s), 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), Waypoint Radius (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), PWM_X (us), MAX_M_W, MAX_C_W, MAC_M, MAX_C");
|
|
dataFile.close();
|
|
}
|
|
if (CONFIG_FILE_SV) {
|
|
SdFile::dateTimeCallback(dateTime);
|
|
configSaveFile = SD.open(nameconfig, FILE_WRITE);
|
|
delay(10);
|
|
if (configSaveFile) {
|
|
configSaveFile.println("Will be used later if we want to be able to save the configuration on startup to check what went wrong afterwards");
|
|
configSaveFile.close();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void getconfig() {
|
|
watchdog.enable(Watchdog::TIMEOUT_1S);
|
|
|
|
if (!SD.begin(chipSelect)) { sd_ok = false; delay(1500); }
|
|
else {
|
|
sd_ok = true;
|
|
if (DEBUG) { Serial.println("Trying to read the file"); }
|
|
File configFile = SD.open("config.txt", FILE_READ);
|
|
if (configFile) {
|
|
if (DEBUG) { Serial.println("Success"); }
|
|
|
|
// 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 {
|
|
if (DEBUG) { Serial.println("Failure"); }
|
|
delay(1500) ;
|
|
}
|
|
}
|
|
}
|