kopia lustrzana https://github.com/YohanHadji/R2Home
Adding new modes / features
YohanHadji
rodzic
b56a259026
commit
3ee23d0414
|
|
@ -1,14 +1,15 @@
|
|||
//---------- CONFIG ----------//
|
||||
|
||||
#define I_WANT_TO_FLY false // Simulated servo movement to test the servo movement :))
|
||||
#define I_WANT_TO_FLY false // Simulated servo movement to test the servo movement :))
|
||||
#define TEST_DIR_RC false // Use channels 0 on the radio to test the direction of the autopilot and the servos, I_WANT_TO_FLY should be set true too.
|
||||
#define BUZZER_TURN false // Buzzer sounds as function of the turn command
|
||||
#define BUZZER_SWEEP false // Buzzer turn on steroïds, should be easier to understand his tricky language ^^
|
||||
#define NO_INIT false // Skip init, for testing only purposes
|
||||
#define NAV_WAYPOINT true // Doing the waypoint sequence before reaching home?
|
||||
#define NAV_HOME false // Should it go home after the waypoint sequence?
|
||||
#define NAV_WAYPOINT false // Doing the waypoint sequence before reaching home?
|
||||
#define NAV_HOME true // Should it go home after the waypoint sequence?
|
||||
#define SD_WRITING true // Should it write on the SD card or not?
|
||||
#define LOW_RATE false // Dataloging at low HZ (if true) instead of 20Hz, for balloon flight
|
||||
#define CONFIG_FILE_SV false // Will also save the config file with the name of the file to debug if the config was wrong
|
||||
#define LED_MODEL 0 // Big led = 1, small led = 0.
|
||||
#define CAM_CYCLE false // If true, camera will only be powered on for 20s every 10min, used to save battery on long HAB flight
|
||||
#define SAFE_TRIGGER false // For HAB flight, will use a safer, but slower methode to detect apogee and transision to descent mode
|
||||
|
|
@ -28,6 +29,7 @@
|
|||
#define LINEAR_MODE 0 // command is linear (0), or linear but with a large deadband set using servo_start etc (1)
|
||||
#define DROP true // R2Home's version, drop or motorised
|
||||
|
||||
#define DEP_MODE 0 // If 0, the timer will be used if 1, the altitude will be used, to trigger deployment once in descent mode
|
||||
#define DESCENT_TIMER 15000
|
||||
#define OPENING_TIMER 6000
|
||||
#define SPIRAL_RECOVER 5000
|
||||
|
|
@ -35,6 +37,7 @@
|
|||
#define VUP 5 // m/s
|
||||
#define VDOWN -6 // m/s
|
||||
|
||||
// PID for navigation, better to not touch them and touch the servo max command settings instead
|
||||
#define NKP 1
|
||||
#define NKI 0.05
|
||||
#define NKD 0.1
|
||||
|
|
@ -43,8 +46,15 @@
|
|||
#define BLOW 3.5
|
||||
#define NO_BATT 4.0
|
||||
|
||||
#define SERVO_MAX_M 2000 // m for map
|
||||
#define SERVO_MAX_C 2000 // c for constrain
|
||||
#define AUTO_GAIN_WEIGHT true // Servo max command will be set automatically at initialization based on given payload weight
|
||||
#define AUTO_GAIN_PRESSURE true // Servo max command will be set automatically during the flight based on the ratio between ground pressure and current pressure
|
||||
|
||||
#define SYSTEM_WEIGHT 500 // System weight in gram
|
||||
#define PAYLOAD_WEIGHT 500 // Payload weight in gram
|
||||
|
||||
// If you choose to not use any of the two auto_gain functions, you can set the max commands with these two lines:
|
||||
#define SERVO_MAX_M_DEF 1600 // m for map
|
||||
#define SERVO_MAX_C_DEF 1375 // c for constrain
|
||||
|
||||
#define TRIG 20
|
||||
#define LEFT_OFFSET 100
|
||||
|
|
@ -61,6 +71,6 @@
|
|||
#define GPS_VS_AVG 1
|
||||
|
||||
#define GPS_SAFE_AVG 50
|
||||
#define BARO_SAFE_AVG 3
|
||||
#define BARO_SAFE_AVG 10
|
||||
|
||||
#define PRE_PE_AVG 50
|
||||
|
|
|
|||
|
|
@ -1,6 +1,12 @@
|
|||
#include <PID_v1.h>
|
||||
#include "config.h"
|
||||
|
||||
int SERVO_MAX_M_W = 0;
|
||||
int SERVO_MAX_C_W = 0;
|
||||
|
||||
int SERVO_MAX_M = 0;
|
||||
int SERVO_MAX_C = 0;
|
||||
|
||||
double Setpoint, Input, Output;
|
||||
PID myPID(&Input, &Output, &Setpoint,NKP, NKI, NKD, DIRECT);
|
||||
|
||||
|
|
@ -16,3 +22,35 @@ float cmpt_cmd(float err) {
|
|||
myPID.Compute();
|
||||
return map(Output, -180, 180, 1000, 2000);
|
||||
}
|
||||
|
||||
void cmpt_weight_gain() {
|
||||
if (AUTO_GAIN_WEIGHT) {
|
||||
int total_weight = SYSTEM_WEIGHT+PAYLOAD_WEIGHT;
|
||||
total_weight = constrain(total_weight, 500, 1500);
|
||||
SERVO_MAX_M_W = map(total_weight, 500, 1500, 2000, 1500);
|
||||
SERVO_MAX_C_W = map(total_weight, 500, 1500, 1500, 1250);
|
||||
}
|
||||
else {
|
||||
SERVO_MAX_M_W = SERVO_MAX_M_DEF;
|
||||
SERVO_MAX_C_W = SERVO_MAX_C_DEF;
|
||||
}
|
||||
}
|
||||
|
||||
void cmpt_pressure_gain(float pressure_ratio) {
|
||||
if (AUTO_GAIN_PRESSURE) {
|
||||
SERVO_MAX_M = map((SERVO_MAX_M_W-1000)/pressure_ratio, 0, 1000, 1000, 2000);
|
||||
SERVO_MAX_C = map((SERVO_MAX_M_W-1000)/pressure_ratio, 0, 1000, 1000, 1500);
|
||||
}
|
||||
else {
|
||||
SERVO_MAX_M = SERVO_MAX_M_W;
|
||||
SERVO_MAX_C = SERVO_MAX_C_W;
|
||||
}
|
||||
}
|
||||
|
||||
String servo_max_cmd_text() {
|
||||
String max_m_w_text = String(SERVO_MAX_M_W);
|
||||
String max_c_w_text = String(SERVO_MAX_C_W);
|
||||
String max_m_text = String(SERVO_MAX_M);
|
||||
String max_c_text = String(SERVO_MAX_C);
|
||||
return max_m_w_text+","+max_c_w_text+","+max_m_text+","+max_c_text ;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -8,12 +8,14 @@ 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 configFile;
|
||||
File configSaveFile;
|
||||
const int chipSelect = BUILTIN_SDCARD;
|
||||
char namebuff[20];
|
||||
unsigned int addr = 0;
|
||||
|
|
@ -89,13 +91,14 @@ void cmpt_string_data(int flight_mode, bool initialised, bool deployed, bool win
|
|||
|
||||
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();
|
||||
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 (!SD.begin(chipSelect)) { sd_ok = false; }
|
||||
else {
|
||||
|
|
@ -104,9 +107,18 @@ 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), 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.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), 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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -115,8 +115,8 @@ 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))) {
|
||||
|
||||
if ((DEP_MODE and (millis()-init_time>DESCENT_TIMER)) or ((!DEP_MODE and merged_alt < dep_altitude))) {
|
||||
flight_mode = 4;
|
||||
EasyBuzzer.beep(3000,100,50,5,500,1);
|
||||
deployed = true;
|
||||
|
|
|
|||
|
|
@ -109,10 +109,15 @@ bool is_descent(int v_trigger, bool mode) {
|
|||
}
|
||||
}
|
||||
|
||||
float v_down(int vdown) {
|
||||
return sqrt((baro_set*100.0)/pressure_baro)*vdown;
|
||||
float pressure_sqrt_ratio() {
|
||||
return sqrt((baro_set*100.0)/pressure_baro);
|
||||
}
|
||||
|
||||
float v_down(int vdown) {
|
||||
return pressure_sqrt_ratio()*vdown;
|
||||
}
|
||||
|
||||
|
||||
String pos_text() {
|
||||
String merged_alt_text = String(merged_alt,3);
|
||||
String gps_weight_text = String(gps_alt_weight,2);
|
||||
|
|
|
|||
Ładowanie…
Reference in New Issue