Advitiy.setPos_J2(propagate(pos, vel, time_J2, 0.5, drag=False)[0]) Advitiy.setVel_J2(propagate(pos, vel, time_J2, 0.5, drag=False)[1]) # Advitiy.setJ2Data(sensor.J2_propagator(Advitiy)) # Estimated quaternion if (estbool == 0): # qBO is same as obtained by integrator Advitiy.setQUEST(defblock.estimator(Advitiy)) # if (estbool == 1): #qBO is obtained using Quest/MEKF # control torque if (contcons == 0): # getting default control torque (zero in our case) Advitiy.setControl_b(defblock.controller(Advitiy)) if (contcons == 1): # getting control torque by omega controller # k = 0.001 torque_control[i * int(Nmodel / Ncontrol):(i + 1) * (int(Nmodel / Ncontrol)), :] \ = -0.001 * Advitiy.getW_BI_b() # print(Advitiy.getW_BI_b()) Advitiy.setControl_b(torque_control[i * int(Nmodel / Ncontrol), :]) # torque applied Advitiy.setAppTorque_b(Advitiy.getControl_b()) for k in range(0, int(Nmodel / Ncontrol)): # loop for environment-cycle
def test_controller(self): qBO = np.array([.0,0.,0.,1.]) state = np.hstack((qBO,np.array([0.10050588,-0.05026119,-0.3014887]))) mySat = satellite.Satellite(state,128.05) self.assertTrue(np.allclose(defblock.controller(mySat),np.zeros(3)))