@ -80,7 +80,7 @@
void do_accel_calibration ( int status_pub , struct vehicle_status_s * status , int mavlink_fd ) {
void do_accel_calibration ( int status_pub , struct vehicle_status_s * status , int mavlink_fd ) {
/* announce change */
/* announce change */
mavlink_log_info ( mavlink_fd , " accelerometer calibration started " ) ;
mavlink_log_info ( mavlink_fd , " accel calibration started " ) ;
/* set to accel calibration mode */
/* set to accel calibration mode */
status - > flag_preflight_accel_calibration = true ;
status - > flag_preflight_accel_calibration = true ;
state_machine_publish ( status_pub , status , mavlink_fd ) ;
state_machine_publish ( status_pub , status , mavlink_fd ) ;
@ -96,7 +96,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
| | param_set ( param_find ( " SENS_ACC_XSCALE " ) , & ( accel_scale [ 0 ] ) )
| | param_set ( param_find ( " SENS_ACC_XSCALE " ) , & ( accel_scale [ 0 ] ) )
| | param_set ( param_find ( " SENS_ACC_YSCALE " ) , & ( accel_scale [ 1 ] ) )
| | param_set ( param_find ( " SENS_ACC_YSCALE " ) , & ( accel_scale [ 1 ] ) )
| | param_set ( param_find ( " SENS_ACC_ZSCALE " ) , & ( accel_scale [ 2 ] ) ) ) {
| | param_set ( param_find ( " SENS_ACC_ZSCALE " ) , & ( accel_scale [ 2 ] ) ) ) {
mavlink_log_critical ( mavlink_fd , " Setting offs or scale failed! " ) ;
mavlink_log_critical ( mavlink_fd , " ERROR: setting offs or scale failed " ) ;
}
}
int fd = open ( ACCEL_DEVICE_PATH , 0 ) ;
int fd = open ( ACCEL_DEVICE_PATH , 0 ) ;
@ -122,7 +122,6 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
}
}
mavlink_log_info ( mavlink_fd , " accel calibration done " ) ;
mavlink_log_info ( mavlink_fd , " accel calibration done " ) ;
tune_confirm ( ) ;
tune_confirm ( ) ;
sleep ( 2 ) ;
sleep ( 2 ) ;
tune_confirm ( ) ;
tune_confirm ( ) ;
@ -142,12 +141,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m
int do_accel_calibration_mesurements ( int mavlink_fd , float accel_offs_scaled [ 3 ] , float accel_scale [ 3 ] ) {
int do_accel_calibration_mesurements ( int mavlink_fd , float accel_offs_scaled [ 3 ] , float accel_scale [ 3 ] ) {
const int samples_num = 2500 ;
const int samples_num = 2500 ;
int sensor_combined_sub = orb_subscribe ( ORB_ID ( sensor_combined ) ) ;
int16_t accel_raw_ref [ 6 ] [ 3 ] ;
int16_t accel_raw_ref [ 6 ] [ 3 ] ;
bool data_collected [ 6 ] = { false , false , false , false , false , false } ;
bool data_collected [ 6 ] = { false , false , false , false , false , false } ;
const char * orientation_strs [ 6 ] = { " x+ " , " x- " , " y+ " , " y- " , " z+ " , " z- " } ;
const char * orientation_strs [ 6 ] = { " x+ " , " x- " , " y+ " , " y- " , " z+ " , " z- " } ;
int sensor_combined_sub = orb_subscribe ( ORB_ID ( sensor_combined ) ) ;
while ( true ) {
while ( true ) {
bool done = true ;
bool done = true ;
char str [ 80 ] ;
char str [ 80 ] ;
@ -159,25 +157,21 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3],
done = false ;
done = false ;
}
}
}
}
if ( done ) {
if ( done )
mavlink_log_info ( mavlink_fd , " all accel measurements complete " ) ;
break ;
break ;
} else {
mavlink_log_info ( mavlink_fd , str ) ;
mavlink_log_info ( mavlink_fd , str ) ;
int orient = detect_orientation ( mavlink_fd , sensor_combined_sub ) ;
int orient = detect_orientation ( mavlink_fd , sensor_combined_sub ) ;
if ( orient < 0 ) {
if ( orient < 0 )
sprintf ( str , " orientation detection error: %i " , orient ) ;
return ERROR ;
mavlink_log_info ( mavlink_fd , str ) ;
return ERROR ;
sprintf ( str , " meas started: %s " , orientation_strs [ orient ] ) ;
}
mavlink_log_info ( mavlink_fd , str ) ;
mavlink_log_info ( mavlink_fd , " accel measurement started " ) ;
read_accelerometer_raw_avg ( sensor_combined_sub , & ( accel_raw_ref [ orient ] [ 0 ] ) , samples_num ) ;
read_accelerometer_raw_avg ( sensor_combined_sub , & ( accel_raw_ref [ orient ] [ 0 ] ) , samples_num ) ;
str_ptr = sprintf ( str , " meas result for %s: [ %i %i %i ] " , orientation_strs [ orient ] , accel_raw_ref [ orient ] [ 0 ] , accel_raw_ref [ orient ] [ 1 ] , accel_raw_ref [ orient ] [ 2 ] ) ;
//mavlink_log_info(mavlink_fd, "accel measurement complete");
mavlink_log_info ( mavlink_fd , str ) ;
str_ptr = sprintf ( str , " complete: %i [ %i %i %i ] " , orient , accel_raw_ref [ orient ] [ 0 ] , accel_raw_ref [ orient ] [ 1 ] , accel_raw_ref [ orient ] [ 2 ] ) ;
data_collected [ orient ] = true ;
mavlink_log_info ( mavlink_fd , str ) ;
tune_confirm ( ) ;
data_collected [ orient ] = true ;
tune_confirm ( ) ;
}
}
}
close ( sensor_combined_sub ) ;
close ( sensor_combined_sub ) ;
@ -186,26 +180,16 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3],
float accel_T [ 3 ] [ 3 ] ;
float accel_T [ 3 ] [ 3 ] ;
int res = calculate_calibration_values ( accel_raw_ref , accel_T , accel_offs , CONSTANTS_ONE_G ) ;
int res = calculate_calibration_values ( accel_raw_ref , accel_T , accel_offs , CONSTANTS_ONE_G ) ;
if ( res ! = 0 ) {
if ( res ! = 0 ) {
mavlink_log_info ( mavlink_fd , " calibration values calculation error " ) ;
mavlink_log_info ( mavlink_fd , " ERROR: calibration values calc error" ) ;
return ERROR ;
return ERROR ;
}
}
char str [ 80 ] ;
sprintf ( str , " accel offsets: [ %i %i %i ] " , accel_offs [ 0 ] , accel_offs [ 1 ] , accel_offs [ 2 ] ) ;
mavlink_log_info ( mavlink_fd , str ) ;
//mavlink_log_info(mavlink_fd, "accel transform matrix:");
for ( int i = 0 ; i < 3 ; i + + ) {
//sprintf(str, "\t[ %0.6f %0.6f %0.6f ]", accel_T[i][0], accel_T[i][1], accel_T[i][2]);
//mavlink_log_info(mavlink_fd, str);
}
/* convert raw accel offset to scaled and transform matrix to scales
/* convert raw accel offset to scaled and transform matrix to scales
* rotation part of transform matrix is not used by now */
* rotation part of transform matrix is not used by now */
for ( int i = 0 ; i < 3 ; i + + ) {
for ( int i = 0 ; i < 3 ; i + + ) {
accel_offs_scaled [ i ] = accel_offs [ i ] * accel_T [ i ] [ i ] ;
accel_offs_scaled [ i ] = accel_offs [ i ] * accel_T [ i ] [ i ] ;
accel_scale [ i ] = accel_T [ i ] [ i ] ;
accel_scale [ i ] = accel_T [ i ] [ i ] ;
}
}
return OK ;
return OK ;
}
}
@ -226,8 +210,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
float ema_len = 0.2f ;
float ema_len = 0.2f ;
/* set "still" threshold to 0.005 m/s^2 */
/* set "still" threshold to 0.005 m/s^2 */
float still_thr2 = pow ( 0.05f / CONSTANTS_ONE_G , 2 ) ;
float still_thr2 = pow ( 0.05f / CONSTANTS_ONE_G , 2 ) ;
/* set accel error threshold to 2 0% of accel vector length */
/* set accel error threshold to 3 0% of accel vector length */
float accel_err_thr = 0.2 f ;
float accel_err_thr = 0.3 f ;
/* still time required in us */
/* still time required in us */
int64_t still_time = 2000000 ;
int64_t still_time = 2000000 ;
struct pollfd fds [ 1 ] = { { . fd = sub_sensor_combined , . events = POLLIN } } ;
struct pollfd fds [ 1 ] = { { . fd = sub_sensor_combined , . events = POLLIN } } ;
@ -264,7 +248,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
/* is still now */
/* is still now */
if ( t_still = = 0 ) {
if ( t_still = = 0 ) {
/* first time */
/* first time */
mavlink_log_info ( mavlink_fd , " still " ) ;
mavlink_log_info ( mavlink_fd , " still... " ) ;
t_still = t ;
t_still = t ;
t_timeout = t + timeout ;
t_timeout = t + timeout ;
} else {
} else {
@ -279,7 +263,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
accel_disp [ 2 ] > still_thr_raw2 * 2.0f ) {
accel_disp [ 2 ] > still_thr_raw2 * 2.0f ) {
/* not still, reset still start time */
/* not still, reset still start time */
if ( t_still ! = 0 ) {
if ( t_still ! = 0 ) {
mavlink_log_info ( mavlink_fd , " moving " ) ;
mavlink_log_info ( mavlink_fd , " moving... " ) ;
t_still = 0 ;
t_still = 0 ;
}
}
}
}
@ -289,17 +273,11 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
return - 3 ;
return - 3 ;
}
}
if ( t > t_timeout ) {
if ( t > t_timeout ) {
mavlink_log_info ( mavlink_fd , " ERROR: timeout " ) ;
return - 1 ;
return - 1 ;
}
}
}
}
float accel_len = sqrt ( accel_len2 ) ;
float accel_len = sqrt ( accel_len2 ) ;
float accel_err_thr_raw = accel_len * accel_err_thr ;
float accel_err_thr_raw = accel_len * accel_err_thr ;
char str [ 80 ] ;
sprintf ( str , " ema: [ %.1f %.1f %.1f ] " , accel_ema [ 0 ] , accel_ema [ 1 ] , accel_ema [ 2 ] ) ;
mavlink_log_info ( mavlink_fd , str ) ;
sprintf ( str , " disp: [ %.1f %.1f %.1f ] " , accel_disp [ 0 ] , accel_disp [ 1 ] , accel_disp [ 2 ] ) ;
mavlink_log_info ( mavlink_fd , str ) ;
if ( fabs ( accel_ema [ 0 ] - accel_len ) < accel_err_thr_raw & &
if ( fabs ( accel_ema [ 0 ] - accel_len ) < accel_err_thr_raw & &
fabs ( accel_ema [ 1 ] ) < accel_err_thr_raw & &
fabs ( accel_ema [ 1 ] ) < accel_err_thr_raw & &
fabs ( accel_ema [ 2 ] ) < accel_err_thr_raw )
fabs ( accel_ema [ 2 ] ) < accel_err_thr_raw )
@ -316,13 +294,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
fabs ( accel_ema [ 1 ] + accel_len ) < accel_err_thr_raw & &
fabs ( accel_ema [ 1 ] + accel_len ) < accel_err_thr_raw & &
fabs ( accel_ema [ 2 ] ) < accel_err_thr_raw )
fabs ( accel_ema [ 2 ] ) < accel_err_thr_raw )
return 3 ; // [ 0, -g, 0 ]
return 3 ; // [ 0, -g, 0 ]
if ( abs ( accel_ema [ 0 ] ) < accel_err_thr_raw & &
if ( f abs( accel_ema [ 0 ] ) < accel_err_thr_raw & &
abs ( accel_ema [ 1 ] ) < accel_err_thr_raw & &
f abs( accel_ema [ 1 ] ) < accel_err_thr_raw & &
abs ( accel_ema [ 2 ] - accel_len ) < accel_err_thr_raw )
f abs( accel_ema [ 2 ] - accel_len ) < accel_err_thr_raw )
return 4 ; // [ 0, 0, g ]
return 4 ; // [ 0, 0, g ]
if ( abs ( accel_ema [ 0 ] ) < accel_err_thr_raw & &
if ( f abs( accel_ema [ 0 ] ) < accel_err_thr_raw & &
abs ( accel_ema [ 1 ] ) < accel_err_thr_raw & &
f abs( accel_ema [ 1 ] ) < accel_err_thr_raw & &
abs ( accel_ema [ 2 ] + accel_len ) < accel_err_thr_raw )
f abs( accel_ema [ 2 ] + accel_len ) < accel_err_thr_raw )
return 5 ; // [ 0, 0, -g ]
return 5 ; // [ 0, 0, -g ]
mavlink_log_info ( mavlink_fd , " ERROR: invalid orientation " ) ;
mavlink_log_info ( mavlink_fd , " ERROR: invalid orientation " ) ;
return - 2 ; // Can't detect orientation
return - 2 ; // Can't detect orientation