aleks/flix
1
0
Fork 0
mirror of https://github.com/okalachev/flix.git synced 2025-08-17 09:06:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Oleg Kalachev
2023-03-26 10:23:30 +03:00
commit e039055c8e
46 changed files with 3049 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
gazebo/CMakeLists.txt Normal file
View File

@@ -0,0 +1,16 @@
project(flix_gazebo)
cmake_minimum_required(VERSION 3.0 FATAL_ERROR)
# === gazebo plugin
find_package(gazebo REQUIRED)
find_package(SDL2 REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")
set(FLIX_SOURCE_DIR ../flix)
include_directories(${FLIX_SOURCE_DIR})
file(GLOB_RECURSE FLIX_INO_FILES ${FLIX_SOURCE_DIR}/*.ino)
add_library(flix SHARED flix.cpp)
target_link_libraries(flix ${GAZEBO_LIBRARIES} ${SDL2_LIBRARIES})

86
gazebo/arduino.hpp Normal file
View File

@@ -0,0 +1,86 @@
// 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:
// }

297
gazebo/flix.cpp Normal file
View File

@@ -0,0 +1,297 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
// https://classic.gazebosim.org/tutorials?tut=plugins_model&cat=write_plugin
// https://classic.gazebosim.org/tutorials?tut=set_velocity&cat=
// https://github.com/gazebosim/gazebo-classic/blob/gazebo11/plugins/ArduCopterPlugin.cc
// https://github.com/gazebosim/gazebo-classic/blob/gazebo11/plugins/ArduCopterPlugin.cc#L510 - motor
// https://classic.gazebosim.org/tutorials?tut=gui_overlay&cat=user_input
// https://github.com/gazebosim/gazebo-classic/blob/gazebo9/examples/plugins/gui_overlay_plugin_time/GUIExampleTimeWidget.cc
// https://github.com/yujinrobot/kobuki_desktop/blob/ea5b7283d92f61efbd1a2185b46e1ad344e7e81a/kobuki_gazebo_plugins/src/gazebo_ros_kobuki_loads.cpp#L29
// https://github.com/osrf/swarm/blob/1a2e4040b12b686ed7a13e32301d538b1c7d0b1d/src/RobotPlugin.cc#L936
// motors thrust: https://www.youtube.com/watch?v=VtKI4Pjx8Sk
// https://github.com/gazebosim/gazebo-classic/tree/master/examples/plugins
// publish to topics https://github.com/wuwushrek/sim_cf/blob/df68af275c9f753d9bf1b0494a4e513d9f4c9a7c/crazyflie_gazebo/src/gazebo_lps_plugin.cpp#L104
// https://github.com/bitcraze/crazyflie-simulation
// GUI overlay:
// https://github.com/gazebosim/gazebo-classic/blob/gazebo9/examples/plugins/gui_overlay_plugin_time/GUIExampleTimeWidget.cc
#include <functional>
#include <cmath>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/rendering/rendering.hh>
#include <gazebo/common/common.hh>
#include <gazebo/sensors/sensors.hh>
#include <gazebo/msgs/msgs.hh>
#include <ignition/math/Vector3.hh>
#include <ignition/math/Pose3.hh>
#include <ignition/math/Quaternion.hh>
#include <iostream>
#include <fstream>
#include "arduino.hpp"
#include "flix.hpp"
#include "turnigy.hpp"
#include "util.ino"
#include "estimate.ino"
#include "control.ino"
using ignition::math::Vector3d;
using ignition::math::Pose3d;
using namespace gazebo;
using namespace std;
Pose3d flu2frd(const Pose3d& p)
{
return ignition::math::Pose3d(p.Pos().X(), -p.Pos().Y(), -p.Pos().Z(),
p.Rot().W(), p.Rot().X(), -p.Rot().Y(), -p.Rot().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];
ofstream log;
public:
void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
{
this->model = _parent;
this->body = this->model->GetLink("body");
this->imu = std::dynamic_pointer_cast<sensors::ImuSensor>(sensors::get_sensor(model->GetScopedName(true) + "::body::imu")); // default::flix::body::imu
if (imu == nullptr) {
gzerr << "IMU sensor not found" << std::endl;
return;
}
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));
this->resetConnection = event::Events::ConnectWorldReset(
std::bind(&ModelFlix::OnReset, this));
initNode();
gzmsg << "Flix plugin loaded" << endl;
}
public:
void OnReset()
{
attitude = Quaternion();
gzmsg << "Flix plugin reset" << endl;
}
void OnUpdate()
{
// this->model->SetLinearVel(ignition::math::Vector3d(.3, 0, 0));
// 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);
// 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();
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
attitude.setYaw(-this->model->WorldPose().Yaw());
// Serial.print("attitude: "); Serial.println(attitude);
// controlMission();
// controlPosition();
// auto pose = flu2frd(this->model->WorldPose());
control();
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();
}
void applyMotorsThrust()
{
// thrusts
const double d = 0.035355;
const double maxThrust = 0.03 * ONE_G; // 30 g, https://www.youtube.com/watch?v=VtKI4Pjx8Sk
// 65 mm prop ~40 g
// std::cout << "fr: " << motors[MOTOR_FRONT_RIGHT]
// << " fl: " << motors[MOTOR_FRONT_LEFT]
// << " rr: " << motors[MOTOR_REAR_RIGHT]
// << " rl: " << motors[MOTOR_REAR_LEFT] << std::endl;
const float scale0 = 1.0, scale1 = 1.1, scale2 = 0.9, scale3 = 1.05;
const float minThrustRel = 0;
// apply min thrust
float mfl = mapff(motors[MOTOR_FRONT_LEFT], 0, 1, minThrustRel, 1);
float mfr = mapff(motors[MOTOR_FRONT_RIGHT], 0, 1, minThrustRel, 1);
float mrl = mapff(motors[MOTOR_REAR_LEFT], 0, 1, minThrustRel, 1);
float mrr = mapff(motors[MOTOR_REAR_RIGHT], 0, 1, minThrustRel, 1);
if (motors[MOTOR_FRONT_LEFT] < 0.001) mfl = 0;
if (motors[MOTOR_FRONT_RIGHT] < 0.001) mfr = 0;
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));
body->AddLinkForce(Vector3d(0.0, 0.0, scale1 * maxThrust * abs(mfr)), Vector3d(d, -d, 0.0));
body->AddLinkForce(Vector3d(0.0, 0.0, scale2 * maxThrust * abs(mrl)), Vector3d(-d, d, 0.0));
body->AddLinkForce(Vector3d(0.0, 0.0, scale3 * maxThrust * abs(mrr)), Vector3d(-d, -d, 0.0));
// TODO: indicate if > 1
// torque
const double maxTorque = 0.0023614413; // 24.08 g*cm
int direction = 1;
// z is counter clockwise, normal rotation direction is minus
body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale0 * maxTorque * motors[MOTOR_FRONT_LEFT]));
body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale1 * -maxTorque * motors[MOTOR_FRONT_RIGHT]));
body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale2 * -maxTorque * motors[MOTOR_REAR_LEFT]));
body->AddRelativeTorque(Vector3d(0.0, 0.0, direction * scale3 * maxTorque * motors[MOTOR_REAR_RIGHT]));
}
void updateEstimatePose() {
if (estimateModel == nullptr) {
return;
}
if (!attitude.finite()) {
// gzerr << "attitude is nan" << std::endl;
return;
}
Pose3d pose(
model->WorldPose().Pos().X(), model->WorldPose().Pos().Y(), model->WorldPose().Pos().Z(),
attitude.w, attitude.x, -attitude.y, -attitude.z // frd to flu
);
// std::cout << pose.Pos().X() << " " << pose.Pos().Y() << " " << pose.Pos().Z() <<
// " " << pose.Rot().W() << " " << pose.Rot().X() << " " << pose.Rot().Y() << " " << pose.Rot().Z() << std::endl;
// calculate attitude estimation error
float angle = Vector::angleBetweenVectors(attitude.rotate(Vector(0, 0, -1)), Vector(0, 0, -1));
if (angle < 0.3) {
//gzwarn << "att err: " << angle << endl;
// TODO: warning
// position under the floor to make it invisible
pose.SetZ(-5);
}
estimateModel->SetWorldPose(pose);
}
void initNode() {
nodeHandle = transport::NodePtr(new transport::Node());
nodeHandle->Init(); // TODO: namespace
motorPub[0] = nodeHandle->Advertise<msgs::Int>("~/motor0");
motorPub[1] = nodeHandle->Advertise<msgs::Int>("~/motor1");
motorPub[2] = nodeHandle->Advertise<msgs::Int>("~/motor2");
motorPub[3] = nodeHandle->Advertise<msgs::Int>("~/motor3");
}
void publishTopics() {
for (int i = 0; i < 4; i++) {
msgs::Int msg;
msg.set_data(static_cast<int>(std::round(motors[i] * 1000)));
motorPub[i]->Publish(msg);
}
}
void logData() {
if (!log.is_open()) return;
log << this->model->GetWorld()->SimTime() << "\t" << rollRatePID.derivative << "\t" << pitchRatePID.derivative << "\n";
}
};
GZ_REGISTER_MODEL_PLUGIN(ModelFlix)

51
gazebo/flix.hpp Normal file
View File

@@ -0,0 +1,51 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#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
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();
// control
void control();
void interpretRC();
static void controlAttitude();
static void controlAttitudeAlter();
static void controlManual();
static void controlRate();
void desaturate(float& a, float& b, float& c, float& d);
static void indicateSaturation();
// mocks
void setLED(bool on) {}

62
gazebo/flix.world Normal file
View File

@@ -0,0 +1,62 @@
<?xml version="1.0"?>
<sdf version="1.4">
<world name="default">
<scene>
<ambient>1 1 1 1</ambient>
</scene>
<gui>
<!-- <camera name="user_camera">
<pose>-1 0 0.3 0 0 0</pose>
<track_visual>
<name>flix</name>
<min_dist>2</min_dist>
<max_dist>2</max_dist>
<use_model_frame>1</use_model_frame>
<static>1</static>
<inherit_yaw>1</inherit_yaw>
</track_visual>
</camera> -->
<camera name="user_camera">
<pose>-2 -0.3 1.5 0 0.5 0.1</pose>
</camera>
</gui>
<physics type="ode">
<!-- <real_time_update_rate>100</real_time_update_rate> -->
<max_step_size>0.001</max_step_size>
</physics>
<include>
<uri>model://floor</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://flix</uri>
<pose>0 0 0.2 0 0 0</pose>
</include>
<include>
<uri>model://Table</uri>
<pose>0.2 1.5 0 0 0 0</pose>
</include>
<!-- TODO: redesign, move to flix model -->
<!-- <box><size>0.1 0.1 0.02</size></box> -->
<model name="flix_estimate">
<static>true</static>
<link name="estimate">
<visual name="estimate">
<pose>0 0 0 0 0 1.57</pose>
<geometry>
<!-- <mesh>
<uri>model://flix/plane.dae</uri>
<scale>0.003 0.003 0.003</scale>
</mesh> -->
<box>
<size>0.125711 0.125711 0.022</size>
</box>
</geometry>
</visual>
</link>
</model>
</world>
</sdf>

527
gazebo/models/flix/flix.dae Normal file
View File

File diff suppressed because one or more lines are too long

172
gazebo/models/flix/flix.sdf Normal file
View File

@@ -0,0 +1,172 @@
<?xml version="1.0"?>
<sdf version="1.5">
<model name="flix">
<static>false</static>
<link name="body">
<inertial>
<mass>0.065</mass>
<inertia>
<ixx>3.55E-5</ixx>
<iyy>4.23E-5</iyy>
<izz>7.47E-5</izz>
</inertia>
</inertial>
<collision name="collision">
<geometry>
<box>
<size>0.125711 0.125711 0.022</size>
</box>
</geometry>
</collision>
<visual name="body">
<geometry>
<mesh><uri>model://flix/flix.dae</uri></mesh>
<!-- <box>
<size>0.125711 0.125711 0.022</size>
</box> -->
<!-- led -->
</geometry>
</visual>
<!-- motors visual -->
<!-- <visual name="prop0">
<geometry>
<mesh><uri>model://flix/prop.dae</uri></mesh>
</geometry>
<pose relative_to="body">-0.035355 0.035355 0.011 0 0 0</pose>
</visual>
<visual name="prop1">
<geometry>
<mesh><uri>model://flix/prop.dae</uri></mesh>
</geometry>
<pose relative_to="body">-0.035355 -0.035355 0.011 0 0 1</pose>
</visual>
<visual name="prop2">
<geometry>
<mesh><uri>model://flix/prop.dae</uri></mesh>
</geometry>
<pose relative_to="body">0.035355 -0.035355 0.011 0 0 0</pose>
</visual>
<visual name="prop3">
<geometry>
<mesh><uri>model://flix/prop.dae</uri></mesh>
</geometry>
<pose relative_to="body">0.035355 0.035355 0.011 0 0 0</pose>
</visual> -->
<sensor name="imu" type="imu">
<always_on>1</always_on>
<visualize>1</visualize>
<update_rate>1000</update_rate>
<imu>
<angular_velocity>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.5e-2</stddev>
<!-- <bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev> -->
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.5e-2</stddev>
<!-- <bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev> -->
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>1.5e-2</stddev>
<!-- <bias_mean>0.0000075</bias_mean>
<bias_stddev>0.0000008</bias_stddev> -->
</noise>
</z>
</angular_velocity>
<linear_acceleration>
<x>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>3.5e-1</stddev>
<!-- <bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev> -->
</noise>
</x>
<y>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>3.5e-1</stddev>
<!-- <bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev> -->
</noise>
</y>
<z>
<noise type="gaussian">
<mean>0.0</mean>
<stddev>3.5e-1</stddev>
<!-- <bias_mean>0.1</bias_mean>
<bias_stddev>0.001</bias_stddev> -->
</noise>
</z>
</linear_acceleration>
</imu>
</sensor>
</link>
<!-- <link name="imu_link">
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>1e-05</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1e-05</iyy>
<iyz>0</iyz>
<izz>1e-05</izz>
</inertia>
</inertial>
<sensor name="imu" type="imu">
<always_on>1</always_on>
<visualize>1</visualize>
<update_rate>50</update_rate>
</sensor>
</link>
<joint name="hokuyo_joint" type="fixed">
<child>imu_link</child>
<parent>body</parent>
</joint> -->
<!-- <link name="motor0"></link>
<link name="motor1"></link>
<link name="motor2"></link>
<link name="motor3"></link>
<joint name="motor0_joint" type="fixed">
<parent>body</parent>
<child>motor0</child>
<pose relative_to="body">-0.035355 0.035355 0 0 0 0</pose>
</joint>
<joint name="motor1_joint" type="fixed">
<parent>body</parent>
<child>motor1</child>
<pose relative_to="body">-0.035355 -0.035355 0 0 0 0</pose>
</joint>
<joint name="motor2_joint" type="fixed">
<parent>body</parent>
<child>motor2</child>
<pose relative_to="body">0.035355 -0.035355 0 0 0 0</pose>
</joint>
<joint name="motor3_joint" type="fixed">
<parent>body</parent>
<child>motor3</child>
<pose relative_to="body">0.035355 0.035355 0 0 0 0</pose>
</joint> -->
<plugin name="flix" filename="libflix.so">
<always_on>1</always_on>
<robotNamespace></robotNamespace>
<visualize>1</visualize>
</plugin>
</model>
</sdf>

14
gazebo/models/flix/model.config Normal file
View File

@@ -0,0 +1,14 @@
<?xml version="1.0"?>
<model>
<name>flix</name>
<version>1.0</version>
<sdf version="1.5">flix.sdf</sdf>
<author>
<name>Oleg Kalachev</name>
<email>okalachev@gmail.com</email>
</author>
<license>Unknown</license>
<description>
Flix quadrotor
</description>
</model>

31
gazebo/models/floor/floor.sdf Normal file
View File

@@ -0,0 +1,31 @@
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="floor">
<static>true</static>
<link name="link">
<pose>0 0 -0.02 0 0 0</pose>
<collision name="collision">
<geometry>
<box>
<size>200 200 .02</size>
</box>
</geometry>
</collision>
<visual name="visual">
<cast_shadows>false</cast_shadows>
<geometry>
<box>
<size>200 200 .02</size>
</box>
</geometry>
<material>
<script>
<uri>model://floor/materials/scripts</uri>
<uri>model://floor/materials/textures</uri>
<name>parquet</name>
</script>
</material>
</visual>
</link>
</model>
</sdf>

20
gazebo/models/floor/materials/scripts/floor.material Normal file
View File

@@ -0,0 +1,20 @@
material parquet
{
technique
{
pass
{
ambient 0.5 0.5 0.5 1.0
diffuse 0.5 0.5 0.5 1.0
specular 0.2 0.2 0.2 1.0 12.5
texture_unit
{
texture floor.jpg
filtering anistropic
max_anisotropy 16
scale 0.01 0.01
}
}
}
}

BIN
gazebo/models/floor/materials/textures/floor.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 11 MiB

19
gazebo/models/floor/model.config Normal file
View File

@@ -0,0 +1,19 @@
<?xml version="1.0"?>
<model>
<name>Floor</name>
<version>1.0</version>
<sdf version="1.5">floor.sdf</sdf>
<author>
<name>Oleg Kalachev</name>
<email>okalachev@gmail.com</email>
</author>
<license>Unknown</license>
<description>
Floor.
</description>
</model>

70
gazebo/turnigy.hpp Normal file
View File

@@ -0,0 +1,70 @@
// Copyright (c) 2023 Oleg Kalachev <okalachev@gmail.com>
// Repository: https://github.com/okalachev/flix
#include <SDL2/SDL.h>
#include <gazebo/gazebo.hh>
#include <iostream>
using namespace std;
static const int16_t channelNeutralMin[] = {-1290, -258, -26833, 0, 0, 0};
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_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;
void turnigyInit()
{
SDL_Init(SDL_INIT_JOYSTICK);
joystick = SDL_JoystickOpen(0);
if (joystick != NULL) {
turnigyInitialized = true;
gzmsg << "Joystick initialized: " << SDL_JoystickNameForIndex(0) << endl;
} else if (!warnShown) {
gzwarn << "Joystick not found, begin waiting for joystick..." << endl;
warnShown = true;
}
}
void turnigyGet()
{
if (!turnigyInitialized) {
turnigyInit();
return;
}
SDL_JoystickUpdate();
for (uint8_t i = 0; i < 4; i++) {
channels[i] = SDL_JoystickGetAxis(joystick, i);
}
normalizeRC();
}
void normalizeRC() {
for (uint8_t i = 0; i < RC_CHANNELS; 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);
// }
} else {
controls[i] = mapf(channels[i], (channelNeutralMin[i] + channelNeutralMax[i]) / 2, channelMax[i], 0, 1);
}
}
controls[RC_CHANNEL_THROTTLE] = constrain(controls[RC_CHANNEL_THROTTLE], 0, 1);
}