aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-28 20:08:53 +00:00
Code Issues Packages Projects Releases Wiki Activity

Port changes from commit 52819e4

Browse Source
This commit is contained in:
Oleg Kalachev 2025-07-15 01:22:18 +03:00
parent 4d1f9de872
commit 0c87d4d634
11 changed files with 98 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
docs/firmware.md
View File

@ -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/s<sup>2</sup>*.
* `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

15
flix/cli.ino
View File

@ -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]);

25
flix/control.ino
View File

@ -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()) {

14
flix/failsafe.ino
View File

@ -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;
}

10
flix/flix.ino
View File

@ -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

24
flix/mavlink.ino
View File

@ -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;
}
}

41
flix/rc.ino
View File

@ -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");

3
gazebo/SBUS.h
View File

@ -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;
};
};

5
gazebo/flix.h
View File

@ -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);

31
gazebo/joystick.h
View File

@ -8,15 +8,16 @@
#include <iostream>
// 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;
}

4
gazebo/simulator.cpp
View File

@ -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();