diff --git a/docs/book/firmware.md b/docs/book/firmware.md
index 6f88b60..f8bb47e 100644
--- a/docs/book/firmware.md
+++ b/docs/book/firmware.md
@@ -10,7 +10,7 @@
* `acc` *(Vector)* — данные с акселерометра, *м/с2*.
* `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.md
index 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/s2*.
* `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 files
diff --git a/flix/cli.ino b/flix/cli.ino
index 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.ino
index 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 mode
yawMode = 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.ino
index 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 failsafe
void rcLossFailsafe() {
- if (t - controlsTime > RC_LOSS_TIMEOUT) {
+ if (t - controlTime > RC_LOSS_TIMEOUT) {
descend();
}
}
@@ -33,9 +33,9 @@ void rcLossFailsafe() {
// Smooth descend on RC lost
void 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.ino
index da2aaa2..531ad18 100644
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -12,8 +12,7 @@
double t = NAN; // current step time, s
float 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 data
Vector acc; // accelerometer data, m/s/s
Vector rates; // filtered angular rates, rad/s
diff --git a/flix/mavlink.ino b/flix/mavlink.ino
index 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 input
float 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.ino
index 612fe34..793e15d 100644
--- a/flix/parameters.ino
+++ b/flix/parameters.ino
@@ -5,8 +5,9 @@
#include
-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.ino
index 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 data
normalizeRC();
- 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());
- for (int i = 0; i < 16; i++) {
- channelMax[i] = channels[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];
- }
- printRCCal();
+ 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 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 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++) {
+ if (abs(out[i] - in[i]) > diff) {
+ ch = i;
+ diff = abs(out[i] - in[i]);
+ }
+ }
+ if (ch >= 0 && diff > 10) { // difference threshold is 10
+ *channel = ch;
+ channelZero[ch] = in[ch];
+ channelMax[ch] = out[ch];
+ } else {
+ *channel = NAN;
+ }
+}
+
+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.h
index 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.h
index 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.cpp
index 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 rc
readRC();
- controls[modeChannel] = 1; // 0 acro, 1 stab
- controls[armedChannel] = 1; // armed
-
estimate();
// correct yaw to the actual yaw