-
Notifications
You must be signed in to change notification settings - Fork 6
/
drone_dynamics_ground_frame.cpp
74 lines (61 loc) · 2.8 KB
/
drone_dynamics_ground_frame.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
#include "drone_dynamics_ground_frame.hpp"
#include <math.h>
using hako::assets::drone::DroneDynamicsGroundFrame;
void DroneDynamicsGroundFrame::run_x(const DroneThrustType &thrust, const DroneTorqueType &torque)
{
(void)torque;
this->next_velocity.data.x =
this->delta_time_sec *
(
- (thrust.data / this->param_mass ) * ( this->cache.cos_phi * this->cache.sin_theta * this->cache.cos_psi + this->cache.sin_phi * this->cache.sin_psi )
- this->param_drag * this->velocity.data.x
)
+
this->velocity.data.x;
this->next_position.data.x = (this->velocity.data.x * this->delta_time_sec) + this->position.data.x;
return;
}
void DroneDynamicsGroundFrame::run_y(const DroneThrustType &thrust, const DroneTorqueType &torque)
{
(void)torque;
this->next_velocity.data.y =
this->delta_time_sec *
(
- (thrust.data / this->param_mass ) * ( this->cache.cos_phi * this->cache.sin_theta * this->cache.sin_psi - this->cache.sin_phi * this->cache.sin_psi )
- this->param_drag * this->velocity.data.y
)
+
this->velocity.data.y;
this->next_position.data.y = (this->velocity.data.y * this->delta_time_sec) + this->position.data.y;
}
void DroneDynamicsGroundFrame::run_z(const DroneThrustType &thrust, const DroneTorqueType &torque)
{
(void)torque;
this->next_velocity.data.z =
this->delta_time_sec *
(
- ( (thrust.data / this->param_mass ) * this->cache.cos_theta * this->cache.cos_phi - GRAVITY )
- this->param_drag * this->velocity.data.z
)
+
this->velocity.data.z;
this->next_position.data.z = (this->velocity.data.z * this->delta_time_sec) + this->position.data.z;
}
void DroneDynamicsGroundFrame::run_rx(const DroneThrustType &thrust, const DroneTorqueType &torque)
{
(void)thrust;
this->next_angularVelocity.data.x = (torque.data.x * this->delta_time_sec) + this->angularVelocity.data.x;
this->next_angle.data.x = (this->angularVelocity.data.x * this->delta_time_sec) + this->angle.data.x;
}
void DroneDynamicsGroundFrame::run_ry(const DroneThrustType &thrust, const DroneTorqueType &torque)
{
(void)thrust;
this->next_angularVelocity.data.y = (torque.data.y * this->delta_time_sec) + this->angularVelocity.data.y;
this->next_angle.data.y = (this->angularVelocity.data.y * this->delta_time_sec) + this->angle.data.y;
}
void DroneDynamicsGroundFrame::run_rz(const DroneThrustType &thrust, const DroneTorqueType &torque)
{
(void)thrust;
this->next_angularVelocity.data.z = (torque.data.z * this->delta_time_sec) + this->angularVelocity.data.z;
this->next_angle.data.z = (this->angularVelocity.data.z * this->delta_time_sec) + this->angle.data.z;
}