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 : multicopter simulator class 17 : */ 18 : 19 : #pragma once 20 : 21 : #include "SIM_Aircraft.h" 22 : #include "SIM_Motor.h" 23 : #include <AP_JSON/AP_JSON.h> 24 : #include <AP_Param/AP_Param.h> 25 : 26 : #ifndef SIM_FRAME_MAX_ACTUATORS 27 : #define SIM_FRAME_MAX_ACTUATORS 32 28 : #endif 29 : 30 : namespace SITL { 31 : 32 : /* 33 : class to describe a multicopter frame type 34 : */ 35 : class Frame { 36 : public: 37 : const char *name; 38 : uint8_t num_motors; 39 : Motor *motors; 40 : 41 1110 : Frame(const char *_name, 42 : uint8_t _num_motors, 43 1110 : Motor *_motors) : 44 1110 : name(_name), 45 1110 : num_motors(_num_motors), 46 1110 : motors(_motors) {} 47 : 48 : #if AP_SIM_ENABLED 49 : // create a frame by name from its template 50 : static Frame *create_frame(const char *name); 51 : 52 : // initialise frame 53 : void init(const char *frame_str); 54 : 55 : // calculate rotational and linear accelerations 56 : void calculate_forces(const Aircraft &aircraft, 57 : const struct sitl_input &input, 58 : Vector3f &rot_accel, Vector3f &body_accel, float* rpm); 59 : #endif // AP_SIM_ENABLED 60 : 61 : static const struct AP_Param::GroupInfo var_info[]; 62 : 63 : float terminal_velocity; 64 : float terminal_rotation_rate; 65 : uint8_t motor_offset; 66 : 67 : float get_current_amp(void); 68 : 69 : // get mass in kg 70 107946341 : float get_mass(void) const { 71 107946341 : return mass; 72 : } 73 : 74 : // scale factor on model mass, used by quadplane to allow for plane components 75 201 : void set_mass_scale(float scale) { 76 201 : mass_scale = scale; 77 201 : } 78 : 79 2220 : float get_model_batt_max_voltage(void) const { return model.maxVoltage; } 80 2220 : float get_model_batt_capacity_ah(void) const { return model.battCapacityAh; } 81 2220 : float get_model_batt_resistance_ohm(void) const { return model.refBatRes; } 82 : 83 : // returns true once when the battery model values have changed 84 : bool battery_changed(void); 85 : 86 : private: 87 : /* 88 : parameters that define the multicopter model. The scalars are 89 : exposed as SIM_FRM_ parameters, defaults come from the parameter 90 : table and can be overridden by loading a json model file 91 : */ 92 : struct Model { 93 : // model mass kg 94 : AP_Float mass; 95 : 96 : // diameter of model 97 : AP_Float diagonal_size; 98 : 99 : /* 100 : the ref values are for a test at fixed angle, used to estimate drag 101 : */ 102 : AP_Float refSpd; // m/s 103 : AP_Float refAngle; // degrees 104 : AP_Float refVoltage; // Volts 105 : AP_Float refCurrent; // Amps 106 : AP_Float refAlt; // altitude AMSL 107 : float refTempC = 25; // temperature C, unused 108 : 109 : // battery resistance reference value in Ohms 110 : AP_Float refBatRes; 111 : 112 : // full pack voltage 113 : AP_Float maxVoltage; 114 : 115 : // battery capacity in Ah. Use zero for unlimited 116 : AP_Float battCapacityAh; 117 : 118 : // CTUN.ThO at hover at refAlt 119 : AP_Float hoverThrOut; 120 : 121 : // MOT_THST_EXPO 122 : AP_Float propExpo; 123 : 124 : // scaling factor for yaw response, deg/sec 125 : AP_Float refRotRate; 126 : 127 : // MOT params are from the reference test 128 : // MOT_PWM_MIN 129 : AP_Float pwmMin; 130 : // MOT_PWM_MAX 131 : AP_Float pwmMax; 132 : // MOT_SPIN_MIN 133 : AP_Float spin_min; 134 : // MOT_SPIN_MAX 135 : AP_Float spin_max; 136 : 137 : // maximum slew rate of motors 138 : AP_Float slew_max; 139 : 140 : // rotor disc area in m**2 141 : // Note that coaxial rotors count as one rotor only when calculating effective disc area 142 : AP_Float disc_area; 143 : 144 : // momentum drag coefficient 145 : AP_Float mdrag_coef; 146 : 147 : // bluff body drag scaling 148 : AP_Float bbdrag_coef; 149 : 150 : // if zero value will be estimated from mass 151 : Vector3f moment_of_inertia; 152 : 153 : Vector3f motor_pos[SIM_FRAME_MAX_ACTUATORS]; 154 : Vector3f motor_thrust_vec[SIM_FRAME_MAX_ACTUATORS]; 155 : float yaw_factor[SIM_FRAME_MAX_ACTUATORS] {0,}; 156 : 157 : // number of motors 158 : float num_motors = 4; 159 : }; 160 : 161 : protected: 162 : // load frame parameters from a json model file 163 : void load_frame_params(const char *model_json); 164 : 165 : // get air density in kg/m^3 166 : float get_air_density(float alt_amsl) const; 167 : 168 : struct Model model; 169 : 170 : private: 171 : // apply parameter based settings, called on each physics step so 172 : // that SIM_FRM_ parameter changes take effect 173 : void update_parameters(void); 174 : 175 : // exposed area times coefficient of drag 176 : float areaCd; 177 : float mass; 178 : float mass_scale = 1.0; 179 : 180 : // effective momentum drag coefficient, possibly scaled down from model.mdrag_coef 181 : float mdrag_coef; 182 : 183 : // moment of inertia in use, from model or estimated from mass 184 : Vector3f moment_of_inertia; 185 : 186 : // last printed EK3 drag suggestions 187 : float last_drag_bcoef; 188 : float last_drag_mcoef; 189 : 190 : // battery model change detection 191 : bool battery_dirty; 192 : float last_batt_voltage; 193 : float last_batt_cap; 194 : float last_batt_res; 195 : 196 : // json parsing helpers 197 : void parse_float(AP_JSON::value val, const char* label, float ¶m); 198 : void parse_vector3(AP_JSON::value val, const char* label, Vector3f ¶m); 199 : }; 200 : }
