fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-30 00:50:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/master' into linux

Browse Source 
Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Conflicts:
	src/modules/commander/accelerometer_calibration.cpp
This commit is contained in:
Mark Charlebois
2015-05-06 15:51:39 -07:00
parent 04b564920f 525e605b92
commit db52227409
20 changed files with 795 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+33
View File
@@ -409,7 +409,33 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
*/
calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
{
/* get total sensor board rotation matrix */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
param_t board_offset_x = param_find("SENS_BOARD_X_OFF");
param_t board_offset_y = param_find("SENS_BOARD_Y_OFF");
param_t board_offset_z = param_find("SENS_BOARD_Z_OFF");
float board_offset[3];
param_get(board_offset_x, &board_offset[0]);
param_get(board_offset_y, &board_offset[1]);
param_get(board_offset_z, &board_offset[2]);
math::Matrix<3, 3> board_rotation_offset;
board_rotation_offset.from_euler(M_DEG_TO_RAD_F * board_offset[0],
M_DEG_TO_RAD_F * board_offset[1],
M_DEG_TO_RAD_F * board_offset[2]);
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
math::Matrix<3, 3> board_rotation;
get_rot_matrix(board_rotation_id, &board_rotation);
/* combine board rotation with offset rotation */
board_rotation = board_rotation_offset * board_rotation;
px4_pollfd_struct_t fds[max_accel_sens];
struct pollfd fds[max_accel_sens];
for (unsigned i = 0; i < max_accel_sens; i++) {
fds[i].fd = subs[i];
@@ -455,6 +481,13 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc
}
}
// rotate sensor measurements from body frame into sensor frame using board rotation matrix
for (unsigned i = 0; i < max_accel_sens; i++) {
math::Vector<3> accel_sum_vec(&accel_sum[i][0]);
accel_sum_vec = board_rotation * accel_sum_vec;
memcpy(&accel_sum[i][0], &accel_sum_vec.data[0], sizeof(accel_sum[i]));
}
for (unsigned s = 0; s < max_accel_sens; s++) {
for (unsigned i = 0; i < 3; i++) {
accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];
src/modules/commander/commander_params.c
+14 -7
View File
@@ -111,7 +111,8 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
/** Datalink loss time threshold
/**
* Datalink loss time threshold
*
* After this amount of seconds without datalink the data link lost mode triggers
*
@@ -122,7 +123,8 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
*/
PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
/** Datalink regain time threshold
/**
* Datalink regain time threshold
*
* After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss'
* flag is set back to false
@@ -134,7 +136,8 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
*/
PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
/** Engine Failure Throttle Threshold
/**
* Engine Failure Throttle Threshold
*
* Engine failure triggers only above this throttle value
*
@@ -144,7 +147,8 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
*/
PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
/** Engine Failure Current/Throttle Threshold
/**
* Engine Failure Current/Throttle Threshold
*
* Engine failure triggers only below this current/throttle value
*
@@ -154,7 +158,8 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
*/
PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
/** Engine Failure Time Threshold
/**
* Engine Failure Time Threshold
*
* Engine failure triggers only if the throttle threshold and the
* current to throttle threshold are violated for this time
@@ -166,7 +171,8 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
*/
PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
/** RC loss time threshold
/**
* RC loss time threshold
*
* After this amount of seconds without RC connection the rc lost flag is set to true
*
@@ -177,7 +183,8 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
*/
PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5);
/** Autosaving of params
/**
* Autosaving of params
*
* If not equal to zero the commander will automatically save parameters to persistent storage once changed.
* Default is on, as the interoperability with currently deployed GCS solutions depends on parameters