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

Merged master

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-14 22:07:49 +02:00
parent 7dacb835b9 dbcb226de6
commit b126f00052
39 changed files with 530 additions and 318 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/PreflightCheck.cpp
+134 -27
View File
@@ -71,7 +71,50 @@
namespace Commander
{
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
static int check_calibration(int fd, const char* param_template, int &devid);
int check_calibration(int fd, const char* param_template, int &devid)
{
bool calibration_found;
/* new style: ask device for calibration state */
int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
calibration_found = (ret == OK);
devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
char s[20];
int instance = 0;
/* old style transition: check param values */
while (!calibration_found) {
sprintf(s, param_template, instance);
param_t parm = param_find(s);
/* if the calibration param is not present, abort */
if (parm == PARAM_INVALID) {
break;
}
/* if param get succeeds */
int calibration_devid;
if (!param_get(parm, &(calibration_devid))) {
/* if the devid matches, exit early */
if (devid == calibration_devid) {
calibration_found = true;
break;
}
}
instance++;
}
return !calibration_found;
}
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
{
bool success = true;
@@ -88,13 +131,9 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
return false;
}
int calibration_devid;
int ret;
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
sprintf(s, "CAL_MAG%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
if (devid != calibration_devid) {
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
success = false;
@@ -115,7 +154,7 @@ out:
return success;
}
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
{
bool success = true;
@@ -132,13 +171,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
return false;
}
int calibration_devid;
int ret;
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
sprintf(s, "CAL_ACC%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
if (devid != calibration_devid) {
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
success = false;
@@ -184,7 +219,7 @@ out:
return success;
}
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
{
bool success = true;
@@ -201,13 +236,9 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
return false;
}
int calibration_devid;
int ret;
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
sprintf(s, "CAL_GYRO%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
if (devid != calibration_devid) {
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
success = false;
@@ -228,7 +259,7 @@ out:
return success;
}
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
{
bool success = true;
@@ -245,6 +276,20 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
return false;
}
device_id = -1000;
// TODO: There is no baro calibration yet, since no external baros exist
// int ret = check_calibration(fd, "CAL_BARO%u_ID");
// if (ret) {
// mavlink_and_console_log_critical(mavlink_fd,
// "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
// success = false;
// goto out;
// }
//out:
px4_close(fd);
return success;
}
@@ -311,49 +356,110 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- MAG ---- */
if (checkMag) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_MAG_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
bool required = (i < max_mandatory_mag_count);
int device_id = -1;
if (!magnometerCheck(mavlink_fd, i, !required) && required) {
if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
failed = true;
}
}
/* ---- ACCEL ---- */
if (checkAcc) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_ACC_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
bool required = (i < max_mandatory_accel_count);
int device_id = -1;
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
failed = true;
}
}
/* ---- GYRO ---- */
if (checkGyro) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
bool required = (i < max_mandatory_gyro_count);
int device_id = -1;
if (!gyroCheck(mavlink_fd, i, !required) && required) {
if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
/* check if the primary device is present */
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
failed = true;
}
}
/* ---- BARO ---- */
if (checkBaro) {
bool prime_found = false;
int32_t prime_id = 0;
param_get(param_find("CAL_BARO_PRIME"), &prime_id);
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_baro_count; i++) {
bool required = (i < max_mandatory_baro_count);
int device_id = -1;
if (!baroCheck(mavlink_fd, i, !required) && required) {
if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
failed = true;
}
if (device_id == prime_id) {
prime_found = true;
}
}
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
failed = true;
}
}
@@ -367,12 +473,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_fd) != OK) {
mavlink_log_critical(mavlink_fd, "RC calibration check failed");
failed = true;
}
}
/* ---- Global Navigation Satellite System receiver ---- */
if(checkGNSS) {
if (checkGNSS) {
if(!gnssCheck(mavlink_fd)) {
failed = true;
}
src/modules/commander/accelerometer_calibration.cpp
+15 -1
View File
@@ -156,6 +156,10 @@ static const int ERROR = -1;
static const char *sensor_name = "accel";
static int32_t device_id[max_accel_sens];
static int device_prio_max = 0;
static int32_t device_id_primary = 0;
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
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);
int mat_invert3(float src[3][3], float dst[3][3]);
@@ -172,7 +176,6 @@ typedef struct {
int do_accel_calibration(int mavlink_fd)
{
int fd;
int32_t device_id[max_accel_sens];
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
@@ -259,6 +262,8 @@ int do_accel_calibration(int mavlink_fd)
bool failed = false;
failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
/* set parameters */
(void)sprintf(str, "CAL_ACC%u_XOFF", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
@@ -370,6 +375,15 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
struct accel_report arp = {};
(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
timestamps[i] = arp.timestamp;
// Get priority
int32_t prio;
orb_priority(worker_data.subs[i], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_id[i];
}
}
if (result == calibrate_return_ok) {
src/modules/commander/gyro_calibration.cpp
+15
View File
@@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd)
1.0f, // z scale
};
int device_prio_max = 0;
int32_t device_id_primary = 0;
for (unsigned s = 0; s < max_gyros; s++) {
char str[30];
@@ -199,6 +202,15 @@ int do_gyro_calibration(int mavlink_fd)
for (unsigned s = 0; s < max_gyros; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
// Get priority
int32_t prio;
orb_priority(worker_data.gyro_sensor_sub[s], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = worker_data.device_id[s];
}
}
int cancel_sub = calibrate_cancel_subscribe();
@@ -258,9 +270,12 @@ int do_gyro_calibration(int mavlink_fd)
}
if (res == OK) {
/* set offset parameters to new values */
bool failed = false;
failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
for (unsigned s = 0; s < max_gyros; s++) {
if (worker_data.device_id[s] != 0) {
char str[30];
src/modules/commander/mag_calibration.cpp
+16 -1
View File
@@ -73,6 +73,10 @@ static constexpr unsigned int calibration_sides = 6; ///< The total number of
static constexpr unsigned int calibration_total_points = 240; ///< The total points per magnetometer
static constexpr unsigned int calibraton_duration_seconds = 42; ///< The total duration the routine is allowed to take
int32_t device_ids[max_mags];
int device_prio_max = 0;
int32_t device_id_primary = 0;
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
/// Data passed to calibration worker routine
@@ -108,7 +112,6 @@ int do_mag_calibration(int mavlink_fd)
// Determine which mags are available and reset each
int32_t device_ids[max_mags];
char str[30];
for (size_t i=0; i<max_mags; i++) {
@@ -434,6 +437,15 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
result = calibrate_return_error;
break;
}
// Get priority
int32_t prio;
orb_priority(worker_data.sub_mag[cur_mag], &prio);
if (prio > device_prio_max) {
device_prio_max = prio;
device_id_primary = device_ids[cur_mag];
}
}
}
}
@@ -550,6 +562,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
}
if (result == calibrate_return_ok) {
(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
int fd_mag = -1;