Making an ESP32-based quadcopter from scratch
Python arduinoautopilotcontrol-theorydroneesp32flight-controlflight-controllermavlinkquadcopterquadcopter-firmwareuav
Make channels definition to rc.ino It's also planned to parametrize them later
Oleg Kalachev821e6b105e89
diff --git a/flix/cli.ino b/flix/cli.inoindex 61cc53e..3749863 100644--- a/flix/cli.ino+++ b/flix/cli.ino@@ -8,6 +8,7 @@extern const int MOTOR_REAR_LEFT, MOTOR_REAR_RIGHT, MOTOR_FRONT_RIGHT, MOTOR_FRONT_LEFT;extern float loopRate;+extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;const char* motd ="\nWelcome to\n"@@ -65,11 +66,11 @@ void doCommand(String& command, String& arg0, String& arg1) {Serial.printf("rate: %f\n", loopRate);} else if (command == "rc") {Serial.printf("Raw: throttle %d yaw %d pitch %d roll %d armed %d mode %d\n",- channels[RC_CHANNEL_THROTTLE], channels[RC_CHANNEL_YAW], channels[RC_CHANNEL_PITCH],- channels[RC_CHANNEL_ROLL], channels[RC_CHANNEL_ARMED], channels[RC_CHANNEL_MODE]);+ channels[throttleChannel], channels[yawChannel], channels[pitchChannel],+ channels[rollChannel], channels[armedChannel], channels[modeChannel]);Serial.printf("Control: throttle %g yaw %g pitch %g roll %g armed %g mode %g\n",- controls[RC_CHANNEL_THROTTLE], controls[RC_CHANNEL_YAW], controls[RC_CHANNEL_PITCH],- controls[RC_CHANNEL_ROLL], controls[RC_CHANNEL_ARMED], controls[RC_CHANNEL_MODE]);+ controls[throttleChannel], controls[yawChannel], controls[pitchChannel],+ controls[rollChannel], controls[armedChannel], controls[modeChannel]);Serial.printf("Mode: %s\n", getModeName());} else if (command == "mot") {Serial.printf("Motors: front-right %g front-left %g rear-right %g rear-left %g\n",diff --git a/flix/control.ino b/flix/control.inoindex 6bab295..8d7b55f 100644--- a/flix/control.ino+++ b/flix/control.ino@@ -54,6 +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;void control() {interpretRC();@@ -71,38 +72,38 @@ void control() {}void interpretRC() {- armed = controls[RC_CHANNEL_THROTTLE] >= 0.05 && controls[RC_CHANNEL_ARMED] >= 0.5;+ armed = controls[throttleChannel] >= 0.05 && controls[armedChannel] >= 0.5;// NOTE: put ACRO or MANUAL modes there if you want to use them- if (controls[RC_CHANNEL_MODE] < 0.25) {+ if (controls[modeChannel] < 0.25) {mode = STAB;- } else if (controls[RC_CHANNEL_MODE] < 0.75) {+ } else if (controls[modeChannel] < 0.75) {mode = STAB;} else {mode = STAB;}- thrustTarget = controls[RC_CHANNEL_THROTTLE];+ thrustTarget = controls[throttleChannel];if (mode == ACRO) {yawMode = YAW_RATE;- ratesTarget.x = controls[RC_CHANNEL_ROLL] * maxRate.x;- ratesTarget.y = controls[RC_CHANNEL_PITCH] * maxRate.y;- ratesTarget.z = -controls[RC_CHANNEL_YAW] * maxRate.z; // positive yaw stick means clockwise rotation in FLU+ 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} else if (mode == STAB) {- yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;+ yawMode = controls[yawChannel] == 0 ? YAW : YAW_RATE;attitudeTarget = Quaternion::fromEulerZYX(Vector(- controls[RC_CHANNEL_ROLL] * tiltMax,- controls[RC_CHANNEL_PITCH] * tiltMax,+ controls[rollChannel] * tiltMax,+ controls[pitchChannel] * tiltMax,attitudeTarget.getYaw()));- ratesTarget.z = -controls[RC_CHANNEL_YAW] * maxRate.z; // positive yaw stick means clockwise rotation in FLU+ ratesTarget.z = -controls[yawChannel] * maxRate.z; // positive yaw stick means clockwise rotation in FLU} else if (mode == MANUAL) {// passthrough modeyawMode = YAW_RATE;- torqueTarget = Vector(controls[RC_CHANNEL_ROLL], controls[RC_CHANNEL_PITCH], -controls[RC_CHANNEL_YAW]) * 0.01;+ torqueTarget = Vector(controls[rollChannel], controls[pitchChannel], -controls[yawChannel]) * 0.01;}if (yawMode == YAW_RATE || !motorsActive()) {diff --git a/flix/failsafe.ino b/flix/failsafe.inoindex 7ddfa65..be36551 100644--- a/flix/failsafe.ino+++ b/flix/failsafe.ino@@ -6,6 +6,8 @@#define RC_LOSS_TIMEOUT 0.2#define DESCEND_TIME 3.0 // time to descend from full throttle to zero+extern int rollChannel, pitchChannel, throttleChannel, yawChannel;+void failsafe() {if (t - controlsTime > RC_LOSS_TIMEOUT) {descend();@@ -15,9 +17,9 @@ void failsafe() {void descend() {// Smooth descend on RC lostmode = STAB;- controls[RC_CHANNEL_ROLL] = 0;- controls[RC_CHANNEL_PITCH] = 0;- controls[RC_CHANNEL_YAW] = 0;- controls[RC_CHANNEL_THROTTLE] -= dt / DESCEND_TIME;- if (controls[RC_CHANNEL_THROTTLE] < 0) controls[RC_CHANNEL_THROTTLE] = 0;+ controls[rollChannel] = 0;+ controls[pitchChannel] = 0;+ controls[throttleChannel] = 0;+ controls[throttleChannel] -= dt / DESCEND_TIME;+ if (controls[throttleChannel] < 0) controls[throttleChannel] = 0;}diff --git a/flix/flix.ino b/flix/flix.inoindex 632b846..b1baa23 100644--- a/flix/flix.ino+++ b/flix/flix.ino@@ -10,18 +10,10 @@#define SERIAL_BAUDRATE 115200#define WIFI_ENABLED 1-#define RC_CHANNELS 16-#define RC_CHANNEL_ROLL 0-#define RC_CHANNEL_PITCH 1-#define RC_CHANNEL_THROTTLE 2-#define RC_CHANNEL_YAW 3-#define RC_CHANNEL_ARMED 4-#define RC_CHANNEL_MODE 5-float t = NAN; // current step time, sfloat dt; // time delta from previous step, s-int16_t channels[RC_CHANNELS]; // raw rc channels-float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for throttle)+int16_t channels[16]; // raw rc channels+float controls[16]; // normalized controls in range [-1..1] ([0..1] for throttle)float controlsTime; // time of the last controls updateVector gyro; // gyroscope dataVector acc; // accelerometer data, m/s/sdiff --git a/flix/mavlink.ino b/flix/mavlink.inoindex b9a9b3a..46e13f8 100644--- a/flix/mavlink.ino+++ b/flix/mavlink.ino@@ -13,6 +13,8 @@#define MAVLINK_CONTROL_SCALE 0.7f#define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f+extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;+void processMavlink() {sendMavlink();receiveMavlink();@@ -88,15 +90,15 @@ void handleMavlink(const void *_msg) {if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {mavlink_manual_control_t manualControl;mavlink_msg_manual_control_decode(msg, &manualControl);- controls[RC_CHANNEL_THROTTLE] = manualControl.z / 1000.0f;- controls[RC_CHANNEL_PITCH] = manualControl.x / 1000.0f * MAVLINK_CONTROL_SCALE;- controls[RC_CHANNEL_ROLL] = manualControl.y / 1000.0f * MAVLINK_CONTROL_SCALE;- controls[RC_CHANNEL_YAW] = manualControl.r / 1000.0f * MAVLINK_CONTROL_SCALE;- controls[RC_CHANNEL_MODE] = 1; // STAB mode- controls[RC_CHANNEL_ARMED] = 1; // armed+ controls[throttleChannel] = manualControl.z / 1000.0f;+ controls[pitchChannel] = manualControl.x / 1000.0f * MAVLINK_CONTROL_SCALE;+ controls[rollChannel] = manualControl.y / 1000.0f * MAVLINK_CONTROL_SCALE;+ controls[yawChannel] = manualControl.r / 1000.0f * MAVLINK_CONTROL_SCALE;+ controls[modeChannel] = 1; // STAB mode+ controls[armedChannel] = 1; // armedcontrolsTime = t;- if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0;+ if (abs(controls[yawChannel]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[yawChannel] = 0;}if (msg->msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) {diff --git a/flix/parameters.ino b/flix/parameters.inoindex 092ae20..3e9a6ff 100644--- a/flix/parameters.ino+++ b/flix/parameters.ino@@ -5,8 +5,8 @@#include <Preferences.h>-extern float channelNeutral[RC_CHANNELS];-extern float channelMax[RC_CHANNELS];+extern float channelNeutral[16];+extern float channelMax[16];Preferences storage;diff --git a/flix/rc.ino b/flix/rc.inoindex 42cb64f..48f5827 100644--- a/flix/rc.ino+++ b/flix/rc.ino@@ -6,8 +6,16 @@#include <SBUS.h>#include "util.h"-float channelNeutral[RC_CHANNELS] = {NAN}; // first element NAN means not calibrated-float channelMax[RC_CHANNELS];+// RC channels mapping:+int rollChannel = 0;+int pitchChannel = 1;+int throttleChannel = 2;+int yawChannel = 3;+int armedChannel = 4;+int modeChannel = 5;++float channelNeutral[16] = {NAN}; // first element NAN means not calibrated+float channelMax[16];SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins@@ -27,7 +35,7 @@ void readRC() {void normalizeRC() {if (isnan(channelNeutral[0])) return; // skip if not calibrated- for (uint8_t i = 0; i < RC_CHANNELS; i++) {+ for (uint8_t i = 0; i < 16; i++) {controls[i] = mapf(channels[i], channelNeutral[i], channelMax[i], 0, 1);}}@@ -37,14 +45,14 @@ void calibrateRC() {Serial.println("··o ··o\n··· ···\n··· ···");delay(4000);for (int i = 0; i < 30; i++) readRC(); // ensure the values are updated- for (int i = 0; i < RC_CHANNELS; i++) {+ for (int i = 0; i < 16; i++) {channelMax[i] = channels[i];}Serial.println("Calibrate RC: move all sticks to neutral positions in 4 seconds");Serial.println("··· ···\n··· ·o·\n·o· ···");delay(4000);for (int i = 0; i < 30; i++) readRC(); // ensure the values are updated- for (int i = 0; i < RC_CHANNELS; i++) {+ for (int i = 0; i < 16; i++) {channelNeutral[i] = channels[i];}printRCCal();diff --git a/gazebo/flix.h b/gazebo/flix.hindex ba76abe..c457d07 100644--- a/gazebo/flix.h+++ b/gazebo/flix.h@@ -17,7 +17,7 @@ float t = NAN;float dt;float motors[4];int16_t channels[16]; // raw rc channels-float controls[RC_CHANNELS];+float controls[16];float controlsTime;Vector acc;Vector gyro;diff --git a/gazebo/joystick.h b/gazebo/joystick.hindex 3833340..9056191 100644--- a/gazebo/joystick.h+++ b/gazebo/joystick.h@@ -7,13 +7,6 @@#include <gazebo/gazebo.hh>#include <iostream>-#define RC_CHANNEL_ROLL 0-#define RC_CHANNEL_PITCH 1-#define RC_CHANNEL_THROTTLE 2-#define RC_CHANNEL_YAW 3-#define RC_CHANNEL_ARMED 5-#define RC_CHANNEL_MODE 4-SDL_Joystick *joystick;bool joystickInit() {diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cppindex 3d5555e..c520b6e 100644--- a/gazebo/simulator.cpp+++ b/gazebo/simulator.cpp@@ -72,8 +72,8 @@ public:// read rcreadRC();- controls[RC_CHANNEL_MODE] = 1; // 0 acro, 1 stab- controls[RC_CHANNEL_ARMED] = 1; // armed+ controls[modeChannel] = 1; // 0 acro, 1 stab+ controls[armedChannel] = 1; // armedestimate();