Update for s2e v8

README.md

@@ -32,5 +32,5 @@ This repository provides the following samples.
 
 ## How to use this branch
 
-- This branch is a sample code for [How To Make New Simulation Scenario](https://github.com/ut-issl/s2e-documents/blob/develop/Tutorials/HowToMakeNewSimulationScenario.md) in the `s2e-documents`.
+- This branch is a sample code for [How To Add Control Algorithms/Direct method](https://github.com/ut-issl/s2e-documents/blob/develop/Tutorials/HowToAddControlAlgorithms.md#2-direct-method) in the `s2e-documents`.
 - Please follow the tutorial to learn how to use the sample code.

settings/user_satellite/satellite.ini

@@ -10,9 +10,9 @@ propagate_mode = RK4
 initialize_mode = MANUAL
 
 // Initial angular velocity at body frame [rad/s]
-initial_angular_velocity_b_rad_s(0) = 0.0
-initial_angular_velocity_b_rad_s(1) = 0.0
-initial_angular_velocity_b_rad_s(2) = 0.0
+initial_angular_velocity_b_rad_s(0) = 0.05
+initial_angular_velocity_b_rad_s(1) = -0.03
+initial_angular_velocity_b_rad_s(2) = 0.01
 
 // Initial quaternion inertial frame to body frame (real part, imaginary part)
 // This value also used in INERTIAL_STABILIZE mode of ControlledAttitude

src/simulation/spacecraft/user_satellite.cpp

@@ -13,3 +13,37 @@ UserSatellite::UserSatellite(const s2e::simulation::SimulationConfiguration *sim
   components_ =
       new UserComponents(dynamics_, structure_, local_environment_, global_environment, simulation_configuration, &clock_generator_, spacecraft_id);
 }
+
+void UserSatellite::Update(const s2e::environment::SimulationTime *simulation_time) {
+  dynamics_->ClearForceTorque();
+
+  // Update local environment and disturbance
+  local_environment_->Update(dynamics_, simulation_time);
+  disturbances_->Update(*local_environment_, *dynamics_, simulation_time);
+
+  // Control algorithm
+  ControlAlgorithm();
+
+  // Add generated force and torque by disturbances
+  dynamics_->AddAcceleration_i_m_s2(disturbances_->GetAcceleration_i_m_s2());
+  dynamics_->AddTorque_b_Nm(disturbances_->GetTorque_b_Nm());
+  dynamics_->AddForce_b_N(disturbances_->GetForce_b_N());
+
+  // Propagate dynamics
+  dynamics_->Update(simulation_time, &(local_environment_->GetCelestialInformation()));
+}
+
+void UserSatellite::ControlAlgorithm(void) {
+  // Sensing
+  s2e::math::Vector<3> observed_omega_b_rad_s = dynamics_->GetAttitude().GetAngularVelocity_b_rad_s();
+
+  // Control algorithm
+  s2e::math::Vector<3> control_torque_b_Nm(0.0);
+  s2e::math::Vector<3> control_force_b_N(0.0);
+  double Kp = 5.0e-3;
+  control_torque_b_Nm = -1.0 * Kp * observed_omega_b_rad_s;
+
+  // Generate force and torque
+  dynamics_->AddTorque_b_Nm(control_torque_b_Nm);
+  dynamics_->AddForce_b_N(control_force_b_N);
+}

src/simulation/spacecraft/user_satellite.hpp

@@ -22,8 +22,18 @@ class UserSatellite : public s2e::spacecraft::Spacecraft {
    */
   UserSatellite(const s2e::simulation::SimulationConfiguration *simulation_configuration,
                 const s2e::environment::GlobalEnvironment *global_environment, const unsigned int spacecraft_id);
+  /**
+   * @fn Update
+   * @brief Override Spacecraft::Update
+   */
+  virtual void Update(const s2e::environment::SimulationTime *simulation_time) override;
 
  private:
+  /**
+   * @fn ControlAlgorithm
+   * @brief Control algorithm
+   */
+  void ControlAlgorithm(void);
 };
 
 #endif  // S2E_SIMULATION_SPACECRAFT_USER_SATELLITE_HPP_