kopia lustrzana https://github.com/YohanHadji/R2Home
Version is flight proven
YohanHadji
rodzic
2a03120871
commit
5b3445af86
|
|
@ -117,5 +117,6 @@ void get_baro(int mode) {
|
|||
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;
|
||||
String baro_pressure_text = String(pressure_baro, 2);
|
||||
return alt_baro_text+","+baro_pressure_text+","+baro_vspeed_text;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -3,8 +3,8 @@
|
|||
int cam = 23;
|
||||
bool camIsOn = false;
|
||||
bool camReady = false;
|
||||
int onDuration = 0;
|
||||
int offDuration = 0;
|
||||
unsigned long onDuration = 0;
|
||||
unsigned long offDuration = 0;
|
||||
|
||||
unsigned long camOn = 0;
|
||||
unsigned long camOff = 0;
|
||||
|
|
|
|||
|
|
@ -4,8 +4,8 @@
|
|||
|
||||
Watchdog watchdog;
|
||||
bool sd_ok = false;
|
||||
int delaySD = 100; // Datalog
|
||||
int delayTLM = 1000; // Tlm
|
||||
unsigned int delaySD = 100; // Datalog
|
||||
unsigned int delayTLM = 1000; // Tlm
|
||||
|
||||
char sdnamebuff[20];
|
||||
String mainSD;
|
||||
|
|
@ -45,7 +45,7 @@ void cmpt_data_rate(int flight_mode) {
|
|||
case 8:
|
||||
case 9:
|
||||
if (LOW_RATE) { delaySD = 200; }
|
||||
else { delaySD = 50; }
|
||||
else { delaySD = 100; }
|
||||
delayTLM = 1000;
|
||||
break;
|
||||
}
|
||||
|
|
@ -69,7 +69,7 @@ void save_data(bool initialised) {
|
|||
}
|
||||
}
|
||||
|
||||
void cmpt_string_data(int flight_mode, bool initialised, bool deployed, bool wing_opened) {
|
||||
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());
|
||||
|
||||
|
|
@ -81,12 +81,13 @@ void cmpt_string_data(int flight_mode, bool initialised, bool deployed, bool win
|
|||
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+","+failsafe_text+","+vbatt_text+","+loop_rate_text;
|
||||
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;
|
||||
|
||||
mainSD = status_text+","+gps_text()+","+baro_text()+","+nav_text()+","+rc_text()+","+servo_text();
|
||||
mainTLM = "/*"+status_text+","+gps_text()+","+baro_text()+","+servo_text()+"/*";
|
||||
|
|
@ -103,7 +104,7 @@ void newfile() {
|
|||
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.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), 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();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -178,7 +178,7 @@ void colorWipe(uint32_t color, int wait) {
|
|||
}
|
||||
}
|
||||
|
||||
void setled(int a, int b, int c, int d, int e) {
|
||||
void setled(int a, int b, int c, int d, unsigned 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; }
|
||||
|
|
|
|||
|
|
@ -7,6 +7,7 @@ bool initialised = false;
|
|||
bool flight_started = false;
|
||||
bool deployed = false;
|
||||
bool wing_opened = false;
|
||||
bool spiral = false;
|
||||
unsigned long init_time = 0;
|
||||
|
||||
|
||||
|
|
@ -43,6 +44,8 @@ void flight_init() {
|
|||
b_al.reset();
|
||||
g_vs.reset();
|
||||
|
||||
cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral);
|
||||
|
||||
ground_altitude = gps.altitude.meters();
|
||||
dep_altitude = (DEP_ALT+ground_altitude);
|
||||
|
||||
|
|
@ -67,7 +70,7 @@ void flight_init() {
|
|||
|
||||
void ready_steady() {
|
||||
|
||||
if (millis()-init_time>=5000) {
|
||||
if (millis()-init_time>=3000) {
|
||||
|
||||
if (is_ascent(VUP, 0)) {
|
||||
flight_mode = 2;
|
||||
|
|
@ -96,7 +99,7 @@ void ready_steady() {
|
|||
//------------------- 2 -------------------//
|
||||
|
||||
void flight_ascent() {
|
||||
if (baro_vspeed<0.5) {
|
||||
if (is_descent(0, 0)) {
|
||||
flight_mode = 1;
|
||||
}
|
||||
if ((gps.altitude.meters()-ground_altitude)>10) {
|
||||
|
|
@ -111,6 +114,7 @@ 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);
|
||||
|
|
@ -124,7 +128,7 @@ void flight_descent() {
|
|||
|
||||
void flight_gliding() {
|
||||
|
||||
if ((millis()-init_time) >= GLIDING_TIMER) {
|
||||
if ((millis()-init_time) >= OPENING_TIMER) {
|
||||
wing_opened = true;
|
||||
|
||||
if (failsafe == true) {
|
||||
|
|
@ -148,6 +152,14 @@ void flight_gliding_auto() {
|
|||
flight_mode = 11;
|
||||
myPID.SetMode(MANUAL);
|
||||
}
|
||||
|
||||
if (is_descent(v_down(-5), 0)) {
|
||||
spiral = true;
|
||||
}
|
||||
|
||||
if (is_ascent(v_down(-2), 0)) {
|
||||
spiral = false;
|
||||
}
|
||||
|
||||
if (failsafe == false) {
|
||||
flight_mode = 6;
|
||||
|
|
|
|||
|
|
@ -69,7 +69,7 @@ void datacmpt() {
|
|||
cog_ok = true;
|
||||
}
|
||||
|
||||
steering = cmpt_servo(channels, cmd_to_waypoint, flight_mode, deployed, failsafe, cog_ok);
|
||||
steering = cmpt_servo(channels, cmd_to_waypoint, flight_mode, deployed, failsafe, cog_ok, spiral);
|
||||
update_servo_cmd(steering, SERVO_RATE);
|
||||
|
||||
cmpt_flight_state();
|
||||
|
|
@ -79,7 +79,7 @@ void datacmpt() {
|
|||
|
||||
if ((millis()-sd)>=delaySD) {
|
||||
sd = millis();
|
||||
cmpt_string_data(flight_mode, initialised, deployed, wing_opened);
|
||||
cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral);
|
||||
save_data(initialised);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -58,7 +58,7 @@ void cmpt_fusion() {
|
|||
}
|
||||
}
|
||||
|
||||
bool cmpt_vertical_state() {
|
||||
void cmpt_vertical_state() {
|
||||
if (new_baro) {
|
||||
new_baro = false;
|
||||
new_baro_fusion = true;
|
||||
|
|
|
|||
|
|
@ -26,7 +26,7 @@ void servo_setup() {
|
|||
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) {
|
||||
servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool deployed, bool failsafe, bool cog_ok, bool spiral) {
|
||||
|
||||
float roll = map(channels[0], 67, 1982, 1000, 2000);
|
||||
float pitch = map(channels[1], 67, 1982, 1000, 2000);
|
||||
|
|
@ -74,7 +74,7 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
|
|||
case 9:
|
||||
case 10:
|
||||
case 5:
|
||||
if (cog_ok) {
|
||||
if (cog_ok and !spiral) {
|
||||
steering_cmpt.right = autopilot;
|
||||
steering_cmpt.left = 3000-autopilot;
|
||||
}
|
||||
|
|
@ -169,7 +169,7 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
|
|||
return steering_cmpt;
|
||||
}
|
||||
|
||||
void update_servo_cmd(servo_cmd steering_apply, int a) {
|
||||
void update_servo_cmd(servo_cmd steering_apply, unsigned int a) {
|
||||
|
||||
if ((millis()-tpwm)>=(1000/a)) {
|
||||
tpwm = millis();
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue