From 0c87d4d63471296f1df49c337183478ec6658aee Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Tue, 15 Jul 2025 01:22:18 +0300 Subject: [PATCH] Port changes from commit 52819e4 --- docs/firmware.md | 2 +- flix/cli.ino | 15 ++++++++------- flix/control.ino | 25 +++++++++++++------------ flix/failsafe.ino | 14 +++++++------- flix/flix.ino | 10 +--------- flix/mavlink.ino | 24 +++++++++++------------- flix/rc.ino | 41 ++++++++++++++++++++++++++++++----------- gazebo/SBUS.h | 3 +++ gazebo/flix.h | 5 ++--- gazebo/joystick.h | 31 ++++++++++++++++++++----------- gazebo/simulator.cpp | 4 ++-- 11 files changed, 98 insertions(+), 76 deletions(-) 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();