void AP_AccelCal::update()
{
if (!get_calibrator(0)) {
// no calibrators
return;
}
if (_started) {
update_status();
AccelCalibrator *cal;
uint8_t num_active_calibrators = 0;
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
num_active_calibrators++;
}
if (num_active_calibrators != _num_active_calibrators) {
fail();
return;
}
if(_start_collect_sample) {
collect_sample();
}
switch(_status) {
case ACCEL_CAL_NOT_STARTED:
fail();
return;
case ACCEL_CAL_WAITING_FOR_ORIENTATION: {
// if we're waiting for orientation, first ensure that all calibrators are on the same step
uint8_t step;
if ((cal = get_calibrator(0)) == nullptr) {
fail();
return;
}
step = cal->get_num_samples_collected()+1;
for(uint8_t i=1 ; (cal = get_calibrator(i)) ; i++) {
if (step != cal->get_num_samples_collected()+1) {
fail();
return;
}
}
// if we're on a new step, print a message describing the step
if (step != _step) {
_step = step;
if(_use_gcs_snoop) {
const char *msg;
switch (step) {
case ACCELCAL_VEHICLE_POS_LEVEL:
msg = "level";
break;
case ACCELCAL_VEHICLE_POS_LEFT:
msg = "on its LEFT side";
break;
case ACCELCAL_VEHICLE_POS_RIGHT:
msg = "on its RIGHT side";
break;
case ACCELCAL_VEHICLE_POS_NOSEDOWN:
msg = "nose DOWN";
break;
case ACCELCAL_VEHICLE_POS_NOSEUP:
msg = "nose UP";
break;
case ACCELCAL_VEHICLE_POS_BACK:
msg = "on its BACK";
break;
default:
fail();
return;
}
_printf("Place vehicle %s and press any key.", msg);
_waiting_for_mavlink_ack = true;
}
}
uint32_t now = AP_HAL::millis();
if (now - _last_position_request_ms > AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS) {
_last_position_request_ms = now;
_gcs->send_accelcal_vehicle_position(step);
}
break;
}
case ACCEL_CAL_COLLECTING_SAMPLE:
// check for timeout
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
cal->check_for_timeout();
}
update_status();
if (_status == ACCEL_CAL_FAILED) {
fail();
}
return;
case ACCEL_CAL_SUCCESS:
// save
if (_saving) {
bool done = true;
for(uint8_t i=0; i<_num_clients; i++) {
if (client_active(i) && _clients[i]->_acal_get_saving()) {
done = false;
break;
}
}
if (done) {
success();
}
return;
} else {
for(uint8_t i=0; i<_num_clients; i++) {
if(client_active(i) && _clients[i]->_acal_get_fail()) {
fail();
return;
}
}
for(uint8_t i=0; i<_num_clients; i++) {
if(client_active(i)) {
_clients[i]->_acal_save_calibrations();
}
}
_saving = true;
}
return;
default:
case ACCEL_CAL_FAILED:
fail();
return;
}
} else if (_last_result != ACCEL_CAL_NOT_STARTED) {
// only continuously report if we have ever completed a calibration
uint32_t now = AP_HAL::millis();
if (now - _last_position_request_ms > AP_ACCELCAL_POSITION_REQUEST_INTERVAL_MS) {
_last_position_request_ms = now;
switch (_last_result) {
case ACCEL_CAL_SUCCESS:
_gcs->send_accelcal_vehicle_position(ACCELCAL_VEHICLE_POS_SUCCESS);
break;
case ACCEL_CAL_FAILED:
_gcs->send_accelcal_vehicle_position(ACCELCAL_VEHICLE_POS_FAILED);
break;
default:
// should never hit this state
break;
}
}
}
}
void AP_AccelCal::update()
{
if (!get_calibrator(0)) {
// no calibrators
return;
}
if (_started) {
update_status();
AccelCalibrator *cal;
uint8_t num_active_calibrators = 0;
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
num_active_calibrators++;
}
if (num_active_calibrators != _num_active_calibrators) {
fail();
return;
}
if(_start_collect_sample) {
collect_sample();
_gcs->set_snoop(NULL);
_start_collect_sample = false;
}
switch(_status) {
case ACCEL_CAL_NOT_STARTED:
fail();
return;
case ACCEL_CAL_WAITING_FOR_ORIENTATION: {
// if we're waiting for orientation, first ensure that all calibrators are on the same step
uint8_t step;
if ((cal = get_calibrator(0)) == NULL) {
fail();
return;
}
step = cal->get_num_samples_collected()+1;
for(uint8_t i=1 ; (cal = get_calibrator(i)) ; i++) {
if (step != cal->get_num_samples_collected()+1) {
fail();
return;
}
}
// if we're on a new step, print a message describing the step
if (step != _step) {
_step = step;
const char *msg;
switch (step) {
case 1:
msg = "level";
break;
case 2:
msg = "on its LEFT side";
break;
case 3:
msg = "on its RIGHT side";
break;
case 4:
msg = "nose DOWN";
break;
case 5:
msg = "nose UP";
break;
case 6:
msg = "on its BACK";
break;
default:
fail();
return;
}
_printf("Place vehicle %s and press any key.", msg);
}
// setup snooping of packets so we can see the COMMAND_ACK
_gcs->set_snoop(_snoop);
break;
}
case ACCEL_CAL_COLLECTING_SAMPLE:
// check for timeout
for(uint8_t i=0; (cal = get_calibrator(i)); i++) {
cal->check_for_timeout();
}
update_status();
if (_status == ACCEL_CAL_FAILED) {
fail();
}
return;
case ACCEL_CAL_SUCCESS:
// save
if (_saving) {
bool done = true;
for(uint8_t i=0; i<_num_clients; i++) {
if (client_active(i) && _clients[i]->_acal_get_saving()) {
done = false;
break;
}
}
if (done) {
success();
}
return;
} else {
for(uint8_t i=0; i<_num_clients; i++) {
if(client_active(i) && _clients[i]->_acal_get_fail()) {
fail();
return;
}
}
for(uint8_t i=0; i<_num_clients; i++) {
if(client_active(i)) {
_clients[i]->_acal_save_calibrations();
}
}
_saving = true;
}
return;
default:
case ACCEL_CAL_FAILED:
fail();
return;
}
}
}