kopia lustrzana https://github.com/YohanHadji/R2Home
Update with new debug mode (make sure to disable it before flight!!)
rodzic
2b43cc9877
commit
7e9e9ea47e
|
@ -54,6 +54,7 @@ void barometer_setup() {
|
||||||
Adafruit_BMP280::FILTER_X16,
|
Adafruit_BMP280::FILTER_X16,
|
||||||
Adafruit_BMP280::STANDBY_MS_1);
|
Adafruit_BMP280::STANDBY_MS_1);
|
||||||
baroset(0, 1);
|
baroset(0, 1);
|
||||||
|
if (DEBUG) { Serial.println("Baro was set correctly"); }
|
||||||
}
|
}
|
||||||
|
|
||||||
void cmpt_vertical_speed_baro(float da, int dt) {
|
void cmpt_vertical_speed_baro(float da, int dt) {
|
||||||
|
@ -64,6 +65,7 @@ void cmpt_vertical_speed_baro(float da, int dt) {
|
||||||
|
|
||||||
void get_baro(int mode) {
|
void get_baro(int mode) {
|
||||||
if ((millis()-baroA)>=10 and mode == 0) {
|
if ((millis()-baroA)>=10 and mode == 0) {
|
||||||
|
if (DEBUG) { Serial.println("Got a Baro message"); }
|
||||||
|
|
||||||
baroA = millis();
|
baroA = millis();
|
||||||
unsigned waitd = millis();
|
unsigned waitd = millis();
|
||||||
|
|
|
@ -13,6 +13,7 @@ int cells = 0;
|
||||||
void battery_setup() {
|
void battery_setup() {
|
||||||
pinMode(vpin, INPUT);
|
pinMode(vpin, INPUT);
|
||||||
voltage.begin();
|
voltage.begin();
|
||||||
|
if (DEBUG) { Serial.println("RC was set correctly"); }
|
||||||
}
|
}
|
||||||
|
|
||||||
void get_vbatt() {
|
void get_vbatt() {
|
||||||
|
|
|
@ -34,6 +34,7 @@ void setcam(int a, int b, int c) {
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
camReady = true;
|
camReady = true;
|
||||||
|
if (DEBUG) { Serial.println("Camera was set correctly"); }
|
||||||
}
|
}
|
||||||
|
|
||||||
void updatecam() {
|
void updatecam() {
|
||||||
|
|
|
@ -16,7 +16,8 @@
|
||||||
#define COG_BRAKE false // Will reduce command if CoG is turning faster than a threshold
|
#define COG_BRAKE false // Will reduce command if CoG is turning faster than a threshold
|
||||||
#define GPS_FREQ 5 // Hz
|
#define GPS_FREQ 5 // Hz
|
||||||
#define SERVO_RATE 50 // Hz
|
#define SERVO_RATE 50 // Hz
|
||||||
#define TLM_MONITOR true // Show telemetry on serial monitor
|
#define TLM_MONITOR false // Show telemetry on serial monitor
|
||||||
|
#define DEBUG true // Will show all the steps on the screen
|
||||||
#define GPS_PORT Serial7
|
#define GPS_PORT Serial7
|
||||||
#define TLM_PORT Serial5
|
#define TLM_PORT Serial5
|
||||||
#define RX_PORT Serial1
|
#define RX_PORT Serial1
|
||||||
|
|
|
@ -14,6 +14,7 @@ void navigation_setup() {
|
||||||
myPID.SetTunings(NKP, NKI, NKD);
|
myPID.SetTunings(NKP, NKI, NKD);
|
||||||
myPID.SetOutputLimits(-180, 180);
|
myPID.SetOutputLimits(-180, 180);
|
||||||
myPID.SetMode(MANUAL);
|
myPID.SetMode(MANUAL);
|
||||||
|
if (DEBUG) { Serial.println("PIDs set correctly"); }
|
||||||
}
|
}
|
||||||
|
|
||||||
float cmpt_cmd(float err) {
|
float cmpt_cmd(float err) {
|
||||||
|
|
|
@ -56,6 +56,7 @@ void cmpt_data_rate(int flight_mode) {
|
||||||
void send_data() {
|
void send_data() {
|
||||||
packet_count = (packet_count +1);
|
packet_count = (packet_count +1);
|
||||||
TLM_PORT.println(mainTLM);
|
TLM_PORT.println(mainTLM);
|
||||||
|
if (DEBUG) { Serial.println("Sending Data"); }
|
||||||
if (TLM_MONITOR) {
|
if (TLM_MONITOR) {
|
||||||
Serial.println(mainTLM);
|
Serial.println(mainTLM);
|
||||||
}
|
}
|
||||||
|
@ -64,6 +65,7 @@ void send_data() {
|
||||||
void save_data(bool initialised) {
|
void save_data(bool initialised) {
|
||||||
if (initialised == true) {
|
if (initialised == true) {
|
||||||
if (sd_ok == true and SD_WRITING == true) {
|
if (sd_ok == true and SD_WRITING == true) {
|
||||||
|
if (DEBUG) { Serial.println("Saving Data"); }
|
||||||
dataFile = SD.open(namebuff, FILE_WRITE);
|
dataFile = SD.open(namebuff, FILE_WRITE);
|
||||||
dataFile.println(mainSD);
|
dataFile.println(mainSD);
|
||||||
dataFile.close();
|
dataFile.close();
|
||||||
|
@ -100,8 +102,11 @@ void newfile() {
|
||||||
sprintf(namebuff, "%s.txt", sdnamebuff);
|
sprintf(namebuff, "%s.txt", sdnamebuff);
|
||||||
sprintf(nameconfig, "config_%s.txt", sdnamebuff);
|
sprintf(nameconfig, "config_%s.txt", sdnamebuff);
|
||||||
|
|
||||||
if (!SD.begin(chipSelect)) { sd_ok = false; }
|
if (DEBUG) { Serial.println("Trying to create a new LOG file"); }
|
||||||
|
|
||||||
|
if (!SD.begin(chipSelect)) { sd_ok = false; if (DEBUG) { Serial.println("Failure"); } }
|
||||||
else {
|
else {
|
||||||
|
if (DEBUG) { Serial.println("Success"); }
|
||||||
sd_ok = true;
|
sd_ok = true;
|
||||||
SdFile::dateTimeCallback(dateTime);
|
SdFile::dateTimeCallback(dateTime);
|
||||||
dataFile = SD.open(namebuff, FILE_WRITE);
|
dataFile = SD.open(namebuff, FILE_WRITE);
|
||||||
|
@ -129,8 +134,10 @@ void getconfig() {
|
||||||
if (!SD.begin(chipSelect)) { sd_ok = false; delay(1500); }
|
if (!SD.begin(chipSelect)) { sd_ok = false; delay(1500); }
|
||||||
else {
|
else {
|
||||||
sd_ok = true;
|
sd_ok = true;
|
||||||
|
if (DEBUG) { Serial.println("Trying to read the file"); }
|
||||||
File configFile = SD.open("config.txt", FILE_READ);
|
File configFile = SD.open("config.txt", FILE_READ);
|
||||||
if (configFile) {
|
if (configFile) {
|
||||||
|
if (DEBUG) { Serial.println("Success"); }
|
||||||
|
|
||||||
// Reading the baro adress
|
// Reading the baro adress
|
||||||
String memory = "";
|
String memory = "";
|
||||||
|
@ -170,6 +177,7 @@ void getconfig() {
|
||||||
configFile.close();
|
configFile.close();
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
if (DEBUG) { Serial.println("Failure"); }
|
||||||
delay(1500) ;
|
delay(1500) ;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -49,6 +49,7 @@ void buzzer_setup() {
|
||||||
tone(BUZZER_PIN, 762);
|
tone(BUZZER_PIN, 762);
|
||||||
delay(200);
|
delay(200);
|
||||||
noTone(BUZZER_PIN);
|
noTone(BUZZER_PIN);
|
||||||
|
if (DEBUG) { Serial.println("Buzzer was set correctly"); }
|
||||||
}
|
}
|
||||||
|
|
||||||
void buzzer_end_setup() {
|
void buzzer_end_setup() {
|
||||||
|
@ -169,6 +170,7 @@ void led_setup() {
|
||||||
strip.begin();
|
strip.begin();
|
||||||
strip.show();
|
strip.show();
|
||||||
strip.setBrightness(255);
|
strip.setBrightness(255);
|
||||||
|
if (DEBUG) { Serial.println("LED was set correctly"); }
|
||||||
}
|
}
|
||||||
|
|
||||||
void colorWipe(uint32_t color, int wait) {
|
void colorWipe(uint32_t color, int wait) {
|
||||||
|
|
|
@ -27,6 +27,7 @@ void cmpt_vertical_speed_gps(float da, int dt) {
|
||||||
void get_gps() {
|
void get_gps() {
|
||||||
|
|
||||||
while (GPS_PORT.available()) {
|
while (GPS_PORT.available()) {
|
||||||
|
if (DEBUG) { Serial.println("Got a GPS message"); }
|
||||||
gps.encode(GPS_PORT.read());
|
gps.encode(GPS_PORT.read());
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -135,6 +136,7 @@ if (e == 1) {
|
||||||
}
|
}
|
||||||
|
|
||||||
GPS_PORT.addMemoryForRead(serial2bufferRead, sizeof(serial2bufferRead));
|
GPS_PORT.addMemoryForRead(serial2bufferRead, sizeof(serial2bufferRead));
|
||||||
|
if (DEBUG) { Serial.println("GPS was set correctly"); }
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -10,8 +10,13 @@ float cmd_to_waypoint = 0;
|
||||||
void setup() {
|
void setup() {
|
||||||
|
|
||||||
Serial.begin(115200);
|
Serial.begin(115200);
|
||||||
|
|
||||||
|
if (DEBUG) { Serial.println("Just barely waking up"); }
|
||||||
|
|
||||||
TLM_PORT.begin(57600);
|
TLM_PORT.begin(57600);
|
||||||
|
|
||||||
|
if (DEBUG) { Serial.println("Ready to initialise all the hardware"); }
|
||||||
|
|
||||||
battery_setup();
|
battery_setup();
|
||||||
buzzer_setup();
|
buzzer_setup();
|
||||||
camera_setup();
|
camera_setup();
|
||||||
|
@ -44,6 +49,11 @@ void loop() {
|
||||||
loop_rate = loop_count;
|
loop_rate = loop_count;
|
||||||
loop_count = 0;
|
loop_count = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
if (DEBUG) {
|
||||||
|
delay(10);
|
||||||
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void getdata() {
|
void getdata() {
|
||||||
|
@ -58,6 +68,7 @@ void datacmpt() {
|
||||||
|
|
||||||
if (new_cog) {
|
if (new_cog) {
|
||||||
new_cog = false;
|
new_cog = false;
|
||||||
|
if (DEBUG) { Serial.println("New direction and command computed"); }
|
||||||
setPoint_waypoint = cmpt_setpoint(current_waypoint);
|
setPoint_waypoint = cmpt_setpoint(current_waypoint);
|
||||||
error_waypoint = cmpt_error(gps.course.deg(), setPoint_waypoint);
|
error_waypoint = cmpt_error(gps.course.deg(), setPoint_waypoint);
|
||||||
cmd_to_waypoint = cmpt_cmd(error_waypoint);
|
cmd_to_waypoint = cmpt_cmd(error_waypoint);
|
||||||
|
|
|
@ -8,6 +8,7 @@ bool lostFrame;
|
||||||
|
|
||||||
void rc_setup() {
|
void rc_setup() {
|
||||||
rx.begin();
|
rx.begin();
|
||||||
|
if (DEBUG) { Serial.println("RC was set correctly"); }
|
||||||
}
|
}
|
||||||
|
|
||||||
void get_rc() {
|
void get_rc() {
|
||||||
|
@ -15,6 +16,7 @@ void get_rc() {
|
||||||
|
|
||||||
if ((channels[3])>=1500 or (channels[3])==0) { failsafe = true; }
|
if ((channels[3])>=1500 or (channels[3])==0) { failsafe = true; }
|
||||||
else { failsafe = false; }
|
else { failsafe = false; }
|
||||||
|
if (DEBUG) { Serial.println("Just got RC (even if no rx is connected)"); }
|
||||||
}
|
}
|
||||||
|
|
||||||
String rc_text() {
|
String rc_text() {
|
||||||
|
|
Ładowanie…
Reference in New Issue