kopia lustrzana https://github.com/YohanHadji/R2Home
380 wiersze
8.0 KiB
C++
380 wiersze
8.0 KiB
C++
#include "config.h"
|
|
#include "data.hpp"
|
|
|
|
int flight_mode = 0;
|
|
int dep_altitude = 0;
|
|
int sep_altitude = 0;
|
|
int prev_flight_mode = 0;
|
|
bool initialised = false;
|
|
bool flight_started = false;
|
|
bool deployed = false;
|
|
bool wing_opened = false;
|
|
bool spiral = false;
|
|
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 -------------------//
|
|
|
|
void flight_init() {
|
|
|
|
if ((gps.satellites.value()>=6 and gps_ok and gps.hdop.value()<150 and millis()>5000) or NO_INIT) {
|
|
|
|
EasyBuzzer.beep(3000,100,50,10,500,1);
|
|
|
|
if ((NAV_WAYPOINT == true) and (NAV_HOME == true)) {
|
|
waypoint[last_waypoint_number].latitude = gps.location.lat();
|
|
waypoint[last_waypoint_number].longitude = gps.location.lng();
|
|
waypoint[last_waypoint_number].radius = HOME_WAYPOINT_THRESHOLD;
|
|
current_waypoint = waypoint[waypoint_number];
|
|
}
|
|
|
|
else if ((NAV_WAYPOINT == false) and (NAV_HOME == true)) {
|
|
current_waypoint.latitude = gps.location.lat();
|
|
current_waypoint.longitude = gps.location.lng();
|
|
current_waypoint.radius = HOME_WAYPOINT_THRESHOLD;
|
|
}
|
|
|
|
else if ((NAV_WAYPOINT == true) and (NAV_HOME == false)) {
|
|
current_waypoint = waypoint[waypoint_number];
|
|
}
|
|
|
|
b_vs.reset();
|
|
b_al.reset();
|
|
g_vs.reset();
|
|
|
|
baroset(gps.altitude.meters(), 1);
|
|
get_baro(1);
|
|
merged_alt = gps.altitude.meters();
|
|
|
|
cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral);
|
|
|
|
ground_altitude = gps.altitude.meters();
|
|
ground_altitude = constrain(ground_altitude, 0, 2000);
|
|
|
|
dep_altitude = (DEP_ALT+ground_altitude);
|
|
|
|
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 {
|
|
myPID.SetMode(MANUAL);
|
|
flight_mode = 7;
|
|
}
|
|
|
|
init_time = millis();
|
|
initialised = true;
|
|
}
|
|
|
|
if (millis()-sat_buzz>5000) {
|
|
sat_buzz = millis();
|
|
EasyBuzzer.beep(2000,50,25,gps.satellites.value(),100,1);
|
|
}
|
|
|
|
}
|
|
|
|
//------------------- 1 -------------------//
|
|
|
|
void ready_steady() {
|
|
|
|
if (millis()-init_time>=3000) {
|
|
|
|
if (is_ascent(VUP, 0)) {
|
|
flight_mode = 2;
|
|
EasyBuzzer.beep(1000,100,50,2,500,1);
|
|
setcam(1, 20, 600);
|
|
}
|
|
|
|
if (is_descent(v_down(VDOWN), 1)) {
|
|
flight_mode = 3;
|
|
EasyBuzzer.beep(3000,100,50,3,500,1);
|
|
setcam(1, 60, 600);
|
|
init_time = millis();
|
|
}
|
|
|
|
if ((atof(fix_type.value()) < 2) and (NO_INIT == false) and (flight_started == false)) {
|
|
flight_mode = 0;
|
|
}
|
|
|
|
if (I_WANT_TO_FLY) {
|
|
flight_mode = 5;
|
|
}
|
|
}
|
|
}
|
|
|
|
//------------------- 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 -------------------//
|
|
|
|
void flight_descent() {
|
|
|
|
if (is_ascent(0, 0)) {
|
|
flight_mode = 1;
|
|
}
|
|
|
|
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;
|
|
init_time = millis();
|
|
setcam(1, 60, 600);
|
|
}
|
|
}
|
|
|
|
//------------------- 4 -------------------//
|
|
|
|
void flight_gliding() {
|
|
|
|
if ((millis()-init_time) >= OPENING_TIMER) {
|
|
wing_opened = true;
|
|
|
|
if (failsafe == true) {
|
|
flight_mode = 5;
|
|
setcam(1, 20, 600);
|
|
myPID.SetMode(AUTOMATIC);
|
|
}
|
|
|
|
else {
|
|
flight_mode = 6;
|
|
myPID.SetMode(MANUAL);
|
|
}
|
|
}
|
|
}
|
|
|
|
//------------------- 5 -------------------//
|
|
|
|
void flight_gliding_auto() {
|
|
|
|
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);
|
|
}
|
|
|
|
if (failsafe == false) {
|
|
flight_mode = 6;
|
|
myPID.SetMode(MANUAL);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
}
|
|
|
|
//------------------- 6 -------------------//
|
|
|
|
void flight_gliding_manual() {
|
|
if (failsafe == true) {
|
|
flight_mode = 5;
|
|
myPID.SetMode(AUTOMATIC);
|
|
}
|
|
}
|
|
|
|
//------------------- 7 -------------------//
|
|
|
|
void on_ground() {
|
|
if (gps.speed.mps()>=LAUNCH_SPEED) {
|
|
flight_mode = 8;
|
|
}
|
|
}
|
|
|
|
//------------------- 8 -------------------//
|
|
|
|
void motorised_man() {
|
|
|
|
if (channels[6] > 1000) {
|
|
flight_mode = 9;
|
|
myPID.SetMode(AUTOMATIC);
|
|
}
|
|
|
|
if (failsafe == true) {
|
|
flight_mode = 10;
|
|
myPID.SetMode(AUTOMATIC);
|
|
}
|
|
}
|
|
|
|
//------------------- 9 -------------------//
|
|
|
|
void motorised_auto() {
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
//------------------- 10 -------------------//
|
|
|
|
void motorised_failSafe() {
|
|
|
|
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 -------------------//
|
|
|
|
void flight_gliding_no_gps() {
|
|
if (gps_ok) {
|
|
flight_mode = 5;
|
|
myPID.SetMode(AUTOMATIC);
|
|
}
|
|
}
|
|
|
|
//------------------- STATE MACHINE -------------------//
|
|
|
|
void cmpt_flight_state() {
|
|
|
|
if (flight_mode!=prev_flight_mode and prev_flight_mode !=0) {
|
|
cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral);
|
|
save_data();
|
|
send_data();
|
|
prev_flight_mode = flight_mode;
|
|
}
|
|
|
|
switch(flight_mode) {
|
|
case 0:
|
|
flight_init();
|
|
setled(255, 0, 0, 25, 2000);
|
|
break;
|
|
|
|
case 1:
|
|
ready_steady();
|
|
setled(0, 255, 0, 25, 500);
|
|
break;
|
|
|
|
case 2:
|
|
flight_ascent();
|
|
setled(0, 0, 255, 25, 2000);
|
|
break;
|
|
|
|
case 3:
|
|
flight_descent();
|
|
setled(255, 128, 0, 25, 1000);
|
|
break;
|
|
|
|
case 4:
|
|
flight_gliding();
|
|
break;
|
|
|
|
case 5:
|
|
flight_gliding_auto();
|
|
setled(255, 255, 0, 25, 1000);
|
|
break;
|
|
|
|
case 6:
|
|
flight_gliding_manual();
|
|
setled(0, 255, 255, 25, 1000);
|
|
break;
|
|
|
|
case 7:
|
|
on_ground();
|
|
setled(128, 0, 255, 25, 1000);
|
|
break;
|
|
|
|
case 8:
|
|
motorised_man();
|
|
setled(0, 0, 255, 25, 500);
|
|
break;
|
|
|
|
case 9:
|
|
motorised_auto();
|
|
setled(0, 255, 255, 25, 500);
|
|
break;
|
|
|
|
case 10:
|
|
motorised_failSafe();
|
|
setled(0, 255, 255, 25, 500);
|
|
break;
|
|
|
|
case 11:
|
|
flight_gliding_no_gps();
|
|
setled(255, 0, 0, 25, 200);
|
|
break;
|
|
}
|
|
}
|