aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-07-27 09:39:33 +00:00
Code Issues Packages Projects Releases Wiki Activity

Make channels definition to rc.ino

Browse Source 
It's also planned to parametrize them later
This commit is contained in:
Oleg Kalachev 2025-01-10 09:37:48 +03:00
parent 568f9dd5b1
commit 821e6b105e
10 changed files with 54 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
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 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",

25
flix/control.ino
View File

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

12
flix/failsafe.ino
View File

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

12
flix/flix.ino
View File

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

16
flix/mavlink.ino
View File

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

4
flix/parameters.ino
View File

@ -5,8 +5,8 @@
#include <Preferences.h>
extern float channelNeutral[RC_CHANNELS];
extern float channelMax[RC_CHANNELS];
extern float channelNeutral[16];
extern float channelMax[16];
Preferences storage;

18
flix/rc.ino
View File

@ -6,8 +6,16 @@
#include <SBUS.h>
#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();

2
gazebo/flix.h
View File

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

7
gazebo/joystick.h
View File

@ -7,13 +7,6 @@
#include <gazebo/gazebo.hh>
#include <iostream>
#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() {

4
gazebo/simulator.cpp
View File

@ -72,8 +72,8 @@ public:
// 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();