Skip to content

armgit / flix

Public Imported from GitHub
0

Making an ESP32-based quadcopter from scratch

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.ino
index 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.ino
index 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 mode
yawMode = 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.ino
index 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 lost
mode = 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.ino
index 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, s
float 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 update
Vector gyro; // gyroscope data
Vector acc; // accelerometer data, m/s/s
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index 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; // armed
controlsTime = 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.ino
index 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.ino
index 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.h
index 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.h
index 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.cpp
index 3d5555e..c520b6e 100644
--- a/gazebo/simulator.cpp
+++ b/gazebo/simulator.cpp
@@ -72,8 +72,8 @@ public:
// read rc
readRC();
- 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; // armed
estimate();