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

Enable Serial input and output in simulator, refactor

Browse Source
This commit is contained in:
Oleg Kalachev 2023-05-24 10:23:12 +03:00
parent 3a05403068
commit aaa8f70166
8 changed files with 225 additions and 210 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
flix/motors.ino
View File

@ -128,4 +128,5 @@ static float thrustToMotorInput(uint8_t n, float thrust)
return mapff(thrust, 0, 1, motorThrust[n][i-1], motorThrust[n][i]);
}
}
return 1;
}

150
gazebo/Arduino.h Normal file
View File

@ -0,0 +1,150 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// Distributed under the MIT License (https://opensource.org/licenses/MIT)
// Partial implementation of Arduino API for simulation
// https://github.com/UFABCRocketDesign/Simulator
#pragma once
#include <iostream>
#include <cmath>
#include <string>
#include <stdio.h>
#include <unistd.h>
#include <sys/poll.h>
using std::cout;
using std::max;
using std::min;
using std::isfinite;
#define PI 3.1415926535897932384626433832795
#define DEG_TO_RAD 0.017453292519943295769236907684886
#define RAD_TO_DEG 57.295779513082320876798154814105
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
long map(long x, long in_min, long in_max, long out_min, long out_max) {
const long run = in_max - in_min;
const long rise = out_max - out_min;
const long delta = x - in_min;
return (delta * rise) / run + out_min;
}
class __FlashStringHelper;
// Arduino String partial implementation
// https://www.arduino.cc/reference/en/language/variables/data-types/stringobject/
class String: public std::string {
public:
long toInt() { return atol(this->c_str()); }
float toFloat() { return atof(this->c_str()); }
bool isEmpty() { return this->empty(); }
};
class Print;
class Printable {
public:
virtual size_t printTo(Print& p) const = 0;
};
class Print {
public:
size_t printf(const char *format, ...)
{
va_list args;
va_start(args, format);
size_t result = vprintf(format, args);
va_end(args);
return result;
}
size_t print(float n, int digits = 2)
{
return printf("%.*f", digits, n);
}
size_t println(float n, int digits = 2)
{
return printf("%.*f\n", digits, n);
}
size_t print(const char* s)
{
return printf("%s", s);
}
size_t println()
{
return print("\n");
}
size_t println(const char* s)
{
return printf("%s\n", s);
}
size_t println(const Printable& p)
{
return p.printTo(*this) + print("\n");
}
size_t print(const String& s)
{
return printf("%s", s.c_str());
}
size_t println(const std::string& s)
{
return printf("%s\n", s.c_str());
}
size_t println(const String& s)
{
return printf("%s\n", s.c_str());
}
};
class HardwareSerial: public Print {
public:
void begin(unsigned long baud) {
// server is running in background by default, so doesn't have access to stdin
// https://github.com/gazebosim/gazebo-classic/blob/d45feeb51f773e63960616880b0544770b8d1ad7/gazebo/gazebo_main.cc#L216
// set foreground process group to current process group to allow reading from stdin
// https://stackoverflow.com/questions/58918188/why-is-stdin-not-propagated-to-child-process-of-different-process-group
signal(SIGTTOU, SIG_IGN);
tcsetpgrp(STDIN_FILENO, getpgrp());
signal(SIGTTOU, SIG_DFL);
};
int available() {
// to implement for Windows, see https://stackoverflow.com/a/71992965/6850197
struct pollfd pfd = { .fd = STDIN_FILENO, .events = POLLIN };
return poll(&pfd, 1, 0) > 0;
}
int read() {
if (available()) {
char c;
::read(STDIN_FILENO, &c, 1); // use raw read to avoid C++ buffering
// https://stackoverflow.com/questions/45238997/does-getchar-function-has-its-own-buffer-to-store-remaining-input
return c;
}
return -1;
}
};
HardwareSerial Serial, Serial2;
void delay(uint32_t ms) {
std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}
unsigned long __micros;
unsigned long micros() {
return __micros;
}

2
gazebo/CMakeLists.txt
View File

@ -12,5 +12,7 @@ set(FLIX_SOURCE_DIR ../flix)
include_directories(${FLIX_SOURCE_DIR})
file(GLOB_RECURSE FLIX_INO_FILES ${FLIX_SOURCE_DIR}/*.ino)
set(CMAKE_BUILD_TYPE RelWithDebInfo)
add_library(flix SHARED flix.cpp)
target_link_libraries(flix ${GAZEBO_LIBRARIES} ${SDL2_LIBRARIES})
target_include_directories(flix PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

17
gazebo/MPU9250.h Normal file
View File

@ -0,0 +1,17 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// Distributed under the MIT License (https://opensource.org/licenses/MIT)
// Mocks of some MPU9250 library functions
#pragma once
class MPU9250 {
public:
float getAccelBiasX_mss() { return 0; }
float getAccelBiasY_mss() { return 0; }
float getAccelBiasZ_mss() { return 0; }
float getGyroBiasX_rads() { return 0; }
float getGyroBiasY_rads() { return 0; }
float getGyroBiasZ_rads() { return 0; }
};

86
gazebo/arduino.hpp
View File

@ -1,86 +0,0 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#include <iostream>
#include <cmath>
#include <string>
using std::cout;
using std::max;
using std::min;
using std::isfinite;
// #define PI 3.1415926535897932384626433832795
#define DEG_TO_RAD 0.017453292519943295769236907684886
#define RAD_TO_DEG 57.295779513082320876798154814105
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))
class Print;
class Printable {
public:
virtual size_t printTo(Print& p) const = 0;
};
class Print {
public:
size_t print(float n, int digits = 2)
{
cout << n;
return 0; // TODO:
}
size_t println(float n, int digits = 2)
{
print(n, digits);
cout << std::endl;
return 0;
}
size_t print(const char* s)
{
cout << s;
return 0;
}
size_t println(const char* s)
{
print(s);
cout << std::endl;
return 0;
}
size_t println(const Printable& p)
{
p.printTo(*this);
cout << std::endl;
return 0;
}
// int available()
// {
// std::string s;
// s << std::cin;
// return s.length();
// }
// char read()
// {
// char c;
// s >> c;
// return c;
// }
};
class HardwareSerial: public Print {};
HardwareSerial Serial, Serial2;
// gazebo::common::Time curTime = this->dataPtr->model->GetWorld()->SimTime();
// unsigned long micros()
// {
// // return (unsigned long) (esp_timer_get_time());
// TODO:
// }

108
gazebo/flix.cpp
View File

@ -34,12 +34,15 @@
#include <iostream>
#include <fstream>
#include "arduino.hpp"
#include "Arduino.h"
#include "flix.hpp"
#include "turnigy.hpp"
#include "util.ino"
#include "time.ino"
#include "turnigy.hpp"
#include "estimate.ino"
#include "control.ino"
#include "log.ino"
#include "cli.ino"
using ignition::math::Vector3d;
using ignition::math::Pose3d;
@ -52,13 +55,17 @@ Pose3d flu2frd(const Pose3d& p)
p.Rot().W(), p.Rot().X(), -p.Rot().Y(), -p.Rot().Z());
}
Vector flu2frd(const Vector3d& v)
{
return Vector(v.X(), -v.Y(), -v.Z());
}
class ModelFlix : public ModelPlugin
{
private:
physics::ModelPtr model, estimateModel;
physics::LinkPtr body;
sensors::ImuSensorPtr imu;
common::Time _stepTime;
event::ConnectionPtr updateConnection, resetConnection;
transport::NodePtr nodeHandle;
transport::PublisherPtr motorPub[4];
@ -78,13 +85,6 @@ public:
this->estimateModel = model->GetWorld()->ModelByName("flix_estimate");
// auto scene = rendering::get_scene();
// motorVisual[0] = scene->GetVisual("motor0");
// motorVisual[1] = scene->GetVisual("motor1");
// motorVisual[2] = scene->GetVisual("motor2");
// motorVisual[3] = scene->GetVisual("motor3");
// motorVisual[0] = model->GetVisual("motor0");
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
std::bind(&ModelFlix::OnUpdate, this));
@ -93,6 +93,8 @@ public:
initNode();
Serial.begin(0);
gzmsg << "Flix plugin loaded" << endl;
}
@ -105,92 +107,28 @@ public:
void OnUpdate()
{
// this->model->SetLinearVel(ignition::math::Vector3d(.3, 0, 0));
__micros = model->GetWorld()->SimTime().Double() * 1000000;
step();
// this->model->GetLink("body")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96));
// this->model->GetLink("body")->SetTorque(1.0, ignition::math::Vector3d(1.0, 0.0, 0.0);
// read imu
rates = flu2frd(imu->AngularVelocity());
acc = flu2frd(imu->LinearAcceleration());
// this->model->GetLink("motor0")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
// this->model->GetLink("motor1")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
// this->model->GetLink("motor2")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4));
// this->model->GetLink("motor3")->AddForce(ignition::math::Vector3d(0.0, 0.0, 1.96/4))
// === GROUNDTRUTH ORIENTATION ===
// auto pose = flu2frd(this->model->WorldPose());
// attitude = Quaternion(pose.Rot().W(), pose.Rot().X(), pose.Rot().Y(), pose.Rot().Z());
// auto vel = this->model->RelativeAngularVel();
// rates = Vector(vel.X(), -vel.Y(), -vel.Z()); // flu to frd
// === GROUNDTRUTH POSITION ===
// auto pose = flu2frd(this->model->WorldPose());
// position = Vector(pose.Pos().X(), pose.Pos().Y(), pose.Pos().Z());
// auto vel = this->model->RelativeLinearVel();
// velocity = Vector(vel.X(), -vel.Y(), -vel.Z());
/// == IMU ===
auto imuRates = imu->AngularVelocity();
rates = Vector(imuRates.X(), -imuRates.Y(), -imuRates.Z()); // flu to frd
auto imuAccel = imu->LinearAcceleration();
acc = Vector(imuAccel.X(), -imuAccel.Y(), -imuAccel.Z()); // flu to frd
// gazebo::common::Time curTime = this->dataPtr->model->GetWorld()->SimTime();
turnigyGet();
// read rc
joystickGet();
controls[RC_CHANNEL_MODE] = 1; // 0 acro, 1 stab
controls[RC_CHANNEL_AUX] = 1; // armed
// std::cout << "yaw: " << yaw << " pitch: " << pitch << " roll: " << roll << " throttle: " << throttle << std::endl;
if (std::isnan(dt)) {
// first step
dt = 0;
} else {
dt = (this->model->GetWorld()->SimTime() - _stepTime).Double();
}
_stepTime = this->model->GetWorld()->SimTime();
stepTime = _stepTime.Double() * 1000000;
// std::cout << "dt: " << dt << std::endl;
estimate();
// fix yaw
// correct yaw to the actual yaw
attitude.setYaw(-this->model->WorldPose().Yaw());
// Serial.print("attitude: "); Serial.println(attitude);
// controlMission();
// controlPosition();
// auto pose = flu2frd(this->model->WorldPose());
control();
parseInput();
applyMotorsThrust();
updateEstimatePose();
// autotune();
/*
working:
const double max_thrust = 2.2;
double thrust = max_thrust * throttle;
// rate setpoint
double rx = roll * 0.1;
double ry = -pitch * 0.1;
double rz = yaw * 0.1;
// this->model->GetLink("body")->AddForce(ignition::math::Vector3d(0.0, 0.0, 2.5 * throttle));
const double d = 0.035355;
double front_left = thrust + ry + rx;
double front_right = thrust + ry - rx;
double rear_left = thrust - ry + rx;
double rear_right = thrust - ry - rx;
if (throttle < 0.1) return;
this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, front_left), ignition::math::Vector3d(d, d, 0.0));
this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, front_right), ignition::math::Vector3d(d, -d, 0.0));
this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, rear_left), ignition::math::Vector3d(-d, d, 0.0));
this->body->AddLinkForce(ignition::math::Vector3d(0.0, 0.0, rear_right), ignition::math::Vector3d(-d, -d, 0.0));
*/
publishTopics();
logData();
}
@ -221,8 +159,6 @@ public:
if (motors[MOTOR_REAR_LEFT] < 0.001) mrl = 0;
if (motors[MOTOR_REAR_RIGHT] < 0.001) mrr = 0;
// const float scale0 = 1.0, scale1 = 1.0, scale2 = 1.0, scale3 = 1.0;
// TODO: min_thrust
body->AddLinkForce(Vector3d(0.0, 0.0, scale0 * maxThrust * abs(mfl)), Vector3d(d, d, 0.0));

41
gazebo/flix.hpp
View File

@ -1,41 +1,31 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// Declarations of some functions and variables in Arduino code
#include <cmath>
#include "vector.hpp"
#include "quaternion.hpp"
#define ONE_G 9.807f
#define RC_CHANNELS 6
// #define RC_CHANNEL_THROTTLE 2
// #define RC_CHANNEL_YAW 3
// #define RC_CHANNEL_PITCH 1
// #define RC_CHANNEL_ROLL 0
// #define RC_CHANNEL_AUX 4
// #define RC_CHANNEL_MODE 5
#define MOTOR_REAR_LEFT 0
#define MOTOR_FRONT_LEFT 3
#define MOTOR_FRONT_RIGHT 2
#define MOTOR_REAR_RIGHT 1
uint32_t startTime;
uint32_t stepTime;
uint32_t steps;
float stepsPerSecond;
float dt;
float motors[4];
int16_t channels[16]; // raw rc channels WARNING: unsigned on hardware
float controls[RC_CHANNELS];
Vector acc;
Vector rates;
Quaternion attitude;
float dt = NAN;
float motors[4]; // normalized motors thrust in range [-1..1]
int16_t channels[16]; // raw rc channels WARNING: unsigned in real life
float controls[RC_CHANNELS]; // normalized controls in range [-1..1] ([0..1] for thrust)
uint32_t stepTime;
// util
float mapf(long x, long in_min, long in_max, float out_min, float out_max);
float mapff(float x, float in_min, float in_max, float out_min, float out_max);
// float hypot3(float x, float y, float z);
// rc
void normalizeRC();
bool calibrating;
// control
void control();
@ -47,5 +37,12 @@ static void controlRate();
void desaturate(float& a, float& b, float& c, float& d);
static void indicateSaturation();
// cli
static void showTable();
static void cliTestMotor(uint8_t n);
// mocks
void setLED(bool on) {}
void setLED(bool on) {};
void calibrateGyro() {};
void fullMotorTest(int n) {};
void sendMotors() {};

30
gazebo/turnigy.hpp
View File

@ -12,23 +12,25 @@ static const int16_t channelNeutralMax[] = {-1032, -258, -27348, 3353, 0, 0};
static const int16_t channelMax[] = {27090, 27090, 27090, 27090, 0, 0};
#define RC_CHANNEL_ROLL 0
#define RC_CHANNEL_PITCH 1
#define RC_CHANNEL_THROTTLE 2
#define RC_CHANNEL_YAW 3
#define RC_CHANNEL_PITCH 1
#define RC_CHANNEL_ROLL 0
#define RC_CHANNEL_AUX 4
#define RC_CHANNEL_MODE 5
static SDL_Joystick *joystick;
bool turnigyInitialized = false, warnShown = false;
bool joystickInitialized = false, warnShown = false;
void turnigyInit()
void normalizeRC();
void joystickInit()
{
SDL_Init(SDL_INIT_JOYSTICK);
joystick = SDL_JoystickOpen(0);
if (joystick != NULL) {
turnigyInitialized = true;
joystickInitialized = true;
gzmsg << "Joystick initialized: " << SDL_JoystickNameForIndex(0) << endl;
} else if (!warnShown) {
gzwarn << "Joystick not found, begin waiting for joystick..." << endl;
@ -36,10 +38,10 @@ void turnigyInit()
}
}
void turnigyGet()
void joystickGet()
{
if (!turnigyInitialized) {
turnigyInit();
if (!joystickInitialized) {
joystickInit();
return;
}
@ -48,20 +50,16 @@ void turnigyGet()
for (uint8_t i = 0; i < 4; i++) {
channels[i] = SDL_JoystickGetAxis(joystick, i);
}
channels[RC_CHANNEL_MODE] = SDL_JoystickGetButton(joystick, 0) ? 1 : 0;
controls[RC_CHANNEL_MODE] = channels[RC_CHANNEL_MODE];
normalizeRC();
}
void normalizeRC() {
for (uint8_t i = 0; i < RC_CHANNELS; i++) {
for (uint8_t i = 0; i < 4; i++) {
if (channels[i] >= channelNeutralMin[i] && channels[i] <= channelNeutralMax[i]) {
controls[i] = 0; // make neutral position strictly equal to 0
// } else if (channels[i] > channelNeutralMax[i]) {
// controls[i] = mapf(channels[i], channelNeutralMax[i], channelMax[i], 0, 1);
// } else {
// float channelMin = channelNeutralMin[i] - (channelMax[i] - channelNeutralMax[i]);
// controls[i] = mapf(channels[i], channelNeutralMin[i], channelMin, -1, 0);
// }
controls[i] = 0;
} else {
controls[i] = mapf(channels[i], (channelNeutralMin[i] + channelNeutralMax[i]) / 2, channelMax[i], 0, 1);
}