mirror of
https://github.com/okalachev/flix.git
synced 2025-07-30 04:48:59 +00:00
Implement auto mode for automatic flight
Use arm/disarm gestures Add arm/disarm commands Add ratesExtra variable for Rename interpretRC to interpretControls Rename controlRate to controlRates Remove USER mode Add invalidate methods for vector and quaternion Add valid/invalid method for vector and quaternion Add valid/invalid function Print armed in rc command Pass auto mode to heartbeat Use actuator_control_target for motors
This commit is contained in:
Oleg Kalachev
parent
2066d05a60
commit
dfceb8a6b2
@ -12,6 +12,7 @@ extern float loopRate, dt;
|
||||
extern double t;
|
||||
extern uint16_t channels[16];
|
||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
|
||||
extern bool armed;
|
||||
|
||||
const char* motd =
|
||||
"\nWelcome to\n"
|
||||
@ -31,6 +32,8 @@ const char* motd =
|
||||
"ps - show pitch/roll/yaw\n"
|
||||
"psq - show attitude quaternion\n"
|
||||
"imu - show IMU data\n"
|
||||
"arm - arm the drone (when no mode switch)\n"
|
||||
"disarm - disarm the drone (when no mode switch)\n"
|
||||
"rc - show RC data\n"
|
||||
"mot - show motor output\n"
|
||||
"log - dump in-RAM log\n"
|
||||
@ -109,6 +112,10 @@ void doCommand(String str, bool echo = false) {
|
||||
printIMUCalibration();
|
||||
print("rate: %.0f\n", loopRate);
|
||||
print("landed: %d\n", landed);
|
||||
} else if (command == "arm") {
|
||||
armed = true;
|
||||
} else if (command == "disarm") {
|
||||
armed = false;
|
||||
} else if (command == "rc") {
|
||||
print("channels: ");
|
||||
for (int i = 0; i < 16; i++) {
|
||||
|
||||
@ -34,8 +34,7 @@
|
||||
#define TILT_MAX radians(30)
|
||||
#define RATES_D_LPF_ALPHA 0.2 // cutoff frequency ~ 40 Hz
|
||||
|
||||
enum { MANUAL, ACRO, STAB, USER } mode = STAB;
|
||||
enum { YAW, YAW_RATE } yawMode = YAW;
|
||||
enum { MANUAL, ACRO, STAB, AUTO } mode = STAB;
|
||||
bool armed = false;
|
||||
|
||||
PID rollRatePID(ROLLRATE_P, ROLLRATE_I, ROLLRATE_D, ROLLRATE_I_LIM, RATES_D_LPF_ALPHA);
|
||||
@ -49,6 +48,7 @@ float tiltMax = TILT_MAX;
|
||||
|
||||
Quaternion attitudeTarget;
|
||||
Vector ratesTarget;
|
||||
Vector ratesExtra; // feedforward rates
|
||||
Vector torqueTarget;
|
||||
float thrustTarget;
|
||||
|
||||
@ -56,63 +56,50 @@ extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRO
|
||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;
|
||||
|
||||
void control() {
|
||||
interpretRC();
|
||||
interpretControls();
|
||||
failsafe();
|
||||
if (mode == STAB) {
|
||||
controlAttitude();
|
||||
controlRate();
|
||||
controlTorque();
|
||||
} else if (mode == ACRO) {
|
||||
controlRate();
|
||||
controlTorque();
|
||||
} else if (mode == MANUAL) {
|
||||
controlTorque();
|
||||
}
|
||||
controlAttitude();
|
||||
controlRates();
|
||||
controlTorque();
|
||||
}
|
||||
|
||||
void interpretRC() {
|
||||
armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
|
||||
|
||||
void interpretControls() {
|
||||
// NOTE: put ACRO or MANUAL modes there if you want to use them
|
||||
if (controlMode < 0.25) {
|
||||
mode = STAB;
|
||||
} else if (controlMode < 0.75) {
|
||||
mode = STAB;
|
||||
} else {
|
||||
mode = STAB;
|
||||
}
|
||||
if (controlMode < 0.25) mode = STAB;
|
||||
if (controlMode < 0.75) mode = STAB;
|
||||
if (controlMode > 0.75) mode = AUTO;
|
||||
if (controlArmed < 0.5) armed = false;
|
||||
|
||||
if (mode == AUTO) return; // pilot is not effective in AUTO mode
|
||||
|
||||
if (landed && controlThrottle == 0 && controlYaw > 0.95) armed = true; // arm gesture
|
||||
if (landed && controlThrottle == 0 && controlYaw < -0.95) armed = false; // disarm gesture
|
||||
|
||||
thrustTarget = controlThrottle;
|
||||
|
||||
if (mode == ACRO) {
|
||||
yawMode = YAW_RATE;
|
||||
ratesTarget.x = controlRoll * maxRate.x;
|
||||
ratesTarget.y = controlPitch* maxRate.y;
|
||||
ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
|
||||
|
||||
} else if (mode == STAB) {
|
||||
yawMode = controlYaw == 0 ? YAW : YAW_RATE;
|
||||
|
||||
attitudeTarget = Quaternion::fromEuler(Vector(
|
||||
controlRoll * tiltMax,
|
||||
controlPitch * tiltMax,
|
||||
attitudeTarget.getYaw()));
|
||||
ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
|
||||
|
||||
} else if (mode == MANUAL) {
|
||||
// passthrough mode
|
||||
yawMode = YAW_RATE;
|
||||
torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
|
||||
if (mode == STAB) {
|
||||
float yawTarget = attitudeTarget.getYaw();
|
||||
if (invalid(yawTarget) || controlYaw != 0) yawTarget = attitude.getYaw(); // reset yaw target if NAN or yaw rate is set
|
||||
attitudeTarget = Quaternion::fromEuler(Vector(controlRoll * tiltMax, controlPitch * tiltMax, yawTarget));
|
||||
ratesExtra = Vector(0, 0, -controlYaw * maxRate.z); // positive yaw stick means clockwise rotation in FLU
|
||||
}
|
||||
|
||||
if (yawMode == YAW_RATE || !motorsActive()) {
|
||||
// update yaw target as we don't have control over the yaw
|
||||
attitudeTarget.setYaw(attitude.getYaw());
|
||||
if (mode == ACRO) {
|
||||
attitudeTarget.invalidate();
|
||||
ratesTarget.x = controlRoll * maxRate.x;
|
||||
ratesTarget.y = controlPitch * maxRate.y;
|
||||
ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU
|
||||
}
|
||||
|
||||
if (mode == MANUAL) { // passthrough mode
|
||||
attitudeTarget.invalidate();
|
||||
ratesTarget.invalidate();
|
||||
torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;
|
||||
}
|
||||
}
|
||||
|
||||
void controlAttitude() {
|
||||
if (!armed) {
|
||||
if (!armed || attitudeTarget.invalid()) { // skip attitude control
|
||||
rollPID.reset();
|
||||
pitchPID.reset();
|
||||
yawPID.reset();
|
||||
@ -125,17 +112,15 @@ void controlAttitude() {
|
||||
|
||||
Vector error = Vector::rotationVectorBetween(upTarget, upActual);
|
||||
|
||||
ratesTarget.x = rollPID.update(error.x, dt);
|
||||
ratesTarget.y = pitchPID.update(error.y, dt);
|
||||
ratesTarget.x = rollPID.update(error.x, dt) + ratesExtra.x;
|
||||
ratesTarget.y = pitchPID.update(error.y, dt) + ratesExtra.y;
|
||||
|
||||
if (yawMode == YAW) {
|
||||
float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
|
||||
ratesTarget.z = yawPID.update(yawError, dt);
|
||||
}
|
||||
float yawError = wrapAngle(attitudeTarget.getYaw() - attitude.getYaw());
|
||||
ratesTarget.z = yawPID.update(yawError, dt) + ratesExtra.z;
|
||||
}
|
||||
|
||||
void controlRate() {
|
||||
if (!armed) {
|
||||
void controlRates() {
|
||||
if (!armed || ratesTarget.invalid()) { // skip rates control
|
||||
rollRatePID.reset();
|
||||
pitchRatePID.reset();
|
||||
yawRatePID.reset();
|
||||
@ -151,7 +136,9 @@ void controlRate() {
|
||||
}
|
||||
|
||||
void controlTorque() {
|
||||
if (!armed) {
|
||||
if (!torqueTarget.valid()) return; // skip torque control
|
||||
|
||||
if (!armed || thrustTarget < 0.05) {
|
||||
memset(motors, 0, sizeof(motors));
|
||||
return;
|
||||
}
|
||||
@ -172,7 +159,7 @@ const char* getModeName() {
|
||||
case MANUAL: return "MANUAL";
|
||||
case ACRO: return "ACRO";
|
||||
case STAB: return "STAB";
|
||||
case USER: return "USER";
|
||||
case AUTO: return "AUTO";
|
||||
default: return "UNKNOWN";
|
||||
}
|
||||
}
|
||||
|
||||
@ -10,32 +10,40 @@ extern double controlTime;
|
||||
extern float controlRoll, controlPitch, controlThrottle, controlYaw;
|
||||
|
||||
void failsafe() {
|
||||
armingFailsafe();
|
||||
rcLossFailsafe();
|
||||
}
|
||||
|
||||
// Prevent arming without zero throttle input
|
||||
void armingFailsafe() {
|
||||
static double zeroThrottleTime;
|
||||
static double armingTime;
|
||||
if (!armed) armingTime = t; // stores the last time when the drone was disarmed, therefore contains arming time
|
||||
if (controlTime > 0 && controlThrottle < 0.05) zeroThrottleTime = controlTime;
|
||||
if (armingTime - zeroThrottleTime > 0.1) armed = false; // prevent arming if there was no zero throttle for 0.1 sec
|
||||
autoFailsafe();
|
||||
}
|
||||
|
||||
// RC loss failsafe
|
||||
void rcLossFailsafe() {
|
||||
if (mode == AUTO) return;
|
||||
if (!armed) return;
|
||||
if (t - controlTime > RC_LOSS_TIMEOUT) {
|
||||
descend();
|
||||
}
|
||||
}
|
||||
|
||||
// Allow pilot to interrupt automatic flight
|
||||
void autoFailsafe() {
|
||||
static float roll, pitch, yaw, throttle;
|
||||
|
||||
if (roll != controlRoll || pitch != controlPitch || yaw != controlYaw || abs(throttle - controlThrottle) > 0.05) {
|
||||
if (mode == AUTO && !isfinite(controlMode)) {
|
||||
print("Failsafe: regain control to pilot\n");
|
||||
mode = STAB; // regain control to the pilot
|
||||
}
|
||||
}
|
||||
|
||||
roll = controlRoll;
|
||||
pitch = controlPitch;
|
||||
yaw = controlYaw;
|
||||
throttle = controlThrottle;
|
||||
}
|
||||
|
||||
// Smooth descend on RC lost
|
||||
void descend() {
|
||||
mode = STAB;
|
||||
controlRoll = 0;
|
||||
controlPitch = 0;
|
||||
controlYaw = 0;
|
||||
controlThrottle -= dt / DESCEND_TIME;
|
||||
if (controlThrottle < 0) controlThrottle = 0;
|
||||
mode = AUTO;
|
||||
thrustTarget -= dt / DESCEND_TIME;
|
||||
if (thrustTarget < 0) thrustTarget = 0;
|
||||
if (thrustTarget == 0) armed = false;
|
||||
}
|
||||
|
||||
@ -12,7 +12,8 @@
|
||||
|
||||
double t = NAN; // current step time, s
|
||||
float dt; // time delta from previous step, s
|
||||
float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode; // pilot's inputs, range [-1, 1]
|
||||
float controlRoll, controlPitch, controlYaw, controlThrottle; // pilot's inputs, range [-1, 1]
|
||||
float controlArmed = NAN, controlMode = NAN;
|
||||
Vector gyro; // gyroscope data
|
||||
Vector acc; // accelerometer data, m/s/s
|
||||
Vector rates; // filtered angular rates, rad/s
|
||||
|
||||
@ -36,7 +36,9 @@ void sendMavlink() {
|
||||
lastSlow = t;
|
||||
|
||||
mavlink_msg_heartbeat_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, MAV_TYPE_QUADROTOR, MAV_AUTOPILOT_GENERIC,
|
||||
MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (armed * MAV_MODE_FLAG_SAFETY_ARMED) | ((mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED),
|
||||
(armed * MAV_MODE_FLAG_SAFETY_ARMED) |
|
||||
(mode == STAB) * MAV_MODE_FLAG_STABILIZE_ENABLED |
|
||||
((mode == AUTO) ? MAV_MODE_FLAG_AUTO_ENABLED : MAV_MODE_FLAG_MANUAL_INPUT_ENABLED),
|
||||
mode, MAV_STATE_STANDBY);
|
||||
sendMessage(&msg);
|
||||
|
||||
@ -57,9 +59,9 @@ void sendMavlink() {
|
||||
channels[0], channels[1], channels[2], channels[3], channels[4], channels[5], channels[6], channels[7], UINT8_MAX);
|
||||
if (channels[0] != 0) sendMessage(&msg); // 0 means no RC input
|
||||
|
||||
float actuator[32];
|
||||
memcpy(actuator, motors, sizeof(motors));
|
||||
mavlink_msg_actuator_output_status_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 4, actuator);
|
||||
float controls[8];
|
||||
memcpy(controls, motors, sizeof(motors));
|
||||
mavlink_msg_actuator_control_target_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time, 0, controls);
|
||||
sendMessage(&msg);
|
||||
|
||||
mavlink_msg_scaled_imu_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, time,
|
||||
@ -102,8 +104,8 @@ void handleMavlink(const void *_msg) {
|
||||
controlPitch = m.x / 1000.0f * mavlinkControlScale;
|
||||
controlRoll = m.y / 1000.0f * mavlinkControlScale;
|
||||
controlYaw = m.r / 1000.0f * mavlinkControlScale;
|
||||
controlMode = 1; // STAB mode
|
||||
controlArmed = 1; // armed
|
||||
controlMode = NAN; // keep mode
|
||||
controlArmed = NAN;
|
||||
controlTime = t;
|
||||
|
||||
if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
|
||||
@ -174,6 +176,39 @@ void handleMavlink(const void *_msg) {
|
||||
doCommand(data, true);
|
||||
}
|
||||
|
||||
if (msg.msgid == MAVLINK_MSG_ID_SET_ATTITUDE_TARGET) {
|
||||
if (mode != AUTO) return;
|
||||
|
||||
mavlink_set_attitude_target_t m;
|
||||
mavlink_msg_set_attitude_target_decode(&msg, &m);
|
||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
||||
|
||||
// copy attitude, rates and thrust targets
|
||||
ratesTarget.x = m.body_roll_rate;
|
||||
ratesTarget.y = -m.body_pitch_rate; // convert to flu
|
||||
ratesTarget.z = -m.body_yaw_rate;
|
||||
attitudeTarget.w = m.q[0];
|
||||
attitudeTarget.x = m.q[1];
|
||||
attitudeTarget.y = -m.q[2];
|
||||
attitudeTarget.z = -m.q[3];
|
||||
thrustTarget = m.thrust;
|
||||
ratesExtra = Vector(0, 0, 0);
|
||||
|
||||
if (m.type_mask & ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE) attitudeTarget.invalidate();
|
||||
|
||||
armed = m.thrust > 0;
|
||||
}
|
||||
|
||||
if (msg.msgid == MAVLINK_MSG_ID_SET_ACTUATOR_CONTROL_TARGET) {
|
||||
if (mode != AUTO) return;
|
||||
|
||||
mavlink_set_actuator_control_target_t m;
|
||||
mavlink_msg_set_actuator_control_target_decode(&msg, &m);
|
||||
if (m.target_system && m.target_system != SYSTEM_ID) return;
|
||||
|
||||
memcpy(motors, m.controls, sizeof(motors)); // copy motor thrusts
|
||||
}
|
||||
|
||||
// Handle commands
|
||||
if (msg.msgid == MAVLINK_MSG_ID_COMMAND_LONG) {
|
||||
mavlink_command_long_t m;
|
||||
@ -188,8 +223,12 @@ void handleMavlink(const void *_msg) {
|
||||
mavlink_msg_autopilot_version_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &response,
|
||||
MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT | MAV_PROTOCOL_CAPABILITY_MAVLINK2, 1, 0, 1, 1, 0, 0, 0, 0, 0, 0, 0);
|
||||
sendMessage(&response);
|
||||
} else {
|
||||
mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_UNSUPPORTED, UINT8_MAX, 0, msg.sysid, msg.compid);
|
||||
}
|
||||
|
||||
if (m.command == MAV_CMD_DO_SET_MODE) {
|
||||
if (!(m.param2 >= 0 && m.param2 <= AUTO)) return;
|
||||
mode = static_cast<decltype(mode)>(m.param2);
|
||||
mavlink_msg_command_ack_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &ack, m.command, MAV_RESULT_ACCEPTED, UINT8_MAX, 0, msg.sysid, msg.compid);
|
||||
sendMessage(&ack);
|
||||
}
|
||||
}
|
||||
|
||||
@ -64,6 +64,21 @@ public:
|
||||
return isfinite(w) && isfinite(x) && isfinite(y) && isfinite(z);
|
||||
}
|
||||
|
||||
bool valid() const {
|
||||
return finite();
|
||||
}
|
||||
|
||||
bool invalid() const {
|
||||
return !valid();
|
||||
}
|
||||
|
||||
void invalidate() {
|
||||
w = NAN;
|
||||
x = NAN;
|
||||
y = NAN;
|
||||
z = NAN;
|
||||
}
|
||||
|
||||
float norm() const {
|
||||
return sqrt(w * w + x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
@ -21,6 +21,20 @@ public:
|
||||
return isfinite(x) && isfinite(y) && isfinite(z);
|
||||
}
|
||||
|
||||
bool valid() const {
|
||||
return finite();
|
||||
}
|
||||
|
||||
bool invalid() const {
|
||||
return !valid();
|
||||
}
|
||||
|
||||
void invalidate() {
|
||||
x = NAN;
|
||||
y = NAN;
|
||||
z = NAN;
|
||||
}
|
||||
|
||||
float norm() const {
|
||||
return sqrt(x * x + y * y + z * z);
|
||||
}
|
||||
|
||||
@ -15,7 +15,8 @@
|
||||
double t = NAN;
|
||||
float dt;
|
||||
float motors[4];
|
||||
float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;
|
||||
float controlRoll, controlPitch, controlYaw, controlThrottle = NAN;
|
||||
float controlArmed = NAN, controlMode = NAN;
|
||||
Vector acc;
|
||||
Vector gyro;
|
||||
Vector rates;
|
||||
@ -28,9 +29,9 @@ void computeLoopRate();
|
||||
void applyGyro();
|
||||
void applyAcc();
|
||||
void control();
|
||||
void interpretRC();
|
||||
void interpretControls();
|
||||
void controlAttitude();
|
||||
void controlRate();
|
||||
void controlRates();
|
||||
void controlTorque();
|
||||
const char* getModeName();
|
||||
void sendMotors();
|
||||
@ -54,8 +55,8 @@ void mavlinkPrint(const char* str);
|
||||
void sendMavlinkPrint();
|
||||
inline Quaternion fluToFrd(const Quaternion &q);
|
||||
void failsafe();
|
||||
void armingFailsafe();
|
||||
void rcLossFailsafe();
|
||||
void autoFailsafe();
|
||||
void descend();
|
||||
int parametersCount();
|
||||
const char *getParameterName(int index);
|
||||
|
||||
@ -56,7 +56,7 @@ class Flix:
|
||||
if e.errno != errno.EADDRINUSE:
|
||||
raise
|
||||
# Port busy - using proxy
|
||||
logger.debug('Listening on port 14560 (proxy)')
|
||||
logger.debug('Listening on port 14555 (proxy)')
|
||||
self.connection: mavutil.mavfile = mavutil.mavlink_connection('udpin:0.0.0.0:14555', source_system=254) # type: ignore
|
||||
self.connection.target_system = system_id
|
||||
self.mavlink: mavlink.MAVLink = self.connection.mav
|
||||
@ -169,8 +169,8 @@ class Flix:
|
||||
msg.chan5_raw, msg.chan6_raw, msg.chan7_raw, msg.chan8_raw]
|
||||
self._trigger('channels', self.channels)
|
||||
|
||||
if isinstance(msg, mavlink.MAVLink_actuator_output_status_message):
|
||||
self.motors = msg.actuator[:4] # type: ignore
|
||||
if isinstance(msg, mavlink.MAVLink_actuator_control_target_message):
|
||||
self.motors = msg.controls[:4] # type: ignore
|
||||
self._trigger('motors', self.motors)
|
||||
|
||||
if isinstance(msg, mavlink.MAVLink_scaled_imu_message):
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user