mirror of
https://github.com/okalachev/flix.git
synced 2025-07-27 09:39:33 +00:00
3cc3014ca0
Return the data the same way as on the real drone without touching channels global vairable
57 lines
1.6 KiB
C++
57 lines
1.6 KiB
C++
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
|
|
// Repository: https://github.com/okalachev/flix
|
|
|
|
// Joystick support for simulation
|
|
|
|
#include <SDL2/SDL.h>
|
|
#include <gazebo/gazebo.hh>
|
|
#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};
|
|
|
|
#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 joystickInitialized = false, warnShown = false;
|
|
|
|
void normalizeRC();
|
|
|
|
void joystickInit() {
|
|
SDL_Init(SDL_INIT_JOYSTICK);
|
|
joystick = SDL_JoystickOpen(0);
|
|
if (joystick != NULL) {
|
|
joystickInitialized = true;
|
|
gzmsg << "Joystick initialized: " << SDL_JoystickNameForIndex(0) << std::endl;
|
|
} else if (!warnShown) {
|
|
gzwarn << "Joystick not found, begin waiting for joystick..." << std::endl;
|
|
warnShown = true;
|
|
}
|
|
|
|
// apply calibration overrides
|
|
extern int channelNeutral[RC_CHANNELS];
|
|
extern int channelMax[RC_CHANNELS];
|
|
memcpy(channelNeutral, channelNeutralOverride, sizeof(channelNeutralOverride));
|
|
memcpy(channelMax, channelMaxOverride, sizeof(channelMaxOverride));
|
|
}
|
|
|
|
bool joystickGet(int16_t ch[16]) {
|
|
if (!joystickInitialized) {
|
|
joystickInit();
|
|
return false;
|
|
}
|
|
|
|
SDL_JoystickUpdate();
|
|
|
|
for (uint8_t i = 0; i < sizeof(channels) / sizeof(channels[0]); i++) {
|
|
ch[i] = SDL_JoystickGetAxis(joystick, i);
|
|
}
|
|
return true;
|
|
}
|