@@ -10,21 +10,31 @@ typedef struct {
1010 bool completed ;
1111} CalibrationState ;
1212
13- static bool my_log_callback (mavsdk_log_level_t level , const char * message , const char * file , int line , void * user_data ) {
13+ static bool my_log_callback (
14+ mavsdk_log_level_t level , const char * message , const char * file , int line , void * user_data )
15+ {
1416 const char * level_str = "UNKNOWN" ;
1517 switch (level ) {
16- case MAVSDK_LOG_LEVEL_DEBUG : level_str = "DEBUG" ; break ;
17- case MAVSDK_LOG_LEVEL_INFO : level_str = "INFO" ; break ;
18- case MAVSDK_LOG_LEVEL_WARN : level_str = "WARN" ; break ;
19- case MAVSDK_LOG_LEVEL_ERROR : level_str = "ERROR" ; break ;
18+ case MAVSDK_LOG_LEVEL_DEBUG :
19+ level_str = "DEBUG" ;
20+ break ;
21+ case MAVSDK_LOG_LEVEL_INFO :
22+ level_str = "INFO" ;
23+ break ;
24+ case MAVSDK_LOG_LEVEL_WARN :
25+ level_str = "WARN" ;
26+ break ;
27+ case MAVSDK_LOG_LEVEL_ERROR :
28+ level_str = "ERROR" ;
29+ break ;
2030 }
21-
31+
2232 if (file ) {
2333 printf ("[%s] %s:%d - %s\n" , level_str , file , line , message );
2434 } else {
2535 printf ("[%s] %s\n" , level_str , message );
2636 }
27-
37+
2838 // Return true to prevent default logging, false to keep both
2939 return true;
3040}
@@ -35,13 +45,13 @@ static void calibration_callback(
3545 void * user_data )
3646{
3747 CalibrationState * state = (CalibrationState * )user_data ;
38-
48+
3949 switch (result ) {
4050 case MAVSDK_CALIBRATION_RESULT_SUCCESS :
4151 printf ("--- Calibration succeeded!\n" );
4252 state -> completed = true;
4353 break ;
44-
54+
4555 case MAVSDK_CALIBRATION_RESULT_NEXT :
4656 if (progress_data .has_progress ) {
4757 printf (" Progress: %.1f%%\n" , progress_data .progress );
@@ -50,52 +60,52 @@ static void calibration_callback(
5060 printf (" Instruction: %s\n" , progress_data .status_text );
5161 }
5262 break ;
53-
63+
5464 case MAVSDK_CALIBRATION_RESULT_FAILED :
5565 printf ("--- Calibration failed!\n" );
5666 state -> completed = true;
5767 break ;
58-
68+
5969 case MAVSDK_CALIBRATION_RESULT_NO_SYSTEM :
6070 printf ("--- Error: No system connected\n" );
6171 state -> completed = true;
6272 break ;
63-
73+
6474 case MAVSDK_CALIBRATION_RESULT_CONNECTION_ERROR :
6575 printf ("--- Error: Connection error\n" );
6676 state -> completed = true;
6777 break ;
68-
78+
6979 case MAVSDK_CALIBRATION_RESULT_BUSY :
7080 printf ("--- Error: Vehicle is busy\n" );
7181 state -> completed = true;
7282 break ;
73-
83+
7484 case MAVSDK_CALIBRATION_RESULT_COMMAND_DENIED :
7585 printf ("--- Error: Command denied\n" );
7686 state -> completed = true;
7787 break ;
78-
88+
7989 case MAVSDK_CALIBRATION_RESULT_TIMEOUT :
8090 printf ("--- Error: Command timed out\n" );
8191 state -> completed = true;
8292 break ;
83-
93+
8494 case MAVSDK_CALIBRATION_RESULT_CANCELLED :
8595 printf ("--- Calibration cancelled\n" );
8696 state -> completed = true;
8797 break ;
88-
98+
8999 case MAVSDK_CALIBRATION_RESULT_FAILED_ARMED :
90100 printf ("--- Error: Cannot calibrate while armed\n" );
91101 state -> completed = true;
92102 break ;
93-
103+
94104 case MAVSDK_CALIBRATION_RESULT_UNSUPPORTED :
95105 printf ("--- Error: Calibration not supported\n" );
96106 state -> completed = true;
97107 break ;
98-
108+
99109 default :
100110 printf ("--- Unknown calibration result\n" );
101111 state -> completed = true;
@@ -106,14 +116,11 @@ static void calibration_callback(
106116static void calibrate_accelerometer (mavsdk_calibration_t calibration )
107117{
108118 printf ("Calibrating accelerometer...\n" );
109-
119+
110120 CalibrationState state = {.completed = false};
111-
112- mavsdk_calibration_calibrate_accelerometer_async (
113- calibration ,
114- calibration_callback ,
115- & state );
116-
121+
122+ mavsdk_calibration_calibrate_accelerometer_async (calibration , calibration_callback , & state );
123+
117124 // Wait for calibration to complete
118125 while (!state .completed ) {
119126 sleep (1 );
@@ -123,14 +130,11 @@ static void calibrate_accelerometer(mavsdk_calibration_t calibration)
123130static void calibrate_gyro (mavsdk_calibration_t calibration )
124131{
125132 printf ("Calibrating gyro...\n" );
126-
133+
127134 CalibrationState state = {.completed = false};
128-
129- mavsdk_calibration_calibrate_gyro_async (
130- calibration ,
131- calibration_callback ,
132- & state );
133-
135+
136+ mavsdk_calibration_calibrate_gyro_async (calibration , calibration_callback , & state );
137+
134138 // Wait for calibration to complete
135139 while (!state .completed ) {
136140 sleep (1 );
@@ -140,14 +144,11 @@ static void calibrate_gyro(mavsdk_calibration_t calibration)
140144static void calibrate_magnetometer (mavsdk_calibration_t calibration )
141145{
142146 printf ("Calibrating magnetometer...\n" );
143-
147+
144148 CalibrationState state = {.completed = false};
145-
146- mavsdk_calibration_calibrate_magnetometer_async (
147- calibration ,
148- calibration_callback ,
149- & state );
150-
149+
150+ mavsdk_calibration_calibrate_magnetometer_async (calibration , calibration_callback , & state );
151+
151152 // Wait for calibration to complete
152153 while (!state .completed ) {
153154 sleep (1 );
@@ -157,49 +158,49 @@ static void calibrate_magnetometer(mavsdk_calibration_t calibration)
157158int main ()
158159{
159160 mavsdk_log_subscribe (my_log_callback , NULL );
160-
161+
161162 // Create MAVSDK instance
162163 mavsdk_configuration_t configuration =
163164 mavsdk_configuration_create_with_component_type (MAVSDK_COMPONENT_TYPE_GROUND_STATION );
164165 mavsdk_t mavsdk = mavsdk_create (configuration );
165166
166167 // Add connection
167- mavsdk_connection_result_t connection_result =
168- mavsdk_add_any_connection (mavsdk , "udpin://0.0.0.0:14540" );
169-
168+ mavsdk_connection_result_t connection_result =
169+ mavsdk_add_any_connection (mavsdk , "udpin://0.0.0.0:14540" );
170+
170171 if (connection_result != MAVSDK_CONNECTION_RESULT_SUCCESS ) {
171172 fprintf (stderr , "Connection failed: %d\n" , connection_result );
172173 mavsdk_destroy (mavsdk );
173174 return 1 ;
174175 }
175-
176+
176177 // Wait for autopilot
177178 printf ("Waiting for autopilot...\n" );
178179 mavsdk_system_t system = mavsdk_first_autopilot (mavsdk , 3 );
179-
180+
180181 if (!system ) {
181182 fprintf (stderr , "Timed out waiting for system\n" );
182183 mavsdk_destroy (mavsdk );
183184 return 1 ;
184185 }
185-
186+
186187 // Create calibration plugin
187188 mavsdk_calibration_t calibration = mavsdk_calibration_create (system );
188-
189+
189190 if (!calibration ) {
190191 fprintf (stderr , "Failed to create calibration plugin\n" );
191192 mavsdk_destroy (mavsdk );
192193 return 1 ;
193194 }
194-
195+
195196 // Run calibrations
196197 calibrate_accelerometer (calibration );
197198 calibrate_gyro (calibration );
198199 calibrate_magnetometer (calibration );
199-
200+
200201 // Cleanup
201202 mavsdk_calibration_destroy (calibration );
202203 mavsdk_destroy (mavsdk );
203-
204+
204205 return 0 ;
205206}
0 commit comments