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

moved commander to C++, preparation for better gyro scale calibration respecting the current attitude for more accurate results

Browse Source
This commit is contained in:
Lorenz Meier 2013-07-20 13:47:51 +02:00
parent da1bf69ce2
commit 68ab69de01
13 changed files with 156 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
src/modules/commander/accelerometer_calibration.c → src/modules/commander/accelerometer_calibration.cpp
View File

@ -33,7 +33,7 @@
****************************************************************************/
/**
* @file accelerometer_calibration.c
* @file accelerometer_calibration.cpp
*
* Implementation of accelerometer calibration.
*
@ -113,8 +113,6 @@
#include <sys/prctl.h>
#include <math.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_accel.h>
@ -123,6 +121,12 @@
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
int detect_orientation(int mavlink_fd, int sub_sensor_combined);
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
@ -229,7 +233,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
sprintf(str, "meas started: %s", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, str);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]);
str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0],
(double)accel_ref[orient][1],
(double)accel_ref[orient][2]);
mavlink_log_info(mavlink_fd, str);
data_collected[orient] = true;
tune_neutral();
@ -274,7 +281,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
float accel_err_thr = 5.0f;
/* still time required in us */
int64_t still_time = 2000000;
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
hrt_abstime t_start = hrt_absolute_time();
/* set timeout to 30s */
@ -342,29 +351,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
}
}
if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
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; // [ g, 0, 0 ]
if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
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; // [ -g, 0, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
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; // [ 0, g, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
fabs(accel_ema[2]) < accel_err_thr )
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; // [ 0, -g, 0 ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
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; // [ 0, 0, g ]
if ( fabs(accel_ema[0]) < accel_err_thr &&
fabs(accel_ema[1]) < accel_err_thr &&
fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
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; // [ 0, 0, -g ]
mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
@ -376,7 +385,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
* Read specified number of accelerometer samples, calculate average and dispersion.
*/
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = sensor_combined_sub;
fds[0].events = POLLIN;
int count = 0;
float accel_sum[3] = { 0.0f, 0.0f, 0.0f };

6
src/modules/commander/airspeed_calibration.c → src/modules/commander/airspeed_calibration.cpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file airspeed_calibration.c
* @file airspeed_calibration.cpp
* Airspeed sensor calibration routine
*/
@ -65,7 +65,9 @@ void do_airspeed_calibration(int mavlink_fd)
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = diff_pres_sub;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);

2
src/modules/commander/baro_calibration.c → src/modules/commander/baro_calibration.cpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file baro_calibration.c
* @file baro_calibration.cpp
* Barometer calibration routine
*/

2
src/modules/commander/calibration_routines.c → src/modules/commander/calibration_routines.cpp
View File

@ -33,7 +33,7 @@
****************************************************************************/
/**
* @file calibration_routines.c
* @file calibration_routines.cpp
* Calibration routines implementations.
*
* @author Lorenz Meier <lm@inf.ethz.ch>

19
src/modules/commander/commander.c → src/modules/commander/commander.cpp
View File

@ -36,13 +36,11 @@
****************************************************************************/
/**
* @file commander.c
* @file commander.cpp
* Main system state machine implementation.
*
*/
#include "commander.h"
#include <nuttx/config.h>
#include <pthread.h>
#include <stdio.h>
@ -97,12 +95,11 @@
#include "rc_calibration.h"
#include "airspeed_calibration.h"
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
extern struct system_load_s system_load;
@ -160,8 +157,10 @@ enum {
*
* The actual stack size should be set in the call
* to task_create().
*
* @ingroup apps
*/
__EXPORT int commander_main(int argc, char *argv[]);
extern "C" __EXPORT int commander_main(int argc, char *argv[]);
/**
* Print the correct usage.

13
src/modules/commander/commander_helper.c → src/modules/commander/commander_helper.cpp
View File

@ -34,7 +34,7 @@
****************************************************************************/
/**
* @file commander_helper.c
* @file commander_helper.cpp
* Commander helper functions implementations
*/
@ -56,6 +56,12 @@
#include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
bool is_multirotor(const struct vehicle_status_s *current_status)
{
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
@ -175,11 +181,6 @@ int led_off(int led)
return ioctl(leds, LED_OFF, led);
}
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);
float battery_remaining_estimate_voltage(float voltage)
{
float ret = 0;

28
src/modules/commander/commander.h → src/modules/commander/commander_params.c
View File

@ -1,10 +1,7 @@
/****************************************************************************
*
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
* Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* Lorenz Meier <lm@inf.ethz.ch>
* Thomas Gubler <thomasgubler@student.ethz.ch>
* Julian Oes <joes@student.ethz.ch>
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -36,19 +33,22 @@
****************************************************************************/
/**
* @file commander.h
* Main system state machine definition.
* @file commander_params.c
*
* Parameters defined by the sensors task.
*
* @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*
*/
#ifndef COMMANDER_H_
#define COMMANDER_H_
#include <nuttx/config.h>
#include <systemlib/param/param.h>
#endif /* COMMANDER_H_ */
PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */
PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f);
PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f);
PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3);

70
src/modules/commander/gyro_calibration.c → src/modules/commander/gyro_calibration.cpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file gyro_calibration.c
* @file gyro_calibration.cpp
* Gyroscope calibration routine
*/
@ -82,7 +82,9 @@ void do_gyro_calibration(int mavlink_fd)
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
@ -105,6 +107,49 @@ void do_gyro_calibration(int mavlink_fd)
gyro_offset[2] = gyro_offset[2] / calibration_count;
if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
1.0f,
gyro_offset[1],
1.0f,
gyro_offset[2],
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
close(fd);
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warnx("WARNING: auto-save of params to storage failed");
mavlink_log_critical(mavlink_fd, "gyro store failed");
// XXX negative tune
return;
}
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
return;
}
/*** --- SCALING --- ***/
@ -136,7 +181,9 @@ void do_gyro_calibration(int mavlink_fd)
break;
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
int poll_ret = poll(fds, 1, 1000);
@ -180,27 +227,28 @@ void do_gyro_calibration(int mavlink_fd)
}
float gyro_scale = baseline_integral / gyro_integral;
float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale };
warnx("gyro scale: yaw (z): %6.4f", gyro_scale);
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale);
if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
1.0f,
gyro_scales[0],
gyro_offset[1],
1.0f,
gyro_scales[1],
gyro_offset[2],
1.0f,
gyro_scales[2],
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))

6
src/modules/commander/mag_calibration.c → src/modules/commander/mag_calibration.cpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file mag_calibration.c
* @file mag_calibration.cpp
* Magnetometer calibration routine
*/
@ -120,7 +120,9 @@ void do_mag_calibration(int mavlink_fd)
calibration_counter < calibration_maxcount) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
struct pollfd fds[1];
fds[0].fd = sub_mag;
fds[0].events = POLLIN;
/* user guidance */
if (hrt_absolute_time() >= axis_deadline &&

21
src/modules/commander/module.mk
View File

@ -36,14 +36,15 @@
#
MODULE_COMMAND = commander
SRCS = commander.c \
state_machine_helper.c \
commander_helper.c \
calibration_routines.c \
accelerometer_calibration.c \
gyro_calibration.c \
mag_calibration.c \
baro_calibration.c \
rc_calibration.c \
airspeed_calibration.c
SRCS = commander.cpp \
commander_params.c \
state_machine_helper.cpp \
commander_helper.cpp \
calibration_routines.cpp \
accelerometer_calibration.cpp \
gyro_calibration.cpp \
mag_calibration.cpp \
baro_calibration.cpp \
rc_calibration.cpp \
airspeed_calibration.cpp

2
src/modules/commander/rc_calibration.c → src/modules/commander/rc_calibration.cpp
View File

@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file rc_calibration.c
* @file rc_calibration.cpp
* Remote Control calibration routine
*/

7
src/modules/commander/state_machine_helper.c → src/modules/commander/state_machine_helper.cpp
View File

@ -34,7 +34,7 @@
****************************************************************************/
/**
* @file state_machine_helper.c
* @file state_machine_helper.cpp
* State machine helper functions implementations
*/
@ -56,6 +56,11 @@
#include "state_machine_helper.h"
#include "commander_helper.h"
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) {

7
src/modules/systemlib/systemlib.h
View File

@ -42,11 +42,12 @@
#include <float.h>
#include <stdint.h>
/** Reboots the board */
extern void up_systemreset(void) noreturn_function;
__BEGIN_DECLS
/** Reboots the board */
//extern void up_systemreset(void) noreturn_function;
#include <../arch/common/up_internal.h>
/** Sends SIGUSR1 to all processes */
__EXPORT void killall(void);