diff --git a/docs/firmware.md b/docs/firmware.md
index 45ad004..fe659a0 100644
--- a/docs/firmware.md
+++ b/docs/firmware.md
@@ -12,7 +12,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[])* — user control inputs from the RC, normalized to [-1, 1] range.
+* `controlRoll`, `controlPitch`, ... *(float[])* — pilot's control inputs, range [-1, 1].
* `motors` *(float[])* — motor outputs, normalized to [0, 1] range; reverse rotation is possible.
## Source files
diff --git a/flix/cli.ino b/flix/cli.ino
index 10714ea..3172cc8 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 uint16_t channels[16];
const char* motd =
"\nWelcome to\n"
@@ -46,13 +47,13 @@ void doCommand(const String& command) {
printIMUCal();
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]);
- Serial.printf("Control: throttle %f yaw %f pitch %f roll %f armed %f mode %f\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]);
- Serial.printf("Mode: %s\n", getModeName());
+ Serial.printf("channels: ");
+ for (int i = 0; i < 16; i++) {
+ Serial.printf("%u ", channels[i]);
+ }
+ Serial.printf("\nroll: %g pitch: %g yaw: %g throttle: %g armed: %g mode: %g\n",
+ controlRoll, controlPitch, controlYaw, controlThrottle, controlArmed, controlMode);
+ Serial.printf("mode: %s\n", getModeName());
} else if (command == "mot") {
Serial.printf("MOTOR front-right %f front-left %f rear-right %f rear-left %f\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 f42d194..f7914f1 100644
--- a/flix/control.ino
+++ b/flix/control.ino
@@ -69,38 +69,39 @@ void control() {
}
void interpretRC() {
- armed = controls[RC_CHANNEL_THROTTLE] >= 0.05 && controls[RC_CHANNEL_ARMED] >= 0.5;
+ armed = controlThrottle >= 0.05 && controlArmed >= 0.5;
+
// NOTE: put ACRO or MANUAL modes there if you want to use them
- if (controls[RC_CHANNEL_MODE] < 0.25) {
+ if (controlMode < 0.25) {
mode = STAB;
- } else if (controls[RC_CHANNEL_MODE] < 0.75) {
+ } else if (controlMode < 0.75) {
mode = STAB;
} else {
mode = STAB;
}
- thrustTarget = controls[RC_CHANNEL_THROTTLE];
+ thrustTarget = controlThrottle;
if (mode == ACRO) {
yawMode = YAW_RATE;
- ratesTarget.x = controls[RC_CHANNEL_ROLL] * ROLLRATE_MAX;
- ratesTarget.y = controls[RC_CHANNEL_PITCH] * PITCHRATE_MAX;
- ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
+ ratesTarget.x = controlRoll * ROLLRATE_MAX;
+ ratesTarget.y = controlPitch * PITCHRATE_MAX;
+ ratesTarget.z = -controlYaw * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
} else if (mode == STAB) {
- yawMode = controls[RC_CHANNEL_YAW] == 0 ? YAW : YAW_RATE;
+ yawMode = controlYaw == 0 ? YAW : YAW_RATE;
attitudeTarget = Quaternion::fromEuler(Vector(
- controls[RC_CHANNEL_ROLL] * TILT_MAX,
- controls[RC_CHANNEL_PITCH] * TILT_MAX,
+ controlRoll * TILT_MAX,
+ controlPitch * TILT_MAX,
attitudeTarget.getYaw()));
- ratesTarget.z = -controls[RC_CHANNEL_YAW] * YAWRATE_MAX; // positive yaw stick means clockwise rotation in FLU
+ ratesTarget.z = -controlYaw * YAWRATE_MAX; // 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(controlRoll, controlPitch, -controlYaw) * 0.01;
}
if (yawMode == YAW_RATE || !motorsActive()) {
diff --git a/flix/failsafe.ino b/flix/failsafe.ino
index 4ff0066..51a7106 100644
--- a/flix/failsafe.ino
+++ b/flix/failsafe.ino
@@ -6,11 +6,11 @@
#define RC_LOSS_TIMEOUT 0.2
#define DESCEND_TIME 3.0 // time to descend from full throttle to zero
-extern float controlsTime;
+extern float controlTime;
// RC loss failsafe
void failsafe() {
- if (t - controlsTime > RC_LOSS_TIMEOUT) {
+ if (t - controlTime > RC_LOSS_TIMEOUT) {
descend();
}
}
@@ -18,9 +18,9 @@ void failsafe() {
// Smooth descend on RC lost
void descend() {
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;
+ 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 f5ae91e..16f9d2a 100644
--- a/flix/flix.ino
+++ b/flix/flix.ino
@@ -10,17 +10,9 @@
#define SERIAL_BAUDRATE 115200
#define WIFI_ENABLED 1
-#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[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 ff4445c..8ff6bf2 100644
--- a/flix/mavlink.ino
+++ b/flix/mavlink.ino
@@ -13,7 +13,7 @@
#define MAVLINK_CONTROL_SCALE 0.7f
#define MAVLINK_CONTROL_YAW_DEAD_ZONE 0.1f
-extern float controlsTime;
+extern float controlTime;
void processMavlink() {
sendMavlink();
@@ -45,10 +45,8 @@ 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);
+ 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);
sendMessage(&msg);
float actuator[32];
@@ -90,15 +88,15 @@ void handleMavlink(const void *_msg) {
if (msg.msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
mavlink_manual_control_t m;
mavlink_msg_manual_control_decode(&msg, &m);
- controls[RC_CHANNEL_THROTTLE] = m.z / 1000.0f;
- controls[RC_CHANNEL_PITCH] = m.x / 1000.0f * MAVLINK_CONTROL_SCALE;
- controls[RC_CHANNEL_ROLL] = m.y / 1000.0f * MAVLINK_CONTROL_SCALE;
- controls[RC_CHANNEL_YAW] = m.r / 1000.0f * MAVLINK_CONTROL_SCALE;
- controls[RC_CHANNEL_MODE] = 1; // STAB mode
- controls[RC_CHANNEL_ARMED] = 1; // armed
- controlsTime = t;
+ controlThrottle = m.z / 1000.0f;
+ controlPitch = m.x / 1000.0f * MAVLINK_CONTROL_SCALE;
+ controlRoll = m.y / 1000.0f * MAVLINK_CONTROL_SCALE;
+ controlYaw = m.r / 1000.0f * MAVLINK_CONTROL_SCALE;
+ controlMode = 1; // STAB mode
+ controlArmed = 1; // armed
+ controlTime = t;
- if (abs(controls[RC_CHANNEL_YAW]) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controls[RC_CHANNEL_YAW] = 0;
+ if (abs(controlYaw) < MAVLINK_CONTROL_YAW_DEAD_ZONE) controlYaw = 0;
}
}
diff --git a/flix/rc.ino b/flix/rc.ino
index 2f2c1ea..c0f4393 100644
--- a/flix/rc.ino
+++ b/flix/rc.ino
@@ -8,10 +8,21 @@
SBUS RC(Serial2); // NOTE: Use RC(Serial2, 16, 17) if you use the old UART2 pins
+uint16_t channels[16]; // raw rc channels
+float controlTime; // time of the last controls update
+
+
// NOTE: use 'cr' command to calibrate the RC and put the values here
-int channelNeutral[] = {995, 883, 200, 972, 512, 512, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-int channelMax[] = {1651, 1540, 1713, 1630, 1472, 1472, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
-float controlsTime; // time of the last controls update
+int channelZero[] = {992, 992, 172, 992, 172, 172, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+int channelMax[] = {1811, 1811, 1811, 1811, 1811, 1811, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+
+// Channels mapping:
+int rollChannel = 0;
+int pitchChannel = 1;
+int throttleChannel = 2;
+int yawChannel = 3;
+int armedChannel = 4;
+int modeChannel = 5;
void setupRC() {
Serial.println("Setup RC");
@@ -21,18 +32,26 @@ 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() {
- 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 = controls[rollChannel];
+ controlPitch = controls[pitchChannel];
+ controlYaw = controls[yawChannel];
+ controlThrottle = controls[throttleChannel];
+ controlArmed = controls[armedChannel];
+ controlMode = controls[modeChannel];
}
void calibrateRC() {
@@ -48,13 +67,13 @@ void calibrateRC() {
delay(4000);
while (!readRC());
for (int i = 0; i < 16; i++) {
- channelNeutral[i] = channels[i];
+ channelZero[i] = channels[i];
}
- printRCCal();
+ printRCCalibration();
}
-void printRCCal() {
- for (int i = 0; i < sizeof(channelNeutral) / sizeof(channelNeutral[0]); i++) Serial.printf("%d ", channelNeutral[i]);
+void printRCCalibration() {
+ for (int i = 0; i < sizeof(channelZero) / sizeof(channelZero[0]); i++) Serial.printf("%d ", channelZero[i]);
Serial.printf("\n");
for (int i = 0; i < sizeof(channelMax) / sizeof(channelMax[0]); i++) Serial.printf("%d ", channelMax[i]);
Serial.printf("\n");
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 d7c259b..c4819e1 100644
--- a/gazebo/flix.h
+++ b/gazebo/flix.h
@@ -15,8 +15,7 @@
float 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;
@@ -36,7 +35,7 @@ void sendMotors();
bool motorsActive();
void doCommand(const String& command);
void normalizeRC();
-void printRCCal();
+void printRCCalibration();
void processMavlink();
void sendMavlink();
void sendMessage(const void *msg);
diff --git a/gazebo/joystick.h b/gazebo/joystick.h
index 714b502..798c15b 100644
--- a/gazebo/joystick.h
+++ b/gazebo/joystick.h
@@ -8,15 +8,16 @@
#include
// simulation calibration overrides, NOTE: use `cr` command and replace with the actual values
-const int channelNeutralOverride[] = {-258, -258, -27349, 0, -27349, 0};
-const int channelMaxOverride[] = {27090, 27090, 27090, 27090, -5676, 1};
+const int channelZeroOverride[] = {1500, 0, 1000, 1500, 1500, 1000};
+const int channelMaxOverride[] = {2000, 2000, 2000, 2000, 2000, 2000};
-#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
+// channels mapping overrides
+const int rollChannelOverride = 3;
+const int pitchChannelOverride = 4;
+const int throttleChannelOverride = 5;
+const int yawChannelOverride = 0;
+const int armedChannelOverride = 2;
+const int modeChannelOverride = 1;
SDL_Joystick *joystick;
@@ -37,12 +38,20 @@ bool joystickInit() {
warnShown = true;
}
- // apply calibration overrides
- extern int channelNeutral[16];
+ // apply overrides
+ extern int channelZero[16];
extern int channelMax[16];
- memcpy(channelNeutral, channelNeutralOverride, sizeof(channelNeutralOverride));
+ memcpy(channelZero, channelZeroOverride, sizeof(channelZeroOverride));
memcpy(channelMax, channelMaxOverride, sizeof(channelMaxOverride));
+ extern int rollChannel, pitchChannel, throttleChannel, yawChannel, armedChannel, modeChannel;
+ rollChannel = rollChannelOverride;
+ pitchChannel = pitchChannelOverride;
+ throttleChannel = throttleChannelOverride;
+ yawChannel = yawChannelOverride;
+ armedChannel = armedChannelOverride;
+ modeChannel = modeChannelOverride;
+
return joystickInitialized;
}
diff --git a/gazebo/simulator.cpp b/gazebo/simulator.cpp
index dd3105b..cf64166 100644
--- a/gazebo/simulator.cpp
+++ b/gazebo/simulator.cpp
@@ -70,8 +70,8 @@ public:
// read rc
readRC();
- controls[RC_CHANNEL_MODE] = 1; // 0 acro, 1 stab
- controls[RC_CHANNEL_ARMED] = 1; // armed
+ controlMode = 1; // 0 acro, 1 stab
+ controlArmed = 1; // armed
estimate();