Coverage for src/gncpy/dynamics/aircraft/simple_multirotor.py: 0%

1"""Implements a multi-rotor dynamics model.""" 

2import numpy as np 

3from scipy import integrate 

4import enum 

5import pathlib 

6import os 

7from ruamel.yaml import YAML 

8 

9import gncpy.wgs84 as wgs84 

10from gncpy.dynamics.basic import DynamicsBase 

11from gncpy.coordinate_transforms import ned_to_LLA 

12 

13 

14yaml = YAML() 

15r2d = 180.0 / np.pi 

16d2r = 1 / r2d 

17 

18 

19class AeroParams: 

20 """Aerodynamic parameters to be parsed by the yaml parser. 

21 

22 Attributes 

23 ---------- 

24 cd : float 

25 Drag coefficent. 

26 """ 

27 

28 def __init__(self): 

29 self.cd = 0 

30 

31 

32class MassParams: 

33 """Mass parameters to be parsed by the yaml parser. 

34 

35 Attributes 

36 ---------- 

37 cg_m : list 

38 Location of the cg in meters 

39 mass_kg : float 

40 Mass in kilograms 

41 inertia_kgm2 : list 

42 Each element is a list such that it creates the moment of inertia matrix. 

43 """ 

44 

45 def __init__(self): 

46 self.cg_m = [] 

47 self.mass_kg = 0 

48 self.inertia_kgm2 = [] 

49 

50 

51class PropParams: 

52 """Propeller parameters to be parsed by the yaml parser. 

53 

54 Attributes 

55 ---------- 

56 poly_thrust : list 

57 coefficents for the polynomial modeling the thrust of the propellers. 

58 poly_torque : list 

59 coefficents for the polynomial modeling the torque from the propellers. 

60 """ 

61 

62 def __init__(self): 

63 self.poly_thrust = [] 

64 self.poly_torque = [] 

65 

66 

67class MotorParams: 

68 """Motor parameters to be parsed by the yaml parser. 

69 

70 Attributes 

71 ---------- 

72 pos_m : list 

73 Each element is a list of the position of the motor in meters. 

74 dir : list 

75 Each element is +/-1 indicating the direction the motor spins. 

76 """ 

77 

78 def __init__(self): 

79 self.pos_m = [] 

80 self.dir = [] 

81 

82 @property 

83 def num_motors(self): 

84 """Read only total number of motors.""" 

85 return len(self.dir) 

86 

87 

88class GeoParams: 

89 """Geometric parameters to be parsed by the yaml parser. 

90 

91 Attributes 

92 ---------- 

93 front_area_m2 : list 

94 Each element is a float representing the front cross sectional area at 

95 different angles. 

96 """ 

97 

98 def __init__(self): 

99 self.front_area_m2 = [] 

100 

101 

102class AircraftParams: 

103 """Aircraft parameters to be parsed by the yaml parser. 

104 

105 Attributes 

106 ---------- 

107 aero : :class:`.AeroParams` 

108 Aerodynamic parameters. 

109 mass : :class:`.MassParams` 

110 Mass parameters. 

111 geo : :class:`.GeoParams` 

112 Geometric parameters 

113 prop : :class:`.PropParams` 

114 Propeller parameters. 

115 motor : :class:`.MotorParams` 

116 Motor parameters 

117 """ 

118 

119 def __init__(self): 

120 self.aero = AeroParams() 

121 self.mass = MassParams() 

122 self.geo = GeoParams() 

123 self.prop = PropParams() 

124 self.motor = MotorParams() 

125 

126 

127yaml.register_class(AeroParams) 

128yaml.register_class(MassParams) 

129yaml.register_class(PropParams) 

130yaml.register_class(MotorParams) 

131yaml.register_class(GeoParams) 

132yaml.register_class(AircraftParams) 

133 

134 

135class Effector: 

136 """Defines an effector.""" 

137 

138 def step(self, input_cmds): 

139 """Converts input commands to effector commands. 

140 

141 Returns 

142 ------- 

143 numpy array 

144 Effector commands 

145 """ 

146 return input_cmds.copy() 

147 

148 

149class ListEnum(list, enum.Enum): 

150 """Helper class for using list values in an enum.""" 

151 

152 def __new__(cls, *args): 

153 """Creates new objects.""" 

154 assert len(args) == 2 

155 try: 

156 inds = list(args[0]) 

157 except TypeError: 

158 inds = [ 

159 args[0], 

160 ] 

161 units = args[1] 

162 

163 obj = list.__new__(cls) 

164 obj._value_ = inds 

165 obj.extend(inds) 

166 obj.units = units 

167 

168 return obj 

169 

170 def __init__(self, *args): 

171 pass 

172 

173 def __str__(self): 

174 """Converts to a string.""" 

175 return self.name 

176 

177 def __eq__(self, other): 

178 """Checks for equality.""" 

179 if self.__class__ is other.__class__: 

180 return self.value == other.value 

181 elif len(self.value) == 1: 

182 return self.value[0] == other 

183 elif len(self.value) == len(other): 

184 return self.value == other 

185 return NotImplemented() 

186 

187 @classmethod 

188 def get_num_states(cls): 

189 """Returns the total number of states in the enum.""" 

190 n_states = -1 

191 for s in dir(cls): 

192 if s[0] == "_": 

193 continue 

194 v = getattr(cls, s) 

195 if max(v) > n_states: 

196 n_states = max(v) 

197 return n_states + 1 

198 

199 

200class v_smap(ListEnum): 

201 """Enum for the vehicle state that pairs the vector indices with units.""" 

202 

203 lat = (0, "rad") 

204 lon = (1, "rad") 

205 alt_wgs84 = (2, "m") 

206 alt_msl = (3, "m") 

207 alt_agl = (47, "m") 

208 ned_pos = ([4, 5, 6], "m") 

209 ned_vel = ([7, 8, 9], "m/s") 

210 ned_accel = ([10, 11, 12], "m/s^2") 

211 pitch = (13, "rad") 

212 roll = (14, "rad") 

213 yaw = (15, "rad") 

214 body_vel = ([16, 17, 18], "m/s") 

215 body_accel = ([19, 20, 21], "m/s^2") 

216 body_rot_rate = ([22, 23, 24], "rad/s") 

217 body_rot_accel = ([25, 26, 27], "rad/s^2") 

218 dyn_pres = (28, "Pa") 

219 airspeed = (29, "m/s") 

220 mach = (30, "") 

221 aoa = (31, "rad") 

222 aoa_rate = (32, "rad/s") 

223 sideslip_ang = (33, "rad") 

224 sideslip_rate = (34, "rad/s") 

225 gnd_trk = (35, "rad") 

226 fp_ang = (36, "rad") 

227 gnd_speed = (37, "m/s") 

228 dcm_earth2body = ([38, 39, 40, 41, 42, 43, 44, 45, 46], "") 

229 

230 @classmethod 

231 def _get_ordered_key(cls, key, append_ind): 

232 lst = [] 

233 for attr_str in dir(cls): 

234 if attr_str[0] == "_": 

235 continue 

236 

237 attr = getattr(cls, attr_str) 

238 multi = len(attr.value) > 1 

239 is_dcm = multi and "dcm" in attr.name 

240 for ii, jj in enumerate(attr.value): 

241 name = getattr(attr, key) 

242 if append_ind: 

243 if is_dcm: 

244 r, c = np.unravel_index([ii], (3, 3)) 

245 name += "_{:d}{:d}".format(r.item(), c.item()) 

246 elif multi: 

247 name += "_{:d}".format(ii) 

248 

249 lst.append((jj, name)) 

250 lst.sort(key=lambda x: x[0]) 

251 return tuple([x[1] for x in lst]) 

252 

253 @classmethod 

254 def get_ordered_names(cls): 

255 """Get the state names in the order they appear in the vector including indices. 

256 

257 For example if the state vector has position p then velocity v this 

258 would return :code:`['p_0', 'p_1', 'p_2', 'v_0', 'v_1', 'v_2']`. 

259 Matrices have row then column as the index in the unraveled order. 

260 

261 Returns 

262 ------- 

263 list 

264 String with format :code:`NAME_SUBINDEX` sorted according to vector order. 

265 """ 

266 return cls._get_ordered_key("name", True) 

267 

268 @classmethod 

269 def get_ordered_units(cls): 

270 """Get a list of units for each state in the vector in sorted order. 

271 

272 Returns 

273 ------- 

274 list 

275 String of the unit of the corresponding element in the state vector. 

276 """ 

277 return cls._get_ordered_key("units", False) 

278 

279 

280class e_smap(ListEnum): 

281 """Enum for the environment state that pairs indices with units.""" 

282 

283 temp = (0, "K") 

284 speed_of_sound = (1, "m/s") 

285 pressure = (2, "Pa") 

286 density = (3, "kg/m^3") 

287 gravity = ([4, 5, 6], "m/s^2") 

288 mag_field = ([7, 8, 9], "uT") 

289 terrain_alt_wgs84 = (10, "m") 

290 

291 

292class Vehicle: 

293 """Implements the base vehicle. 

294 

295 Attributes 

296 ---------- 

297 state : numpy array 

298 State of the aircraft. 

299 params : :class:`.AircraftParams` 

300 Parameters of the aircraft. 

301 ref_lat : float 

302 Reference lattiude in radians (for converting to NED) 

303 ref_lon : float 

304 Reference longitude in radians (for converting to NED) 

305 takenoff : bool 

306 Flag indicating if the vehicle has taken off yet. 

307 """ 

308 

309 __slots__ = ("state", "params", "ref_lat", "ref_lon", "takenoff") 

310 

311 def __init__(self, params): 

312 """Initialize an object. 

313 

314 Parameters 

315 ---------- 

316 params : :class:`.AircraftParams` 

317 Parameters of the aircraft. 

318 """ 

319 self.state = np.nan * np.ones(v_smap.get_num_states()) 

320 self.params = params 

321 

322 self.ref_lat = np.nan 

323 self.ref_lon = np.nan 

324 self.takenoff = False 

325 

326 def _get_dcm_earth2body(self): 

327 return self.state[v_smap.dcm_earth2body].reshape((3, 3)) 

328 

329 def set_dcm_earth2body(self, mat): 

330 """Sets the dcm in the state vector. 

331 

332 Parameters 

333 ---------- 

334 mat : 3x3 numpy array 

335 Value to assign to the dcm. 

336 """ 

337 self.state[v_smap.dcm_earth2body] = mat.flatten() 

338 

339 def _calc_aero_force_mom(self, dyn_pres, body_vel): 

340 mom = np.zeros(3) 

341 xy_spd = np.linalg.norm(body_vel[0:2]) 

342 if xy_spd < np.finfo(float).eps: 

343 inc_ang = 0 

344 else: 

345 inc_ang = np.arctan(xy_spd / body_vel[2]) 

346 

347 lut_npts = len(self.params.geo.front_area_m2) 

348 front_area = np.interp( 

349 inc_ang * 180 / np.pi, 

350 np.linspace(-180, 180, lut_npts), 

351 self.params.geo.front_area_m2, 

352 ) 

353 

354 vel_mag = np.linalg.norm(-body_vel) 

355 if np.abs(vel_mag) < np.finfo(float).eps: 

356 force = np.zeros(3) 

357 else: 

358 force = -body_vel / vel_mag * front_area * dyn_pres * self.params.aero.cd 

359 

360 return force.ravel(), mom 

361 

362 def _calc_grav_force_mom(self, gravity, dcm_earth2body): 

363 mom = np.zeros(3) 

364 force = dcm_earth2body @ (gravity * self.params.mass.mass_kg).reshape((3, 1)) 

365 

366 return force.ravel(), mom 

367 

368 def _calc_prop_force_mom(self, motor_cmds): 

369 # motor model 

370 m_thrust = -np.polynomial.Polynomial(self.params.prop.poly_thrust[-1::-1])( 

371 motor_cmds 

372 ) 

373 m_torque = -np.polynomial.Polynomial(self.params.prop.poly_torque[-1::-1])( 

374 motor_cmds 

375 ) 

376 m_torque = np.sum(m_torque * np.array(self.params.motor.dir)) 

377 

378 # thrust to moment 

379 motor_mom = np.zeros(3) 

380 for ii, m_pos in enumerate(np.array(self.params.motor.pos_m)): 

381 dx = m_pos[0] - self.params.mass.cg_m[0] 

382 dy = m_pos[1] - self.params.mass.cg_m[1] 

383 motor_mom[0] += dy * m_thrust[ii] 

384 motor_mom[1] += -dx * m_thrust[ii] 

385 

386 motor_mom[2] += m_torque 

387 

388 # calc force 

389 force = np.zeros(3) 

390 force[2] = np.min([0, np.sum(m_thrust).item()]) 

391 

392 return force, motor_mom 

393 

394 def _calc_force_mom(self, gravity, motor_cmds): 

395 a_f, a_m = self._calc_aero_force_mom( 

396 self.state[v_smap.dyn_pres], self.state[v_smap.body_vel] 

397 ) 

398 g_f, g_m = self._calc_grav_force_mom(gravity, self._get_dcm_earth2body()) 

399 p_f, p_m = self._calc_prop_force_mom(motor_cmds) 

400 

401 if not self.takenoff: 

402 self.takenoff = -p_f[2] > g_f[2] 

403 

404 if self.takenoff: 

405 return (a_f + g_f + p_f, a_m + g_m + p_m) 

406 else: 

407 return np.zeros(a_f.shape), np.zeros(a_m.shape) 

408 

409 def eul_to_dcm(self, r1, r2, r3): 

410 """Convert euler angles to a DCM. 

411 

412 Parameters 

413 ---------- 

414 r1 : flaot 

415 First rotation applied about the z axis (radians). 

416 r2 : float 

417 Second rotation applied about the y axis (radians). 

418 r3 : float 

419 Third rotation applied about the x axis (radians). 

420 

421 Returns 

422 ------- 

423 3x3 numpy array 

424 3-2-1 DCM. 

425 """ 

426 cx = np.cos(r3) 

427 cy = np.cos(r2) 

428 cz = np.cos(r1) 

429 sx = np.sin(r3) 

430 sy = np.sin(r2) 

431 sz = np.sin(r1) 

432 

433 return np.array( 

434 [ 

435 [cy * cz, cy * sz, -sy], 

436 [sx * sy * cz - cx * sz, sx * sy * sz + cx * cz, sx * cy], 

437 [cx * sy * cz + sx * sz, cx * sy * sz - sx * cz, cx * cy], 

438 ] 

439 ) 

440 

441 def _six_dof_model(self, force, mom, dt): 

442 def ode_ang(t, x, m): 

443 s_phi = np.sin(x[0]) 

444 c_phi = np.cos(x[0]) 

445 t_theta = np.tan(x[1]) 

446 c_theta = np.cos(x[1]) 

447 if np.abs(c_theta) < np.finfo(float).eps: 

448 c_theta = np.finfo(float).eps * np.sign(c_theta) 

449 eul_dot_mat = np.array( 

450 [ 

451 [1, s_phi * t_theta, c_phi * t_theta], 

452 [0, c_phi, -s_phi], 

453 [0, s_phi / c_theta, c_phi / c_theta], 

454 ] 

455 ) 

456 

457 xdot = np.zeros(6) 

458 xdot[0:3] = eul_dot_mat @ x[3:6] 

459 xdot[3:6] = np.linalg.inv(self.params.mass.inertia_kgm2) @ ( 

460 m - np.cross(x[3:6], self.params.mass.inertia_kgm2 @ x[3:6]) 

461 ) 

462 

463 return xdot 

464 

465 def ode(t, x, f, m): 

466 # x is NED pos x/y/z, body vel x/y/z, phi, theta, psi, body omega x/y/z 

467 dcm_e2b = self.eul_to_dcm(x[8], x[7], x[6]) 

468 # uvw = x[3:6] 

469 # eul = x[6:9] 

470 # pqr = x[9:12] 

471 

472 xdot = np.zeros(12) 

473 xdot[0:3] = dcm_e2b.T @ x[3:6] 

474 xdot[3:6] = f / self.params.mass.mass_kg + np.cross(x[3:6], x[9:12]) 

475 xdot[6:12] = ode_ang(t, x[6:12], m) 

476 

477 return xdot 

478 

479 r = integrate.ode(ode).set_integrator("dopri5").set_f_params(force, mom) 

480 eul_inds = v_smap.roll + v_smap.pitch + v_smap.yaw 

481 x0 = np.concatenate( 

482 ( 

483 self.state[v_smap.ned_pos].flatten(), 

484 self.state[v_smap.body_vel].flatten(), 

485 self.state[eul_inds].flatten(), 

486 self.state[v_smap.body_rot_rate].flatten(), 

487 ) 

488 ) 

489 r.set_initial_value(x0, 0) 

490 y = r.integrate(dt) 

491 

492 if not r.successful(): 

493 raise RuntimeError("Integration failed.") 

494 

495 ned_pos = y[0:3] 

496 body_vel = y[3:6] 

497 roll = y[6] 

498 pitch = y[7] 

499 yaw = y[8] 

500 body_rot_rate = y[9:12] 

501 

502 # get dcm 

503 dcm_earth2body = self.eul_to_dcm(x0[8], x0[7], x0[6]) 

504 

505 ned_vel = dcm_earth2body.T @ body_vel 

506 

507 xdot = ode(dt, x0, force, mom) 

508 body_accel = xdot[3:6] 

509 body_rot_accel = xdot[9:12] 

510 ned_accel = body_accel - np.cross(x0[3:6], x0[9:12]) 

511 

512 return ( 

513 ned_vel, 

514 ned_pos, 

515 roll, 

516 pitch, 

517 yaw, 

518 dcm_earth2body, 

519 body_vel, 

520 body_rot_rate, 

521 body_rot_accel, 

522 body_accel, 

523 ned_accel, 

524 ) 

525 

526 def calc_derived_states( 

527 self, dt, terrain_alt_wgs84, density, speed_of_sound, ned_vel, ned_pos, body_vel 

528 ): 

529 """Calculates the parts of the state vector derived from other parts. 

530 

531 Parameters 

532 ---------- 

533 dt : float 

534 Delta time since last update. 

535 terrain_alt_wgs84 : float 

536 Altitude of the terrain relative to WGS-84 model in meters. 

537 density : float 

538 Density of the atmosphere. 

539 speed_of_sound : float 

540 Speed of sound in m/s. 

541 ned_vel : numpy array 

542 Velocity in NED frame. 

543 ned_pos : numpy array 

544 Body position in NED frame. 

545 body_vel : numpy array 

546 Velocity of the body in body frame. 

547 

548 Returns 

549 ------- 

550 gnd_trk : float 

551 Ground track. 

552 gnd_speed : float 

553 Ground speed. 

554 fp_ang : float 

555 flight path angle (radians). 

556 dyn_pres : float 

557 dynamic pressure. 

558 aoa : float 

559 Angle of attack (radians). 

560 airspeed : float 

561 Airspeed in m/s. 

562 sideslip_ang : float 

563 Sideslip angle in radians. 

564 aoa_rate : float 

565 Rate of change in the AoA (rad/s). 

566 sideslip_rate : float 

567 Rate of change in the sideslip angle (rad/s). 

568 mach : float 

569 Mach number. 

570 lat : float 

571 Latitude (radians). 

572 lon : float 

573 Longitude (radians). 

574 alt_wgs84 : float 

575 Altitude relative to WGS-84 model (meters). 

576 alt_agl : float 

577 Altitude relative to ground (meters). 

578 alt_msl : float 

579 Altitude relative to mean sea level (meters). 

580 """ 

581 gnd_trk = np.arctan2(ned_vel[1], ned_vel[0]) 

582 gnd_speed = np.linalg.norm(ned_vel[0:2]) 

583 fp_ang = np.arctan2(-ned_vel[2], gnd_speed) 

584 

585 dyn_pres = 0.5 * density * np.sum(body_vel * body_vel) 

586 

587 aoa = np.arctan2(body_vel[2], body_vel[0]) 

588 airspeed = np.linalg.norm(body_vel) 

589 if np.abs(airspeed) <= np.finfo(float).eps: 

590 sideslip_ang = 0 

591 else: 

592 sideslip_ang = np.arcsin(body_vel[1] / airspeed) 

593 

594 aoa_rate = (aoa - self.state[v_smap.aoa]) / dt 

595 sideslip_rate = (sideslip_ang - self.state[v_smap.sideslip_ang]) / dt 

596 

597 mach = airspeed / speed_of_sound 

598 

599 lla = ned_to_LLA( 

600 ned_pos.reshape((3, 1)), self.ref_lat, self.ref_lon, terrain_alt_wgs84 

601 ) 

602 lat = lla[0] 

603 lon = lla[1] 

604 alt_wgs84 = lla[2] 

605 

606 alt_agl = alt_wgs84 - terrain_alt_wgs84 

607 alt_msl = wgs84.convert_wgs_to_msl(lla[0], lla[1], lla[2]) 

608 

609 return ( 

610 gnd_trk, 

611 gnd_speed, 

612 fp_ang, 

613 dyn_pres, 

614 aoa, 

615 airspeed, 

616 sideslip_ang, 

617 aoa_rate, 

618 sideslip_rate, 

619 mach, 

620 lat, 

621 lon, 

622 alt_wgs84, 

623 alt_agl, 

624 alt_msl, 

625 ) 

626 

627 def step(self, dt, terrain_alt_wgs84, gravity, density, speed_of_sound, motor_cmds): 

628 """Perform one update step for the vehicle. 

629 

630 This updates the internal state vector of the vehicle. 

631 

632 Parameters 

633 ---------- 

634 dt : float 

635 Delta time since last update. 

636 terrain_alt_wgs84 : float 

637 Altitude of the terrain relative to WGS-84 model in meters. 

638 gravity : numpy array 

639 gravity vector. 

640 density : float 

641 Density of the atmosphere. 

642 speed_of_sound : float 

643 Speed of sound in m/s. 

644 motor_cmds : numpy array 

645 Commands to the motors in normalized range. 

646 """ 

647 force, mom = self._calc_force_mom(gravity, motor_cmds) 

648 

649 ( 

650 ned_vel, 

651 ned_pos, 

652 roll, 

653 pitch, 

654 yaw, 

655 dcm_earth2body, 

656 body_vel, 

657 body_rot_rate, 

658 body_rot_accel, 

659 body_accel, 

660 ned_accel, 

661 ) = self._six_dof_model(force, mom, dt) 

662 

663 ( 

664 gnd_trk, 

665 gnd_speed, 

666 fp_ang, 

667 dyn_pres, 

668 aoa, 

669 airspeed, 

670 sideslip_ang, 

671 aoa_rate, 

672 sideslip_rate, 

673 mach, 

674 lat, 

675 lon, 

676 alt_wgs84, 

677 alt_agl, 

678 alt_msl, 

679 ) = self.calc_derived_states( 

680 dt, terrain_alt_wgs84, density, speed_of_sound, ned_vel, ned_pos, body_vel 

681 ) 

682 

683 # update state 

684 self.state[v_smap.ned_vel] = ned_vel 

685 self.state[v_smap.ned_pos] = ned_pos 

686 self.state[v_smap.roll] = roll 

687 self.state[v_smap.pitch] = pitch 

688 self.state[v_smap.yaw] = yaw 

689 self.set_dcm_earth2body(dcm_earth2body) 

690 self.state[v_smap.body_vel] = body_vel 

691 self.state[v_smap.body_rot_rate] = body_rot_rate 

692 self.state[v_smap.body_rot_accel] = body_rot_accel 

693 self.state[v_smap.body_accel] = body_accel 

694 self.state[v_smap.ned_accel] = ned_accel 

695 self.state[v_smap.gnd_trk] = gnd_trk 

696 self.state[v_smap.gnd_speed] = gnd_speed 

697 self.state[v_smap.fp_ang] = fp_ang 

698 self.state[v_smap.dyn_pres] = dyn_pres 

699 self.state[v_smap.aoa] = aoa 

700 self.state[v_smap.aoa_rate] = aoa_rate 

701 self.state[v_smap.airspeed] = airspeed 

702 self.state[v_smap.sideslip_ang] = sideslip_ang 

703 self.state[v_smap.sideslip_rate] = sideslip_rate 

704 self.state[v_smap.mach] = mach 

705 self.state[v_smap.lat] = lat 

706 self.state[v_smap.lon] = lon 

707 self.state[v_smap.alt_wgs84] = alt_wgs84 

708 self.state[v_smap.alt_agl] = alt_agl 

709 self.state[v_smap.alt_msl] = alt_msl 

710 

711 

712class Environment: 

713 """Environment model. 

714 

715 Attributes 

716 ---------- 

717 state : numpy array 

718 Environment state vector. 

719 """ 

720 

721 def __init__(self): 

722 """Initialize an object.""" 

723 self.state = np.nan * np.ones(e_smap.get_num_states()) 

724 

725 def _lower_atmo(self, alt_km): 

726 """Model of the lower atmosphere. 

727 

728 This code is extracted from the version hosted on 

729 https://www.pdas.com/atmos.html of Public Domain Aerospace Software. 

730 """ 

731 REARTH = 6356.7523 # polar radius of the earth, kilometers 

732 GZERO = 9.80665 # sea level accel. of gravity, m/s^2 

733 MOLWT_ZERO = 28.9644 # molecular weight of air at sea level 

734 RSTAR = 8314.32 # perfect gas constant, N-m/(kmol-K) 

735 GMR = 1000 * GZERO * MOLWT_ZERO / RSTAR # hydrostatic constant, kelvins/km 

736 

737 TEMP0 = 288.15 # sea-level temperature, kelvins 

738 PRES0 = 101325.0 # sea-level pressure, Pa 

739 DENSITY0 = 1.225 # sea-level density, kg/cu.m 

740 ASOUND0 = 340.294 # speed of sound at S.L. m/sec 

741 

742 htab = [0.0, 11.0, 20.0, 32.0, 47.0, 51.0, 71.0, 84.852] 

743 ttab = [288.15, 216.65, 216.65, 228.65, 270.65, 270.65, 214.65, 186.946] 

744 ptab = [ 

745 1.0, 

746 2.2336110e-1, 

747 5.4032950e-2, 

748 8.5666784e-3, 

749 1.0945601e-3, 

750 6.6063531e-4, 

751 3.9046834e-5, 

752 3.68501e-6, 

753 ] 

754 gtab = [-6.5, 0.0, 1.0, 2.8, 0, -2.8, -2.0, 0.0] 

755 

756 # convert from geometric (height above geoid) to geopotential altitude 

757 h = alt_km * REARTH / (alt_km + REARTH) 

758 

759 # binary search for atmo layer 

760 i = 0 

761 j = len(htab) 

762 while j > i + 1: 

763 k = (i + j) // 2 

764 if h < htab[k]: 

765 j = k 

766 else: 

767 i = k 

768 tgrad = gtab[i] # temp. gradient of local layer 

769 tbase = ttab[i] # base temp. of local layer 

770 deltah = h - htab[i] # height above local base 

771 tlocal = tbase + tgrad * deltah # local temperature 

772 theta = tlocal / ttab[0] # temperature ratio 

773 

774 if 0.0 == tgrad: 

775 delta = ptab[i] * np.exp(-GMR * deltah / tbase).item() 

776 else: 

777 delta = ptab[i] * (tbase / tlocal) ** (GMR / tgrad) 

778 sigma = delta / theta 

779 

780 return ( 

781 sigma * DENSITY0, 

782 delta * PRES0, 

783 theta * TEMP0, 

784 ASOUND0 * np.sqrt(theta), 

785 ) 

786 

787 def _atmo(self, alt_msl): 

788 alt_km = alt_msl / 1000.0 

789 if alt_km < 86: 

790 return self._lower_atmo(alt_km) 

791 else: 

792 raise NotImplementedError( 

793 "Upper atmosphere model (>86 km) not implemented." 

794 ) 

795 

796 def step(self, lat, lon, alt_wgs84, alt_msl): 

797 """Perform one update step for the environment. 

798 

799 This updates the internal state vector. 

800 

801 Parameters 

802 ---------- 

803 lat : float 

804 Latitude (radians). 

805 lon : float 

806 Longitude (radians). 

807 alt_wgs84 : float 

808 Altitude relative to WGS-84 model (meters). 

809 alt_msl : float 

810 Altitude relative to mean sea level (meters). 

811 """ 

812 density, pres, temp, spd_snd = self._atmo(alt_msl) 

813 gravity = wgs84.calc_gravity(lat, alt_wgs84).ravel() 

814 

815 # update state 

816 self.state[e_smap.temp] = temp 

817 self.state[e_smap.speed_of_sound] = spd_snd 

818 self.state[e_smap.pressure] = pres 

819 self.state[e_smap.density] = density 

820 self.state[e_smap.gravity] = gravity 

821 

822 

823class SimpleMultirotor(DynamicsBase): 

824 """Implements functions for a generic multi-rotor. 

825 

826 Attributes 

827 ---------- 

828 effector : :class:`.Effector` 

829 Effectors for the vehicle. 

830 env : :class:`.Environment` 

831 Environment the vehicle is in. 

832 vehicle : :class:`.Vehicle` 

833 Base vehicle class defining its physics. 

834 """ 

835 

836 state_names = v_smap.get_ordered_names() 

837 """List of vehicle state names.""" 

838 

839 state_units = v_smap.get_ordered_units() 

840 """List of vehicle state units.""" 

841 

842 state_map = v_smap 

843 """Map of states to indices with units.""" 

844 

845 def __init__(