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

Update navigation and PIDs, version is flight proven

pull/1/head
Mathis et Yohan 2022-02-17 20:02:25 +01:00
rodzic fe363b8d3a
commit 0a91fdd6c6
1 zmienionych plików z 104 dodań i 69 usunięć
Pokaż statystyki Ściągnij plik aktualizacji Ściągnij plik porównania Expand all files Collapse all files

173
RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.02/R2Home_OBC_V1.02.ino
Wyświetl plik

@ -8,6 +8,7 @@
#include <movingAvg.h> #include <movingAvg.h>
#include <Adafruit_NeoPixel.h> #include <Adafruit_NeoPixel.h>
#include <EEPROM.h> #include <EEPROM.h>
#include <PID_v1.h>
// ----------------------------------- SETUP PANEL ----------------------------------- // // ----------------------------------- SETUP PANEL ----------------------------------- //
@ -39,21 +40,10 @@
#define blow 3.5 #define blow 3.5
#define no_batt 4.0 #define no_batt 4.0
#define servo_man_min 1000 #define servo_max_m 1900
#define servo_man_max 2000 #define servo_max_c 1550
#define servo_auto_min 1000 #define trig 20
#define servo_auto_max 2000
#define servo_left_min 2000
#define servo_left_start 1300
#define servo_left_max 1000
#define servo_right_min 2000
#define servo_right_start 1300
#define servo_right_max 1000
#define trig 10
#define left_offset 100 #define left_offset 100
#define right_offset 100 #define right_offset 100
@ -64,9 +54,16 @@
int dep_altitude = dep_alt; int dep_altitude = dep_alt;
int cog_count = 2; int cog_count = 2;
double Setpoint, Input, Output;
// NAV PIDs // // NAV PIDs //
float NKp = 1; float NKp = 1;
float NKd = 0; float NKi = 0.05;
float NKd = 0.1;
PID myPID(&Input, &Output, &Setpoint,NKp, NKi, NKd, DIRECT);
double long sim_cmd_time = 0; double long sim_cmd_time = 0;
float sim_cmd = 0; float sim_cmd = 0;
@ -158,6 +155,7 @@ float next_cog = 0;
float ratecog = 0; float ratecog = 0;
float prev_cog_b = 0; float prev_cog_b = 0;
float cmd_mult = 0; float cmd_mult = 0;
float cruise_cmd = 0;
struct gps_location { struct gps_location {
double latitude = 0; double latitude = 0;
@ -341,6 +339,10 @@ void setup() {
flight_rebooted = true; flight_rebooted = true;
} }
myPID.SetTunings(NKp, NKi, NKd);
myPID.SetOutputLimits(-180, 180);
myPID.SetMode(MANUAL);
tone(buzzer, 582); tone(buzzer, 582);
delay(200); delay(200);
@ -371,7 +373,6 @@ void loop() {
} }
void getdata() { void getdata() {
// -------------------------- Get GPS -------------------------- // // -------------------------- Get GPS -------------------------- //
@ -444,8 +445,11 @@ void datacmpt() {
if (gps.course.isUpdated()) { if (gps.course.isUpdated()) {
new_cog = true; new_cog = true;
if (cog_valid(cog_count) == true) { if (cog_valid(cog_count) == true) {
cog_ok = true; cog_ok = true;
setPoint_Home = TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),lat_B,lon_B); if (flight_mode != 11) {
setPoint_Home = TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),lat_B,lon_B);
}
else { setPoint_Home = cruise_cmd; }
errHome = getangle(gps.course.deg(), setPoint_Home); errHome = getangle(gps.course.deg(), setPoint_Home);
} }
else { cog_ok = false; last_errHome = errHome; } else { cog_ok = false; last_errHome = errHome; }
@ -458,38 +462,40 @@ void datacmpt() {
if (new_cog == true) { if (new_cog == true) {
raterror = getangle(last_errHome+180,errHome+180)*gps_freq; raterror = getangle(last_errHome+180,errHome+180)*gps_freq;
//raterror = rs.reading(raterror); ratecog = getangle(prev_cog_b,gps.course.deg())*gps_freq;
last_errHome = errHome;
prev_cog_b = gps.course.deg();
new_cog = false;
ratecog = getangle(prev_cog_b,gps.course.deg())*gps_freq; float PIDsum = (NKp*errHome)+(NKd*raterror);
//ratecog = rc.reading(ratecog);
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);
cmd_mult = 1; Input = -errHome;
cmdHome = PIDsum*cmd_mult ; Setpoint = 0;
myPID.Compute();
//cmdHome = PIDsum
cmdHome = Output;
if (vspeed<vdown-5) { spiral = true; } if (vspeed<vdown-5) { spiral = true; }
if (vspeed>vdown-3) { spiral = false; } if (vspeed>vdown-3) { spiral = false; }
if (spiral == true) { cmdHome = 0; } if (spiral == true) { cmdHome = 0; }
new_cog = false;
last_errHome = errHome;
prev_cog_b = gps.course.deg();
float cmdRate = cmdHome-ratecog; if ((TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),lat_B,lon_B))<10) {
myPID.SetOutputLimits(-120, 120);
}
steer_auto = map(cmdHome, -180, +180, 1000, 2000); if ((TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),lat_B,lon_B))>10) {
steer_auto = constrain(steer_auto, servo_auto_min, servo_auto_max); myPID.SetOutputLimits(-180, 180);
}
} }
} }
else { steer_auto = 1500; } else { cmdHome = 0; }
if (i_want_to_fly == true) { steer_auto = map(cmdHome, -180, +180, 1000, 2000);
steer_auto = constrain(steer_auto, 1000, 2000);
if (i_want_to_fly == true) {
flight_mode = 5; flight_mode = 5;
steer_auto = map(sim_cmd, -180, +180, 1000, 2000); steer_auto = map(sim_cmd, -180, +180, 1000, 2000);
} }
@ -497,24 +503,23 @@ void datacmpt() {
// -------------------- Vertical Speed -------------------- // // -------------------- Vertical Speed -------------------- //
if (new_baro == true) { if (new_baro == true) {
new_baro = false; new_baro = false;
if ((initialised == false) and (reboot_state != 1)) { baro_set = (baro_set + ((0-alt_baro)/100)); } if ((initialised == false) and (reboot_state != 1)) { baro_set = (baro_set + ((0-alt_baro)/100)); }
if (millis()<=5000) { alt_baro = 0; } if (millis()<=5000) { alt_baro = 0; }
float da = (alt_baro - prev_alt); float da = (alt_baro - prev_alt);
if (abs(da) < 50) { if (abs(da) < 50) {
float vps = (da/(dt/1000)); float vps = (da/(dt/1000));
vspeed = vs.reading(vps*100); vspeed = vs.reading(vps*100);
vspeed = (vspeed/100); vspeed = (vspeed/100);
} }
prev_alt = alt_baro; prev_alt = alt_baro;
}
}
// -------------------- Alt Fusion -------------------- // // -------------------- Alt Fusion -------------------- //
@ -600,11 +605,11 @@ void datacmpt() {
String lat_B_text = String(lat_B,5); String lat_B_text = String(lat_B,5);
String lon_B_text = String(lon_B,5); String lon_B_text = String(lon_B,5);
String next_cog_text = String(next_cog); String ratecog_text = String(ratecog);
String waypoint_text = String(waypoint_number); String waypoint_text = String(waypoint_number);
String waypoint_distance = String(TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),lat_B,lon_B)); String waypoint_distance = String(TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),lat_B,lon_B));
String nav_text = 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+","+waypoint_text+","+waypoint_distance; String nav_text = merged_alt_text+","+baro_weight_text+","+gps_weight_text+","+setPoint_Home_text+","+errHome_text+","+raterror_text+","+ratecog_text+","+cmd_mult_text+","+lat_B_text+","+lon_B_text+","+waypoint_text+","+waypoint_distance;
String a_text = String(channels[0]); String a_text = String(channels[0]);
String b_text = String(channels[1]); String b_text = String(channels[1]);
@ -800,6 +805,11 @@ void flight_state() {
motorised_failSafe(); motorised_failSafe();
setled(0, 255, 255, 25, 500); setled(0, 255, 255, 25, 500);
break; break;
case 11:
motorised_cruise();
setled(0, 255, 255, 25, 500);
break;
} }
} }
@ -969,8 +979,9 @@ void landed() {
void motorised_man() { void motorised_man() {
if (channels[6] > 1000) { flight_mode = 9; if (record_home == true) {newfile();} } if (channels[6] > 1000) { flight_mode = 9; if (record_home == true) {newfile();} myPID.SetMode(AUTOMATIC); }
if (failSafe == true) { flight_mode = 10; if (record_home == true) {newfile();} } 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();} myPID.SetMode(AUTOMATIC); }
} }
//------------------- 9 -------------------// //------------------- 9 -------------------//
@ -978,7 +989,6 @@ void motorised_man() {
void motorised_auto() { void motorised_auto() {
navigation(); navigation();
if (channels[6] < 1000) { flight_mode = 8; } if (channels[6] < 1000) { flight_mode = 8; }
} }
@ -986,8 +996,23 @@ void motorised_auto() {
void motorised_failSafe() { void motorised_failSafe() {
navigation(); navigation();
if (failSafe == false) { flight_mode = 8; } if (failSafe == false) { flight_mode = 8; myPID.SetMode(MANUAL); }
} }
//------------------- 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 ----------------------------------------------------- // // ----------------------------------------------------- Apply command ----------------------------------------------------- //
void applycmd() { void applycmd() {
@ -1030,13 +1055,16 @@ void applycmd() {
break; break;
case 9: case 9:
case 10: case 10:
case 11:
case 5: case 5:
servo_right = steer_auto; servo_right = steer_auto;
servo_left = 3000-steer_auto; servo_left = 3000-steer_auto;
break; break;
} }
// ---------- Stage 2 - Linear mode ---------- // // ---------- Stage 2 - Linear mode ---------- //
switch (linear_mode) { switch (linear_mode) {
@ -1045,20 +1073,27 @@ void applycmd() {
case 1: // control start at servo_start with an offset case 1: // control start at servo_start with an offset
if (servo_left>(1500+trig)) { if (servo_left>(1500+trig)) {
servo_left = map(servo_left, 1500, 2000, 1500+left_offset, 2000); //Serial.println("1");
servo_left = map(servo_left, 1500, 2000, 1500+left_offset, servo_max_m);
} }
else if (servo_left<(1500-trig)) { else if (servo_left<(1500-trig)) {
servo_left = map(servo_left, 1500, 1000, 1500-left_offset, 1000); //Serial.println("2");
servo_left = map(servo_left, 1000, 1500, 1000, 1500-left_offset);
} }
if (servo_right>(1500+trig)) { if (servo_right>(1500+trig)) {
servo_right = map(servo_right, 1500, 2000, 1500+right_offset, 2000); //Serial.println("3");
servo_right = map(servo_right, 1500, 2000, 1500+right_offset, servo_max_m);
} }
else if (servo_left<(1500-trig)) { else if (servo_right<(1500-trig)) {
servo_right = map(servo_right, 1500, 1000, 1500-right_offset, 1000); //Serial.println("4");
servo_right = map(servo_right, 1000, 1500, 1000, 1500-right_offset);
} }
break; break;
} }
//Serial.print(servo_left); Serial.print(", "); Serial.println(servo_right);
// ---------- Stage 3 - Control mode ---------- // // ---------- Stage 3 - Control mode ---------- //
switch (control_mode) { switch (control_mode) {
@ -1066,8 +1101,8 @@ void applycmd() {
break; break;
case 1: // neutral is hands up case 1: // neutral is hands up
servo_right = constrain(map(servo_right, 1500, 2000, 1000, 2000), 1000, 2000); servo_right = constrain(map(servo_right, 1500, servo_max_m, 1000, servo_max_m), 1000, servo_max_c);
servo_left = constrain(map(servo_left, 1500, 2000, 1000, 2000), 1000, 2000); servo_left = constrain(map(servo_left, 1500, servo_max_m, 1000, servo_max_m), 1000, servo_max_c);
break; break;
} }
@ -1298,7 +1333,7 @@ float getangle(float a, float b) {
if ((a-b) < 0) { angle = (-360) +(b-a);} if ((a-b) < 0) { angle = (-360) +(b-a);}
else { angle = (360) + (b-a);} else { angle = (360) + (b-a);}
} }
return angle; return angle;
} }
void setcam(int a) { void setcam(int a) {
@ -1414,7 +1449,7 @@ void newfile() {
dataFile = SD.open(namebuff, FILE_WRITE); dataFile = SD.open(namebuff, FILE_WRITE);
delay(10); delay(10);
if (dataFile) { 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), Loopmin (µS), Loopmax (µS), Loopmean (µ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), WaypointNumber (text), Distance (m), 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.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), Loopmin (µS), Loopmax (µS), Loopmean (µ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), Rate_Cog (deg), Cmd_mult (text), LatB (deg), LonB (deg), WaypointNumber (text), Distance (m), 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(); dataFile.close();
EEPROM.put(90, time_number); EEPROM.put(90, time_number);
} }