Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : simple electric motor simulation class
17 : */
18 :
19 : #pragma once
20 :
21 : #include <AP_Math/AP_Math.h>
22 : #include <SITL/SITL_Input.h>
23 :
24 : namespace SITL {
25 :
26 : /*
27 : class to describe a motor position
28 : */
29 : class Motor {
30 : public:
31 : float angle;
32 : float yaw_factor;
33 : uint8_t servo;
34 : uint8_t display_order;
35 :
36 : // support for tilting motors
37 : int8_t roll_servo = -1;
38 : float roll_min, roll_max;
39 : int8_t pitch_servo = -1;
40 : float pitch_min, pitch_max;
41 :
42 : // support for variable-pitch (collective) rotors; the motor's
43 : // own servo channel commands blade pitch and rotor speed comes
44 : // from a shared rotor-speed-control servo
45 : int8_t rsc_servo = -1;
46 :
47 : // support for servo slew rate
48 : enum {SERVO_NORMAL, SERVO_RETRACT} servo_type;
49 : float servo_rate = 0.24; // seconds per 60 degrees
50 : uint64_t last_change_usec;
51 : float last_roll_value, last_pitch_value;
52 :
53 456885 : Motor(uint8_t _servo, float _angle, float _yaw_factor, uint8_t _display_order) :
54 456885 : angle(_angle), // angle in degrees from front
55 456885 : yaw_factor(_yaw_factor), // positive is clockwise
56 456885 : servo(_servo), // what servo output drives this motor
57 456885 : display_order(_display_order) // order for clockwise display
58 : {
59 456885 : position.x = cosf(radians(angle));
60 456885 : position.y = sinf(radians(angle));
61 456885 : position.z = 0;
62 :
63 456885 : thrust_vector.x = 0;
64 456885 : thrust_vector.y = 0;
65 456885 : thrust_vector.z = -1;
66 456885 : }
67 :
68 : /*
69 : alternative constructor for variable-pitch rotors
70 : */
71 8580 : Motor(uint8_t _servo, float _angle, float _yaw_factor, uint8_t _display_order,
72 8580 : int8_t _rsc_servo) :
73 8580 : angle(_angle), // angle in degrees from front
74 8580 : yaw_factor(_yaw_factor), // positive is clockwise
75 8580 : servo(_servo), // what servo output drives this motor's blade pitch
76 8580 : display_order(_display_order), // order for clockwise display
77 8580 : rsc_servo(_rsc_servo) // what servo output drives the rotor speed
78 : {
79 8580 : position.x = cosf(radians(angle));
80 8580 : position.y = sinf(radians(angle));
81 8580 : position.z = 0;
82 :
83 8580 : thrust_vector.x = 0;
84 8580 : thrust_vector.y = 0;
85 8580 : thrust_vector.z = -1;
86 8580 : }
87 :
88 : /*
89 : alternative constructor for tiltable motors
90 : */
91 34320 : Motor(uint8_t _servo, float _angle, float _yaw_factor, uint8_t _display_order,
92 : int8_t _roll_servo, float _roll_min, float _roll_max,
93 34320 : int8_t _pitch_servo, float _pitch_min, float _pitch_max) :
94 34320 : angle(_angle), // angle in degrees from front
95 34320 : yaw_factor(_yaw_factor), // positive is clockwise
96 34320 : servo(_servo), // what servo output drives this motor
97 34320 : display_order(_display_order), // order for clockwise display
98 34320 : roll_servo(_roll_servo),
99 34320 : roll_min(_roll_min),
100 34320 : roll_max(_roll_max),
101 34320 : pitch_servo(_pitch_servo),
102 34320 : pitch_min(_pitch_min),
103 34320 : pitch_max(_pitch_max)
104 : {
105 34320 : position.x = cosf(radians(angle));
106 34320 : position.y = sinf(radians(angle));
107 34320 : position.z = 0;
108 :
109 34320 : thrust_vector.x = 0;
110 34320 : thrust_vector.y = 0;
111 34320 : thrust_vector.z = -1;
112 34320 : }
113 :
114 : void calculate_forces(const struct sitl_input &input,
115 : uint8_t motor_offset,
116 : Vector3f &torque, // Newton meters
117 : Vector3f &thrust, // Z is down, Newtons
118 : const Vector3f &velocity_air_bf,
119 : const Vector3f &gyro, // rad/sec
120 : float air_density,
121 : float voltage,
122 : bool use_drag);
123 :
124 : uint16_t update_servo(uint16_t demand, uint64_t time_usec, float &last_value) const;
125 :
126 : // get current
127 : float get_current(void) const;
128 :
129 : // convert a PWM value to a thrust demand from 0 to 1
130 : float pwm_to_command(float pwm) const;
131 :
132 : // setup motor key parameters
133 : void setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min, float _spin_max, float _expo, float _slew_max,
134 : float _diagonal_size, float _power_factor, float _voltage_max, float _effective_prop_area,
135 : float _velocity_max, Vector3f _position, Vector3f _thrust_vector, float _yaw_factor,
136 : float _true_prop_area, float _momentum_drag_coefficient);
137 :
138 : // override slew limit
139 : void set_slew_max(float _slew_max) {
140 : slew_max = _slew_max;
141 : }
142 :
143 17508600 : float get_command(void) const {
144 17508600 : return last_command;
145 : }
146 :
147 : // calculate thrust of motor
148 : float calc_thrust(float command, float air_density, float velocity_in, float voltage_scale) const;
149 :
150 : private:
151 : float mot_pwm_min;
152 : float mot_pwm_max;
153 : float mot_spin_min;
154 : float mot_spin_max;
155 : float mot_expo;
156 : float slew_max;
157 : float current;
158 : float power_factor;
159 : float voltage_max;
160 : float effective_prop_area;
161 : float max_outflow_velocity;
162 : float true_prop_area;
163 : float momentum_drag_coefficient;
164 : float diagonal_size;
165 :
166 : float last_command;
167 : uint64_t last_calc_us;
168 :
169 : Vector3f position;
170 : Vector3f thrust_vector;
171 : };
172 :
173 : }
|
