AP_AccelCal: make collect_sample responsible for state changes

Also only set the snoop if the step has changed
This commit is contained in:
Francisco Ferreira 2016-11-12 09:38:40 +00:00 committed by Tom Pittenger
parent 34649ef66a
commit c07f377eb1
1 changed files with 3 additions and 4 deletions

View File

@ -45,8 +45,6 @@ void AP_AccelCal::update()
}
if(_start_collect_sample) {
collect_sample();
_gcs->set_snoop(nullptr);
_start_collect_sample = false;
}
switch(_status) {
case ACCEL_CAL_NOT_STARTED:
@ -96,9 +94,9 @@ void AP_AccelCal::update()
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);
}
// setup snooping of packets so we can see the COMMAND_ACK
_gcs->set_snoop(_snoop);
break;
}
case ACCEL_CAL_COLLECTING_SAMPLE:
@ -246,6 +244,7 @@ void AP_AccelCal::collect_sample()
}
// setup snooping of packets so we can see the COMMAND_ACK
_gcs->set_snoop(nullptr);
_start_collect_sample = false;
update_status();
}