From 01f6805a5b7c43ce34de666f4addd8f5a96cc07d Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Fri, 10 Jan 2025 09:26:20 +0300 Subject: [PATCH] Make channels definition to rc.ino It's also planned to parametrize them later --- flix/cli.ino | 9 +++++---- flix/control.ino | 25 +++++++++++++------------ flix/failsafe.ino | 12 +++++++----- flix/flix.ino | 12 ++---------- flix/mavlink.ino | 16 +++++++++------- flix/rc.ino | 18 +++++++++++++----- gazebo/flix.h | 2 +- gazebo/joystick.h | 7 ------- gazebo/simulator.cpp | 4 ++-- 9 files changed, 52 insertions(+), 53 deletions(-) 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/rc.ino b/flix/rc.ino index 42cb64f..48f5827 100644 --- a/flix/rc.ino +++ b/flix/rc.ino @@ -6,8 +6,16 @@ #include #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 #include -#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 @@ class ModelFlix : public ModelPlugin { // 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();