src/modules/commander/accelerometer_calibration.cpp
/*
 * Read specified number of accelerometer samples, calculate average and dispersion.
 */
Read specified number of accelerometer samples, calculate average and dispersion.
Read specified number of accelerometer samples calculate average and dispersion .
int read_accelerometer_avg(int (&subs)[max_sens], float (&accel_avg)[max_sens][6][3], unsigned orient, unsigned samples_num)
{
	struct pollfd fds[max_sens];
	for (unsigned i = 0; i < max_sens; i++) {
		fds[i].fd = subs[i];
		fds[i].events = POLLIN;
	}
	unsigned counts[max_sens] = { 0 };
	float accel_sum[max_sens][3];
	memset(accel_sum, 0, sizeof(accel_sum));
	unsigned errcount = 0;
	while (counts[0] < samples_num) {
		int poll_ret = poll(&fds[0], max_sens, 1000);
		if (poll_ret > 0) {
			for (unsigned s = 0; s < max_sens; s++) {
				bool changed;
				orb_check(subs[s], &changed);
				if (changed) {
					struct accel_report arp;
					orb_copy(ORB_ID(sensor_accel), subs[s], &arp);
					accel_sum[s][0] += arp.x;
					accel_sum[s][1] += arp.y;
					accel_sum[s][2] += arp.z;
					counts[s]++;
				}
			}
		} else {
			errcount++;
			continue;
		}
		if (errcount > samples_num / 10) {
			return ERROR;
		}
	}
	for (unsigned s = 0; s < max_sens; s++) {
		for (unsigned i = 0; i < 3; i++) {
			accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
			warnx("input: s:%u, axis: %u, orient: %u cnt: %u -> %8.4f", s, i, orient, counts[s], (double)accel_avg[s][orient][i]);
		}
	}
	return OK;
}
int read_accelerometer_avg ( int ( & subs ) [ max_sens ] , float ( & accel_avg ) [ max_sens ] [ 6 ] [ 3 ] , unsigned orient , unsigned samples_num ) { struct pollfd fds [ max_sens ] ; for ( unsigned i = 0 ; i < max_sens ; i ++ ) { fds [ i ] . fd = subs [ i ] ; fds [ i ] . events = POLLIN ; } unsigned counts [ max_sens ] = { 0 } ; float accel_sum [ max_sens ] [ 3 ] ; memset ( accel_sum , 0 , sizeof ( accel_sum ) ) ; unsigned errcount = 0 ; while ( counts [ 0 ] < samples_num ) { int poll_ret = poll ( & fds [ 0 ] , max_sens , 1000 ) ; if ( poll_ret > 0 ) { for ( unsigned s = 0 ; s < max_sens ; s ++ ) { bool changed ; orb_check ( subs [ s ] , & changed ) ; if ( changed ) { struct accel_report arp ; orb_copy ( ORB_ID ( sensor_accel ) , subs [ s ] , & arp ) ; accel_sum [ s ] [ 0 ] += arp . x ; accel_sum [ s ] [ 1 ] += arp . y ; accel_sum [ s ] [ 2 ] += arp . z ; counts [ s ] ++ ; } } } else { errcount ++ ; continue ; } if ( errcount > samples_num / 10 ) { return ERROR ; } } for ( unsigned s = 0 ; s < max_sens ; s ++ ) { for ( unsigned i = 0 ; i < 3 ; i ++ ) { accel_avg [ s ] [ orient ] [ i ] = accel_sum [ s ] [ i ] / counts [ s ] ; warnx ( " " , s , i , orient , counts [ s ] , ( double ) accel_avg [ s ] [ orient ] [ i ] ) ; } } return OK ; }
Read specified number of accelerometer samples, calculate average and dispersion.
Read specified number of accelerometer samples calculate average and dispersion .
/* use the first sensor to pace the readout, but do per-sensor counts */