simulation/physicsSim.py at main · Bronco-Space/simulation · GitHub

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
75
76
77
78
79
80
81
82
83
84
85
86
87
88
import bpy
from math import sqrt
from pyquaternion import Quaternion
import numpy as np
import mathutils as mat
import os
import sys

dir = os.path.dirname(bpy.data.filepath)
if not dir in sys.path:
    sys.path.append(dir)

from imu import test_imu

satQuat = bpy.data.objects["cubesat"].rotation_quaternion                                                       #initial value needed for qCurrent
qCurrent = Quaternion(satQuat[0], satQuat[1], satQuat[2], satQuat[3]).normalised
angVarQuat = Quaternion(1,0,0,0)

class physicSim:

    def gravForce(self):

        cube = bpy.data.objects["cubesat"].location * 10000
        cubeR = sqrt((cube[0]**2) + (cube[1]**2) + (cube[2]**2))                                                #in meters
        massC = 1.75                                                                                            #in kg


        earthDim = (bpy.data.objects["EarthSurface"].dimensions * 10000)
        earthR = earthDim[0]/2
        dens = 5515                                                                                             #kg/m^3
        volE = (4/3)*(3.14159)* (earthR)**3                                                                     #m^3
        massE = dens * volE                                                                                     #kg
        gravC = (6.67408 * 10**(-11))
        unitVectR = cube / cubeR

        force = -((gravC * massE *massC) / (cubeR**2)) * unitVectR                                              #newtons

        return force

    def gravAccel(self):
        cube = bpy.data.objects["cubesat"].location * 10000
        cubeR = sqrt((cube[0]**2) + (cube[1]**2) + (cube[2]**2))                                                #in m


        earthDim = (bpy.data.objects["EarthSurface"].dimensions * 10000)
        earthR = earthDim[0]/2
        dens = 5515                                                                                             #kg/m^3
        volE = (4/3)*(3.14159)* (earthR)**3                                                                     #m^3
        massE = dens * volE                                                                                     #kg
        unitVectR = cube / cubeR
        gravC = (6.67408 * 10**(-11))

        accel = -((gravC * massE) / (cubeR ** 2))* unitVectR                                                    #meters/s^2

        return accel

    def AngVar(self, dx, dy, dz):

        global qCurrent
        global angVarQuat

        self.dx = dx
        self.dy = dy
        self.dz = dz
        qx = Quaternion(axis=(1.0, 0.0, 0.0), degrees = dx).normalised
        qy = Quaternion(axis=(0.0, 1.0, 0.0), degrees = dy).normalised
        qz = Quaternion(axis=(0.0, 0.0, 1.0), degrees = dz).normalised

        qDelta = (qx*qy*qz).normalised
        qNew = (qDelta * qCurrent).normalised
        bpy.data.objects["cubesat"].rotation_quaternion = mat.Quaternion((qNew[0], qNew[1], qNew[2], qNew[3]))  #Orientation Set

        angVarQuat = qNew * qCurrent.conjugate #readout of current ang var
        qCurrent = qNew
        test_imu.Update(angVarQuat) # UPDATES IMU AND DISPLAYS VALUES
        return qCurrent

#test indepent of timestep
x = 0
a = physicSim()
while x < 10:
    a.AngVar(10,0,0)
    x = x + 1