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;
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;
223 
224  // *** GETTERS and SETTERS *** //
225  // ***************************** //
229  void setHeadingRad(double rad) override
230  { yaw = stopAndTurn ? rad - M_PI : rad;}
234  double getHeadingRad() override {return yaw;}
239  glm::dvec3 & getSpeedXyByReference(){return speed_xy;}
245 
246 
256  glm::dvec3 getCurrentDirection() override;
260  bool canStopAndTurn() const override {return true;}
261 
262  // *** CONTROL STEP *** //
263  // ********************** //
264  void doControlStep(int simFrequency_hz) override;
269  double computeLiftSinkRate();
273  void computeXYSpeed(int simFrequency_hz);
277  void computeEngineForce(double zForceTarget);
283  void computeRotationAngles(int simFrequency_hz);
291  void computeAlignmentAngles();
298  void computeTurningAngles();
305  void rotate(double roll, double pitch, double yaw);
310  void handleRoute(int simFrequency_hz);
316  inline double computeSlowdownStep(double const speedMagnitude) const
317  {return cfg_slowdownFactor * speedMagnitude;}
323  inline double computeSpeedupStep(double const speedMagnitude) const
324  {return cfg_speedupFactor * speedMagnitude;}
325 };
static const glm::dvec3 up
z : up-down direction
Definition: Directions.h:26
Class representing a helicopter platform.
Definition: HelicopterPlatform.h:12
void initLeg() override
Definition: HelicopterPlatform.cpp:88
double getHeadingRad() override
Definition: HelicopterPlatform.h:234
void setHeadingRad(double rad) override
Definition: HelicopterPlatform.h:229
bool cache_aligning
Definition: HelicopterPlatform.h:168
double computeLiftSinkRate()
Compute the lift/sink rate.
Definition: HelicopterPlatform.cpp:214
double roll
Helicopter roll angle.
Definition: HelicopterPlatform.h:40
Rotation getDirectionalAttitude() override
Directional attitude for HelicopterPlatform considers only the XY direction.
Definition: HelicopterPlatform.h:252
double cache_xyDistanceThreshold
XY distance threshold to handle turning computation.
Definition: HelicopterPlatform.h:172
void computeAlignmentAngles()
Compute the rotation angles for alignment process.
Definition: HelicopterPlatform.cpp:303
void doControlStep(int simFrequency_hz) override
Control step for simple phyisics platform simulation.
Definition: HelicopterPlatform.cpp:200
double lastSign
Helicopter rotation sign coming from previous simulation step.
Definition: HelicopterPlatform.h:48
double cfg_pitch_speed
Pitch rotation speed (units per second)
Definition: HelicopterPlatform.h:64
double yaw
Helicopter yaw angle.
Definition: HelicopterPlatform.h:36
void prepareSimulation(int simFrequency_hz) override
Definition: HelicopterPlatform.cpp:54
Rotation & getRotationByReference()
Obtain rotation assistance r by reference.
Definition: HelicopterPlatform.h:244
double pitch
Helicopter pitch angle.
Definition: HelicopterPlatform.h:44
double cfg_max_pitch_offset
Maximum boundary for pitch angle with respect to base pitch.
Definition: HelicopterPlatform.h:72
double cfg_speedupFactor
Helicopter speedup factor.
Definition: HelicopterPlatform.h:98
bool cache_speedUpFinished
Flag specifying if helicopter speed-up stage has finished (true) or not (false)
Definition: HelicopterPlatform.h:177
void updateStaticCache() override
Definition: HelicopterPlatform.cpp:114
void initLegManual() override
Definition: HelicopterPlatform.cpp:64
double ef_xy_max
Max engine force over XY plane.
Definition: HelicopterPlatform.h:32
void handleRoute(int simFrequency_hz)
Determine remaining iterations and start turning mode if necessary.
Definition: HelicopterPlatform.cpp:401
bool canStopAndTurn() const override
Definition: HelicopterPlatform.h:260
HelicopterPlatform()=default
Default helicopter platform constructor.
glm::dvec3 & getSpeedXyByReference()
Obtain xy speed vector by reference.
Definition: HelicopterPlatform.h:239
void computeRotationAngles(int simFrequency_hz)
Compute rotation angles (roll, pitch, yaw)
Definition: HelicopterPlatform.cpp:282
double computeSlowdownStep(double const speedMagnitude) const
Compute speed magnitude after a slowdown step.
Definition: HelicopterPlatform.h:316
int cache_turnIterations
Count of turn iterations.
Definition: HelicopterPlatform.h:158
double cfg_pitchStepMagnitude
Pitch step magnitude.
Definition: HelicopterPlatform.h:107
void computeTurningAngles()
Compute the rotation angles for turning process.
Definition: HelicopterPlatform.cpp:328
bool waypointReached() override
Definition: HelicopterPlatform.cpp:92
double cfg_pitch_base
Base pitch angle for the helicopter.
Definition: HelicopterPlatform.h:52
void computeEngineForce(double zForceTarget)
Compute the engine force.
Definition: HelicopterPlatform.cpp:277
double cfg_alignmentThreshold
Helicopter alignment threshold. A helicopter will be considered as aligned when the difference betwee...
Definition: HelicopterPlatform.h:131
void computeNonSmoothSlowdownDist()
Compute the slowdown distance. This is necessary when using the slowdown mode with no smooth turn....
Definition: HelicopterPlatform.cpp:170
bool cache_turning
Flag specifying if helicopter is currently turning (true) or not (false)
Definition: HelicopterPlatform.h:163
double cfg_roll_speed
Roll rotation speed (units per second)
Definition: HelicopterPlatform.h:60
double computeSpeedupStep(double const speedMagnitude) const
Compute speed magnitude after a speedup step.
Definition: HelicopterPlatform.h:323
glm::dvec3 speed_xy
Helicopter speed vector over xy plane.
Definition: HelicopterPlatform.h:136
double cfg_max_pitch
Maximum boundary for pitch angle (radians)
Definition: HelicopterPlatform.h:76
double cfg_min_pitch
Minimum boundary for pitch angle (radians)
Definition: HelicopterPlatform.h:80
glm::dvec3 getCurrentDirection() override
Definition: HelicopterPlatform.cpp:121
void rotate(double roll, double pitch, double yaw)
Rotate helicopter.
Definition: HelicopterPlatform.cpp:383
glm::dvec3 lastSpeed_xy
Helicopter speed vector over xy plane from previous simulation step.
Definition: HelicopterPlatform.h:141
Rotation dirAttitudeXY
Directional attitude over XY plane for HelicopterPlatform.
Definition: HelicopterPlatform.h:151
double cfg_max_roll_offset
Maximum boundary for roll angle with respect to base roll.
Definition: HelicopterPlatform.h:68
double cfg_slowdown_magnitude
Slowdown step magnitude.
Definition: HelicopterPlatform.h:24
void computeTurnDistanceThreshold()
Compute the distance threshold with respect to expected turn.
Definition: HelicopterPlatform.cpp:130
double cfg_slowdown_dist_xy
Distance threshold to start slowdown process when not in smooth turn mode.
Definition: HelicopterPlatform.h:20
double cfg_rollStepMagnitude
Roll step magnitude.
Definition: HelicopterPlatform.h:116
double cfg_yawStepMagnitude
Yaw step magnitude.
Definition: HelicopterPlatform.h:125
void computeXYSpeed(int simFrequency_hz)
Compute the speed through XY and its impact on pitch.
Definition: HelicopterPlatform.cpp:233
Rotation r
Rotation instance to assist helicopter rotation computation.
Definition: HelicopterPlatform.h:146
double cfg_yaw_speed
Yaw rotation speed (units per second)
Definition: HelicopterPlatform.h:56
double cfg_slowdownFactor
Helicopter slowdown factor.
Definition: HelicopterPlatform.h:89
double cfg_speedup_magnitude
Speedup step magnitude.
Definition: HelicopterPlatform.h:28
bool stopAndTurn
Flag to specify if platform must work in stop and turn mode (true) or not (false)....
Definition: Platform.h:117
Definition: Rotation.h:80
Class representing a simple phyiscs platform.
Definition: SimplePhysicsPlatform.h:8