Commit 68ab69de authored by Lorenz Meier's avatar Lorenz Meier
Browse files

moved commander to C++, preparation for better gyro scale calibration...

moved commander to C++, preparation for better gyro scale calibration respecting the current attitude for more accurate results
parent da1bf69c
......@@ -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 };
......
......@@ -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);
......
......@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file baro_calibration.c
* @file baro_calibration.cpp
* Barometer calibration routine
*/
......
......@@ -33,7 +33,7 @@
****************************************************************************/
/**
* @file calibration_routines.c
* @file calibration_routines.cpp
* Calibration routines implementations.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
......
......@@ -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.
......
......@@ -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;
......
/****************************************************************************
*
* 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);
......@@ -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))
......
......@@ -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 &&
......
......@@ -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
......@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file rc_calibration.c
* @file rc_calibration.cpp
* Remote Control calibration routine
*/
......
......@@ -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) {
......
......@@ -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);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment