main
YohanHadji 2022-06-04 00:02:19 +02:00
rodzic d373085564
commit d76d9fddf1
15 zmienionych plików z 239 dodań i 109 usunięć

Wyświetl plik

@ -5,7 +5,7 @@
Watchdog watchdog;
Adafruit_BMP280 bmp(&Wire);
Adafruit_BMP280 bmp(&BARO_PORT);
int baro_adress = 0x00;
double baro_set = 1000.0;

Wyświetl plik

@ -0,0 +1,15 @@
// This file is used as an "add on", the only function defined here should be "gps_location where to go(float gps_latitude, float gps_longitude)"
// This function takes in parameter the current position, and should return a waypoint goal, which of course should be choosen based on the current position.
#include <PID_v1.h>
#include "config.h"
double Setpoint, Input, Output;
PID myPID(&Input, &Output, &Setpoint,NKP, NKI, NKD, DIRECT);
float cmpt_cmd(float err) {
Input = -err;
Setpoint = 0;
myPID.Compute();
return map(Output, -180, 180, 1000, 2000);
}

Wyświetl plik

@ -6,22 +6,24 @@
#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_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 true // If true, camera will only be powered on for 20s every 10min, used to save battery on long HAB flight
#define LED_MODEL 1 // 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
#define COG_BRAKE false // Will reduce command if CoG is turning faster than a threshold
#define GPS_FREQ 5 // Hz
#define SERVO_RATE 50 // Hz
#define TLM_MONITOR true // Show telemetry on serial monitor
#define DEBUG false // Will show all the steps on the screen
#define X_SERVO false // Is an additional servo used for separation from balloon or D servo is used with two positions?
#define GPS_PORT Serial7
#define TLM_PORT Serial5
#define RX_PORT Serial1
#define BARO_PORT Wire
#define TIME_OUT 300
//---------- FLIGHT SETTINGS ----------//
@ -29,28 +31,32 @@
#define RC_MODE 1 // only roll (0), pitch and roll mixed (1), pitch and roll separated (2)
#define CONTROL_MODE 1 // neutral position at the center (0) or with "hands up" (1)
#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 DROP false // R2Home's version, drop or motorised
#define LAUNCH_SPEED 0 // m/s, in motorised version needed horizontal speed to go into controled mode
#define DEP_MODE 1 // If 0, the ALTITUDE will be used if 1, the TIMER will be used, to trigger deployment once in descent mode
#define DESCENT_TIMER 4000
#define OPENING_TIMER 3000
#define SPIRAL_RECOVER 5000
#define DEP_ALT 300 // m above ground altitude
#define SEP_ALT 10 // m above sea level
#define VUP 1 // m/s
#define VDOWN -1 // m/s
#define DESCENT_TIMER 500
#define OPENING_TIMER 5000
#define SPIRAL_RECOVER 1000
#define DEP_ALT 1000 // m above ground altitude
#define ALT_MODE 1 // If 1, sep altitude is above ground, if 0, sep altitude is above sea level
#define SEP_ALT 100 // m above sea level
#define VUP 3 // m/s
#define VDOWN -2 // 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
// PID for navigation
#define NKP 3
#define NKI 0.2
#define NKD 1
#define BCRITICAL 3.4
#define BLOW 3.5
#define NO_BATT 4.0
#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 AUTO_GAIN_WEIGHT false // Servo max command will be set automatically at initialization based on given payload weight
#define AUTO_GAIN_PRESSURE false // 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
@ -73,7 +79,7 @@
#define GPS_VS_AVG 1
#define GPS_SAFE_AVG 10
#define BARO_SAFE_AVG 5
#define GPS_SAFE_AVG 5
#define BARO_SAFE_AVG 1
#define PRE_PE_AVG 50

Wyświetl plik

@ -1,5 +1,6 @@
#include <PID_v1.h>
#include "config.h"
#include "cmd_cmpt.hpp"
int SERVO_MAX_M_W = 0;
int SERVO_MAX_C_W = 0;
@ -7,9 +8,6 @@ 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);
unsigned long time_gain = 0;
void navigation_setup() {
@ -19,13 +17,6 @@ void navigation_setup() {
if (DEBUG) { Serial.println("PIDs set correctly"); }
}
float cmpt_cmd(float err) {
Input = -err;
Setpoint = 0;
myPID.Compute();
return map(Output, -180, 180, 1000, 2000);
}
void cmpt_weight_gain() {
if (AUTO_GAIN_WEIGHT) {
int total_weight = SYSTEM_WEIGHT+PAYLOAD_WEIGHT;

Wyświetl plik

@ -1,7 +1,6 @@
#include <SD.h>
#include "feedback.hpp"
bool sd_ok = false;
unsigned int delaySD = 100; // Datalog
unsigned int delayTLM = 1000; // Tlm

Wyświetl plik

@ -163,6 +163,9 @@ void buzzer_batt() {
void update_buzzer() {
sweep_beep_update();
EasyBuzzer.update();
if (arming_error) {
EasyBuzzer.singleBeep(3000,200);
}
}
void led_setup() {

Wyświetl plik

@ -14,6 +14,10 @@ bool separation = false;
unsigned long spiral_time = 0;
unsigned long init_time = 0;
float setPoint_waypoint = 0;
float error_waypoint = 0;
float cmd_to_waypoint = 0;
//------------------- 0 -------------------//
@ -54,13 +58,22 @@ void flight_init() {
ground_altitude = constrain(ground_altitude, 0, 2000);
dep_altitude = (DEP_ALT+ground_altitude);
sep_altitude = SEP_ALT;
if (ALT_MODE) {
sep_altitude = (SEP_ALT+ground_altitude);
}
else {
sep_altitude = SEP_ALT;
}
setcam(1, 60, 60);
newfile();
if (DROP == true) { flight_mode = 1;}
else { flight_mode = 8; }
else {
myPID.SetMode(MANUAL);
flight_mode = 7;
}
init_time = millis();
initialised = true;
@ -105,15 +118,19 @@ void ready_steady() {
//------------------- 2 -------------------//
void flight_ascent() {
if (is_descent(0, 0)) {
flight_mode = 1;
}
}
if ((gps.altitude.meters()-ground_altitude)>10) {
flight_started = true;
}
if (gps.altitude.meters()>sep_altitude and gps_ok) {
separation = true;
}
}
//------------------- 3 -------------------//
@ -163,12 +180,14 @@ void flight_gliding_auto() {
}
if (is_descent(v_down(-5), 1)) {
myPID.SetMode(MANUAL);
spiral = true;
spiral_time = millis();
}
if (millis()-spiral_time>SPIRAL_RECOVER) {
spiral = false;
myPID.SetMode(AUTOMATIC);
}
if (failsafe == false) {
@ -179,8 +198,15 @@ void flight_gliding_auto() {
if (I_WANT_TO_FLY == true) {
flight_mode = 5;
}
if (new_cog) {
new_cog = false;
if (DEBUG) { Serial.println("New direction and command computed"); }
setPoint_waypoint = cmpt_setpoint(gps.location.lat(), gps.location.lng(), where_to_go(gps.location.lat(), gps.location.lng()));
error_waypoint = cmpt_error(gps.course.deg(), setPoint_waypoint);
cmd_to_waypoint = cmpt_cmd(error_waypoint);
}
navigation();
}
//------------------- 6 -------------------//
@ -194,7 +220,10 @@ void flight_gliding_manual() {
//------------------- 7 -------------------//
void landed() {
void on_ground() {
if (gps.speed.mps()>=LAUNCH_SPEED) {
flight_mode = 8;
}
}
//------------------- 8 -------------------//
@ -216,7 +245,14 @@ void motorised_man() {
void motorised_auto() {
navigation();
if (new_cog) {
new_cog = false;
if (DEBUG) { Serial.println("New direction and command computed"); }
setPoint_waypoint = cmpt_setpoint(gps.location.lat(), gps.location.lng(), where_to_go(gps.location.lat(), gps.location.lng()));
error_waypoint = cmpt_error(gps.course.deg(), setPoint_waypoint);
cmd_to_waypoint = cmpt_cmd(error_waypoint);
}
if (channels[6] < 1000) {
flight_mode = 8;
}
@ -226,12 +262,38 @@ void motorised_auto() {
void motorised_failSafe() {
navigation();
if (new_cog) {
new_cog = false;
if (DEBUG) { Serial.println("New direction and command computed"); }
setPoint_waypoint = cmpt_setpoint(gps.location.lat(), gps.location.lng(), where_to_go(gps.location.lat(), gps.location.lng()));
error_waypoint = cmpt_error(gps.course.deg(), setPoint_waypoint);
cmd_to_waypoint = cmpt_cmd(error_waypoint);
}
if (failsafe == false) {
flight_mode = 8;
myPID.SetMode(MANUAL);
}
/* DANGEROUS: WOULD SET MOTOR TO MAX!!!
if (!gps_ok) {
flight_mode = 11;
myPID.SetMode(MANUAL);
}
*/
if (is_descent(v_down(-5), 1)) {
myPID.SetMode(MANUAL);
spiral = true;
spiral_time = millis();
}
if (millis()-spiral_time>SPIRAL_RECOVER) {
spiral = false;
myPID.SetMode(AUTOMATIC);
}
}
//------------------- 11 -------------------//
@ -290,7 +352,7 @@ void cmpt_flight_state() {
break;
case 7:
landed();
on_ground();
setled(128, 0, 255, 25, 1000);
break;

Wyświetl plik

@ -54,8 +54,6 @@ void get_gps() {
}
}
void sendPacket(byte *packet, byte len){
for (byte i = 0; i < len; i++) { GPS_PORT.write(packet[i]); }
}
@ -161,7 +159,6 @@ String date_time() {
return date_year+":"+date_month+":"+date_day+","+time_hour+":"+time_minute+":"+time_second;
}
String gps_text() {
String lat_A_text = String(gps.location.lat(), 10);
String lon_A_text = String(gps.location.lng(), 10);

Wyświetl plik

@ -7,9 +7,6 @@
servo_cmd steering;
float setPoint_waypoint = 0;
float error_waypoint = 0;
float cmd_to_waypoint = 0;
void setup() {
@ -68,7 +65,7 @@ void loop() {
void getdata() {
get_gps();
get_baro(0);
get_baro(0);
get_rc();
get_vbatt();
@ -76,15 +73,7 @@ void getdata() {
void datacmpt() {
cmpt_pressure_gain(pressure_sqrt_ratio());
if (new_cog) {
new_cog = false;
if (DEBUG) { Serial.println("New direction and command computed"); }
setPoint_waypoint = cmpt_setpoint(current_waypoint);
error_waypoint = cmpt_error(gps.course.deg(), setPoint_waypoint);
cmd_to_waypoint = cmpt_cmd(error_waypoint);
}
cmpt_pressure_gain(pressure_sqrt_ratio());
if (I_WANT_TO_FLY) {
cmd_to_waypoint = sim();

Wyświetl plik

@ -1,17 +1,8 @@
#include "config.h"
#include "position.hpp"
#include "servo.hpp"
#include "where_to_go.hpp"
struct gps_location {
double latitude = 0;
double longitude = 0;
double radius = 0;
};
gps_location waypoint[17];
gps_location current_waypoint;
int waypoint_number = 0;
int last_waypoint_number = 0;
float getangle(float a, float b) {
@ -24,36 +15,17 @@ float getangle(float a, float b) {
return angle;
}
float cmpt_setpoint(gps_location waypoint) {
return TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),waypoint.latitude,waypoint.longitude);
float cmpt_setpoint(float gps_latitude, float gps_longitude, gps_location waypoint) {
return TinyGPSPlus::courseTo(gps_latitude, gps_longitude, waypoint.latitude, waypoint.longitude);
}
float cmpt_error(float cog, float setPoint) {
return getangle(cog, setPoint);
}
void navigation() {
if (NAV_WAYPOINT == true) {
float distance_to = TinyGPSPlus::distanceBetween(gps.location.lat(),gps.location.lng(),current_waypoint.latitude,current_waypoint.longitude);
if (distance_to < current_waypoint.radius) {
if (waypoint_number < 15) {
waypoint_number++;
}
if ((waypoint[waypoint_number].latitude !=0) and (waypoint[waypoint_number].longitude !=0)) {
current_waypoint = waypoint[waypoint_number];
}
}
}
}
String control_text() {
String setPoint_Home_text = String(cmpt_setpoint(current_waypoint),2);
String errHome_text = String(cmpt_error(gps.course.deg(), cmpt_setpoint(current_waypoint)),2);
String setPoint_Home_text = String(cmpt_setpoint(gps.location.lat(), gps.location.lng(), current_waypoint),2);
String errHome_text = String(cmpt_error(gps.course.deg(), cmpt_setpoint(gps.location.lat(), gps.location.lng(), current_waypoint)),2);
String lat_B_text = String(current_waypoint.latitude,5);
String lon_B_text = String(current_waypoint.longitude,5);
String waypoint_text = String(waypoint_number);

Wyświetl plik

@ -120,7 +120,6 @@ 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);

Wyświetl plik

@ -6,17 +6,29 @@ uint16_t channels[16];
bool failsafe;
bool lostFrame;
unsigned long last_rc(0);
void rc_setup() {
rx.begin();
if (DEBUG) { Serial.println("RC was set correctly"); }
}
void get_rc() {
rx.read(&channels[0], &failsafe, &lostFrame);
if ((channels[3])>=1500 or (channels[3])==0) { failsafe = true; }
else { failsafe = false; }
if (DEBUG) { Serial.println("Just got RC (even if no rx is connected)"); }
rx.read(&channels[0], &failsafe, &lostFrame);
if (channels[3]==0) { failsafe = true; }
//else { failsafe = false; }
if (DEBUG) { Serial.println("Just got RC (even if no rx is connected)"); }
/*
Serial.print(channels[0]); Serial.print(",");
Serial.print(channels[1]); Serial.print(",");
Serial.print(channels[2]); Serial.print(",");
Serial.print(channels[4]); Serial.print(",");
Serial.print(channels[5]); Serial.print(",");
Serial.println(channels[6]);
*/
}
String rc_text() {

Wyświetl plik

@ -1,9 +1,10 @@
#include "config.h"
#include "control.hpp"
#include <PWMServo.h>
#include <movingAvg.h>
PWMServo steer;
PWMServo deploy;
PWMServo aux;
PWMServo aux_deploy;
PWMServo left;
PWMServo right;
@ -14,6 +15,9 @@ int servo_right = 0;
int servo_aux = 0;
int servo_aux_deploy = 0;
bool armed = false;
bool arming_error = false;
unsigned long tpwm = 0;
struct servo_cmd {
@ -26,17 +30,17 @@ struct servo_cmd {
void servo_setup() {
left.attach(6, 1000, 2000);
right.attach(7, 1000, 2000);
if (DROP == true) { deploy.attach(8, 1000, 2000); }
else { esc.attach(8, 1000, 2000); }
aux.attach(8, 1000, 2000);
aux_deploy.attach(9, 1000, 2000);
}
servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool deployed, bool failsafe, bool cog_ok, bool spiral, bool separation) {
float roll = map(channels[0], 67, 1982, 1000, 2000);
float pitch = map(channels[1], 67, 1982, 1000, 2000);
float aux = map(channels[2], 67, 1982, 1000, 2000);
float sw = map(channels[6], 67, 1982, 1000, 2000);
float roll = map(channels[0], 172, 1811, 1000, 2000);
float pitch = map(channels[1], 172, 1811, 1000, 2000);
float aux = map(channels[2], 365, 1681, 1000, 2000);
float sw = map(channels[6], 172, 1811, 1000, 2000);
roll = constrain(roll, 1000, 2000);
pitch = constrain(pitch, 1000, 2000);
@ -49,6 +53,7 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
case 0:
case 1:
case 6:
case 7:
case 8:
// ---------- Stage 1 - RC mode ---------- //
switch (RC_MODE) {
@ -93,6 +98,11 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
steering_cmpt.right = 1500;
steering_cmpt.left = 1500;
break;
case 4:
steering_cmpt.right = 2000;
steering_cmpt.left = 2000;
break;
}
@ -138,10 +148,9 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
case 1:
case 2:
case 3:
case 4:
case 5:
case 6:
case 7:
case 4:
case 6:
// Deployment Servo
if (deployed == true) {
steering_cmpt.aux = 1000;
@ -160,8 +169,15 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
else {
steering_cmpt.aux_deploy = 2000;
}
if (!X_SERVO) {
steering_cmpt.aux_deploy = map(steering_cmpt.aux_deploy, 1000, 2000, 1500, 2000);
steering_cmpt.aux_deploy = constrain(steering_cmpt.aux_deploy, 1500, 2000);
steering_cmpt.aux = min(steering_cmpt.aux, steering_cmpt.aux_deploy);
}
break;
case 7:
case 8:
steering_cmpt.aux = aux;
break;
@ -172,10 +188,40 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
break;
case 11:
steering_cmpt.aux = 1000;
steering_cmpt.aux = 2000;
steering_cmpt.aux_deploy = 2000;
break;
}
if (!DROP) {
switch (armed) {
case true:
if (channels[4] < 1000) {
armed = false;
arming_error = false;
}
break;
case false:
if (channels[4] > 1000 and aux >1001) {
arming_error = true;
}
else if (channels[4] > 1000 and aux <=1001 and !arming_error and flight_mode !=0) {
armed = true;
arming_error = false;
}
else if (channels[4] > 1000 and flight_mode ==0) {
arming_error = true;
}
if (channels[4] < 1000) {
arming_error = false;
}
}
if (!armed and !DROP) {
steering_cmpt.aux = 1000;
}
}
servo_left = steering_cmpt.left;
servo_right = steering_cmpt.right;
@ -191,7 +237,7 @@ void update_servo_cmd(servo_cmd steering_apply, unsigned int a) {
tpwm = millis();
left.write(map(steering_apply.left, 1000, 2000, 0, 180));
right.write(map(steering_apply.right, 1000, 2000, 0, 180));
deploy.write(map(steering_apply.aux, 1000, 2000, 0, 180));
aux.write(map(steering_apply.aux, 1000, 2000, 0, 180));
aux_deploy.write(map(steering_apply.aux_deploy, 1000, 2000, 0, 180));
}

Wyświetl plik

@ -0,0 +1,39 @@
// This file is used as an "add on", the only function defined here should be "gps_location where to go(float gps_latitude, float gps_longitude)"
// This function takes in parameter the current position, and should return a waypoint goal, which of course should be choosen based on the current position.
#include "config.h"
#include <TinyGPS++.h>
struct gps_location {
double latitude = 0;
double longitude = 0;
double radius = 0;
};
gps_location waypoint[17];
gps_location current_waypoint;
int waypoint_number = 0;
gps_location where_to_go(float gps_latitude, float gps_longitude) {
if (NAV_WAYPOINT == true) {
float distance_to = TinyGPSPlus::distanceBetween(gps_latitude, gps_longitude, current_waypoint.latitude, current_waypoint.longitude);
if (waypoint_number < 15) {
if (distance_to < current_waypoint.radius) {
if ((waypoint[waypoint_number+1].latitude !=0) and (waypoint[waypoint_number+1].longitude !=0)) {
waypoint_number++;
current_waypoint = waypoint[waypoint_number];
}
}
}
}
return current_waypoint;
}

Wyświetl plik

@ -24,7 +24,7 @@ unsigned long tread = 0;
unsigned long tchange = 0;
unsigned long tparallax = 0;
float Kp = 0.75;
float Kp = 0.6;
float Kd = 0;
float lasterror = 0;
@ -103,7 +103,7 @@ void updatecmd(int a) {
// ------------------------------------------------------------------------------------ //
parallax_steer_b = map(parallax_steer_a, 1000, 2000, 1100, 1900); // this is the ONLY line to change to reduce the range, for example, "[...] 1450, 1550)"
parallax_steer_b = map(parallax_steer_a, 1000, 2000, 1150, 1900); // this is the ONLY line to change to reduce the range, for example, "[...] 1450, 1550)"
Serial.print(parallax_steer_b); Serial.print(","); Serial.println(feedback_value);