fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Thomas Gubler 2013-11-03 18:38:34 +01:00
commit fa4533e359
6 changed files with 57 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/lib/external_lgpl/tecs/tecs.h
View File

@ -54,7 +54,9 @@ public:
_SPE_est(0.0f),
_SKE_est(0.0f),
_SPEdot(0.0f),
_SKEdot(0.0f) {
_SKEdot(0.0f),
_vel_dot(0.0f),
_STEdotErrLast(0.0f) {
}

6
src/lib/mathlib/math/arm/Matrix.hpp
View File

@ -121,10 +121,10 @@ public:
for (size_t i = 0; i < getRows(); i++) {
for (size_t j = 0; j < getCols(); j++) {
float sig;
int exp;
int exponent;
float num = (*this)(i, j);
float2SigExp(num, sig, exp);
printf("%6.3fe%03.3d,", (double)sig, exp);
float2SigExp(num, sig, exponent);
printf("%6.3fe%03d ", (double)sig, exponent);
}
printf("\n");

6
src/lib/mathlib/math/arm/Vector.hpp
View File

@ -109,10 +109,10 @@ public:
inline void print() const {
for (size_t i = 0; i < getRows(); i++) {
float sig;
int exp;
int exponent;
float num = (*this)(i);
float2SigExp(num, sig, exp);
printf("%6.3fe%03.3d,", (double)sig, exp);
float2SigExp(num, sig, exponent);
printf("%6.3fe%03d ", (double)sig, exponent);
}
printf("\n");

29
src/modules/commander/mag_calibration.cpp
View File

@ -73,7 +73,7 @@ int do_mag_calibration(int mavlink_fd)
/* maximum 500 values */
const unsigned int calibration_maxcount = 500;
unsigned int calibration_counter = 0;
unsigned int calibration_counter;
struct mag_scale mscale_null = {
0.0f,
@ -99,28 +99,34 @@ int do_mag_calibration(int mavlink_fd)
res = ioctl(fd, MAGIOCCALIBRATE, fd);
if (res != OK) {
mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale");
mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
/* this is non-fatal - mark it accordingly */
res = OK;
}
}
close(fd);
float *x;
float *y;
float *z;
float *x = NULL;
float *y = NULL;
float *z = NULL;
if (res == OK) {
/* allocate memory */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
x = (float *)malloc(sizeof(float) * calibration_maxcount);
y = (float *)malloc(sizeof(float) * calibration_maxcount);
z = (float *)malloc(sizeof(float) * calibration_maxcount);
x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
if (x == NULL || y == NULL || z == NULL) {
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
res = ERROR;
return res;
}
} else {
/* exit */
return ERROR;
}
if (res == OK) {
@ -136,6 +142,8 @@ int do_mag_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
calibration_counter = 0;
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) {
@ -178,6 +186,7 @@ int do_mag_calibration(int mavlink_fd)
float sphere_radius;
if (res == OK) {
/* sphere fit */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
@ -270,7 +279,7 @@ int do_mag_calibration(int mavlink_fd)
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return res;
}
return res;
}

4
src/modules/uORB/uORB.cpp
View File

@ -249,8 +249,10 @@ ORBDevNode::close(struct file *filp)
} else {
SubscriberData *sd = filp_to_sd(filp);
if (sd != nullptr)
if (sd != nullptr) {
hrt_cancel(&sd->update_call);
delete sd;
}
}
return CDev::close(filp);

28
src/systemcmds/preflight_check/preflight_check.c
View File

@ -109,7 +109,7 @@ int preflight_check_main(int argc, char *argv[])
/* ---- ACCEL ---- */
close(fd);
fd = open(ACCEL_DEVICE_PATH, 0);
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
@ -119,6 +119,29 @@ int preflight_check_main(int argc, char *argv[])
goto system_eval;
}
/* check measurement result range */
struct accel_report acc;
ret = read(fd, &acc, sizeof(acc));
if (ret == sizeof(acc)) {
/* evaluate values */
if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) {
warnx("accel with spurious values");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2");
/* this is frickin' fatal */
fail_on_error = true;
system_ok = false;
goto system_eval;
}
} else {
warnx("accel read failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ");
/* this is frickin' fatal */
fail_on_error = true;
system_ok = false;
goto system_eval;
}
/* ---- GYRO ---- */
close(fd);
@ -193,9 +216,10 @@ system_eval:
/* stop alarm */
ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
/* switch on leds */
/* switch off leds */
led_on(leds, LED_BLUE);
led_on(leds, LED_AMBER);
close(leds);
if (fail_on_error) {
/* exit with error message */