kopia lustrzana https://github.com/YohanHadji/R2Home
253 wiersze
6.2 KiB
C++
253 wiersze
6.2 KiB
C++
#include "config.h"
|
|
#include "control.hpp"
|
|
#include <PWMServo.h>
|
|
#include <movingAvg.h>
|
|
|
|
PWMServo steer;
|
|
PWMServo aux;
|
|
PWMServo aux_deploy;
|
|
PWMServo left;
|
|
PWMServo right;
|
|
PWMServo esc;
|
|
|
|
int servo_left = 0;
|
|
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 {
|
|
float left = 1500;
|
|
float right = 1500;
|
|
float aux = 1500;
|
|
float aux_deploy = 1500;
|
|
};
|
|
|
|
void servo_setup() {
|
|
left.attach(6, 1000, 2000);
|
|
right.attach(7, 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], 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);
|
|
aux = constrain(aux, 1000, 2000);
|
|
sw = constrain(sw, 1000, 2000);
|
|
|
|
servo_cmd steering_cmpt ;
|
|
|
|
switch(flight_mode) {
|
|
case 0:
|
|
case 1:
|
|
case 6:
|
|
case 7:
|
|
case 8:
|
|
// ---------- Stage 1 - RC mode ---------- //
|
|
switch (RC_MODE) {
|
|
|
|
case 0: // roll only
|
|
steering_cmpt.right = roll;
|
|
steering_cmpt.left = 3000-roll;
|
|
break;
|
|
|
|
case 1: // pitch and roll mixed
|
|
steering_cmpt.right = roll;
|
|
steering_cmpt.left = 3000-roll;
|
|
steering_cmpt.left = steering_cmpt.left+(pitch-1500);
|
|
steering_cmpt.right = steering_cmpt.right+(pitch-1500);
|
|
break;
|
|
|
|
case 2: // pitch and roll separated
|
|
steering_cmpt.left = roll;
|
|
steering_cmpt.right = pitch;
|
|
break;
|
|
}
|
|
if (failsafe) {
|
|
steering_cmpt.left = 1500;
|
|
steering_cmpt.right = 1500;
|
|
}
|
|
break;
|
|
|
|
case 9:
|
|
case 10:
|
|
case 5:
|
|
if (cog_ok and !spiral) {
|
|
steering_cmpt.right = autopilot;
|
|
steering_cmpt.left = 3000-autopilot;
|
|
}
|
|
else {
|
|
steering_cmpt.right = 1500;
|
|
steering_cmpt.left = 1500;
|
|
}
|
|
break;
|
|
|
|
case 11:
|
|
steering_cmpt.right = 1500;
|
|
steering_cmpt.left = 1500;
|
|
break;
|
|
|
|
case 4:
|
|
steering_cmpt.right = 2000;
|
|
steering_cmpt.left = 2000;
|
|
break;
|
|
|
|
}
|
|
|
|
// ---------- Stage 2 - Linear mode ---------- //
|
|
switch (LINEAR_MODE) {
|
|
|
|
case 0: // control is fully linear
|
|
break;
|
|
|
|
case 1: // control start at servo_start with an offset
|
|
if (steering_cmpt.left>=(1500+TRIG)) {
|
|
steering_cmpt.left = map(steering_cmpt.left, 1500, 2000, 1500+LEFT_OFFSET, SERVO_MAX_M);
|
|
}
|
|
else if (steering_cmpt.left<=(1500-TRIG)) {
|
|
steering_cmpt.left = map(steering_cmpt.left, 1000, 1500, 1000, 1500-LEFT_OFFSET);
|
|
}
|
|
if (steering_cmpt.right>=(1500+TRIG)) {
|
|
steering_cmpt.right = map(steering_cmpt.right, 1500, 2000, 1500+RIGHT_OFFSET, SERVO_MAX_M);
|
|
}
|
|
else if (steering_cmpt.right<=(1500-TRIG)) {
|
|
steering_cmpt.right = map(steering_cmpt.right, 1000, 1500, 1000, 1500-RIGHT_OFFSET);
|
|
}
|
|
break;
|
|
}
|
|
|
|
// ---------- Stage 3 - Control mode ---------- //
|
|
switch (CONTROL_MODE) {
|
|
|
|
case 0: // neutral is center
|
|
break;
|
|
|
|
case 1: // neutral is hands up
|
|
steering_cmpt.right = constrain(map(steering_cmpt.right, 1500, 2000, 1000, SERVO_MAX_M), 1000, SERVO_MAX_C);
|
|
steering_cmpt.left = constrain(map(steering_cmpt.left, 1500, 2000, 1000, SERVO_MAX_M), 1000, SERVO_MAX_C);
|
|
break;
|
|
}
|
|
|
|
// -------------------------- Deployment Servo and ESC -------------------------- //
|
|
|
|
switch(flight_mode) {
|
|
|
|
case 0:
|
|
case 1:
|
|
case 2:
|
|
case 3:
|
|
case 5:
|
|
case 4:
|
|
case 6:
|
|
// Deployment Servo
|
|
if (deployed == true) {
|
|
steering_cmpt.aux = 1000;
|
|
}
|
|
else {
|
|
if (failsafe == true) {
|
|
steering_cmpt.aux = 2000;
|
|
}
|
|
else { steering_cmpt.aux = sw; }
|
|
}
|
|
|
|
// Separation Servo
|
|
if (separation == true) {
|
|
steering_cmpt.aux_deploy = 1000;
|
|
}
|
|
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;
|
|
|
|
case 9:
|
|
case 10:
|
|
steering_cmpt.aux = 1000;
|
|
break;
|
|
|
|
case 11:
|
|
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;
|
|
servo_aux = steering_cmpt.aux;
|
|
servo_aux_deploy = steering_cmpt.aux_deploy;
|
|
|
|
return steering_cmpt;
|
|
}
|
|
|
|
void update_servo_cmd(servo_cmd steering_apply, unsigned int a) {
|
|
|
|
if ((millis()-tpwm)>=(1000/a)) {
|
|
tpwm = millis();
|
|
left.write(map(steering_apply.left, 1000, 2000, 0, 180));
|
|
right.write(map(steering_apply.right, 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));
|
|
}
|
|
|
|
}
|
|
|
|
String servo_text() {
|
|
String left_text = String(servo_left);
|
|
String right_text = String(servo_right);
|
|
String aux_text = String(servo_aux);
|
|
String aux_deploy_text = String(servo_aux_deploy);
|
|
return left_text+","+right_text+","+aux_text+","+aux_deploy_text;
|
|
}
|