Mercurial > public > ostc4
annotate Discovery/Src/data_central.c @ 683:6f5a18bb25be Betatest
Bugfix: Set actual gas after basic dive settings were created:
Previous version showed unexpected warnings (ppo2 low) and deco data in simulator mode in case the dive mode had been changed before (e.g. from CCR to OC). Root cause was the update of the gas list before basic settings were set in the divesettings. To solve this the gas selection function has been moved behind the section initializing the gas variables.
| author | Ideenmodellierer |
|---|---|
| date | Mon, 16 May 2022 20:50:26 +0200 |
| parents | 8775d3dc6325 |
| children | 8a2337c7af52 |
| rev | line source |
|---|---|
| 38 | 1 /** |
| 2 ****************************************************************************** | |
| 3 * @copyright heinrichs weikamp | |
| 4 * @file data_central.c | |
| 5 * @author heinrichs weikamp gmbh | |
| 6 * @date 10-November-2014 | |
| 7 * @version V1.0.2 | |
| 8 * @since 10-Nov-2014 | |
| 9 * @brief All the data EXCEPT | |
| 10 * - settings (settings.c) | |
| 11 * feste Werte, die nur an der Oberfl�che ge�ndert werden | |
| 12 * - dataIn and dataOut (data_exchange.h and data_exchange_main.c) | |
| 13 * Austausch mit Small CPU | |
| 14 * @bug | |
| 15 * @warning | |
| 16 @verbatim | |
| 17 ============================================================================== | |
| 18 ##### SDiveState Real and Sim ##### | |
| 19 ============================================================================== | |
| 20 [..] SDiveSettings | |
| 21 copy of parts of Settings that are necessary during the dive | |
| 22 and could be modified during the dive without post dive changes. | |
| 23 | |
| 24 [..] SLifeData | |
| 25 written in DataEX_copy_to_LifeData(); | |
| 26 block 1 "lifedata" set by SmallCPU in stateReal | |
| 27 block 2 "actualGas" set by main CPU from user input and send to Small CPU | |
| 28 block 3 "calculated data" set by main CPU based on "lifedata" | |
| 29 | |
| 30 [..] SVpm | |
| 31 | |
| 32 [..] SEvents | |
| 33 | |
| 34 [..] SDecoinfo | |
| 35 | |
| 36 [..] mode | |
| 37 set by SmallCPU in stateReal, can be surface, dive, ... | |
| 38 | |
| 39 [..] data_old__lost_connection_to_slave | |
| 40 set by DataEX_copy_to_LifeData(); | |
| 41 | |
| 42 ============================================================================== | |
| 43 ##### SDiveState Deco ##### | |
| 44 ============================================================================== | |
| 45 [..] kjbkldafj�lasdfjasdf | |
| 46 | |
| 47 ============================================================================== | |
| 48 ##### decoLock ##### | |
| 49 ============================================================================== | |
| 50 [..] The handler that synchronizes the data between IRQ copy and main deco loop | |
| 51 | |
| 52 | |
| 53 @endverbatim | |
| 54 ****************************************************************************** | |
| 55 * @attention | |
| 56 * | |
| 57 * <h2><center>© COPYRIGHT(c) 2015 heinrichs weikamp</center></h2> | |
| 58 * | |
| 59 ****************************************************************************** | |
| 60 */ | |
| 61 | |
| 62 /* Includes ------------------------------------------------------------------*/ | |
| 63 #include <string.h> | |
|
539
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
64 #include <math.h> |
| 38 | 65 #include "data_central.h" |
| 66 #include "calc_crush.h" | |
| 67 #include "decom.h" | |
| 68 #include "stm32f4xx_hal.h" | |
| 69 #include "settings.h" | |
| 70 #include "data_exchange_main.h" | |
| 71 #include "ostc.h" // for button adjust on hw testboard 1 | |
| 72 #include "tCCR.h" | |
| 73 #include "crcmodel.h" | |
| 662 | 74 #include "configuration.h" |
| 38 | 75 |
|
225
2bb1db22b5f5
cleanup: random set of cleanups
Jan Mulder <jlmulder@xs4all.nl>
parents:
149
diff
changeset
|
76 static SDiveState stateReal = { 0 }; |
| 38 | 77 SDiveState stateSim = { 0 }; |
| 78 SDiveState stateDeco = { 0 }; | |
| 79 | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
80 static SDevice stateDevice = |
| 38 | 81 { |
| 82 /* max is 0x7FFFFFFF, min is 0x80000000 but also defined in stdint.h :-) */ | |
| 83 | |
| 84 /* count, use 0 */ | |
| 85 .batteryChargeCompleteCycles.value_int32 = 0, | |
| 86 .batteryChargeCycles.value_int32 = 0, | |
| 87 .diveCycles.value_int32 = 0, | |
| 88 .hoursOfOperation.value_int32 = 0, | |
| 89 | |
| 90 /* max values, use min. */ | |
| 91 .temperatureMaximum.value_int32 = INT32_MIN, | |
| 92 .depthMaximum.value_int32 = INT32_MIN, | |
| 93 | |
| 94 /* min values, use max. */ | |
| 95 .temperatureMinimum.value_int32 = INT32_MAX, | |
| 96 .voltageMinimum.value_int32 = INT32_MAX, | |
| 97 }; | |
| 98 | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
99 static SVpmRepetitiveData stateVPM = |
| 38 | 100 { |
| 101 .repetitive_variables_not_valid = 1, | |
| 102 .is_data_from_RTE_CPU = 0, | |
| 103 }; | |
| 104 | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
105 const SDiveState *stateUsed = &stateReal; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
106 SDiveState *stateUsedWrite = &stateReal; |
| 38 | 107 |
|
539
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
108 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
109 #define COMPASS_FRACTION (4.0f) /* delay till value changes to new actual */ |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
110 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
111 static float compass_compensated = 0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
112 |
| 38 | 113 void set_stateUsedToReal(void) |
| 114 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
115 stateUsed = stateUsedWrite = &stateReal; |
| 38 | 116 } |
| 117 | |
| 118 void set_stateUsedToSim(void) | |
| 119 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
120 stateUsed = stateUsedWrite = &stateSim; |
| 38 | 121 } |
| 122 | |
| 123 _Bool is_stateUsedSetToSim(void) | |
| 124 { | |
|
225
2bb1db22b5f5
cleanup: random set of cleanups
Jan Mulder <jlmulder@xs4all.nl>
parents:
149
diff
changeset
|
125 return stateUsed == &stateSim; |
| 38 | 126 } |
| 127 | |
| 128 const SDiveState * stateRealGetPointer(void) | |
| 129 { | |
| 130 return &stateReal; | |
| 131 } | |
| 132 | |
| 133 SDiveState * stateRealGetPointerWrite(void) | |
| 134 { | |
| 135 return &stateReal; | |
| 136 } | |
| 137 | |
| 138 | |
| 139 const SDiveState * stateSimGetPointer(void) | |
| 140 { | |
| 141 return &stateSim; | |
| 142 } | |
| 143 | |
| 144 | |
| 145 SDiveState * stateSimGetPointerWrite(void) | |
| 146 { | |
| 147 return &stateSim; | |
| 148 } | |
| 149 | |
| 150 | |
| 151 const SDevice * stateDeviceGetPointer(void) | |
| 152 { | |
| 153 return &stateDevice; | |
| 154 } | |
| 155 | |
| 156 | |
| 157 SDevice * stateDeviceGetPointerWrite(void) | |
| 158 { | |
| 159 return &stateDevice; | |
| 160 } | |
| 161 | |
| 162 | |
| 163 const SVpmRepetitiveData * stateVpmRepetitiveDataGetPointer(void) | |
| 164 { | |
| 165 return &stateVPM; | |
| 166 } | |
| 167 | |
| 168 | |
| 169 SVpmRepetitiveData * stateVpmRepetitiveDataGetPointerWrite(void) | |
| 170 { | |
| 171 return &stateVPM; | |
| 172 } | |
| 173 | |
| 174 | |
| 175 uint32_t time_elapsed_ms(uint32_t ticksstart,uint32_t ticksnow) | |
| 176 { | |
| 177 if(ticksstart <= ticksnow) | |
| 178 return ticksnow - ticksstart; | |
| 179 else | |
| 180 return 0xFFFFFFFF - ticksstart + ticksnow; | |
| 181 } | |
| 182 | |
| 183 | |
| 184 uint8_t decoLock = DECO_CALC_undefined; | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
185 |
|
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
186 static int descent_rate_meter_per_min = 20; |
|
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
187 static int max_depth = 70; |
|
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
188 static int bottom_time = 10; |
| 38 | 189 |
| 190 _Bool vpm_crush(SDiveState* pDiveState); | |
| 191 void setSimulationValues(int _ascent_rate_meter_per_min, int _descent_rate_meter_per_min, int _max_depth, int _bottom_time ) | |
| 192 { | |
| 193 descent_rate_meter_per_min = _descent_rate_meter_per_min; | |
| 194 max_depth = _max_depth; | |
| 195 bottom_time = _bottom_time; | |
| 196 } | |
| 197 | |
| 198 int current_second(void) { | |
| 199 | |
| 200 return HAL_GetTick() / 1000; | |
| 201 } | |
| 202 | |
| 203 #define OXY_ONE_SIXTIETH_PART 0.0166667f | |
| 204 | |
| 205 uint8_t calc_MOD(uint8_t gasId) | |
| 206 { | |
| 207 int16_t oxygen, maxppO2, result; | |
| 208 SSettings *pSettings; | |
| 209 | |
| 210 pSettings = settingsGetPointer(); | |
| 211 | |
| 212 oxygen = (int16_t)(pSettings->gas[gasId].oxygen_percentage); | |
| 213 | |
| 214 if(pSettings->gas[gasId].note.ub.deco > 0) | |
| 215 maxppO2 =(int16_t)(pSettings->ppO2_max_deco); | |
| 216 else | |
| 217 maxppO2 =(int16_t)(pSettings->ppO2_max_std); | |
| 218 | |
| 219 result = 10 * maxppO2; | |
| 220 result /= oxygen; | |
| 221 result -= 10; | |
| 222 | |
| 223 if(result < 0) | |
| 224 return 0; | |
| 225 | |
| 226 if(result > 255) | |
| 227 return 255; | |
| 228 | |
| 229 return result; | |
| 230 } | |
| 231 | |
| 232 float get_ambiant_pressure_simulation(long dive_time_seconds, float surface_pressure_bar ) | |
| 233 { | |
| 234 static | |
| 235 long descent_time; | |
| 236 float depth_meter; | |
| 237 | |
| 238 descent_time = 60 * max_depth / descent_rate_meter_per_min; | |
| 239 | |
| 240 if(dive_time_seconds <= descent_time) | |
| 241 { | |
| 242 depth_meter = ((float)(dive_time_seconds * descent_rate_meter_per_min)) / 60; | |
| 243 return surface_pressure_bar + depth_meter / 10; | |
| 244 } | |
| 245 //else if(dive_time_seconds <= (descent_time + bottom_time * 60)) | |
| 246 return surface_pressure_bar + max_depth / 10; | |
| 247 | |
| 248 | |
| 249 | |
| 250 } | |
| 251 | |
| 252 void UpdateLifeDataTest(SDiveState * pDiveState) | |
| 253 { | |
| 254 static int last_second = -1; | |
| 255 int now = current_second(); | |
| 256 if(last_second == now) | |
| 257 return; | |
| 258 last_second = now; | |
| 259 | |
| 260 pDiveState->lifeData.dive_time_seconds += 1; | |
| 261 pDiveState->lifeData.pressure_ambient_bar = get_ambiant_pressure_simulation(pDiveState->lifeData.dive_time_seconds,pDiveState->lifeData.pressure_surface_bar); | |
| 262 | |
| 263 pDiveState->lifeData.depth_meter = (pDiveState->lifeData.pressure_ambient_bar - pDiveState->lifeData.pressure_surface_bar) * 10.0f; | |
| 264 if(pDiveState->lifeData.max_depth_meter < pDiveState->lifeData.depth_meter) | |
| 265 pDiveState->lifeData.max_depth_meter = pDiveState->lifeData.depth_meter; | |
| 266 decom_tissues_exposure(1, &pDiveState->lifeData); | |
| 267 pDiveState->lifeData.ppO2 = decom_calc_ppO2( pDiveState->lifeData.pressure_ambient_bar, &pDiveState->lifeData.actualGas); | |
| 268 decom_oxygen_calculate_cns(& pDiveState->lifeData.cns, pDiveState->lifeData.ppO2); | |
| 269 | |
| 270 vpm_crush(pDiveState); | |
| 271 } | |
| 272 | |
| 273 | |
| 274 _Bool vpm_crush(SDiveState* pDiveState) | |
| 275 { | |
| 276 int i = 0; | |
| 277 static float starting_ambient_pressure = 0; | |
| 278 static float ending_ambient_pressure = 0; | |
| 279 static float time_calc_begin = -1; | |
| 280 static float initial_helium_pressure[16]; | |
| 281 static float initial_nitrogen_pressure[16]; | |
| 282 ending_ambient_pressure = pDiveState->lifeData.pressure_ambient_bar * 10; | |
| 283 | |
| 284 if((pDiveState->lifeData.dive_time_seconds <= 4) || (starting_ambient_pressure >= ending_ambient_pressure)) | |
| 285 { | |
| 286 time_calc_begin = pDiveState->lifeData.dive_time_seconds; | |
| 287 starting_ambient_pressure = pDiveState->lifeData.pressure_ambient_bar * 10; | |
| 288 for( i = 0; i < 16; i++) | |
| 289 { | |
| 290 initial_helium_pressure[i] = pDiveState->lifeData.tissue_helium_bar[i] * 10; | |
| 291 initial_nitrogen_pressure[i] = pDiveState->lifeData.tissue_nitrogen_bar[i] * 10; | |
| 292 } | |
| 293 return false; | |
| 294 } | |
| 295 if(pDiveState->lifeData.dive_time_seconds - time_calc_begin >= 4) | |
| 296 { | |
| 297 if(ending_ambient_pressure > starting_ambient_pressure + 0.5f) | |
| 298 { | |
| 299 float rate = (ending_ambient_pressure - starting_ambient_pressure) * 60 / 4; | |
| 300 calc_crushing_pressure(&pDiveState->lifeData, &pDiveState->vpm, initial_helium_pressure, initial_nitrogen_pressure, starting_ambient_pressure, rate); | |
| 301 | |
| 302 time_calc_begin = pDiveState->lifeData.dive_time_seconds; | |
| 303 starting_ambient_pressure = pDiveState->lifeData.pressure_ambient_bar * 10; | |
| 304 for( i = 0; i < 16; i++) | |
| 305 { | |
| 306 initial_helium_pressure[i] = pDiveState->lifeData.tissue_helium_bar[i] * 10; | |
| 307 initial_nitrogen_pressure[i] = pDiveState->lifeData.tissue_nitrogen_bar[i] * 10; | |
| 308 } | |
| 309 | |
| 310 return true; | |
| 311 } | |
| 312 | |
| 313 } | |
| 314 return false; | |
| 315 }; | |
| 316 | |
| 317 | |
| 318 void createDiveSettings(void) | |
| 319 { | |
| 662 | 320 int i; |
| 38 | 321 SSettings* pSettings = settingsGetPointer(); |
| 322 | |
| 323 stateReal.diveSettings.compassHeading = pSettings->compassBearing; | |
| 324 stateReal.diveSettings.ascentRate_meterperminute = 10; | |
| 325 | |
| 326 stateReal.diveSettings.diveMode = pSettings->dive_mode; | |
| 327 stateReal.diveSettings.CCR_Mode = pSettings->CCR_Mode; | |
| 662 | 328 if((stateReal.diveSettings.diveMode == DIVEMODE_PSCR) && (stateReal.diveSettings.CCR_Mode == CCRMODE_FixedSetpoint)) |
| 329 { | |
| 330 /* TODO: update selection of sensor used on/off (currently sensor/fixpoint). As PSCR has no fixed setpoint change to simulated ppo2 if sensors are not active */ | |
| 331 stateReal.diveSettings.CCR_Mode = CCRMODE_Simulation; | |
| 332 } | |
| 333 | |
| 334 if(isLoopMode(stateReal.diveSettings.diveMode)) | |
| 38 | 335 stateReal.diveSettings.ccrOption = 1; |
| 336 else | |
| 337 stateReal.diveSettings.ccrOption = 0; | |
| 338 memcpy(stateReal.diveSettings.gas, pSettings->gas,sizeof(pSettings->gas)); | |
| 339 memcpy(stateReal.diveSettings.setpoint, pSettings->setpoint,sizeof(pSettings->setpoint)); | |
|
683
6f5a18bb25be
Bugfix: Set actual gas after basic dive settings were created:
Ideenmodellierer
parents:
682
diff
changeset
|
340 |
|
6f5a18bb25be
Bugfix: Set actual gas after basic dive settings were created:
Ideenmodellierer
parents:
682
diff
changeset
|
341 setActualGasFirst(&stateReal.lifeData); |
|
6f5a18bb25be
Bugfix: Set actual gas after basic dive settings were created:
Ideenmodellierer
parents:
682
diff
changeset
|
342 |
| 38 | 343 stateReal.diveSettings.gf_high = pSettings->GF_high; |
| 344 stateReal.diveSettings.gf_low = pSettings->GF_low; | |
| 345 stateReal.diveSettings.input_next_stop_increment_depth_bar = ((float)pSettings->stop_increment_depth_meter) / 10.0f; | |
| 346 stateReal.diveSettings.last_stop_depth_bar = ((float)pSettings->last_stop_depth_meter) / 10.0f; | |
| 347 stateReal.diveSettings.vpm_conservatism = pSettings->VPM_conservatism.ub.standard; | |
| 348 stateReal.diveSettings.deco_type.uw = pSettings->deco_type.uw; | |
| 349 stateReal.diveSettings.fallbackOption = pSettings->fallbackToFixedSetpoint; | |
| 350 stateReal.diveSettings.ppo2sensors_deactivated = pSettings->ppo2sensors_deactivated; | |
| 351 stateReal.diveSettings.future_TTS_minutes = pSettings->future_TTS; | |
| 352 | |
| 662 | 353 stateReal.diveSettings.pscr_lung_ratio = pSettings->pscr_lung_ratio; |
| 354 stateReal.diveSettings.pscr_o2_drop = pSettings->pscr_o2_drop; | |
| 355 | |
| 356 if(stateReal.diveSettings.diveMode == DIVEMODE_PSCR) | |
| 357 { | |
| 358 for(i=0; i<5; i++) | |
| 359 { | |
| 360 stateReal.diveSettings.decogaslist[i].pscr_factor = 1.0 / stateReal.diveSettings.pscr_lung_ratio * stateReal.diveSettings.pscr_o2_drop; | |
| 361 } | |
| 362 } | |
| 363 | |
| 38 | 364 decom_CreateGasChangeList(&stateReal.diveSettings, &stateReal.lifeData); // decogaslist |
| 365 stateReal.diveSettings.internal__pressure_first_stop_ambient_bar_as_upper_limit_for_gf_low_otherwise_zero = 0; | |
| 366 | |
| 367 /* for safety */ | |
| 368 stateReal.diveSettings.input_second_to_last_stop_depth_bar = stateReal.diveSettings.last_stop_depth_bar + stateReal.diveSettings.input_next_stop_increment_depth_bar; | |
| 369 /* and the proper calc */ | |
| 662 | 370 for(i = 1; i <10; i++) |
| 38 | 371 { |
| 372 if(stateReal.diveSettings.input_next_stop_increment_depth_bar * i > stateReal.diveSettings.last_stop_depth_bar) | |
| 373 { | |
| 374 stateReal.diveSettings.input_second_to_last_stop_depth_bar = stateReal.diveSettings.input_next_stop_increment_depth_bar * i; | |
| 375 break; | |
| 376 } | |
| 377 } | |
| 378 } | |
| 379 | |
| 380 | |
| 381 void copyDiveSettingsToSim(void) | |
| 382 { | |
| 383 memcpy(&stateSim, &stateReal, sizeof(stateReal)); | |
| 384 } | |
| 385 | |
| 386 | |
| 387 void copyVpmRepetetiveDataToSim(void) | |
| 388 { | |
| 389 SDiveState * pSimData = stateSimGetPointerWrite(); | |
| 390 const SVpmRepetitiveData * pVpmData = stateVpmRepetitiveDataGetPointer(); | |
| 391 | |
| 392 if(pVpmData->is_data_from_RTE_CPU) | |
| 393 { | |
| 394 for(int i=0; i<16;i++) | |
| 395 { | |
| 396 pSimData->vpm.adjusted_critical_radius_he[i] = pVpmData->adjusted_critical_radius_he[i]; | |
| 397 pSimData->vpm.adjusted_critical_radius_n2[i] = pVpmData->adjusted_critical_radius_n2[i]; | |
| 398 | |
| 399 pSimData->vpm.adjusted_crushing_pressure_he[i] = pVpmData->adjusted_crushing_pressure_he[i]; | |
| 400 pSimData->vpm.adjusted_crushing_pressure_n2[i] = pVpmData->adjusted_crushing_pressure_n2[i]; | |
| 401 | |
| 402 pSimData->vpm.initial_allowable_gradient_he[i] = pVpmData->initial_allowable_gradient_he[i]; | |
| 403 pSimData->vpm.initial_allowable_gradient_n2[i] = pVpmData->initial_allowable_gradient_n2[i]; | |
| 404 | |
| 405 pSimData->vpm.max_actual_gradient[i] = pVpmData->max_actual_gradient[i]; | |
| 406 } | |
| 407 pSimData->vpm.repetitive_variables_not_valid = pVpmData->repetitive_variables_not_valid; | |
| 408 } | |
| 409 } | |
| 410 | |
| 411 | |
| 662 | 412 |
| 413 | |
| 38 | 414 void updateSetpointStateUsed(void) |
| 415 { | |
| 682 | 416 if(!isLoopMode(stateUsed->diveSettings.diveMode)) |
| 38 | 417 { |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
418 stateUsedWrite->lifeData.actualGas.setPoint_cbar = 0; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
419 stateUsedWrite->lifeData.ppO2 = decom_calc_ppO2(stateUsed->lifeData.pressure_ambient_bar, &stateUsed->lifeData.actualGas); |
| 38 | 420 } |
| 421 else | |
| 422 { | |
| 423 if(stateUsed->diveSettings.CCR_Mode == CCRMODE_Sensors) | |
| 424 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
425 stateUsedWrite->lifeData.actualGas.setPoint_cbar = get_ppO2SensorWeightedResult_cbar(); |
| 38 | 426 } |
| 662 | 427 #ifdef ENABLE_PSCR_MODE |
| 428 if(stateUsed->diveSettings.diveMode == DIVEMODE_PSCR) /* calculate a ppO2 value based on assumptions ( transfered approach from hwos code) */ | |
| 429 { | |
| 430 stateUsedWrite->lifeData.ppo2Simulated_bar = decom_calc_SimppO2_O2based(stateUsed->lifeData.pressure_ambient_bar, stateReal.diveSettings.gas[stateUsed->lifeData.actualGas.GasIdInSettings].oxygen_percentage, stateUsed->lifeData.actualGas.pscr_factor); | |
| 431 if(stateUsed->diveSettings.CCR_Mode == CCRMODE_Simulation) | |
| 432 { | |
| 433 stateUsedWrite->lifeData.actualGas.setPoint_cbar = stateUsedWrite->lifeData.ppo2Simulated_bar * 100; | |
| 434 } | |
| 435 } | |
| 436 #endif | |
| 437 /* limit calculated value to the physically possible if needed */ | |
| 38 | 438 if((stateUsed->lifeData.pressure_ambient_bar * 100) < stateUsed->lifeData.actualGas.setPoint_cbar) |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
439 stateUsedWrite->lifeData.ppO2 = stateUsed->lifeData.pressure_ambient_bar; |
| 38 | 440 else |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
441 stateUsedWrite->lifeData.ppO2 = ((float)stateUsed->lifeData.actualGas.setPoint_cbar) / 100; |
| 38 | 442 } |
| 443 } | |
| 444 | |
| 445 void setActualGasFirst(SLifeData *lifeData) | |
| 446 { | |
| 447 SSettings* pSettings = settingsGetPointer(); | |
| 448 uint8_t start = 0; | |
| 449 uint8_t gasId = 0; | |
| 450 uint8_t setpoint_cbar = 0; | |
| 451 | |
| 662 | 452 if(isLoopMode(pSettings->dive_mode)) |
| 38 | 453 { |
| 454 setpoint_cbar = pSettings->setpoint[1].setpoint_cbar; | |
| 455 start = NUM_OFFSET_DILUENT+1; | |
| 456 } | |
| 457 else | |
| 458 { | |
| 459 setpoint_cbar = 0; | |
| 460 start = 1; | |
| 461 } | |
| 462 | |
| 463 gasId = start; | |
| 464 for(int i=start;i<=NUM_GASES+start;i++) | |
| 465 { | |
| 466 if(pSettings->gas[i].note.ub.first) | |
| 467 { | |
| 468 gasId = i; | |
| 469 break; | |
| 470 } | |
| 471 } | |
| 472 setActualGas(lifeData, gasId, setpoint_cbar); | |
| 473 } | |
| 474 | |
| 475 void setActualGasAir(SLifeData *lifeData) | |
| 476 { | |
| 477 uint8_t nitrogen; | |
| 478 nitrogen = 79; | |
| 479 lifeData->actualGas.GasIdInSettings = 0; | |
| 480 lifeData->actualGas.nitrogen_percentage = nitrogen; | |
| 481 lifeData->actualGas.helium_percentage =0; | |
| 482 lifeData->actualGas.setPoint_cbar = 0; | |
| 483 lifeData->actualGas.change_during_ascent_depth_meter_otherwise_zero = 0; | |
| 662 | 484 lifeData->actualGas.AppliedDiveMode = stateUsed->diveSettings.diveMode; |
| 38 | 485 } |
| 486 | |
| 487 | |
| 488 void setActualGas(SLifeData *lifeData, uint8_t gasId, uint8_t setpoint_cbar) | |
| 489 { | |
| 490 SSettings* pSettings = settingsGetPointer(); | |
| 491 uint8_t nitrogen; | |
| 492 | |
| 493 nitrogen = 100; | |
| 494 nitrogen -= pSettings->gas[gasId].oxygen_percentage; | |
| 495 nitrogen -= pSettings->gas[gasId].helium_percentage; | |
| 496 | |
| 497 lifeData->actualGas.GasIdInSettings = gasId; | |
| 498 lifeData->actualGas.nitrogen_percentage = nitrogen; | |
| 499 lifeData->actualGas.helium_percentage = pSettings->gas[gasId].helium_percentage; | |
| 500 lifeData->actualGas.setPoint_cbar = setpoint_cbar; | |
| 501 lifeData->actualGas.change_during_ascent_depth_meter_otherwise_zero = 0; | |
| 682 | 502 lifeData->actualGas.AppliedDiveMode = stateUsed->diveSettings.diveMode; |
| 662 | 503 lifeData->actualGas.pscr_factor = 1.0 / pSettings->pscr_lung_ratio * pSettings->pscr_o2_drop; |
| 504 if(isLoopMode(pSettings->dive_mode) && (gasId > NUM_OFFSET_DILUENT)) | |
| 38 | 505 lifeData->lastDiluent_GasIdInSettings = gasId; |
| 506 } | |
| 507 | |
| 508 | |
| 509 void setActualGas_DM(SLifeData *lifeData, uint8_t gasId, uint8_t setpoint_cbar) | |
| 510 { | |
| 511 if(stateUsed->diveSettings.ccrOption && gasId < 6) | |
| 512 { | |
| 513 if(lifeData->actualGas.GasIdInSettings != gasId) | |
| 514 { | |
| 515 SSettings* pSettings = settingsGetPointer(); | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
516 stateUsedWrite->events.bailout = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
517 stateUsedWrite->events.info_bailoutO2 = pSettings->gas[gasId].oxygen_percentage; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
518 stateUsedWrite->events.info_bailoutHe = pSettings->gas[gasId].helium_percentage; |
| 38 | 519 } |
| 520 } | |
| 521 else | |
| 522 { | |
| 523 if(lifeData->actualGas.GasIdInSettings != gasId) | |
| 524 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
525 stateUsedWrite->events.gasChange = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
526 stateUsedWrite->events.info_GasChange = gasId; |
| 38 | 527 } |
| 528 if( lifeData->actualGas.setPoint_cbar != setpoint_cbar) | |
| 529 { | |
| 530 // setPoint_cbar = 255 -> change to sensor mode | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
531 stateUsedWrite->events.setpointChange = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
532 stateUsedWrite->events.info_SetpointChange = setpoint_cbar; |
| 38 | 533 } |
| 534 } | |
| 535 setActualGas(lifeData, gasId, setpoint_cbar); | |
| 536 } | |
| 537 | |
| 538 void setActualGas_ExtraGas(SLifeData *lifeData, uint8_t oxygen, uint8_t helium, uint8_t setpoint_cbar) | |
| 539 { | |
| 540 uint8_t nitrogen; | |
| 541 | |
| 542 nitrogen = 100; | |
| 543 nitrogen -= oxygen; | |
| 544 nitrogen -= helium; | |
| 545 | |
| 546 if((lifeData->actualGas.nitrogen_percentage != nitrogen) || (lifeData->actualGas.helium_percentage != helium)) | |
| 547 { | |
| 281 | 548 stateUsedWrite->events.manualGasSet = 1; |
| 549 stateUsedWrite->events.info_manualGasSetHe = helium; | |
| 550 stateUsedWrite->events.info_manualGasSetO2 = oxygen; | |
| 38 | 551 } |
| 552 if( lifeData->actualGas.setPoint_cbar != setpoint_cbar) | |
| 553 { | |
|
271
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
554 stateUsedWrite->events.setpointChange = 1; |
|
1303747b5ba2
cleanup: also write gas and setpoint changes in simulator mode
Jan Mulder <jlmulder@xs4all.nl>
parents:
270
diff
changeset
|
555 stateUsedWrite->events.info_SetpointChange = setpoint_cbar; |
| 38 | 556 } |
| 557 lifeData->actualGas.GasIdInSettings = 0; | |
| 558 lifeData->actualGas.nitrogen_percentage = nitrogen; | |
| 559 lifeData->actualGas.helium_percentage = helium; | |
| 560 lifeData->actualGas.setPoint_cbar = setpoint_cbar; | |
| 561 lifeData->actualGas.change_during_ascent_depth_meter_otherwise_zero = 0; | |
| 662 | 562 lifeData->actualGas.AppliedDiveMode = stateUsed->diveSettings.diveMode; |
| 38 | 563 } |
| 564 | |
| 565 void setButtonResponsiveness(uint8_t *ButtonSensitivyList) | |
| 566 { | |
| 567 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 568 | |
| 569 for(int i=0; i<4; i++) | |
| 570 { | |
| 571 pDataOut->data.buttonResponsiveness[i] = settingsHelperButtonSens_translate_percentage_to_hwOS_values(ButtonSensitivyList[i]); | |
| 572 } | |
| 573 pDataOut->setButtonSensitivityNow = 1; | |
| 574 } | |
| 575 | |
| 576 | |
| 577 void setDate(RTC_DateTypeDef Sdate) | |
| 578 { | |
| 579 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 580 | |
| 581 pDataOut->data.newDate = Sdate; | |
| 582 pDataOut->setDateNow = 1; | |
| 583 } | |
| 584 | |
| 585 | |
| 586 void setTime(RTC_TimeTypeDef Stime) | |
| 587 { | |
| 588 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 589 | |
| 590 pDataOut->data.newTime = Stime; | |
| 591 pDataOut->setTimeNow = 1; | |
| 592 } | |
| 593 | |
| 594 | |
| 595 void setBatteryPercentage(uint8_t newChargePercentage) | |
| 596 { | |
| 597 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 598 | |
| 599 pDataOut->data.newBatteryGaugePercentageFloat = settingsGetPointer()->lastKnownBatteryPercentage; | |
| 600 pDataOut->setBatteryGaugeNow = 1; | |
| 601 } | |
| 602 | |
| 603 | |
| 604 void calibrateCompass(void) | |
| 605 { | |
| 606 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 607 pDataOut->calibrateCompassNow = 1; | |
| 608 } | |
| 609 | |
| 610 | |
| 611 void clearDeco(void) | |
| 612 { | |
| 613 SDataReceiveFromMaster *pDataOut = dataOutGetPointer(); | |
| 614 pDataOut->clearDecoNow = 1; | |
| 615 | |
| 616 stateRealGetPointerWrite()->cnsHigh_at_the_end_of_dive = 0; | |
| 617 stateRealGetPointerWrite()->decoMissed_at_the_end_of_dive = 0; | |
| 618 } | |
| 619 | |
| 620 | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
621 static int32_t helper_days_from_civil(int32_t y, uint32_t m, uint32_t d) |
| 38 | 622 { |
| 623 y += 2000; | |
| 624 y -= m <= 2; | |
| 625 int32_t era = (y >= 0 ? y : y-399) / 400; | |
| 626 uint32_t yoe = (uint32_t)(y - era * 400); // [0, 399] | |
| 627 uint32_t doy = (153*(m + (m > 2 ? -3 : 9)) + 2)/5 + d-1; // [0, 365] | |
| 628 uint32_t doe = yoe * 365 + yoe/4 - yoe/100 + doy; // [0, 146096] | |
| 629 return era * 146097 + (int32_t)(doe) - 719468; | |
| 630 } | |
| 631 | |
| 632 | |
|
300
5ca177d2df5d
cleanup: remove commented/unused code, make static
Jan Mulder <jlmulder@xs4all.nl>
parents:
281
diff
changeset
|
633 static uint8_t helper_weekday_from_days(int32_t z) |
| 38 | 634 { |
| 635 return (uint8_t)(z >= -4 ? (z+4) % 7 : (z+5) % 7 + 6); | |
| 636 } | |
| 637 | |
| 638 | |
| 639 void setWeekday(RTC_DateTypeDef *sDate) | |
| 640 { | |
| 641 uint8_t day; | |
| 642 // [0, 6] -> [Sun, Sat] | |
| 643 day = helper_weekday_from_days(helper_days_from_civil(sDate->Year, sDate->Month, sDate->Date)); | |
| 644 // [1, 7] -> [Mon, Sun] | |
| 645 if(day == 0) | |
| 646 day = 7; | |
| 647 sDate->WeekDay = day; | |
| 648 } | |
| 649 | |
| 650 | |
| 651 void translateDate(uint32_t datetmpreg, RTC_DateTypeDef *sDate) | |
| 652 { | |
| 653 datetmpreg = (uint32_t)(datetmpreg & RTC_DR_RESERVED_MASK); | |
| 654 | |
| 655 /* Fill the structure fields with the read parameters */ | |
| 656 sDate->Year = (uint8_t)((datetmpreg & (RTC_DR_YT | RTC_DR_YU)) >> 16); | |
| 657 sDate->Month = (uint8_t)((datetmpreg & (RTC_DR_MT | RTC_DR_MU)) >> 8); | |
| 658 sDate->Date = (uint8_t)(datetmpreg & (RTC_DR_DT | RTC_DR_DU)); | |
| 659 sDate->WeekDay = (uint8_t)((datetmpreg & (RTC_DR_WDU)) >> 13); | |
| 660 | |
| 661 /* Convert the date structure parameters to Binary format */ | |
| 662 sDate->Year = (uint8_t)RTC_Bcd2ToByte(sDate->Year); | |
| 663 sDate->Month = (uint8_t)RTC_Bcd2ToByte(sDate->Month); | |
| 664 sDate->Date = (uint8_t)RTC_Bcd2ToByte(sDate->Date); | |
| 665 } | |
| 666 | |
| 667 void translateTime(uint32_t tmpreg, RTC_TimeTypeDef *sTime) | |
| 668 { | |
| 669 tmpreg = (uint32_t)(tmpreg & RTC_TR_RESERVED_MASK); | |
| 670 | |
| 671 /* Fill the structure fields with the read parameters */ | |
| 672 sTime->Hours = (uint8_t)((tmpreg & (RTC_TR_HT | RTC_TR_HU)) >> 16); | |
| 673 sTime->Minutes = (uint8_t)((tmpreg & (RTC_TR_MNT | RTC_TR_MNU)) >>8); | |
| 674 sTime->Seconds = (uint8_t)(tmpreg & (RTC_TR_ST | RTC_TR_SU)); | |
| 675 sTime->TimeFormat = (uint8_t)((tmpreg & (RTC_TR_PM)) >> 16); | |
| 676 | |
| 677 /* Convert the time structure parameters to Binary format */ | |
| 678 sTime->Hours = (uint8_t)RTC_Bcd2ToByte(sTime->Hours); | |
| 679 sTime->Minutes = (uint8_t)RTC_Bcd2ToByte(sTime->Minutes); | |
| 680 sTime->Seconds = (uint8_t)RTC_Bcd2ToByte(sTime->Seconds); | |
| 681 sTime->SubSeconds = 0; | |
| 682 } | |
| 683 | |
|
270
2e58a4094770
feature, debug: make simulator write a logbook entry
Jan Mulder <jlmulder@xs4all.nl>
parents:
225
diff
changeset
|
684 void resetEvents(const SDiveState *pStateUsed) |
| 38 | 685 { |
|
270
2e58a4094770
feature, debug: make simulator write a logbook entry
Jan Mulder <jlmulder@xs4all.nl>
parents:
225
diff
changeset
|
686 memset((void *)&pStateUsed->events, 0, sizeof(SEvents)); |
| 38 | 687 } |
| 688 | |
| 689 | |
| 690 uint32_t CRC_CalcBlockCRC_moreThan768000(uint32_t *buffer1, uint32_t *buffer2, uint32_t words) | |
| 691 { | |
| 692 cm_t crc_model; | |
| 693 uint32_t word_to_do; | |
| 694 uint8_t byte_to_do; | |
| 695 int i; | |
| 696 | |
| 697 // Values for the STM32F generator. | |
| 698 | |
| 699 crc_model.cm_width = 32; // 32-bit CRC | |
| 700 crc_model.cm_poly = 0x04C11DB7; // CRC-32 polynomial | |
| 701 crc_model.cm_init = 0xFFFFFFFF; // CRC initialized to 1's | |
| 702 crc_model.cm_refin = FALSE; // CRC calculated MSB first | |
| 703 crc_model.cm_refot = FALSE; // Final result is not bit-reversed | |
| 704 crc_model.cm_xorot = 0x00000000; // Final result XOR'ed with this | |
| 705 | |
| 706 cm_ini(&crc_model); | |
| 707 | |
| 708 while (words--) | |
| 709 { | |
| 710 // The STM32F10x hardware does 32-bit words at a time!!! | |
| 711 if(words > (768000/4)) | |
| 712 word_to_do = *buffer2++; | |
| 713 else | |
| 714 word_to_do = *buffer1++; | |
| 715 | |
| 716 // Do all bytes in the 32-bit word. | |
| 717 | |
| 718 for (i = 0; i < sizeof(word_to_do); i++) | |
| 719 { | |
| 720 // We calculate a *byte* at a time. If the CRC is MSB first we | |
| 721 // do the next MS byte and vica-versa. | |
| 722 | |
| 723 if (crc_model.cm_refin == FALSE) | |
| 724 { | |
| 725 // MSB first. Do the next MS byte. | |
| 726 | |
| 727 byte_to_do = (uint8_t) ((word_to_do & 0xFF000000) >> 24); | |
| 728 word_to_do <<= 8; | |
| 729 } | |
| 730 else | |
| 731 { | |
| 732 // LSB first. Do the next LS byte. | |
| 733 | |
| 734 byte_to_do = (uint8_t) (word_to_do & 0x000000FF); | |
| 735 word_to_do >>= 8; | |
| 736 } | |
| 737 | |
| 738 cm_nxt(&crc_model, byte_to_do); | |
| 739 } | |
| 740 } | |
| 741 | |
| 742 // Return the final result. | |
| 743 | |
| 744 return (cm_crc(&crc_model)); | |
| 745 } | |
| 746 | |
| 747 | |
| 748 uint32_t CRC_CalcBlockCRC(uint32_t *buffer, uint32_t words) | |
| 749 { | |
| 750 cm_t crc_model; | |
| 751 uint32_t word_to_do; | |
| 752 uint8_t byte_to_do; | |
| 753 int i; | |
| 754 | |
| 755 // Values for the STM32F generator. | |
| 756 | |
| 757 crc_model.cm_width = 32; // 32-bit CRC | |
| 758 crc_model.cm_poly = 0x04C11DB7; // CRC-32 polynomial | |
| 759 crc_model.cm_init = 0xFFFFFFFF; // CRC initialized to 1's | |
| 760 crc_model.cm_refin = FALSE; // CRC calculated MSB first | |
| 761 crc_model.cm_refot = FALSE; // Final result is not bit-reversed | |
| 762 crc_model.cm_xorot = 0x00000000; // Final result XOR'ed with this | |
| 763 | |
| 764 cm_ini(&crc_model); | |
| 765 | |
| 766 while (words--) | |
| 767 { | |
| 768 // The STM32F10x hardware does 32-bit words at a time!!! | |
| 769 | |
| 770 word_to_do = *buffer++; | |
| 771 | |
| 772 // Do all bytes in the 32-bit word. | |
| 773 | |
| 774 for (i = 0; i < sizeof(word_to_do); i++) | |
| 775 { | |
| 776 // We calculate a *byte* at a time. If the CRC is MSB first we | |
| 777 // do the next MS byte and vica-versa. | |
| 778 | |
| 779 if (crc_model.cm_refin == FALSE) | |
| 780 { | |
| 781 // MSB first. Do the next MS byte. | |
| 782 | |
| 783 byte_to_do = (uint8_t) ((word_to_do & 0xFF000000) >> 24); | |
| 784 word_to_do <<= 8; | |
| 785 } | |
| 786 else | |
| 787 { | |
| 788 // LSB first. Do the next LS byte. | |
| 789 | |
| 790 byte_to_do = (uint8_t) (word_to_do & 0x000000FF); | |
| 791 word_to_do >>= 8; | |
| 792 } | |
| 793 | |
| 794 cm_nxt(&crc_model, byte_to_do); | |
| 795 } | |
| 796 } | |
| 797 | |
| 798 // Return the final result. | |
| 799 | |
| 800 return (cm_crc(&crc_model)); | |
| 801 } | |
| 802 | |
|
302
eba8d1eb5bef
bugfix, cleanup: keep both is_ambient_pressure_close_to_surface in sync
Jan Mulder <jlmulder@xs4all.nl>
parents:
300
diff
changeset
|
803 // This code is also in RTE. Keep it in sync when editing |
| 38 | 804 _Bool is_ambient_pressure_close_to_surface(SLifeData *lifeData) |
| 805 { | |
|
310
95928ef3986f
Make dive mode detection more advanced
Jan Mulder <jlmulder@xs4all.nl>
parents:
302
diff
changeset
|
806 if (lifeData->pressure_ambient_bar > 1.16) |
|
95928ef3986f
Make dive mode detection more advanced
Jan Mulder <jlmulder@xs4all.nl>
parents:
302
diff
changeset
|
807 return false; |
|
95928ef3986f
Make dive mode detection more advanced
Jan Mulder <jlmulder@xs4all.nl>
parents:
302
diff
changeset
|
808 else if(lifeData->pressure_ambient_bar < (lifeData->pressure_surface_bar + 0.1f)) |
| 38 | 809 return true; |
| 810 else | |
| 811 return false; | |
| 812 } | |
|
539
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
813 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
814 void compass_Inertia(float newHeading) |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
815 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
816 float newTarget = newHeading; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
817 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
818 if(settingsGetPointer()->compassInertia == 0) |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
819 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
820 compass_compensated = newHeading; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
821 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
822 else |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
823 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
824 if((compass_compensated > 270.0) && (newHeading < 90.0)) /* transition passing 0 clockwise */ |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
825 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
826 newTarget = newHeading + 360.0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
827 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
828 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
829 if((compass_compensated < 90.0) && (newHeading > 270.0)) /* transition passing 0 counter clockwise */ |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
830 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
831 newTarget = newHeading - 360.0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
832 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
833 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
834 compass_compensated = compass_compensated + ((newTarget - compass_compensated) / (COMPASS_FRACTION * (settingsGetPointer()->compassInertia))); |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
835 if(compass_compensated < 0.0) |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
836 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
837 compass_compensated += 360.0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
838 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
839 if(compass_compensated >= 360.0) |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
840 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
841 compass_compensated -= 360.0; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
842 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
843 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
844 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
845 |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
846 float compass_getCompensated() |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
847 { |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
848 return compass_compensated; |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
849 } |
|
d784f281833a
Added inertia simulation for compass heading:
Ideenmodellierer
parents:
310
diff
changeset
|
850 |
| 662 | 851 uint8_t isLoopMode(uint8_t Mode) |
| 852 { | |
| 853 uint8_t retVal = 0; | |
| 854 if((Mode == DIVEMODE_CCR) || (Mode == DIVEMODE_PSCR)) | |
| 855 { | |
| 856 retVal = 1; | |
| 857 } | |
| 858 return retVal; | |
| 859 } |