config_applier.cpp
Go to the documentation of this file.
1 // File header
2 #include <config_applier.h>
3 
5 
9 //---------------------------------------------------------------------//
10 //- Constructor -//
11 //---------------------------------------------------------------------//
12 
13 ConfigApplier::ConfigApplier(SbgEComHandle &ref_sbg_com_handle):
14 m_reboot_needed_(false),
15 m_ref_sbg_com_handle(ref_sbg_com_handle)
16 {
17 
18 }
19 
20 //---------------------------------------------------------------------//
21 //- Private methods -//
22 //---------------------------------------------------------------------//
23 
24 void ConfigApplier::checkConfigurationGet(const SbgErrorCode& ref_sbg_error_code, const std::string& ref_conf_title) const
25 {
26  if (ref_sbg_error_code == SBG_INVALID_PARAMETER)
27  {
28  ROS_WARN("SBG_DRIVER - [Config] Configuration %s is not available for the connected device.", ref_conf_title.c_str());
29  }
30  else if (ref_sbg_error_code != SBG_NO_ERROR)
31  {
32  std::string error_message("[Config] Unable to get the ");
33  error_message.append(ref_conf_title);
34  error_message.append(" configuration : ");
35  error_message.append(sbgErrorCodeToString(ref_sbg_error_code));
36 
37  throw ros::Exception(error_message);
38  }
39 }
40 
41 void ConfigApplier::checkConfigurationApplied(const SbgErrorCode& ref_sbg_error_code, const std::string& ref_conf_title)
42 {
43  if (ref_sbg_error_code == SBG_INVALID_PARAMETER)
44  {
45  ROS_WARN("SBG_DRIVER - [Config] Configuration %s is not available for the connected device.", ref_conf_title.c_str());
46  }
47  else if (ref_sbg_error_code != SBG_NO_ERROR)
48  {
49  std::string error_message("[Config] Unable to set the ");
50  error_message.append(ref_conf_title);
51  error_message.append(" configuration : ");
52  error_message.append(sbgErrorCodeToString(ref_sbg_error_code));
53 
54  throw ros::Exception(error_message);
55  }
56  else
57  {
58  ROS_INFO("SBG_DRIVER - [Config] %s updated on the device.", ref_conf_title.c_str());
59  m_reboot_needed_ = true;
60  }
61 }
62 
64 {
65  //
66  // Get the initial condition of the device, compare with the loaded parameters.
67  // If the conditions are different, update the device configuration with the loaded parameters.
68  //
69  SbgEComInitConditionConf init_condition;
70  SbgErrorCode error_code;
71 
72  error_code = sbgEComCmdSensorGetInitCondition(&m_ref_sbg_com_handle, &init_condition);
73 
74  checkConfigurationGet(error_code, std::string("Init conditions"));
75 
76  if ((init_condition.year != ref_init_condition.year)
77  || (init_condition.month != ref_init_condition.month)
78  || (init_condition.day != ref_init_condition.day)
79  || !areEquals(init_condition.altitude, ref_init_condition.altitude)
80  || !areEquals(init_condition.latitude, ref_init_condition.latitude)
81  || !areEquals(init_condition.longitude, ref_init_condition.longitude))
82  {
83  error_code = sbgEComCmdSensorSetInitCondition(&m_ref_sbg_com_handle, &ref_init_condition);
84 
85  checkConfigurationApplied(error_code, std::string("Init conditions"));
86  }
87 }
88 
90 {
91  //
92  // Get the motion profile ID, and compare with the loaded one parameter.
93  // If the profiles are different, update the device with the loaded one.
94  //
95  SbgEComModelInfo motion_profile;
96  SbgErrorCode error_code;
97 
98  error_code = sbgEComCmdSensorGetMotionProfileInfo(&m_ref_sbg_com_handle, &motion_profile);
99 
100  checkConfigurationGet(error_code, std::string("Motion profile"));
101 
102  if (motion_profile.id != ref_motion_profile.id)
103  {
104  error_code = sbgEComCmdSensorSetMotionProfileId(&m_ref_sbg_com_handle, ref_motion_profile.id);
105 
106  checkConfigurationApplied(error_code, std::string("Motion profile"));
107  }
108 }
109 
111 {
112  //
113  // Get the IMU alignement and level arms, and compare with the parameters.
114  // If the alignement are differents, update the device with the loaded parameters.
115  //
116  SbgErrorCode error_code;
117  SbgEComSensorAlignmentInfo sensor_alignement;
118  float level_arms_device[3];
119 
120  error_code = sbgEComCmdSensorGetAlignmentAndLeverArm(&m_ref_sbg_com_handle, &sensor_alignement, level_arms_device);
121 
122  checkConfigurationGet(error_code, std::string("IMU alignement"));
123 
124  SbgVector3<float> level_arms_vector = SbgVector3<float>(level_arms_device, 3);
125 
126  if ((level_arms_vector != ref_level_arms)
127  || (sensor_alignement.axisDirectionX != ref_sensor_align.axisDirectionX)
128  || (sensor_alignement.axisDirectionY != ref_sensor_align.axisDirectionY)
129  || !areEquals(sensor_alignement.misRoll, ref_sensor_align.misRoll)
130  || !areEquals(sensor_alignement.misPitch, ref_sensor_align.misPitch)
131  || !areEquals(sensor_alignement.misYaw, ref_sensor_align.misYaw))
132  {
133  error_code = sbgEComCmdSensorSetAlignmentAndLeverArm(&m_ref_sbg_com_handle, &ref_sensor_align, level_arms_vector.data());
134 
135  checkConfigurationApplied(error_code, std::string("IMU alignement"));
136  }
137 }
138 
140 {
141  //
142  // Get the aiding assignement, and compare with the loaded parameters.
143  // If the assignement are differents, udpdate the device with the loaded parameters.
144  //
145  SbgEComAidingAssignConf aiding_assign;
146  SbgErrorCode error_code;
147 
148  error_code = sbgEComCmdSensorGetAidingAssignment(&m_ref_sbg_com_handle, &aiding_assign);
149 
150  checkConfigurationGet(error_code, std::string("Aiding assignement"));
151 
152  if ((aiding_assign.gps1Port != ref_aiding_assign.gps1Port)
153  || (aiding_assign.gps1Sync != ref_aiding_assign.gps1Sync)
154  || (aiding_assign.odometerPinsConf != ref_aiding_assign.odometerPinsConf)
155  || (aiding_assign.rtcmPort != ref_aiding_assign.rtcmPort))
156  {
157  error_code = sbgEComCmdSensorSetAidingAssignment(&m_ref_sbg_com_handle, &aiding_assign);
158 
159  checkConfigurationApplied(error_code, std::string("Aiding assignement"));
160  }
161 }
162 
164 {
165  //
166  // Get the magnetometer model, and compare with the loaded parameter.
167  // If the model are different, update the device with the loaded parameter.
168  //
169  SbgEComModelInfo model_info;
170  SbgErrorCode error_code;
171 
172  error_code = sbgEComCmdMagGetModelInfo(&m_ref_sbg_com_handle, &model_info);
173 
174  checkConfigurationGet(error_code, std::string("Magnetometer model"));
175 
176  if (model_info.id != ref_mag_model.id)
177  {
178  error_code = sbgEComCmdMagSetModelId(&m_ref_sbg_com_handle, ref_mag_model.id);
179 
180  checkConfigurationApplied(error_code, std::string("Magnetometer model"));
181  }
182 }
183 
185 {
186  //
187  // Get the magnetometer rejection model, and compare with the loaded parameter.
188  // If the model are different, update the device with the loaded parameter.
189  //
190  SbgEComMagRejectionConf mag_rejection;
191  SbgErrorCode error_code;
192 
193  error_code = sbgEComCmdMagGetRejection(&m_ref_sbg_com_handle, &mag_rejection);
194 
195  checkConfigurationGet(error_code, std::string("Magnetometer rejection"));
196 
197  if (mag_rejection.magneticField != ref_mag_rejection.magneticField)
198  {
199  error_code = sbgEComCmdMagSetRejection(&m_ref_sbg_com_handle, &ref_mag_rejection);
200 
201  checkConfigurationApplied(error_code, std::string("Magnetometer rejection"));
202  }
203 }
204 
206 {
207  //
208  // Get the Gnss model, and compare with the loaded model.
209  // If the models are different, update the device with the loaded model.
210  //
211  SbgEComModelInfo model_info;
212  SbgErrorCode error_code;
213 
214  error_code = sbgEComCmdGnss1GetModelInfo(&m_ref_sbg_com_handle, &model_info);
215 
216  checkConfigurationGet(error_code, std::string("Gnss model"));
217 
218  if (model_info.id != ref_gnss_model.id)
219  {
220  error_code = sbgEComCmdGnss1SetModelId(&m_ref_sbg_com_handle, ref_gnss_model.id);
221 
222  checkConfigurationApplied(error_code, std::string("Gnss model"));
223  }
224 }
225 
227 {
228  //
229  // Get the Gnss level arm, and compare with the loaded parameters.
230  // If the level arms are different, update the device with the loaded parameters.
231  //
232  SbgEComGnssInstallation gnss_installation;
233  SbgErrorCode error_code;
234  SbgVector3<float> gnss_device_primary;
235  SbgVector3<float> gnss_device_secondary;
236  SbgVector3<float> gnss_config_primary;
237  SbgVector3<float> gnss_config_secondary;
238 
239  error_code = sbgEComCmdGnss1InstallationGet(&m_ref_sbg_com_handle, &gnss_installation);
240 
241  checkConfigurationGet(error_code, std::string("Gnss level arms"));
242 
243  gnss_device_primary = SbgVector3<float>(gnss_installation.leverArmPrimary, 3);
244  gnss_device_secondary = SbgVector3<float>(gnss_installation.leverArmSecondary, 3);
245  gnss_config_primary = SbgVector3<float>(ref_gnss_installation.leverArmPrimary, 3);
246  gnss_config_secondary = SbgVector3<float>(ref_gnss_installation.leverArmSecondary, 3);
247 
248  if ((gnss_device_primary != gnss_config_primary)
249  || (gnss_device_secondary != gnss_config_secondary)
250  || (gnss_installation.leverArmPrimaryPrecise != ref_gnss_installation.leverArmPrimaryPrecise)
251  || (gnss_installation.leverArmSecondaryMode != ref_gnss_installation.leverArmSecondaryMode))
252  {
253  error_code = sbgEComCmdGnss1InstallationSet(&m_ref_sbg_com_handle, &ref_gnss_installation);
254 
255  checkConfigurationApplied(error_code, std::string("Gnss level arms"));
256  }
257 }
258 
260 {
261  //
262  // Get the Gnss rejection, and compare with the loaded parameters.
263  // If the rejection are different, update the device with the loaded parameters.
264  //
265  SbgEComGnssRejectionConf rejection;
266  SbgErrorCode error_code;
267 
268  error_code = sbgEComCmdGnss1GetRejection(&m_ref_sbg_com_handle, &rejection);
269 
270  checkConfigurationGet(error_code, std::string("Gnss rejection"));
271 
272  if ((rejection.hdt != ref_gnss_rejection.hdt)
273  || (rejection.position != ref_gnss_rejection.position)
274  || (rejection.velocity != ref_gnss_rejection.velocity))
275  {
276  error_code = sbgEComCmdGnss1SetRejection(&m_ref_sbg_com_handle, &ref_gnss_rejection);
277 
278  checkConfigurationApplied(error_code, std::string("Gnss rejection"));
279  }
280 }
281 
283 {
284  //
285  // Get the odometer configuration, and compare with the loaded parameters.
286  // If the conf are different, update the device with the loaded parameters.
287  //
288  SbgEComOdoConf odom_conf;
289  SbgErrorCode error_code;
290 
291  error_code = sbgEComCmdOdoGetConf(&m_ref_sbg_com_handle, &odom_conf);
292 
293  checkConfigurationGet(error_code, std::string("Odometer"));
294 
295  if (!areEquals(odom_conf.gain, ref_odometer.gain)
296  || (odom_conf.gainError != ref_odometer.gainError)
297  || (odom_conf.reverseMode != ref_odometer.reverseMode))
298  {
299  error_code = sbgEComCmdOdoSetConf(&m_ref_sbg_com_handle, &ref_odometer);
300 
301  checkConfigurationApplied(error_code, std::string("Odometer"));
302  }
303 }
304 
306 {
307  //
308  // Get the odometer level arm, and compare with the loaded parameters.
309  // If the level arms are different, update the device with the loaded parameters.
310  //
311  float lever_arm[3];
312  SbgErrorCode error_code;
313 
314  error_code = sbgEComCmdOdoGetLeverArm(&m_ref_sbg_com_handle, lever_arm);
315 
316  checkConfigurationGet(error_code, std::string("Odometer level arms"));
317 
318  SbgVector3<float> lever_arm_device = SbgVector3<float>(lever_arm, 3);
319 
320  if (lever_arm_device != odometer_level_arms)
321  {
322  error_code = sbgEComCmdOdoSetLeverArm(&m_ref_sbg_com_handle, lever_arm_device.data());
323 
324  checkConfigurationApplied(error_code, std::string("Odometer level arms"));
325  }
326 }
327 
329 {
330  //
331  // Get the odometer rejection mode, and compare with the loaded parameter.
332  // If the mode are different, update the device with the loaded parameter.
333  //
334  SbgEComOdoRejectionConf odom_rejection;
335  SbgErrorCode error_code;
336 
337  error_code = sbgEComCmdOdoGetRejection(&m_ref_sbg_com_handle, &odom_rejection);
338 
339  checkConfigurationGet(error_code, std::string("Odometer rejection"));
340 
341  if (odom_rejection.velocity != ref_odometer_rejection.velocity)
342  {
343  error_code = sbgEComCmdOdoSetRejection(&m_ref_sbg_com_handle, &ref_odometer_rejection);
344 
345  checkConfigurationApplied(error_code, std::string("Odometer rejection"));
346  }
347 }
348 
350 {
351  SbgErrorCode error_code;
352  SbgEComOutputMode current_output_mode;
353 
354  //
355  // Get the current output mode for the device and the selected log ID.
356  // If output modes are different, udpate the device mode with the one loaded from the parameters.
357  //
358  error_code = sbgEComCmdOutputGetConf(&m_ref_sbg_com_handle, output_port, ref_log_output.message_class, ref_log_output.message_id, &current_output_mode);
359 
360  if (error_code == SBG_INVALID_PARAMETER)
361  {
362  ROS_WARN("SBG_DRIVER - [Config] Output is not available for this device : Class [%d] - Id [%d]", ref_log_output.message_class, ref_log_output.message_id);
363  }
364  else if (error_code != SBG_NO_ERROR)
365  {
366  std::string error_message("[Config] Unable to get output for the device : Class [");
367  error_message.append(std::to_string(ref_log_output.message_class));
368  error_message.append("] - Id [");
369  error_message.append(std::to_string(ref_log_output.message_id));
370  error_message.append("] : ");
371  error_message.append(sbgErrorCodeToString(error_code));
372 
373  throw ros::Exception(error_message);
374  }
375  else if (current_output_mode != ref_log_output.output_mode)
376  {
377  error_code = sbgEComCmdOutputSetConf(&m_ref_sbg_com_handle, output_port, ref_log_output.message_class, ref_log_output.message_id, ref_log_output.output_mode);
378 
379  if (error_code != SBG_NO_ERROR)
380  {
381  std::string error_message("[Config] Unable to set the output configuration : Class[");
382  error_message.append(std::to_string(ref_log_output.message_class));
383  error_message.append("] - Id [");
384  error_message.append(std::to_string(ref_log_output.message_id));
385  error_message.append("] : ");
386  error_message.append(sbgErrorCodeToString(error_code));
387 
388  throw ros::Exception(error_message);
389  }
390  else
391  {
392  m_reboot_needed_ = true;
393  }
394  }
395 }
396 
397 //---------------------------------------------------------------------//
398 //- Parameters -//
399 //---------------------------------------------------------------------//
400 
401 //---------------------------------------------------------------------//
402 //- Operations -//
403 //---------------------------------------------------------------------//
404 
405 void ConfigApplier::applyConfiguration(const ConfigStore& ref_config_store)
406 {
407  //
408  // Configure the connected device.
409  //
410  configureInitCondition(ref_config_store.getInitialConditions());
411  configureMotionProfile(ref_config_store.getMotionProfile());
412  configureImuAlignement(ref_config_store.getSensorAlignement(), ref_config_store.getSensorLevelArms());
414  configureMagModel(ref_config_store.getMagnetometerModel());
416  configureGnssModel(ref_config_store.getGnssModel());
418  configureGnssRejection(ref_config_store.getGnssRejection());
419  configureOdometer(ref_config_store.getOdometerConf());
422 
423  //
424  // Configure the output, with all output defined in the store.
425  //
426  const std::vector<ConfigStore::SbgLogOutput>& ref_output_modes = ref_config_store.getOutputModes();
427 
428  for (const ConfigStore::SbgLogOutput& ref_output : ref_output_modes)
429  {
430  configureOutput(ref_config_store.getOutputPort(), ref_output);
431  }
432 
433  //
434  // Save configuration if needed.
435  //
436  if (m_reboot_needed_)
437  {
439  }
440 }
441 
443 {
444  SbgErrorCode error_code;
445 
447 
448  if (error_code != SBG_NO_ERROR)
449  {
450  ROS_ERROR("Unable to save the settings on the SBG device - %s", sbgErrorCodeToString(error_code));
451  }
452  else
453  {
454  ROS_INFO("SBG_DRIVER - Settings saved and device rebooted.");
455  }
456 }
SbgErrorCode sbgEComCmdSensorSetAlignmentAndLeverArm(SbgEComHandle *pHandle, const SbgEComSensorAlignmentInfo *pAlignConf, const float leverArm[3])
SbgErrorCode sbgEComCmdMagGetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo)
Definition: sbgEComCmdMag.c:29
SbgEComAxisDirection axisDirectionX
SbgErrorCode sbgEComCmdGnss1SetRejection(SbgEComHandle *pHandle, const SbgEComGnssRejectionConf *pRejectConf)
void configureImuAlignement(const SbgEComSensorAlignmentInfo &ref_sensor_align, const SbgVector3< float > &ref_level_arms)
SbgEComRejectionMode velocity
SbgErrorCode sbgEComCmdOdoGetConf(SbgEComHandle *pHandle, SbgEComOdoConf *pOdometerConf)
Definition: sbgEComCmdOdo.c:26
void configureMotionProfile(const SbgEComModelInfo &ref_motion_profile)
SbgEComOutputMode output_mode
Definition: config_store.h:71
Apply configuration to the device.
SbgEComModulePortAssignment gps1Port
const SbgEComModelInfo & getGnssModel(void) const
SbgErrorCode sbgEComCmdSettingsAction(SbgEComHandle *pHandle, SbgEComSettingsAction action)
void configureOutput(SbgEComOutputPort output_port, const ConfigStore::SbgLogOutput &ref_log_output)
const SbgEComMagRejectionConf & getMagnetometerRejection(void) const
SbgErrorCode sbgEComCmdSensorGetAidingAssignment(SbgEComHandle *pHandle, SbgEComAidingAssignConf *pConf)
void configureOdometerLevelArm(const SbgVector3< float > &odometer_level_arms)
static const char * sbgErrorCodeToString(SbgErrorCode errorCode)
Definition: sbgErrorCodes.h:72
SbgErrorCode sbgEComCmdOutputSetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode conf)
SbgEComAxisDirection axisDirectionY
SbgErrorCode sbgEComCmdSensorGetMotionProfileInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo)
const SbgEComGnssInstallation & getGnssInstallation(void) const
void configureMagRejection(const SbgEComMagRejectionConf &ref_mag_rejection)
SbgErrorCode sbgEComCmdGnss1SetModelId(SbgEComHandle *pHandle, uint32_t id)
SbgEComRejectionMode hdt
SbgErrorCode sbgEComCmdOdoGetLeverArm(SbgEComHandle *pHandle, float leverArm[3])
SbgEComRejectionMode velocity
Definition: sbgEComCmdOdo.h:44
SbgEComHandle & m_ref_sbg_com_handle
const T * data(void) const
Definition: sbg_vector3.h:172
#define ROS_WARN(...)
const SbgEComInitConditionConf & getInitialConditions(void) const
SbgEComGnssInstallationMode leverArmSecondaryMode
SbgEComModuleSyncAssignment gps1Sync
void configureOdometerRejection(const SbgEComOdoRejectionConf &ref_odometer_rejection)
SbgErrorCode sbgEComCmdSensorSetAidingAssignment(SbgEComHandle *pHandle, const SbgEComAidingAssignConf *pConf)
const SbgVector3< float > & getSensorLevelArms(void) const
SbgEComOutputPort getOutputPort(void) const
SbgEComModulePortAssignment rtcmPort
SbgErrorCode sbgEComCmdGnss1GetRejection(SbgEComHandle *pHandle, SbgEComGnssRejectionConf *pRejectConf)
void saveConfiguration(void)
SbgErrorCode sbgEComCmdGnss1InstallationSet(SbgEComHandle *pHandle, const SbgEComGnssInstallation *pGnssInstallation)
#define ROS_INFO(...)
const SbgEComOdoRejectionConf & getOdometerRejection(void) const
void configureGnssModel(const SbgEComModelInfo &ref_gnss_model)
const SbgVector3< float > & getOdometerLevelArms(void) const
SbgErrorCode sbgEComCmdOdoSetRejection(SbgEComHandle *pHandle, const SbgEComOdoRejectionConf *pRejectConf)
const SbgEComModelInfo & getMagnetometerModel(void) const
bool areEquals(T firstValue, T secondValue)
Definition: sbg_vector3.h:56
SbgErrorCode sbgEComCmdSensorGetInitCondition(SbgEComHandle *pHandle, SbgEComInitConditionConf *pConf)
SbgEComOdometerPinAssignment odometerPinsConf
const SbgEComGnssRejectionConf & getGnssRejection(void) const
SbgErrorCode sbgEComCmdOdoGetRejection(SbgEComHandle *pHandle, SbgEComOdoRejectionConf *pRejectConf)
SbgErrorCode sbgEComCmdMagSetModelId(SbgEComHandle *pHandle, uint32_t id)
Definition: sbgEComCmdMag.c:15
SbgErrorCode sbgEComCmdOutputGetConf(SbgEComHandle *pHandle, SbgEComOutputPort outputPort, SbgEComClass classId, SbgEComMsgId msgId, SbgEComOutputMode *pConf)
SbgErrorCode sbgEComCmdGnss1InstallationGet(SbgEComHandle *pHandle, SbgEComGnssInstallation *pGnssInstallation)
const SbgEComOdoConf & getOdometerConf(void) const
const std::vector< SbgLogOutput > & getOutputModes(void) const
void configureGnssRejection(const SbgEComGnssRejectionConf &ref_gnss_rejection)
SbgErrorCode sbgEComCmdGnss1GetModelInfo(SbgEComHandle *pHandle, SbgEComModelInfo *pModelInfo)
void applyConfiguration(const ConfigStore &ref_config_store)
void configureOdometer(const SbgEComOdoConf &ref_odometer)
void configureInitCondition(const SbgEComInitConditionConf &ref_init_condition)
enum _SbgEComOutputPort SbgEComOutputPort
void checkConfigurationGet(const SbgErrorCode &ref_sbg_error_code, const std::string &ref_conf_title) const
void configureAidingAssignement(const SbgEComAidingAssignConf &ref_aiding_assign)
const SbgEComModelInfo & getMotionProfile(void) const
SbgEComRejectionMode magneticField
SbgErrorCode sbgEComCmdOdoSetLeverArm(SbgEComHandle *pHandle, const float leverArm[3])
SbgErrorCode sbgEComCmdOdoSetConf(SbgEComHandle *pHandle, const SbgEComOdoConf *pOdometerConf)
Definition: sbgEComCmdOdo.c:92
SbgErrorCode sbgEComCmdMagGetRejection(SbgEComHandle *pHandle, SbgEComMagRejectionConf *pRejectConf)
SbgEComRejectionMode position
enum _SbgEComOutputMode SbgEComOutputMode
SbgErrorCode sbgEComCmdSensorSetMotionProfileId(SbgEComHandle *pHandle, uint32_t id)
void configureGnssInstallation(const SbgEComGnssInstallation &ref_gnss_installation)
#define ROS_ERROR(...)
void configureMagModel(const SbgEComModelInfo &ref_mag_model)
const SbgEComAidingAssignConf & getAidingAssignement(void) const
SbgErrorCode sbgEComCmdSensorSetInitCondition(SbgEComHandle *pHandle, const SbgEComInitConditionConf *pConf)
SbgErrorCode sbgEComCmdMagSetRejection(SbgEComHandle *pHandle, const SbgEComMagRejectionConf *pRejectConf)
enum _SbgErrorCode SbgErrorCode
const SbgEComSensorAlignmentInfo & getSensorAlignement(void) const
SbgErrorCode sbgEComCmdSensorGetAlignmentAndLeverArm(SbgEComHandle *pHandle, SbgEComSensorAlignmentInfo *pAlignConf, float leverArm[3])
void checkConfigurationApplied(const SbgErrorCode &ref_sbg_error_code, const std::string &ref_conf_title)


sbg_driver
Author(s): SBG Systems
autogenerated on Sat Sep 3 2022 02:53:35