kopia lustrzana https://github.com/YohanHadji/R2Home
Adding X servo capability
rodzic
ffd806f3d6
commit
342497b33d
|
@ -5,7 +5,7 @@
|
|||
#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 false // Doing the waypoint sequence before reaching home?
|
||||
#define NAV_WAYPOINT true // 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
|
||||
|
@ -35,6 +35,7 @@
|
|||
#define OPENING_TIMER 3000
|
||||
#define SPIRAL_RECOVER 5000
|
||||
#define DEP_ALT 300 // m above ground
|
||||
#define SEP_ALT 10000 // m above sea level
|
||||
#define VUP 1 // m/s
|
||||
#define VDOWN -1 // m/s
|
||||
|
||||
|
|
|
@ -111,7 +111,7 @@ 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), MAX_M_W, MAX_C_W, MAC_M, MAX_C");
|
||||
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), PWM_X (us), MAX_M_W, MAX_C_W, MAC_M, MAX_C");
|
||||
dataFile.close();
|
||||
}
|
||||
if (CONFIG_FILE_SV) {
|
||||
|
|
|
@ -9,6 +9,7 @@ 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;
|
||||
|
||||
|
@ -107,6 +108,9 @@ void flight_ascent() {
|
|||
if ((gps.altitude.meters()-ground_altitude)>10) {
|
||||
flight_started = true;
|
||||
}
|
||||
if (gps.altitude.meters()>SEP_ALT and gps_ok) {
|
||||
separation = true;
|
||||
}
|
||||
}
|
||||
|
||||
//------------------- 3 -------------------//
|
||||
|
|
|
@ -87,7 +87,7 @@ void datacmpt() {
|
|||
cog_ok = true;
|
||||
}
|
||||
|
||||
steering = cmpt_servo(channels, cmd_to_waypoint, flight_mode, deployed, failsafe, cog_ok, spiral);
|
||||
steering = cmpt_servo(channels, cmd_to_waypoint, flight_mode, deployed, failsafe, cog_ok, spiral, separation);
|
||||
update_servo_cmd(steering, SERVO_RATE);
|
||||
|
||||
cmpt_flight_state();
|
||||
|
|
|
@ -3,7 +3,8 @@
|
|||
#include <PWMServo.h>
|
||||
|
||||
PWMServo steer;
|
||||
PWMServo deploy;
|
||||
PWMServo deploy;
|
||||
PWMServo aux_deploy;
|
||||
PWMServo left;
|
||||
PWMServo right;
|
||||
PWMServo esc;
|
||||
|
@ -11,6 +12,7 @@ PWMServo esc;
|
|||
int servo_left = 0;
|
||||
int servo_right = 0;
|
||||
int servo_aux = 0;
|
||||
int servo_aux_deploy = 0;
|
||||
|
||||
unsigned long tpwm = 0;
|
||||
|
||||
|
@ -18,6 +20,7 @@ struct servo_cmd {
|
|||
float left = 1500;
|
||||
float right = 1500;
|
||||
float aux = 1500;
|
||||
float aux_deploy = 1500;
|
||||
};
|
||||
|
||||
void servo_setup() {
|
||||
|
@ -25,9 +28,10 @@ void servo_setup() {
|
|||
right.attach(7, 1000, 2000);
|
||||
if (DROP == true) { deploy.attach(8, 1000, 2000); }
|
||||
else { esc.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) {
|
||||
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);
|
||||
|
@ -132,7 +136,6 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
|
|||
|
||||
case 0:
|
||||
case 1:
|
||||
case 2:
|
||||
case 3:
|
||||
case 4:
|
||||
case 5:
|
||||
|
@ -149,6 +152,15 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
|
|||
}
|
||||
break;
|
||||
|
||||
case 2:
|
||||
if (separation == true) {
|
||||
steering_cmpt.aux_deploy = 1000;
|
||||
}
|
||||
else {
|
||||
steering_cmpt.aux_deploy = 2000;
|
||||
}
|
||||
break;
|
||||
|
||||
case 8:
|
||||
steering_cmpt.aux = aux;
|
||||
break;
|
||||
|
@ -166,6 +178,7 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
|
|||
servo_left = steering_cmpt.left;
|
||||
servo_right = steering_cmpt.right;
|
||||
servo_aux = steering_cmpt.aux;
|
||||
servo_aux_deploy = steering_cmpt.aux_deploy;
|
||||
|
||||
return steering_cmpt;
|
||||
}
|
||||
|
@ -177,6 +190,7 @@ void update_servo_cmd(servo_cmd steering_apply, unsigned int a) {
|
|||
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_deploy.write(map(steering_apply.aux_deploy, 1000, 2000, 0, 180));
|
||||
}
|
||||
|
||||
}
|
||||
|
@ -185,5 +199,6 @@ String servo_text() {
|
|||
String left_text = String(servo_left);
|
||||
String right_text = String(servo_right);
|
||||
String aux_text = String(servo_aux);
|
||||
return left_text+","+right_text+","+aux_text;
|
||||
String aux_deploy_text = String(servo_aux_deploy);
|
||||
return left_text+","+right_text+","+aux_text+","+aux_deploy_text;
|
||||
}
|
||||
|
|
Ładowanie…
Reference in New Issue