kopia lustrzana https://github.com/YohanHadji/R2Home
115 wiersze
2.0 KiB
C++
115 wiersze
2.0 KiB
C++
// This code is V3.08
|
|
// Updated and tested on April 10th 10h21 UTC
|
|
// Ready for flight
|
|
|
|
#include <movingAvg.h>
|
|
#include "flight_state.hpp"
|
|
|
|
servo_cmd steering;
|
|
|
|
|
|
void setup() {
|
|
|
|
Serial.begin(115200);
|
|
|
|
if (DEBUG) { Serial.println("Just barely waking up"); }
|
|
|
|
TLM_PORT.begin(57600);
|
|
|
|
if (DEBUG) { Serial.println("Ready to initialise all the hardware"); }
|
|
|
|
battery_setup();
|
|
buzzer_setup();
|
|
camera_setup();
|
|
gps_setup(57600, GPS_FREQ, 2, 1, 0); // baud, Hz, mode, nmea, cog filter (0 = Off, 1 = On)
|
|
led_setup();
|
|
rc_setup();
|
|
servo_setup();
|
|
position_setup();
|
|
|
|
getconfig();
|
|
|
|
watchdog.reset();
|
|
|
|
barometer_setup();
|
|
navigation_setup();
|
|
|
|
cmpt_weight_gain();
|
|
|
|
buzzer_end_setup();
|
|
}
|
|
|
|
void loop() {
|
|
|
|
watchdog.reset();
|
|
|
|
getdata(); // Getting data from all the sensors
|
|
datacmpt(); // Using this data to do all what we have to do
|
|
loop_count++;
|
|
|
|
if ((millis()-tloop)>=1000) {
|
|
tloop = millis();
|
|
loop_rate = loop_count;
|
|
loop_count = 0;
|
|
}
|
|
|
|
if (DEBUG) {
|
|
delay(10);
|
|
}
|
|
|
|
if (TEST_SEP_SERVO and millis()>10000) {
|
|
separation = true;
|
|
}
|
|
|
|
}
|
|
|
|
void getdata() {
|
|
get_gps();
|
|
get_baro(0);
|
|
get_rc();
|
|
get_vbatt();
|
|
|
|
}
|
|
|
|
void datacmpt() {
|
|
|
|
cmpt_pressure_gain(pressure_sqrt_ratio());
|
|
|
|
if (I_WANT_TO_FLY) {
|
|
cmd_to_waypoint = sim();
|
|
//Serial.println(cmd_to_waypoint);
|
|
cog_ok = true;
|
|
}
|
|
|
|
steering = cmpt_servo(channels, cmd_to_waypoint, flight_mode, deployed, failsafe, cog_ok, spiral, separation);
|
|
update_servo_cmd(steering, SERVO_RATE);
|
|
|
|
cmpt_flight_state();
|
|
cmpt_data_rate(flight_mode);
|
|
|
|
if (initialised) {
|
|
cmpt_vertical_state();
|
|
cmpt_fusion();
|
|
}
|
|
|
|
if ((millis()-sd)>=delaySD) {
|
|
sd = millis();
|
|
cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral);
|
|
if (initialised) {
|
|
save_data();
|
|
}
|
|
}
|
|
|
|
if (millis()-tlm>=delayTLM) {
|
|
tlm = millis();
|
|
send_data();
|
|
}
|
|
|
|
buzzer_turn(flight_mode, setPoint_waypoint);
|
|
buzzer_batt();
|
|
update_buzzer();
|
|
updateled();
|
|
updatecam();
|
|
|
|
}
|