void thin_samples()
{
if (_sample_buffer == null)
{
return;
}
if (_samples_collected == 0)
{
return;
}
_samples_thinned = 0;
// shuffle the samples http://en.wikipedia.org/wiki/Fisher%E2%80%93Yates_shuffle
// this is so that adjacent samples don't get sequentially eliminated
for (uint16_t i = (uint16_t)(_samples_collected - 1); i >= 1; i--)
{
uint16_t j = (uint16_t)(get_random16() % (i + 1));
CompassSample temp = _sample_buffer[i];
_sample_buffer[i] = _sample_buffer[j];
_sample_buffer[j] = temp;
}
for (uint16_t i = 0; i < _samples_collected; i++)
{
if (!accept_sample(_sample_buffer[i]))
{
_sample_buffer[i] = _sample_buffer[_samples_collected - 1];
_samples_collected--;
_samples_thinned++;
}
}
update_completion_mask();
}
bool accept_sample(CompassSample sample)
{
return(accept_sample(sample.get()));
}
bool set_status(compass_cal_status_t status)
{
if (status != compass_cal_status_t.COMPASS_CAL_NOT_STARTED && _status == status)
{
return(true);
}
switch (status)
{
case compass_cal_status_t.COMPASS_CAL_NOT_STARTED:
reset_state();
_status = compass_cal_status_t.COMPASS_CAL_NOT_STARTED;
if (_sample_buffer != null)
{
free(_sample_buffer);
_sample_buffer = null;
}
return(true);
case compass_cal_status_t.COMPASS_CAL_WAITING_TO_START:
reset_state();
_status = compass_cal_status_t.COMPASS_CAL_WAITING_TO_START;
set_status(compass_cal_status_t.COMPASS_CAL_RUNNING_STEP_ONE);
return(true);
case compass_cal_status_t.COMPASS_CAL_RUNNING_STEP_ONE:
if (_status != compass_cal_status_t.COMPASS_CAL_WAITING_TO_START)
{
return(false);
}
if (_attempt == 1 && (AP_HAL.millis() - _start_time_ms) * 1.0e-3f < _delay_start_sec)
{
return(false);
}
if (_sample_buffer == null)
{
_sample_buffer = new CompassSample[COMPASS_CAL_NUM_SAMPLES];
for (var i = 0; i < _sample_buffer.Length; i++)
{
_sample_buffer[i] = new CompassSample();
}
//(CompassSample) malloc(Marshal.SizeOf(CompassSample) *
// COMPASS_CAL_NUM_SAMPLES);
}
if (_sample_buffer != null)
{
initialize_fit();
_status = compass_cal_status_t.COMPASS_CAL_RUNNING_STEP_ONE;
return(true);
}
return(false);
case compass_cal_status_t.COMPASS_CAL_RUNNING_STEP_TWO:
if (_status != compass_cal_status_t.COMPASS_CAL_RUNNING_STEP_ONE)
{
return(false);
}
thin_samples();
initialize_fit();
_status = compass_cal_status_t.COMPASS_CAL_RUNNING_STEP_TWO;
return(true);
case compass_cal_status_t.COMPASS_CAL_SUCCESS:
if (_status != compass_cal_status_t.COMPASS_CAL_RUNNING_STEP_TWO)
{
return(false);
}
if (_sample_buffer != null)
{
free(_sample_buffer);
_sample_buffer = null;
}
_status = compass_cal_status_t.COMPASS_CAL_SUCCESS;
return(true);
case compass_cal_status_t.COMPASS_CAL_FAILED:
if (_status == compass_cal_status_t.COMPASS_CAL_NOT_STARTED)
{
return(false);
}
if (_retry && set_status(compass_cal_status_t.COMPASS_CAL_WAITING_TO_START))
{
_attempt++;
return(true);
}
if (_sample_buffer != null)
{
free(_sample_buffer);
_sample_buffer = null;
}
_status = compass_cal_status_t.COMPASS_CAL_FAILED;
return(true);
default:
return(false);
}
;
}
