Helios++
Helios software for LiDAR simulations
HelicopterPlatform.h
1 #pragma once
2 
3 #include "SimplePhysicsPlatform.h"
4 
5 #include <glm/glm.hpp>
6 
7 #include "maths/Rotation.h"
8 
13 public:
14  // *** ATTRIBUTES *** //
15  // ******************** //
20  double cfg_slowdown_dist_xy = 5.0;
24  double cfg_slowdown_magnitude = 2.0;
28  double cfg_speedup_magnitude = 2.0;
32  double ef_xy_max = 0.1;
36  double yaw = 0.0;
40  double roll = 0.0;
44  double pitch = 0.0;
48  double lastSign = 1.0;
52  double cfg_pitch_base = -0.087;
56  double cfg_yaw_speed = 1.5;
60  double cfg_roll_speed = 0.5;
64  double cfg_pitch_speed = 1.5;
68  double cfg_max_roll_offset = 0.45;
72  double cfg_max_pitch_offset = 0.61;
76  double cfg_max_pitch = cfg_pitch_base + cfg_max_pitch_offset;
80  double cfg_min_pitch = cfg_pitch_base - cfg_max_pitch_offset;
131  double cfg_alignmentThreshold = 0.001;
132 
136  glm::dvec3 speed_xy = glm::dvec3(0, 0, 0);
141  glm::dvec3 lastSpeed_xy = glm::dvec3(0, 0, 0);
142 
146  Rotation r = Rotation(glm::dvec3(1, 0, 0), 0);
152 
153  // *** CACHE ATTRIBUTES *** //
154  // ************************** //
163  bool cache_turning = false;
168  bool cache_aligning = false;
177  bool cache_speedUpFinished = false;
178 
179 public:
180  // *** CONSTRUCTION / DESTRUCTION *** //
181  // ************************************ //
185  HelicopterPlatform() = default;
186  std::shared_ptr<Platform> clone() override;
187  void _clone(std::shared_ptr<Platform> p) override;
188 
189  // *** M E T H O D S *** //
190  // *********************** //
194  void prepareSimulation(int simFrequency_hz) override;
198  void initLegManual() override;
202  void initLeg() override;
206  bool waypointReached() override;
210  void updateStaticCache() override;
215 
216  // *** GETTERS and SETTERS *** //
217  // ***************************** //
221  void setHeadingRad(double rad) override
222  { yaw = stopAndTurn ? rad - M_PI : rad;}
226  double getHeadingRad() override {return yaw;}
231  glm::dvec3 & getSpeedXyByReference(){return speed_xy;}
237 
238 
248  glm::dvec3 getCurrentDirection() override;
252  bool canStopAndTurn() override {return true;}
253 
254  // *** CONTROL STEP *** //
255  // ********************** //
256  void doControlStep(int simFrequency_hz) override;
261  double computeLiftSinkRate();
265  void computeXYSpeed(int simFrequency_hz);
269  void computeEngineForce(double zForceTarget);
275  void computeRotationAngles(int simFrequency_hz);
283  void computeAlignmentAngles();
290  void computeTurningAngles();
297  void rotate(double roll, double pitch, double yaw);
302  void handleRoute(int simFrequency_hz);
308  inline double computeSlowdownStep(double speedMagnitude)
309  {return cfg_slowdownFactor * speedMagnitude;}
315  inline double computeSpeedupStep(double speedMagnitude)
316  {return cfg_speedupFactor * speedMagnitude;}
317 };
void computeRotationAngles(int simFrequency_hz)
Compute rotation angles (roll, pitch, yaw)
Definition: HelicopterPlatform.cpp:247
double cfg_roll_speed
Roll rotation speed (units per second)
Definition: HelicopterPlatform.h:60
double cfg_slowdown_dist_xy
Distance threshold to start slowdown process when not in smooth turn mode.
Definition: HelicopterPlatform.h:20
void computeTurnDistanceThreshold()
Compute the distance threshold with respect to expected turn.
Definition: HelicopterPlatform.cpp:126
Class representing a simple phyiscs platform.
Definition: SimplePhysicsPlatform.h:8
void initLegManual() override
Definition: HelicopterPlatform.cpp:63
Rotation dirAttitudeXY
Directional attitude over XY plane for HelicopterPlatform.
Definition: HelicopterPlatform.h:151
double lastSign
Helicopter rotation sign coming from previous simulation step.
Definition: HelicopterPlatform.h:48
double cfg_min_pitch
Minimum boundary for pitch angle (radians)
Definition: HelicopterPlatform.h:80
double cfg_alignmentThreshold
Helicopter alignment threshold. A helicopter will be considered as aligned when the difference betwee...
Definition: HelicopterPlatform.h:131
double computeSpeedupStep(double speedMagnitude)
Compute speed magnitude after a speedup step.
Definition: HelicopterPlatform.h:315
double pitch
Helicopter pitch angle.
Definition: HelicopterPlatform.h:44
glm::dvec3 getCurrentDirection() override
Definition: HelicopterPlatform.cpp:116
void doControlStep(int simFrequency_hz) override
Control step for simple phyisics platform simulation.
Definition: HelicopterPlatform.cpp:168
double cfg_pitch_speed
Pitch rotation speed (units per second)
Definition: HelicopterPlatform.h:64
bool cache_turning
Flag specifying if helicopter is currently turning (true) or not (false)
Definition: HelicopterPlatform.h:163
double yaw
Helicopter yaw angle.
Definition: HelicopterPlatform.h:36
Definition: Rotation.h:80
void handleRoute(int simFrequency_hz)
Determine remaining iterations and start turning mode if necessary.
Definition: HelicopterPlatform.cpp:366
glm::dvec3 speed_xy
Helicopter speed vector over xy plane.
Definition: HelicopterPlatform.h:136
double ef_xy_max
Max engine force over XY plane.
Definition: HelicopterPlatform.h:32
void setHeadingRad(double rad) override
Definition: HelicopterPlatform.h:221
void computeAlignmentAngles()
Compute the rotation angles for alignment process.
Definition: HelicopterPlatform.cpp:268
int cache_turnIterations
Count of turn iterations.
Definition: HelicopterPlatform.h:158
glm::dvec3 & getSpeedXyByReference()
Obtain xy speed vector by reference.
Definition: HelicopterPlatform.h:231
void initLeg() override
Definition: HelicopterPlatform.cpp:87
void rotate(double roll, double pitch, double yaw)
Rotate helicopter.
Definition: HelicopterPlatform.cpp:348
bool cache_speedUpFinished
Flag specifying if helicopter speed-up stage has finished (true) or not (false)
Definition: HelicopterPlatform.h:177
bool canStopAndTurn() override
Definition: HelicopterPlatform.h:252
double cfg_max_roll_offset
Maximum boundary for roll angle with respect to base roll.
Definition: HelicopterPlatform.h:68
void updateStaticCache() override
Definition: HelicopterPlatform.cpp:110
bool stopAndTurn
Flag to specify if platform must work in stop and turn mode (true) or not (false). Not all platforms support this mode, so it will only be used when possible.
Definition: Platform.h:115
double cfg_max_pitch_offset
Maximum boundary for pitch angle with respect to base pitch.
Definition: HelicopterPlatform.h:72
double cfg_yawStepMagnitude
Yaw step magnitude.
Definition: HelicopterPlatform.h:125
void prepareSimulation(int simFrequency_hz) override
Definition: HelicopterPlatform.cpp:54
Rotation r
Rotation instance to assist helicopter rotation computation.
Definition: HelicopterPlatform.h:146
double cfg_speedup_magnitude
Speedup step magnitude.
Definition: HelicopterPlatform.h:28
Rotation getDirectionalAttitude() override
Directional attitude for HelicopterPlatform considers only the XY direction.
Definition: HelicopterPlatform.h:244
double getHeadingRad() override
Definition: HelicopterPlatform.h:226
Rotation & getRotationByReference()
Obtain rotation assistance r by reference.
Definition: HelicopterPlatform.h:236
bool cache_aligning
Definition: HelicopterPlatform.h:168
double cfg_rollStepMagnitude
Roll step magnitude.
Definition: HelicopterPlatform.h:116
void computeEngineForce(double zForceTarget)
Compute the engine force.
Definition: HelicopterPlatform.cpp:242
HelicopterPlatform()=default
Default helicopter platform constructor.
double computeSlowdownStep(double speedMagnitude)
Compute speed magnitude after a slowdown step.
Definition: HelicopterPlatform.h:308
glm::dvec3 lastSpeed_xy
Helicopter speed vector over xy plane from previous simulation step.
Definition: HelicopterPlatform.h:141
void computeXYSpeed(int simFrequency_hz)
Compute the speed through XY and its impact on pitch.
Definition: HelicopterPlatform.cpp:201
void computeTurningAngles()
Compute the rotation angles for turning process.
Definition: HelicopterPlatform.cpp:293
double roll
Helicopter roll angle.
Definition: HelicopterPlatform.h:40
double cfg_speedupFactor
Helicopter speedup factor.
Definition: HelicopterPlatform.h:98
Class representing a helicopter platform.
Definition: HelicopterPlatform.h:12
double cfg_max_pitch
Maximum boundary for pitch angle (radians)
Definition: HelicopterPlatform.h:76
bool waypointReached() override
Definition: HelicopterPlatform.cpp:91
double cfg_pitchStepMagnitude
Pitch step magnitude.
Definition: HelicopterPlatform.h:107
static const glm::dvec3 up
z : up-down direction
Definition: Directions.h:25
double computeLiftSinkRate()
Compute the lift/sink rate.
Definition: HelicopterPlatform.cpp:182
double cfg_slowdown_magnitude
Slowdown step magnitude.
Definition: HelicopterPlatform.h:24
double cfg_yaw_speed
Yaw rotation speed (units per second)
Definition: HelicopterPlatform.h:56
double cache_xyDistanceThreshold
XY distance threshold to handle turning computation.
Definition: HelicopterPlatform.h:172
double cfg_slowdownFactor
Helicopter slowdown factor.
Definition: HelicopterPlatform.h:89
double cfg_pitch_base
Base pitch angle for the helicopter.
Definition: HelicopterPlatform.h:52