static void begin_accept(pni_acceptor_t *acceptor, accept_result_t *result)
{
if (acceptor->listen_sock->closing) {
if (result) {
free(result);
acceptor->accept_queue_size--;
}
if (acceptor->accept_queue_size == 0)
acceptor->signalled = true;
return;
}
if (result) {
reset_accept_result(result);
} else {
if (acceptor->accept_queue_size < IOCP_MAX_ACCEPTS &&
pn_list_size(acceptor->accepts) == acceptor->accept_queue_size ) {
result = accept_result(acceptor->listen_sock);
acceptor->accept_queue_size++;
} else {
// an async accept is still pending or max concurrent accepts already hit
return;
}
}
result->new_sock = create_same_type_socket(acceptor->listen_sock);
if (result->new_sock) {
// Not yet connected.
result->new_sock->read_closed = true;
result->new_sock->write_closed = true;
bool success = acceptor->fn_accept_ex(acceptor->listen_sock->socket, result->new_sock->socket,
result->address_buffer, 0, IOCP_SOCKADDRMAXLEN, IOCP_SOCKADDRMAXLEN,
&result->unused, (LPOVERLAPPED) result);
if (!success && WSAGetLastError() != ERROR_IO_PENDING) {
result->base.status = WSAGetLastError();
pn_list_add(acceptor->accepts, result);
pni_events_update(acceptor->listen_sock, acceptor->listen_sock->events | PN_READABLE);
} else {
acceptor->listen_sock->ops_in_progress++;
// This socket is equally involved in the async operation.
result->new_sock->ops_in_progress++;
}
} else {
iocpdesc_fail(acceptor->listen_sock, WSAGetLastError(), "create accept socket");
}
}
// collect and avg sample to be passed onto LSQ estimator after all requisite orientations are done
void AccelCalibrator::new_sample(const Vector3f& delta_velocity, float dt) {
if (_status != ACCEL_CAL_COLLECTING_SAMPLE) {
return;
}
if (_samples_collected >= _conf_num_samples) {
set_status(ACCEL_CAL_FAILED);
return;
}
_sample_buffer[_samples_collected].delta_velocity += delta_velocity;
_sample_buffer[_samples_collected].delta_time += dt;
_last_samp_frag_collected_ms = AP_HAL::millis();
if (_sample_buffer[_samples_collected].delta_time > _conf_sample_time) {
Vector3f sample = _sample_buffer[_samples_collected].delta_velocity/_sample_buffer[_samples_collected].delta_time;
if (!accept_sample(sample)) {
set_status(ACCEL_CAL_FAILED);
return;
}
_samples_collected++;
if (_samples_collected >= _conf_num_samples) {
run_fit(MAX_ITERATIONS, _fitness);
if (_fitness < _conf_tolerance && accept_result()) {
set_status(ACCEL_CAL_SUCCESS);
} else {
set_status(ACCEL_CAL_FAILED);
}
} else {
set_status(ACCEL_CAL_WAITING_FOR_ORIENTATION);
}
}
}
