Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Major rework of rc subsystem Implement channels mapping calibration. Store mapping in parameters. Get rid of `controls` array and store control inputs in `controlRoll`, `controlPitch`, ... variables. Move `channels` variable to rc.ino, channels are not involved when controled using mavlink. 'Neutral' values are renamed to 'zero' - more precise naming. `controlsTime` is renamed to `controlTime`. Use unsigned values for channels. Make channel values in simulation more alike to real world: unsigned values in range [1000, 2000]. Send RC_CHANNELS_RAW instead of RC_CHANNELS_SCALED via mavlink Don't send channels data via mavlink if rc is not used
Oleg Kalachev52819e403b83
diff --git a/docs/book/firmware.md b/docs/book/firmware.mdindex 6f88b60..f8bb47e 100644--- a/docs/book/firmware.md+++ b/docs/book/firmware.md@@ -10,7 +10,7 @@* `acc` *(Vector)* — данные с акселерометра, *м/с<sup>2</sup>*.* `rates` *(Vector)* — отфильтрованные угловые скорости, *рад/с*.* `attitude` *(Quaternion)* — оценка ориентации (положения) дрона.-* `controls` *(float[])* — команды управления от пилота, в диапазоне [-1, 1].+* `controlRoll`, `controlPitch`, ... *(float[])* — команды управления от пилота, в диапазоне [-1, 1].* `motors` *(float[])* — выходные сигналы на моторы, в диапазоне [0, 1].## Исходные файлыdiff --git a/docs/firmware.md b/docs/firmware.mdindex 8dc9884..5b672cd 100644--- a/docs/firmware.md+++ b/docs/firmware.md@@ -14,7 +14,7 @@ The main loop is running at 1000 Hz. All the dataflow is happening through globa* `acc` *(Vector)* — acceleration data from the accelerometer, *m/s<sup>2</sup>*.* `rates` *(Vector)* — filtered angular rates, *rad/s*.* `attitude` *(Quaternion)* — estimated attitude (orientation) of drone.-* `controls` *(float[])* — pilot's control inputs, range [-1, 1].+* `controlRoll`, `controlPitch`, ... *(float[])* — pilot's control inputs, range [-1, 1].* `motors` *(float[])* — motor outputs, range [0, 1].## Source filesdiff --git a/flix/cli.ino b/flix/cli.inoindex f611106..afc602f 100644--- a/flix/cli.ino+++ b/flix/cli.ino@@ -10,7 +10,8 @@extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;extern float loopRate, dt;extern double t;-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;+extern uint16_t channels[16];+extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;const char* motd ="\nWelcome to\n"@@ -109,13 +110,13 @@ void doCommand(String str, bool echo = false) {print("rate: %.0f\n", loopRate);print("landed: %d\n", landed);} else if (command == "rc") {- print("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",- channels[throttleChannel], channels[yawChannel], channels[pitchChannel],- channels[rollChannel], channels[armedChannel], channels[modeChannel]);- print("Control: throttle %g yaw %g pitch %g roll %g armed %g mode %g\n",- controls[throttleChannel], controls[yawChannel], controls[pitchChannel],- controls[rollChannel], controls[armedChannel], controls[modeChannel]);- print("Mode: %s\n", getModeName());+ print("channels: ");+ for (int i = 0; i < 16; i++) {+ print("%u ", channels[i]);+ }+ print("\nroll: %g pitch: %g yaw: %g throttle: %g armed: %g mode: %g\n",+ controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode);+ print("mode: %s\n", getModeName());} else if (command == "mot") {print("Motors: front-right %g front-left %g rear-right %g rear-left %g\n",motors[MOTOR_FRONT_RIGHT], motors[MOTOR_FRONT_LEFT], motors[MOTOR_REAR_RIGHT], motors[MOTOR_REAR_LEFT]);diff --git a/flix/control.ino b/flix/control.inoindex 0b3ff32..623e8fb 100644--- a/flix/control.ino+++ b/flix/control.ino@@ -21,7 +21,7 @@#define YAWRATE_I 0.0#define YAWRATE_D 0.0#define YAWRATE_I_LIM 0.3-#define ROLL_P 4.5+#define ROLL_P 6#define ROLL_I 0#define ROLL_D 0#define PITCH_P ROLL_P@@ -54,7 +54,7 @@ Vector torqueTarget;float thrustTarget;extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;+extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;void control() {interpretRC();@@ -72,39 +72,38 @@ void control() {}void interpretRC() {- armed = controls[throttleChannel] >= 0.05 &&- (controls[armedChannel] >= 0.5 || isnan(controls[armedChannel])); // assume armed if armed channel is not defined+ armed = controlThrottle >= 0.05 && controlArmed >= 0.5;// NOTE: put ACRO or MANUAL modes there if you want to use them- if (controls[modeChannel] < 0.25) {+ if (controlMode < 0.25) {mode = STAB;- } else if (controls[modeChannel] < 0.75) {+ } else if (controlMode < 0.75) {mode = STAB;} else {mode = STAB;}- thrustTarget = controls[throttleChannel];+ thrustTarget = controlThrottle;if (mode == ACRO) {yawMode = YAW_RATE;- ratesTarget.x = controls[rollChannel] * maxRate.x;- ratesTarget.y = controls[pitchChannel] * maxRate.y;- ratesTarget.z = -controls[yawChannel] * maxRate.z; // positive yaw stick means clockwise rotation in FLU+ 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 = controls[yawChannel] == 0 ? YAW : YAW_RATE;+ yawMode = controlYaw == 0 ? YAW : YAW_RATE;attitudeTarget = Quaternion::fromEuler(Vector(- controls[rollChannel] * tiltMax,- controls[pitchChannel] * tiltMax,+ controlRoll * tiltMax,+ controlPitch * tiltMax,attitudeTarget.getYaw()));- ratesTarget.z = -controls[yawChannel] * maxRate.z; // positive yaw stick means clockwise rotation in FLU+ ratesTarget.z = -controlYaw * maxRate.z; // positive yaw stick means clockwise rotation in FLU} else if (mode == MANUAL) {// passthrough modeyawMode = YAW_RATE;- torqueTarget = Vector(controls[rollChannel], controls[pitchChannel], -controls[yawChannel]) * 0.01;+ torqueTarget = Vector(controlRoll, controlPitch, -controlYaw) * 0.01;}if (yawMode == YAW_RATE || !motorsActive()) {diff --git a/flix/failsafe.ino b/flix/failsafe.inoindex b217994..0d9d646 100644--- a/flix/failsafe.ino+++ b/flix/failsafe.ino@@ -6,8 +6,8 @@#define RC_LOSS_TIMEOUT 0.2#define DESCEND_TIME 3.0 // time to descend from full throttle to zero-extern double controlsTime;-extern int rollChannel, pitchChannel, throttleChannel, yawChannel;+extern double controlTime;+extern float controlRoll, controlPitch, controlThrottle, controlYaw;void failsafe() {armingFailsafe();@@ -19,13 +19,13 @@ 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 (controlsTime > 0 && controls[throttleChannel] < 0.05) zeroThrottleTime = controlsTime;+ 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}// RC loss failsafevoid rcLossFailsafe() {- if (t - controlsTime > RC_LOSS_TIMEOUT) {+ if (t - controlTime > RC_LOSS_TIMEOUT) {descend();}}@@ -33,9 +33,9 @@ void rcLossFailsafe() {// Smooth descend on RC lostvoid descend() {mode = STAB;- controls[rollChannel] = 0;- controls[pitchChannel] = 0;- controls[yawChannel] = 0;- controls[throttleChannel] -= dt / DESCEND_TIME;- if (controls[throttleChannel] < 0) controls[throttleChannel] = 0;+ controlRoll = 0;+ controlPitch = 0;+ controlYaw = 0;+ controlThrottle -= dt / DESCEND_TIME;+ if (controlThrottle < 0) controlThrottle = 0;}diff --git a/flix/flix.ino b/flix/flix.inoindex da2aaa2..531ad18 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -12,8 +12,7 @@double t = NAN; // current step time, sfloat dt; // time delta from previous step, s-int16_t channels[16]; // raw rc channels-float controls[16]; // normalized controls in range [-1..1] ([0..1] for throttle)+float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode; // pilot's inputs, range [-1, 1]Vector gyro; // gyroscope dataVector acc; // accelerometer data, m/s/sVector rates; // filtered angular rates, rad/sdiff --git a/flix/mavlink.ino b/flix/mavlink.inoindex a38661b..c3cded8 100644--- a/flix/mavlink.ino+++ b/flix/mavlink.ino@@ -15,8 +15,8 @@float mavlinkControlScale = 0.7;String mavlinkPrintBuffer;-extern double controlsTime;-extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;+extern double controlTime;+extern float controlRoll, controlPitch, controlThrottle, controlYaw, controlArmed, controlMode;void processMavlink() {sendMavlink();@@ -54,11 +54,9 @@ void sendMavlink() {time, att.w, att.x, att.y, att.z, rates.x, rates.y, rates.z, zeroQuat);sendMessage(&msg);- mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlsTime * 1000, 0,- controls[0] * 10000, controls[1] * 10000, controls[2] * 10000,- controls[3] * 10000, controls[4] * 10000, controls[5] * 10000,- INT16_MAX, INT16_MAX, UINT8_MAX);- sendMessage(&msg);+ mavlink_msg_rc_channels_raw_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlTime * 1000, 0,+ 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 inputfloat actuator[32];memcpy(actuator, motors, sizeof(motors));@@ -101,15 +99,15 @@ void handleMavlink(const void *_msg) {mavlink_msg_manual_control_decode(&msg, &m);if (m.target && m.target != SYSTEM_ID) return; // 0 is broadcast- controls[throttleChannel] = m.z / 1000.0f;- controls[pitchChannel] = m.x / 1000.0f * mavlinkControlScale;- controls[rollChannel] = m.y / 1000.0f * mavlinkControlScale;- controls[yawChannel] = m.r / 1000.0f * mavlinkControlScale;- controls[modeChannel] = 1; // STAB mode- controls[armedChannel] = 1; // armed- controlsTime = t;+ controlThrottle = m.z / 1000.0f;+ 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+ controlTime = t;- if (abs(controls[yawChannel]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[yawChannel] = 0;+ if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;}if (msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {diff --git a/flix/parameters.ino b/flix/parameters.inoindex 612fe34..793e15d 100644--- a/flix/parameters.ino+++ b/flix/parameters.ino@@ -5,8 +5,9 @@#include <Preferences.h>-extern float channelNeutral[16];+extern float channelZero[16];extern float channelMax[16];+extern float rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;extern float mavlinkControlScale;Preferences storage;@@ -49,14 +50,14 @@ Parameter parameters[] = {{"ACC_SCALE_Y", &accScale.y},{"ACC_SCALE_Z", &accScale.z},// rc- {"RC_NEUTRAL_0", &channelNeutral[0]},- {"RC_NEUTRAL_1", &channelNeutral[1]},- {"RC_NEUTRAL_2", &channelNeutral[2]},- {"RC_NEUTRAL_3", &channelNeutral[3]},- {"RC_NEUTRAL_4", &channelNeutral[4]},- {"RC_NEUTRAL_5", &channelNeutral[5]},- {"RC_NEUTRAL_6", &channelNeutral[6]},- {"RC_NEUTRAL_7", &channelNeutral[7]},+ {"RC_ZERO_0", &channelZero[0]},+ {"RC_ZERO_1", &channelZero[1]},+ {"RC_ZERO_2", &channelZero[2]},+ {"RC_ZERO_3", &channelZero[3]},+ {"RC_ZERO_4", &channelZero[4]},+ {"RC_ZERO_5", &channelZero[5]},+ {"RC_ZERO_6", &channelZero[6]},+ {"RC_ZERO_7", &channelZero[7]},{"RC_MAX_0", &channelMax[0]},{"RC_MAX_1", &channelMax[1]},{"RC_MAX_2", &channelMax[2]},@@ -65,6 +66,12 @@ Parameter parameters[] = {{"RC_MAX_5", &channelMax[5]},{"RC_MAX_6", &channelMax[6]},{"RC_MAX_7", &channelMax[7]},+ {"RC_ROLL", &rollChannel},+ {"RC_PITCH", &pitchChannel},+ {"RC_THROTTLE", &throttleChannel},+ {"RC_YAW", &yawChannel},+ {"RC_ARMED", &armedChannel},+ {"RC_MODE", &modeChannel},#if WIFI_ENABLED// MAVLink{"MAV_CTRL_SCALE", &mavlinkControlScale},diff --git a/flix/rc.ino b/flix/rc.inoindex e78cb42..678973a 100644--- a/flix/rc.ino+++ b/flix/rc.ino@@ -8,17 +8,14 @@SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins-// RC channels mapping:-int rollChannel = 0;-int pitchChannel = 1;-int throttleChannel = 2;-int yawChannel = 3;-int armedChannel = 4;-int modeChannel = 5;+uint16_t channels[16]; // raw rc channels+double controlTime; // time of the last controls update+float channelZero[16]; // calibration zero values+float channelMax[16]; // calibration max values++// Channels mapping (using float to store in parameters):+float rollChannel = NAN, pitchChannel = NAN, throttleChannel = NAN, yawChannel = NAN, armedChannel = NAN, modeChannel = NAN;-double controlsTime; // time of the last controls update-float channelNeutral[16] = {NAN}; // first element NAN means not calibrated-float channelMax[16];void setupRC() {print("Setup RC\n");@@ -28,42 +25,76 @@ void setupRC() {bool readRC() {if (RC.read()) {SBUSData data = RC.data();- memcpy(channels, data.ch, sizeof(channels)); // copy channels data+ for (int i = 0; i < 16; i++) channels[i] = data.ch[i]; // copy channels datanormalizeRC();- controlsTime = t;+ controlTime = t;return true;}return false;}void normalizeRC() {- if (isnan(channelNeutral[0])) return; // skip if not calibrated- for (uint8_t i = 0; i < 16; i++) {- controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);+ float controls[16];+ for (int i = 0; i < 16; i++) {+ controls[i] = mapf(channels[i], channelZero[i], channelMax[i], 0, 1);}+ // Update control values+ controlRoll = rollChannel >= 0 ? controls[(int)rollChannel] : NAN;+ controlPitch = pitchChannel >= 0 ? controls[(int)pitchChannel] : NAN;+ controlYaw = yawChannel >= 0 ? controls[(int)yawChannel] : NAN;+ controlThrottle = throttleChannel >= 0 ? controls[(int)throttleChannel] : NAN;+ controlArmed = armedChannel >= 0 ? controls[(int)armedChannel] : 1; // assume armed by default+ controlMode = modeChannel >= 0 ? controls[(int)modeChannel] : NAN;}void calibrateRC() {- print("Calibrate RC: move all sticks to maximum positions [4 sec]\n");- print("··o ··o\n··· ···\n··· ···\n");- pause(4);- while (!readRC());+ uint16_t zero[16];+ uint16_t center[16];+ uint16_t max[16];+ print("1/9 Calibrating RC: put all switches to default positions [3 sec]\n");+ pause(3);+ calibrateRCChannel(NULL, zero, zero, "2/9 Move sticks [3 sec]\n... ...\n... .o.\n.o. ...\n");+ calibrateRCChannel(NULL, center, center, "3/9 Move sticks [3 sec]\n... ...\n.o. .o.\n... ...\n");+ calibrateRCChannel(&throttleChannel, zero, max, "4/9 Move sticks [3 sec]\n.o. ...\n... .o.\n... ...\n");+ calibrateRCChannel(&yawChannel, center, max, "5/9 Move sticks [3 sec]\n... ...\n..o .o.\n... ...\n");+ calibrateRCChannel(&pitchChannel, zero, max, "6/9 Move sticks [3 sec]\n... .o.\n... ...\n.o. ...\n");+ calibrateRCChannel(&rollChannel, zero, max, "7/9 Move sticks [3 sec]\n... ...\n... ..o\n.o. ...\n");+ calibrateRCChannel(&armedChannel, zero, max, "8/9 Switch to armed [3 sec]\n");+ calibrateRCChannel(&modeChannel, zero, max, "9/9 Disarm and switch mode to max [3 sec]\n");+ printRCCalibration();+}++void calibrateRCChannel(float *channel, uint16_t in[16], uint16_t out[16], const char *str) {+ print("%s", str);+ pause(3);+ for (int i = 0; i < 30; i++) readRC(); // try update 30 times max+ memcpy(out, channels, sizeof(channels));++ if (channel == NULL) return; // no channel to calibrate++ // Find channel that changed the most between in and out+ int ch = -1, diff = 0;for (int i = 0; i < 16; i++) {- channelMax[i] = channels[i];+ if (abs(out[i] - in[i]) > diff) {+ ch = i;+ diff = abs(out[i] - in[i]);+ }}- print("Calibrate RC: move all sticks to neutral positions [4 sec]\n");- print("··· ···\n··· ·o·\n·o· ···\n");- pause(4);- while (!readRC());- for (int i = 0; i < 16; i++) {- channelNeutral[i] = channels[i];+ if (ch >= 0 && diff > 10) { // difference threshold is 10+ *channel = ch;+ channelZero[ch] = in[ch];+ channelMax[ch] = out[ch];+ } else {+ *channel = NAN;}- printRCCal();}-void printRCCal() {- for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) print("%g ", channelNeutral[i]);- print("\n");- for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) print("%g ", channelMax[i]);- print("\n");+void printRCCalibration() {+ print("Control Ch Zero Max\n");+ print("Roll %-7g%-7g%-7g\n", rollChannel, rollChannel >= 0 ? channelZero[(int)rollChannel] : NAN, rollChannel >= 0 ? channelMax[(int)rollChannel] : NAN);+ print("Pitch %-7g%-7g%-7g\n", pitchChannel, pitchChannel >= 0 ? channelZero[(int)pitchChannel] : NAN, pitchChannel >= 0 ? channelMax[(int)pitchChannel] : NAN);+ print("Yaw %-7g%-7g%-7g\n", yawChannel, yawChannel >= 0 ? channelZero[(int)yawChannel] : NAN, yawChannel >= 0 ? channelMax[(int)yawChannel] : NAN);+ print("Throttle %-7g%-7g%-7g\n", throttleChannel, throttleChannel >= 0 ? channelZero[(int)throttleChannel] : NAN, throttleChannel >= 0 ? channelMax[(int)throttleChannel] : NAN);+ print("Armed %-7g%-7g%-7g\n", armedChannel, armedChannel >= 0 ? channelZero[(int)armedChannel] : NAN, armedChannel >= 0 ? channelMax[(int)armedChannel] : NAN);+ print("Mode %-7g%-7g%-7g\n", modeChannel, modeChannel >= 0 ? channelZero[(int)modeChannel] : NAN, modeChannel >= 0 ? channelMax[(int)modeChannel] : NAN);}diff --git a/gazebo/SBUS.h b/gazebo/SBUS.hindex f57db7f..eb6b2ec 100644--- a/gazebo/SBUS.h+++ b/gazebo/SBUS.h@@ -18,6 +18,9 @@ public:SBUSData data() {SBUSData data;joystickGet(data.ch);+ for (int i = 0; i < 16; i++) {+ data.ch[i] = map(data.ch[i], -32768, 32767, 1000, 2000); // convert to pulse width style+ }return data;};};diff --git a/gazebo/flix.h b/gazebo/flix.hindex 035d160..2891303 100644--- a/gazebo/flix.h+++ b/gazebo/flix.h@@ -15,8 +15,7 @@double t = NAN;float dt;float motors[4];-int16_t channels[16]; // raw rc channels-float controls[16];+float controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode;Vector acc;Vector gyro;Vector rates;@@ -41,9 +40,10 @@ void print(const char* format, ...);void pause(float duration);void doCommand(String str, bool echo);void handleInput();-void calibrateRC();void normalizeRC();-void printRCCal();+void calibrateRC();+void calibrateRCChannel(float *channel, uint16_t zero[16], uint16_t max[16], const char *str);+void printRCCalibration();void dumpLog();void processMavlink();void sendMavlink();diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cppindex bcfa4ef..316d2bf 100644--- a/gazebo/simulator.cpp+++ b/gazebo/simulator.cpp@@ -71,11 +71,7 @@ public:gyro = Vector(imu->AngularVelocity().X(), imu->AngularVelocity().Y(), imu->AngularVelocity().Z());acc = this->accFilter.update(Vector(imu->LinearAcceleration().X(), imu->LinearAcceleration().Y(), imu->LinearAcceleration().Z()));- // read rcreadRC();- controls[modeChannel] = 1; // 0 acro, 1 stab- controls[armedChannel] = 1; // armed-estimate();// correct yaw to the actual yaw