kopia lustrzana https://github.com/YohanHadji/R2Home
Update code
rodzic
d373085564
commit
d76d9fddf1
|
@ -5,7 +5,7 @@
|
||||||
|
|
||||||
Watchdog watchdog;
|
Watchdog watchdog;
|
||||||
|
|
||||||
Adafruit_BMP280 bmp(&Wire);
|
Adafruit_BMP280 bmp(&BARO_PORT);
|
||||||
int baro_adress = 0x00;
|
int baro_adress = 0x00;
|
||||||
double baro_set = 1000.0;
|
double baro_set = 1000.0;
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
}
|
|
@ -6,22 +6,24 @@
|
||||||
#define BUZZER_TURN false // Buzzer sounds as function of the turn command
|
#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 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 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 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 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 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 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 LED_MODEL 1 // 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 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 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 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 true // Show telemetry on serial monitor
|
||||||
#define DEBUG false // Will show all the steps on the screen
|
#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 GPS_PORT Serial7
|
||||||
#define TLM_PORT Serial5
|
#define TLM_PORT Serial5
|
||||||
#define RX_PORT Serial1
|
#define RX_PORT Serial1
|
||||||
|
#define BARO_PORT Wire
|
||||||
#define TIME_OUT 300
|
#define TIME_OUT 300
|
||||||
|
|
||||||
//---------- FLIGHT SETTINGS ----------//
|
//---------- FLIGHT SETTINGS ----------//
|
||||||
|
@ -29,28 +31,32 @@
|
||||||
#define RC_MODE 1 // only roll (0), pitch and roll mixed (1), pitch and roll separated (2)
|
#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 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 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 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 DESCENT_TIMER 500
|
||||||
#define OPENING_TIMER 3000
|
#define OPENING_TIMER 5000
|
||||||
#define SPIRAL_RECOVER 5000
|
#define SPIRAL_RECOVER 1000
|
||||||
#define DEP_ALT 300 // m above ground altitude
|
#define DEP_ALT 1000 // m above ground altitude
|
||||||
#define SEP_ALT 10 // m above sea level
|
#define ALT_MODE 1 // If 1, sep altitude is above ground, if 0, sep altitude is above sea level
|
||||||
#define VUP 1 // m/s
|
#define SEP_ALT 100 // m above sea level
|
||||||
#define VDOWN -1 // m/s
|
#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
|
// PID for navigation
|
||||||
#define NKD 0.1
|
#define NKP 3
|
||||||
|
#define NKI 0.2
|
||||||
|
#define NKD 1
|
||||||
|
|
||||||
#define BCRITICAL 3.4
|
#define BCRITICAL 3.4
|
||||||
#define BLOW 3.5
|
#define BLOW 3.5
|
||||||
#define NO_BATT 4.0
|
#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_WEIGHT false // 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_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 SYSTEM_WEIGHT 500 // System weight in gram
|
||||||
#define PAYLOAD_WEIGHT 500 // Payload weight in gram
|
#define PAYLOAD_WEIGHT 500 // Payload weight in gram
|
||||||
|
@ -73,7 +79,7 @@
|
||||||
|
|
||||||
#define GPS_VS_AVG 1
|
#define GPS_VS_AVG 1
|
||||||
|
|
||||||
#define GPS_SAFE_AVG 10
|
#define GPS_SAFE_AVG 5
|
||||||
#define BARO_SAFE_AVG 5
|
#define BARO_SAFE_AVG 1
|
||||||
|
|
||||||
#define PRE_PE_AVG 50
|
#define PRE_PE_AVG 50
|
||||||
|
|
|
@ -1,5 +1,6 @@
|
||||||
#include <PID_v1.h>
|
#include <PID_v1.h>
|
||||||
#include "config.h"
|
#include "config.h"
|
||||||
|
#include "cmd_cmpt.hpp"
|
||||||
|
|
||||||
int SERVO_MAX_M_W = 0;
|
int SERVO_MAX_M_W = 0;
|
||||||
int SERVO_MAX_C_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_M = 0;
|
||||||
int SERVO_MAX_C = 0;
|
int SERVO_MAX_C = 0;
|
||||||
|
|
||||||
double Setpoint, Input, Output;
|
|
||||||
PID myPID(&Input, &Output, &Setpoint,NKP, NKI, NKD, DIRECT);
|
|
||||||
|
|
||||||
unsigned long time_gain = 0;
|
unsigned long time_gain = 0;
|
||||||
|
|
||||||
void navigation_setup() {
|
void navigation_setup() {
|
||||||
|
@ -19,13 +17,6 @@ void navigation_setup() {
|
||||||
if (DEBUG) { Serial.println("PIDs set correctly"); }
|
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() {
|
void cmpt_weight_gain() {
|
||||||
if (AUTO_GAIN_WEIGHT) {
|
if (AUTO_GAIN_WEIGHT) {
|
||||||
int total_weight = SYSTEM_WEIGHT+PAYLOAD_WEIGHT;
|
int total_weight = SYSTEM_WEIGHT+PAYLOAD_WEIGHT;
|
||||||
|
|
|
@ -1,7 +1,6 @@
|
||||||
#include <SD.h>
|
#include <SD.h>
|
||||||
#include "feedback.hpp"
|
#include "feedback.hpp"
|
||||||
|
|
||||||
|
|
||||||
bool sd_ok = false;
|
bool sd_ok = false;
|
||||||
unsigned int delaySD = 100; // Datalog
|
unsigned int delaySD = 100; // Datalog
|
||||||
unsigned int delayTLM = 1000; // Tlm
|
unsigned int delayTLM = 1000; // Tlm
|
||||||
|
|
|
@ -163,6 +163,9 @@ void buzzer_batt() {
|
||||||
void update_buzzer() {
|
void update_buzzer() {
|
||||||
sweep_beep_update();
|
sweep_beep_update();
|
||||||
EasyBuzzer.update();
|
EasyBuzzer.update();
|
||||||
|
if (arming_error) {
|
||||||
|
EasyBuzzer.singleBeep(3000,200);
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void led_setup() {
|
void led_setup() {
|
||||||
|
|
|
@ -14,6 +14,10 @@ bool separation = false;
|
||||||
unsigned long spiral_time = 0;
|
unsigned long spiral_time = 0;
|
||||||
unsigned long init_time = 0;
|
unsigned long init_time = 0;
|
||||||
|
|
||||||
|
float setPoint_waypoint = 0;
|
||||||
|
float error_waypoint = 0;
|
||||||
|
float cmd_to_waypoint = 0;
|
||||||
|
|
||||||
|
|
||||||
//------------------- 0 -------------------//
|
//------------------- 0 -------------------//
|
||||||
|
|
||||||
|
@ -54,13 +58,22 @@ void flight_init() {
|
||||||
ground_altitude = constrain(ground_altitude, 0, 2000);
|
ground_altitude = constrain(ground_altitude, 0, 2000);
|
||||||
|
|
||||||
dep_altitude = (DEP_ALT+ground_altitude);
|
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);
|
setcam(1, 60, 60);
|
||||||
newfile();
|
newfile();
|
||||||
|
|
||||||
if (DROP == true) { flight_mode = 1;}
|
if (DROP == true) { flight_mode = 1;}
|
||||||
else { flight_mode = 8; }
|
else {
|
||||||
|
myPID.SetMode(MANUAL);
|
||||||
|
flight_mode = 7;
|
||||||
|
}
|
||||||
|
|
||||||
init_time = millis();
|
init_time = millis();
|
||||||
initialised = true;
|
initialised = true;
|
||||||
|
@ -105,15 +118,19 @@ void ready_steady() {
|
||||||
//------------------- 2 -------------------//
|
//------------------- 2 -------------------//
|
||||||
|
|
||||||
void flight_ascent() {
|
void flight_ascent() {
|
||||||
|
|
||||||
if (is_descent(0, 0)) {
|
if (is_descent(0, 0)) {
|
||||||
flight_mode = 1;
|
flight_mode = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
if ((gps.altitude.meters()-ground_altitude)>10) {
|
if ((gps.altitude.meters()-ground_altitude)>10) {
|
||||||
flight_started = true;
|
flight_started = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
if (gps.altitude.meters()>sep_altitude and gps_ok) {
|
if (gps.altitude.meters()>sep_altitude and gps_ok) {
|
||||||
separation = true;
|
separation = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------- 3 -------------------//
|
//------------------- 3 -------------------//
|
||||||
|
@ -163,12 +180,14 @@ void flight_gliding_auto() {
|
||||||
}
|
}
|
||||||
|
|
||||||
if (is_descent(v_down(-5), 1)) {
|
if (is_descent(v_down(-5), 1)) {
|
||||||
|
myPID.SetMode(MANUAL);
|
||||||
spiral = true;
|
spiral = true;
|
||||||
spiral_time = millis();
|
spiral_time = millis();
|
||||||
}
|
}
|
||||||
|
|
||||||
if (millis()-spiral_time>SPIRAL_RECOVER) {
|
if (millis()-spiral_time>SPIRAL_RECOVER) {
|
||||||
spiral = false;
|
spiral = false;
|
||||||
|
myPID.SetMode(AUTOMATIC);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (failsafe == false) {
|
if (failsafe == false) {
|
||||||
|
@ -179,8 +198,15 @@ void flight_gliding_auto() {
|
||||||
if (I_WANT_TO_FLY == true) {
|
if (I_WANT_TO_FLY == true) {
|
||||||
flight_mode = 5;
|
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 -------------------//
|
//------------------- 6 -------------------//
|
||||||
|
@ -194,7 +220,10 @@ void flight_gliding_manual() {
|
||||||
|
|
||||||
//------------------- 7 -------------------//
|
//------------------- 7 -------------------//
|
||||||
|
|
||||||
void landed() {
|
void on_ground() {
|
||||||
|
if (gps.speed.mps()>=LAUNCH_SPEED) {
|
||||||
|
flight_mode = 8;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//------------------- 8 -------------------//
|
//------------------- 8 -------------------//
|
||||||
|
@ -216,7 +245,14 @@ void motorised_man() {
|
||||||
|
|
||||||
void motorised_auto() {
|
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) {
|
if (channels[6] < 1000) {
|
||||||
flight_mode = 8;
|
flight_mode = 8;
|
||||||
}
|
}
|
||||||
|
@ -226,12 +262,38 @@ void motorised_auto() {
|
||||||
|
|
||||||
void motorised_failSafe() {
|
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) {
|
if (failsafe == false) {
|
||||||
flight_mode = 8;
|
flight_mode = 8;
|
||||||
myPID.SetMode(MANUAL);
|
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 -------------------//
|
//------------------- 11 -------------------//
|
||||||
|
@ -290,7 +352,7 @@ void cmpt_flight_state() {
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 7:
|
case 7:
|
||||||
landed();
|
on_ground();
|
||||||
setled(128, 0, 255, 25, 1000);
|
setled(128, 0, 255, 25, 1000);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
|
|
@ -54,8 +54,6 @@ void get_gps() {
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void sendPacket(byte *packet, byte len){
|
void sendPacket(byte *packet, byte len){
|
||||||
for (byte i = 0; i < len; i++) { GPS_PORT.write(packet[i]); }
|
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;
|
return date_year+":"+date_month+":"+date_day+","+time_hour+":"+time_minute+":"+time_second;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
String gps_text() {
|
String gps_text() {
|
||||||
String lat_A_text = String(gps.location.lat(), 10);
|
String lat_A_text = String(gps.location.lat(), 10);
|
||||||
String lon_A_text = String(gps.location.lng(), 10);
|
String lon_A_text = String(gps.location.lng(), 10);
|
||||||
|
|
|
@ -7,9 +7,6 @@
|
||||||
|
|
||||||
servo_cmd steering;
|
servo_cmd steering;
|
||||||
|
|
||||||
float setPoint_waypoint = 0;
|
|
||||||
float error_waypoint = 0;
|
|
||||||
float cmd_to_waypoint = 0;
|
|
||||||
|
|
||||||
void setup() {
|
void setup() {
|
||||||
|
|
||||||
|
@ -68,7 +65,7 @@ void loop() {
|
||||||
|
|
||||||
void getdata() {
|
void getdata() {
|
||||||
get_gps();
|
get_gps();
|
||||||
get_baro(0);
|
get_baro(0);
|
||||||
get_rc();
|
get_rc();
|
||||||
get_vbatt();
|
get_vbatt();
|
||||||
|
|
||||||
|
@ -76,15 +73,7 @@ void getdata() {
|
||||||
|
|
||||||
void datacmpt() {
|
void datacmpt() {
|
||||||
|
|
||||||
cmpt_pressure_gain(pressure_sqrt_ratio());
|
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (I_WANT_TO_FLY) {
|
if (I_WANT_TO_FLY) {
|
||||||
cmd_to_waypoint = sim();
|
cmd_to_waypoint = sim();
|
||||||
|
|
|
@ -1,17 +1,8 @@
|
||||||
#include "config.h"
|
#include "config.h"
|
||||||
#include "position.hpp"
|
#include "position.hpp"
|
||||||
#include "servo.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;
|
int last_waypoint_number = 0;
|
||||||
|
|
||||||
float getangle(float a, float b) {
|
float getangle(float a, float b) {
|
||||||
|
@ -24,36 +15,17 @@ float getangle(float a, float b) {
|
||||||
return angle;
|
return angle;
|
||||||
}
|
}
|
||||||
|
|
||||||
float cmpt_setpoint(gps_location waypoint) {
|
float cmpt_setpoint(float gps_latitude, float gps_longitude, gps_location waypoint) {
|
||||||
return TinyGPSPlus::courseTo(gps.location.lat(),gps.location.lng(),waypoint.latitude,waypoint.longitude);
|
return TinyGPSPlus::courseTo(gps_latitude, gps_longitude, waypoint.latitude, waypoint.longitude);
|
||||||
}
|
}
|
||||||
|
|
||||||
float cmpt_error(float cog, float setPoint) {
|
float cmpt_error(float cog, float setPoint) {
|
||||||
return getangle(cog, 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 control_text() {
|
||||||
String setPoint_Home_text = String(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(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 lat_B_text = String(current_waypoint.latitude,5);
|
||||||
String lon_B_text = String(current_waypoint.longitude,5);
|
String lon_B_text = String(current_waypoint.longitude,5);
|
||||||
String waypoint_text = String(waypoint_number);
|
String waypoint_text = String(waypoint_number);
|
||||||
|
|
|
@ -120,7 +120,6 @@ float v_down(int vdown) {
|
||||||
return pressure_sqrt_ratio()*vdown;
|
return pressure_sqrt_ratio()*vdown;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
String pos_text() {
|
String pos_text() {
|
||||||
String merged_alt_text = String(merged_alt,3);
|
String merged_alt_text = String(merged_alt,3);
|
||||||
String gps_weight_text = String(gps_alt_weight,2);
|
String gps_weight_text = String(gps_alt_weight,2);
|
||||||
|
|
|
@ -6,17 +6,29 @@ uint16_t channels[16];
|
||||||
bool failsafe;
|
bool failsafe;
|
||||||
bool lostFrame;
|
bool lostFrame;
|
||||||
|
|
||||||
|
unsigned long last_rc(0);
|
||||||
|
|
||||||
void rc_setup() {
|
void rc_setup() {
|
||||||
rx.begin();
|
rx.begin();
|
||||||
if (DEBUG) { Serial.println("RC was set correctly"); }
|
if (DEBUG) { Serial.println("RC was set correctly"); }
|
||||||
}
|
}
|
||||||
|
|
||||||
void get_rc() {
|
void get_rc() {
|
||||||
rx.read(&channels[0], &failsafe, &lostFrame);
|
|
||||||
|
rx.read(&channels[0], &failsafe, &lostFrame);
|
||||||
if ((channels[3])>=1500 or (channels[3])==0) { failsafe = true; }
|
if (channels[3]==0) { failsafe = true; }
|
||||||
else { failsafe = false; }
|
//else { failsafe = false; }
|
||||||
if (DEBUG) { Serial.println("Just got RC (even if no rx is connected)"); }
|
|
||||||
|
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() {
|
String rc_text() {
|
||||||
|
|
|
@ -1,9 +1,10 @@
|
||||||
#include "config.h"
|
#include "config.h"
|
||||||
#include "control.hpp"
|
#include "control.hpp"
|
||||||
#include <PWMServo.h>
|
#include <PWMServo.h>
|
||||||
|
#include <movingAvg.h>
|
||||||
|
|
||||||
PWMServo steer;
|
PWMServo steer;
|
||||||
PWMServo deploy;
|
PWMServo aux;
|
||||||
PWMServo aux_deploy;
|
PWMServo aux_deploy;
|
||||||
PWMServo left;
|
PWMServo left;
|
||||||
PWMServo right;
|
PWMServo right;
|
||||||
|
@ -14,6 +15,9 @@ int servo_right = 0;
|
||||||
int servo_aux = 0;
|
int servo_aux = 0;
|
||||||
int servo_aux_deploy = 0;
|
int servo_aux_deploy = 0;
|
||||||
|
|
||||||
|
bool armed = false;
|
||||||
|
bool arming_error = false;
|
||||||
|
|
||||||
unsigned long tpwm = 0;
|
unsigned long tpwm = 0;
|
||||||
|
|
||||||
struct servo_cmd {
|
struct servo_cmd {
|
||||||
|
@ -26,17 +30,17 @@ struct servo_cmd {
|
||||||
void servo_setup() {
|
void servo_setup() {
|
||||||
left.attach(6, 1000, 2000);
|
left.attach(6, 1000, 2000);
|
||||||
right.attach(7, 1000, 2000);
|
right.attach(7, 1000, 2000);
|
||||||
if (DROP == true) { deploy.attach(8, 1000, 2000); }
|
aux.attach(8, 1000, 2000);
|
||||||
else { esc.attach(8, 1000, 2000); }
|
|
||||||
aux_deploy.attach(9, 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) {
|
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 roll = map(channels[0], 172, 1811, 1000, 2000);
|
||||||
float pitch = map(channels[1], 67, 1982, 1000, 2000);
|
float pitch = map(channels[1], 172, 1811, 1000, 2000);
|
||||||
float aux = map(channels[2], 67, 1982, 1000, 2000);
|
float aux = map(channels[2], 365, 1681, 1000, 2000);
|
||||||
float sw = map(channels[6], 67, 1982, 1000, 2000);
|
float sw = map(channels[6], 172, 1811, 1000, 2000);
|
||||||
|
|
||||||
roll = constrain(roll, 1000, 2000);
|
roll = constrain(roll, 1000, 2000);
|
||||||
pitch = constrain(pitch, 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 0:
|
||||||
case 1:
|
case 1:
|
||||||
case 6:
|
case 6:
|
||||||
|
case 7:
|
||||||
case 8:
|
case 8:
|
||||||
// ---------- Stage 1 - RC mode ---------- //
|
// ---------- Stage 1 - RC mode ---------- //
|
||||||
switch (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.right = 1500;
|
||||||
steering_cmpt.left = 1500;
|
steering_cmpt.left = 1500;
|
||||||
break;
|
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 1:
|
||||||
case 2:
|
case 2:
|
||||||
case 3:
|
case 3:
|
||||||
case 4:
|
|
||||||
case 5:
|
case 5:
|
||||||
case 6:
|
case 4:
|
||||||
case 7:
|
case 6:
|
||||||
// Deployment Servo
|
// Deployment Servo
|
||||||
if (deployed == true) {
|
if (deployed == true) {
|
||||||
steering_cmpt.aux = 1000;
|
steering_cmpt.aux = 1000;
|
||||||
|
@ -160,8 +169,15 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
|
||||||
else {
|
else {
|
||||||
steering_cmpt.aux_deploy = 2000;
|
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;
|
break;
|
||||||
|
|
||||||
|
case 7:
|
||||||
case 8:
|
case 8:
|
||||||
steering_cmpt.aux = aux;
|
steering_cmpt.aux = aux;
|
||||||
break;
|
break;
|
||||||
|
@ -172,10 +188,40 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case 11:
|
case 11:
|
||||||
steering_cmpt.aux = 1000;
|
steering_cmpt.aux = 2000;
|
||||||
steering_cmpt.aux_deploy = 2000;
|
steering_cmpt.aux_deploy = 2000;
|
||||||
break;
|
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_left = steering_cmpt.left;
|
||||||
servo_right = steering_cmpt.right;
|
servo_right = steering_cmpt.right;
|
||||||
|
@ -191,7 +237,7 @@ void update_servo_cmd(servo_cmd steering_apply, unsigned int a) {
|
||||||
tpwm = millis();
|
tpwm = millis();
|
||||||
left.write(map(steering_apply.left, 1000, 2000, 0, 180));
|
left.write(map(steering_apply.left, 1000, 2000, 0, 180));
|
||||||
right.write(map(steering_apply.right, 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));
|
aux_deploy.write(map(steering_apply.aux_deploy, 1000, 2000, 0, 180));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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;
|
||||||
|
|
||||||
|
}
|
|
@ -24,7 +24,7 @@ unsigned long tread = 0;
|
||||||
unsigned long tchange = 0;
|
unsigned long tchange = 0;
|
||||||
unsigned long tparallax = 0;
|
unsigned long tparallax = 0;
|
||||||
|
|
||||||
float Kp = 0.75;
|
float Kp = 0.6;
|
||||||
float Kd = 0;
|
float Kd = 0;
|
||||||
float lasterror = 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);
|
Serial.print(parallax_steer_b); Serial.print(","); Serial.println(feedback_value);
|
||||||
|
|
||||||
|
|
Ładowanie…
Reference in New Issue