public bool check_for_timeout()
{
uint32_t tnow = AP_HAL.millis();
if (running() && tnow - _last_sample_ms > 1000)
{
_retry = false;
set_status(compass_cal_status_t.COMPASS_CAL_FAILED);
return(true);
}
return(false);
}
public void start(bool retry, float delay, uint16_t offset_max)
{
if (running())
{
return;
}
_offset_max = offset_max;
_attempt = 1;
_retry = retry;
_delay_start_sec = delay;
_start_time_ms = AP_HAL.millis();
set_status(compass_cal_status_t.COMPASS_CAL_WAITING_TO_START);
}
public void new_sample(Vector3f sample)
{
_last_sample_ms = AP_HAL.millis();
if (_status == compass_cal_status_t.COMPASS_CAL_WAITING_TO_START)
{
set_status(compass_cal_status_t.COMPASS_CAL_RUNNING_STEP_ONE);
}
if (running() && _samples_collected < COMPASS_CAL_NUM_SAMPLES && accept_sample(sample))
{
update_completion_mask(sample);
_sample_buffer[_samples_collected].set(sample);
_samples_collected++;
}
}
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);
}
;
}
