HC3

1//      @mpla
2//      copyright 12024
3//      MIT License
4
5//   Permission is hereby granted, free of charge,
6//   to any person obtaining a copy of this software 
7//   and associated documentation files (the “Software”),
8//   to deal in the Software without restriction,
9//   including without limitation the rights to use,
10//   copy, modify, merge, publish, distribute, sublicense,
11//   and/or sell copies of the Software, and to permit
12//   persons to whom the Software is furnished to do so,
13//   subject to the following conditions:
14
15
16const float G = 6.67430e-11;             // (m^3 kg^-1 s^-2)
17const float M_earth = 5.972e24;          // (kg)
18const float R_earth = 6371000;           // (m)
19const float rho = 1.225;                 // (kg/m^3)
20const float drag_coeff = 2.2;
21const float area = 0.01;                 // (m^2)
22const float mass_satellite = 1.3;        // (kg)
23const float communicationPower = 0.5;    // (LoRa)
24const float sensorPower = 0.05;          // (50mW)
25const float controlSystemPower = 0.2;    // (reaction wheels)
26const float microcontrollerPower = 0.03; // (30mA)
27const float MAGNETIC_THRESHOLD = 1.5; 
28const float GYROSCOPE_THRESHOLD = 5.0; 
29const float ACCELEROMETER_THRESHOLD = 0.5;
30
31float altitude = 110000; //  LEO (m)
32float velocity = 7800;   // (m/s)
33float pos_x = 0, pos_y = R_earth + altitude; // Χ0
34float vel_x = velocity, vel_y = 0;
35float dt = 1; // (1s)
36float magneticFieldX, magneticFieldY, magneticFieldZ;
37float gyroX, gyroY, gyroZ;
38float accelX, accelY, accelZ;
39
40unsigned long communicationTime = 1800; 
41unsigned long sensorTime = 300;         
42unsigned long controlTime = 600;        
43unsigned long microcontrollerTime = 3600; 
44
45const float max_acceleration = 10.0; // m/s²
46const float max_angular_velocity = 20.0; // rad/s
47const float max_magnetic_field = 50.0;   // μT
48
49
50float calculateEnergy(float power, unsigned long time) {
51  return power * time; 
52}
53
54float atmospheric_density(float altitude) {
55  if (altitude > 100000) return 0; 
56  return rho * exp(-altitude / 8500.0); 
57}
58
59
60float gravitational_force(float altitude) {
61  float r = R_earth + altitude;
62  return G * M_earth * mass_satellite / (r * r);
63}
64
65
66float drag_force(float velocity, float altitude) {
67  float density = atmospheric_density(altitude);
68  return 0.5 * density * drag_coeff * area * velocity * velocity;
69}
70
71
72float accelerometer_reading(float acc_x, float acc_y) {
73  return sqrt(acc_x * acc_x + acc_y * acc_y);
74}
75
76
77float gyroscope_reading(float angular_velocity) {
78  return angular_velocity;
79}
80
81
82float magnetometer_reading(float altitude) {
83  return 40.0 * exp(-altitude / 20000.0); 
84}
85
86void check_sensor_limits(float acceleration, float angular_velocity, float magnetic_field) {
87  if (acceleration > max_acceleration) {
88    Serial.println("a out of order");
89  }
90  if (angular_velocity > max_angular_velocity) {
91    Serial.println("Ω-u out of order");
92  }
93  if (magnetic_field > max_magnetic_field) {
94    Serial.println("T out of order");
95  }
96}
97
98void update_position_and_velocity() {
99  
100  float r = sqrt(pos_x * pos_x + pos_y * pos_y);
101
102  float F_gravity = gravitational_force(r - R_earth);
103
104  float F_drag = drag_force(velocity, r - R_earth);
105
106 
107  float acc_x = -F_gravity * (pos_x / r) / mass_satellite;
108  float acc_y = -F_gravity * (pos_y / r) / mass_satellite;
109
110  
111  acc_x -= (F_drag / mass_satellite) * (vel_x / velocity);
112  acc_y -= (F_drag / mass_satellite) * (vel_y / velocity);
113
114  
115  vel_x += acc_x * dt;
116  vel_y += acc_y * dt;
117
118 
119  pos_x += vel_x * dt;
120  pos_y += vel_y * dt;
121
122  
123  velocity = sqrt(vel_x * vel_x + vel_y * vel_y);
124
125  
126  float acceleration = accelerometer_reading(acc_x, acc_y);
127  float angular_velocity = gyroscope_reading(velocity / r);
128  float magnetic_field = magnetometer_reading(r - R_earth);
129
130  check_sensor_limits(acceleration, angular_velocity, magnetic_field);
131
132  Serial.print("H: ");
133  Serial.print(r - R_earth);
134  Serial.print(" m, U: ");
135  Serial.print(velocity);
136  Serial.print(" m/s, a: ");
137  Serial.print(acceleration);
138  Serial.print(" m/s², T: ");
139  Serial.print(magnetic_field);
140  Serial.println(" μT");
141}
142
143void setup() {
144  Serial.begin(9600);
145
146  
147  float energyCommunication = calculateEnergy(communicationPower, communicationTime);
148  float energySensor = calculateEnergy(sensorPower, sensorTime);
149  float energyControl = calculateEnergy(controlSystemPower, controlTime);
150  float energyMicrocontroller = calculateEnergy(microcontrollerPower, microcontrollerTime);
151
152 
153  Serial.print("W-J:\n");
154  Serial.print("Communication: ");
155  Serial.print(energyCommunication);
156  Serial.println(" J");
157
158  Serial.print("Sensors: ");
159  Serial.print(energySensor);
160  Serial.println(" J");
161
162  Serial.print("GSN: ");
163  Serial.print(energyControl);
164  Serial.println(" J");
165
166  Serial.print("Micro Controler: ");
167  Serial.print(energyMicrocontroller);
168  Serial.println(" J");
169
170
171  float totalEnergy = energyCommunication + energySensor + energyControl + energyMicrocontroller;
172  Serial.print("F-J: ");
173  Serial.print(totalEnergy);
174  Serial.println(" J");
175}
176
177void loop() {
178  
179  magneticFieldX = analogRead(A0);  
180  magneticFieldY = analogRead(A1);  
181  magneticFieldZ = analogRead(A2);  
182
183  gyroX = analogRead(A3);  
184  gyroY = analogRead(A4);  
185  gyroZ = analogRead(A5);  
186
187  accelX = analogRead(A6); 
188  accelY = analogRead(A7); 
189  accelZ = analogRead(A8); 
190
191  
192  if (abs(magneticFieldX) > MAGNETIC_THRESHOLD || abs(magneticFieldY) > MAGNETIC_THRESHOLD || abs(magneticFieldZ) > MAGNETIC_THRESHOLD) {
193    Serial.println("T error");
194    
195  }
196
197  if (abs(gyroX) > GYROSCOPE_THRESHOLD || abs(gyroY) > GYROSCOPE_THRESHOLD || abs(gyroZ) > GYROSCOPE_THRESHOLD) {
198    Serial.println("R error");
199    
200  }
201
202  if (abs(accelX) > ACCELEROMETER_THRESHOLD || abs(accelY) > ACCELEROMETER_THRESHOLD || abs(accelZ) > ACCELEROMETER_THRESHOLD) {
203    Serial.println("a error");
204    
205  }
206
207  delay(1000); 
208}