src/modules/commander/accelerometer_calibration.cpp
/**
 * Wait for vehicle become still and detect it's orientation.
 *
 * @param mavlink_fd the MAVLink file descriptor to print output to
 * @param subs the accelerometer subscriptions. Only the first one will be used.
 *
 * @return 0..5 according to orientation when vehicle is still and ready for measurements,
 * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
 */
Wait for vehicle become still and detect it's orientation.
@param mavlink_fd the MAVLink file descriptor to print output to
@param subs the accelerometer subscriptions. Only the first one will be used.

@return 0..5 according to orientation when vehicle is still and ready for measurements,
ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
Wait for vehicle become still and detect it ' s orientation . @param mavlink_fd the MAVLink file descriptor to print output to @param subs the accelerometer subscriptions . Only the first one will be used . @return 0 .. 5 according to orientation when vehicle is still and ready for measurements ERROR if vehicle is not still after 30s or orientation error is more than 5m / s^2
int detect_orientation(int mavlink_fd, int (&subs)[max_sens])
{
	const unsigned ndim = 3;
	struct accel_report sensor;
	float accel_ema[ndim] = { 0.0f };
	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
	float ema_len = 0.5f;
	float still_thr2 = powf(0.25f, 2);
	float accel_err_thr = 5.0f;
	hrt_abstime still_time = 2000000;
	struct pollfd fds[1];
	fds[0].fd = subs[0];
	fds[0].events = POLLIN;
	hrt_abstime t_start = hrt_absolute_time();
	hrt_abstime timeout = 30000000;
	hrt_abstime t_timeout = t_start + timeout;
	hrt_abstime t = t_start;
	hrt_abstime t_prev = t_start;
	hrt_abstime t_still = 0;
	unsigned poll_errcount = 0;
	while (true) {
		int poll_ret = poll(fds, 1, 1000);
		if (poll_ret) {
			orb_copy(ORB_ID(sensor_accel), subs[0], &sensor);
			t = hrt_absolute_time();
			float dt = (t - t_prev) / 1000000.0f;
			t_prev = t;
			float w = dt / ema_len;
			for (unsigned i = 0; i < ndim; i++) {
				float di = 0.0f;
				switch (i) {
					case 0:
						di = sensor.x;
						break;
					case 1:
						di = sensor.y;
						break;
					case 2:
						di = sensor.z;
						break;
				}
				float d = di - accel_ema[i];
				accel_ema[i] += d * w;
				d = d * d;
				accel_disp[i] = accel_disp[i] * (1.0f - w);
				if (d > still_thr2 * 8.0f) {
					d = still_thr2 * 8.0f;
				}
				if (d > accel_disp[i]) {
					accel_disp[i] = d;
				}
			}
			if (accel_disp[0] < still_thr2 &&
			    accel_disp[1] < still_thr2 &&
			    accel_disp[2] < still_thr2) {
				if (t_still == 0) {
					mavlink_and_console_log_info(mavlink_fd, "detected rest position, hold still...");
					t_still = t;
					t_timeout = t + timeout;
				} else {
					if (t > t_still + still_time) {
						break;
					}
				}
			} else if (accel_disp[0] > still_thr2 * 4.0f ||
				   accel_disp[1] > still_thr2 * 4.0f ||
				   accel_disp[2] > still_thr2 * 4.0f) {
				if (t_still != 0) {
					mavlink_and_console_log_info(mavlink_fd, "detected motion, hold still...");
					sleep(3);
					t_still = 0;
				}
			}
		} else if (poll_ret == 0) {
			poll_errcount++;
		}
		if (t > t_timeout) {
			poll_errcount++;
		}
		if (poll_errcount > 1000) {
			mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
			return ERROR;
		}
	}
	if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return 0;        
	}
	if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return 1;        
	}
	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return 2;        
	}
	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
	    fabsf(accel_ema[2]) < accel_err_thr) {
		return 3;        
	}
	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
		return 4;        
	}
	if (fabsf(accel_ema[0]) < accel_err_thr &&
	    fabsf(accel_ema[1]) < accel_err_thr &&
	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
		return 5;        
	}
	mavlink_and_console_log_critical(mavlink_fd, "ERROR: invalid orientation");
	return ERROR;	
}
int detect_orientation ( int mavlink_fd , int ( & subs ) [ max_sens ] ) { const unsigned ndim = 3 ; struct accel_report sensor ; float accel_ema [ ndim ] = { 0.0f } ; float accel_disp [ 3 ] = { 0.0f , 0.0f , 0.0f } ; float ema_len = 0.5f ; float still_thr2 = powf ( 0.25f , 2 ) ; float accel_err_thr = 5.0f ; hrt_abstime still_time = 2000000 ; struct pollfd fds [ 1 ] ; fds [ 0 ] . fd = subs [ 0 ] ; fds [ 0 ] . events = POLLIN ; hrt_abstime t_start = hrt_absolute_time ( ) ; hrt_abstime timeout = 30000000 ; hrt_abstime t_timeout = t_start + timeout ; hrt_abstime t = t_start ; hrt_abstime t_prev = t_start ; hrt_abstime t_still = 0 ; unsigned poll_errcount = 0 ; while ( true ) { int poll_ret = poll ( fds , 1 , 1000 ) ; if ( poll_ret ) { orb_copy ( ORB_ID ( sensor_accel ) , subs [ 0 ] , & sensor ) ; t = hrt_absolute_time ( ) ; float dt = ( t - t_prev ) / 1000000.0f ; t_prev = t ; float w = dt / ema_len ; for ( unsigned i = 0 ; i < ndim ; i ++ ) { float di = 0.0f ; switch ( i ) { case 0 : di = sensor . x ; break ; case 1 : di = sensor . y ; break ; case 2 : di = sensor . z ; break ; } float d = di - accel_ema [ i ] ; accel_ema [ i ] += d * w ; d = d * d ; accel_disp [ i ] = accel_disp [ i ] * ( 1.0f - w ) ; if ( d > still_thr2 * 8.0f ) { d = still_thr2 * 8.0f ; } if ( d > accel_disp [ i ] ) { accel_disp [ i ] = d ; } } if ( accel_disp [ 0 ] < still_thr2 && accel_disp [ 1 ] < still_thr2 && accel_disp [ 2 ] < still_thr2 ) { if ( t_still == 0 ) { mavlink_and_console_log_info ( mavlink_fd , " " ) ; t_still = t ; t_timeout = t + timeout ; } else { if ( t > t_still + still_time ) { break ; } } } else if ( accel_disp [ 0 ] > still_thr2 * 4.0f || accel_disp [ 1 ] > still_thr2 * 4.0f || accel_disp [ 2 ] > still_thr2 * 4.0f ) { if ( t_still != 0 ) { mavlink_and_console_log_info ( mavlink_fd , " " ) ; sleep ( 3 ) ; t_still = 0 ; } } } else if ( poll_ret == 0 ) { poll_errcount ++ ; } if ( t > t_timeout ) { poll_errcount ++ ; } if ( poll_errcount > 1000 ) { mavlink_and_console_log_critical ( mavlink_fd , CAL_FAILED_SENSOR_MSG ) ; return ERROR ; } } if ( fabsf ( accel_ema [ 0 ] - CONSTANTS_ONE_G ) < accel_err_thr && fabsf ( accel_ema [ 1 ] ) < accel_err_thr && fabsf ( accel_ema [ 2 ] ) < accel_err_thr ) { return 0 ; } if ( fabsf ( accel_ema [ 0 ] + CONSTANTS_ONE_G ) < accel_err_thr && fabsf ( accel_ema [ 1 ] ) < accel_err_thr && fabsf ( accel_ema [ 2 ] ) < accel_err_thr ) { return 1 ; } if ( fabsf ( accel_ema [ 0 ] ) < accel_err_thr && fabsf ( accel_ema [ 1 ] - CONSTANTS_ONE_G ) < accel_err_thr && fabsf ( accel_ema [ 2 ] ) < accel_err_thr ) { return 2 ; } if ( fabsf ( accel_ema [ 0 ] ) < accel_err_thr && fabsf ( accel_ema [ 1 ] + CONSTANTS_ONE_G ) < accel_err_thr && fabsf ( accel_ema [ 2 ] ) < accel_err_thr ) { return 3 ; } if ( fabsf ( accel_ema [ 0 ] ) < accel_err_thr && fabsf ( accel_ema [ 1 ] ) < accel_err_thr && fabsf ( accel_ema [ 2 ] - CONSTANTS_ONE_G ) < accel_err_thr ) { return 4 ; } if ( fabsf ( accel_ema [ 0 ] ) < accel_err_thr && fabsf ( accel_ema [ 1 ] ) < accel_err_thr && fabsf ( accel_ema [ 2 ] + CONSTANTS_ONE_G ) < accel_err_thr ) { return 5 ; } mavlink_and_console_log_critical ( mavlink_fd , " " ) ; return ERROR ; }
Wait for vehicle become still and detect it's orientation.
Wait for vehicle become still and detect it ' s orientation .
/* exponential moving average of accel */ /* max-hold dispersion of accel */ /* EMA time constant in seconds*/ /* set "still" threshold to 0.25 m/s^2 */ /* set accel error threshold to 5m/s^2 */ /* still time required in us */ /* set timeout to 30s */ /* wait blocking for new data */ /* still detector with hysteresis */ /* is still now */ /* first time */ /* still since t_still */ /* vehicle is still, exit from the loop to detection of its orientation */ /* not still, reset still start time */ // [ g, 0, 0 ] // [ -g, 0, 0 ] // [ 0, g, 0 ] // [ 0, -g, 0 ] // [ 0, 0, g ] // [ 0, 0, -g ] // Can't detect orientation