forked from Archive/PX4-Autopilot
Shrinking the main commander file a bit
This commit is contained in:
Julian Oes
parent
9ce2b62eb5
commit
0ecc9c4bf4
|
|
@ -70,15 +70,25 @@
|
|||
*/
|
||||
|
||||
#include "accelerometer_calibration.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <unistd.h>
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
#include <fcntl.h>
|
||||
#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>
|
||||
#include <systemlib/conversions.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
void do_accel_calibration(int mavlink_fd);
|
||||
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);
|
||||
|
|
@ -355,7 +365,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) {
|
|||
float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
|
||||
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
|
||||
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
|
||||
if (det == 0.0)
|
||||
if (det == 0.0f)
|
||||
return ERROR; // Singular matrix
|
||||
|
||||
dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
|
||||
|
|
|
|||
|
|
@ -9,7 +9,6 @@
|
|||
#define ACCELEROMETER_CALIBRATION_H_
|
||||
|
||||
#include <stdint.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
|
||||
void do_accel_calibration(int mavlink_fd);
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,111 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file airspeed_calibration.c
|
||||
* Airspeed sensor calibration routine
|
||||
*/
|
||||
|
||||
#include "airspeed_calibration.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <poll.h>
|
||||
#include <math.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/differential_pressure.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
void do_airspeed_calibration(int mavlink_fd)
|
||||
{
|
||||
/* give directions */
|
||||
mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
|
||||
|
||||
const int calibration_count = 2500;
|
||||
|
||||
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
struct differential_pressure_s diff_pres;
|
||||
|
||||
int calibration_counter = 0;
|
||||
float diff_pres_offset = 0.0f;
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
||||
diff_pres_offset += diff_pres.differential_pressure_pa;
|
||||
calibration_counter++;
|
||||
|
||||
} else if (poll_ret == 0) {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
diff_pres_offset = diff_pres_offset / calibration_count;
|
||||
|
||||
if (isfinite(diff_pres_offset)) {
|
||||
|
||||
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
||||
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
|
||||
}
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
}
|
||||
|
||||
//char buf[50];
|
||||
//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
|
||||
//mavlink_log_info(mavlink_fd, buf);
|
||||
mavlink_log_info(mavlink_fd, "airspeed calibration done");
|
||||
|
||||
tune_positive();
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
|
||||
}
|
||||
|
||||
close(diff_pres_sub);
|
||||
}
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file gyro_calibration.h
|
||||
* Airspeed sensor calibration routine
|
||||
*/
|
||||
|
||||
#ifndef AIRSPEED_CALIBRATION_H_
|
||||
#define AIRSPEED_CALIBRATION_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
void do_airspeed_calibration(int mavlink_fd);
|
||||
|
||||
#endif /* AIRSPEED_CALIBRATION_H_ */
|
||||
|
|
@ -0,0 +1,54 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file baro_calibration.c
|
||||
* Barometer calibration routine
|
||||
*/
|
||||
|
||||
#include "baro_calibration.h"
|
||||
|
||||
#include <poll.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/param/param.h>
|
||||
|
||||
void do_baro_calibration(int mavlink_fd)
|
||||
{
|
||||
// TODO implement this
|
||||
return;
|
||||
}
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mag_calibration.h
|
||||
* Barometer calibration routine
|
||||
*/
|
||||
|
||||
#ifndef BARO_CALIBRATION_H_
|
||||
#define BARO_CALIBRATION_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
void do_baro_calibration(int mavlink_fd);
|
||||
|
||||
#endif /* BARO_CALIBRATION_H_ */
|
||||
|
|
@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[],
|
|||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -91,15 +91,15 @@
|
|||
#include <systemlib/err.h>
|
||||
#include <systemlib/cpuload.h>
|
||||
|
||||
#include "commander_helper.h"
|
||||
#include "state_machine_helper.h"
|
||||
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <drivers/drv_baro.h>
|
||||
|
||||
#include "calibration_routines.h"
|
||||
#include "accelerometer_calibration.h"
|
||||
#include "gyro_calibration.h"
|
||||
#include "mag_calibration.h"
|
||||
#include "baro_calibration.h"
|
||||
#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 */
|
||||
|
|
@ -110,6 +110,9 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
|
|||
|
||||
extern struct system_load_s system_load;
|
||||
|
||||
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
|
||||
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
|
||||
|
||||
/* Decouple update interval and hysteris counters, all depends on intervals */
|
||||
#define COMMANDER_MONITORING_INTERVAL 50000
|
||||
#define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f))
|
||||
|
|
@ -130,7 +133,6 @@ extern struct system_load_s system_load;
|
|||
|
||||
/* File descriptors */
|
||||
static int leds;
|
||||
static int buzzer;
|
||||
static int mavlink_fd;
|
||||
|
||||
/* flags */
|
||||
|
|
@ -167,27 +169,17 @@ __EXPORT int commander_main(int argc, char *argv[]);
|
|||
*/
|
||||
int commander_thread_main(int argc, char *argv[]);
|
||||
|
||||
int buzzer_init(void);
|
||||
void buzzer_deinit(void);
|
||||
int led_init(void);
|
||||
void led_deinit(void);
|
||||
int led_toggle(int led);
|
||||
int led_on(int led);
|
||||
int led_off(int led);
|
||||
void tune_error(void);
|
||||
void tune_positive(void);
|
||||
void tune_neutral(void);
|
||||
void tune_negative(void);
|
||||
void do_reboot(void);
|
||||
void do_gyro_calibration(void);
|
||||
void do_mag_calibration(void);
|
||||
void do_rc_calibration(void);
|
||||
void do_airspeed_calibration(void);
|
||||
|
||||
|
||||
void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety);
|
||||
|
||||
int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
|
||||
// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state);
|
||||
|
||||
|
||||
|
||||
|
|
@ -206,23 +198,6 @@ void usage(const char *reason);
|
|||
*/
|
||||
// static void cal_bsort(float a[], int n);
|
||||
|
||||
int buzzer_init()
|
||||
{
|
||||
buzzer = open("/dev/tone_alarm", O_WRONLY);
|
||||
|
||||
if (buzzer < 0) {
|
||||
warnx("Buzzer: open fail\n");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void buzzer_deinit()
|
||||
{
|
||||
close(buzzer);
|
||||
}
|
||||
|
||||
|
||||
int led_init()
|
||||
{
|
||||
|
|
@ -268,27 +243,6 @@ int led_off(int led)
|
|||
return ioctl(leds, LED_OFF, led);
|
||||
}
|
||||
|
||||
|
||||
void tune_error()
|
||||
{
|
||||
ioctl(buzzer, TONE_SET_ALARM, 2);
|
||||
}
|
||||
|
||||
void tune_positive()
|
||||
{
|
||||
ioctl(buzzer, TONE_SET_ALARM, 3);
|
||||
}
|
||||
|
||||
void tune_neutral()
|
||||
{
|
||||
ioctl(buzzer, TONE_SET_ALARM, 4);
|
||||
}
|
||||
|
||||
void tune_negative()
|
||||
{
|
||||
ioctl(buzzer, TONE_SET_ALARM, 5);
|
||||
}
|
||||
|
||||
void do_reboot()
|
||||
{
|
||||
mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM");
|
||||
|
|
@ -298,424 +252,6 @@ void do_reboot()
|
|||
}
|
||||
|
||||
|
||||
void do_rc_calibration()
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "trim calibration starting");
|
||||
|
||||
/* XXX fix this */
|
||||
// if (current_status.offboard_control_signal_lost) {
|
||||
// mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
|
||||
// return;
|
||||
// }
|
||||
|
||||
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
struct manual_control_setpoint_s sp;
|
||||
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
|
||||
|
||||
/* set parameters */
|
||||
float p = sp.roll;
|
||||
param_set(param_find("TRIM_ROLL"), &p);
|
||||
p = sp.pitch;
|
||||
param_set(param_find("TRIM_PITCH"), &p);
|
||||
p = sp.yaw;
|
||||
param_set(param_find("TRIM_YAW"), &p);
|
||||
|
||||
/* store to permanent storage */
|
||||
/* auto-save */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
|
||||
}
|
||||
|
||||
tune_positive();
|
||||
|
||||
mavlink_log_info(mavlink_fd, "trim calibration done");
|
||||
}
|
||||
|
||||
void do_mag_calibration()
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
|
||||
|
||||
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
|
||||
struct mag_report mag;
|
||||
|
||||
/* 45 seconds */
|
||||
uint64_t calibration_interval = 45 * 1000 * 1000;
|
||||
|
||||
/* maximum 2000 values */
|
||||
const unsigned int calibration_maxcount = 500;
|
||||
unsigned int calibration_counter = 0;
|
||||
|
||||
/* limit update rate to get equally spaced measurements over time (in ms) */
|
||||
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
|
||||
|
||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
/* erase old calibration */
|
||||
struct mag_scale mscale_null = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
|
||||
warn("WARNING: failed to set scale / offsets for mag");
|
||||
mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
|
||||
}
|
||||
|
||||
/* calibrate range */
|
||||
if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
|
||||
warnx("failed to calibrate scale");
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
/* calibrate offsets */
|
||||
|
||||
// uint64_t calibration_start = hrt_absolute_time();
|
||||
|
||||
uint64_t axis_deadline = hrt_absolute_time();
|
||||
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
|
||||
|
||||
const char axislabels[3] = { 'X', 'Y', 'Z'};
|
||||
int axis_index = -1;
|
||||
|
||||
float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
|
||||
if (x == NULL || y == NULL || z == NULL) {
|
||||
warnx("mag cal failed: out of memory");
|
||||
mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
|
||||
warnx("x:%p y:%p z:%p\n", x, y, z);
|
||||
return;
|
||||
}
|
||||
|
||||
while (hrt_absolute_time() < calibration_deadline &&
|
||||
calibration_counter < calibration_maxcount) {
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
|
||||
|
||||
/* user guidance */
|
||||
if (hrt_absolute_time() >= axis_deadline &&
|
||||
axis_index < 3) {
|
||||
|
||||
axis_index++;
|
||||
|
||||
char buf[50];
|
||||
sprintf(buf, "please rotate around %c", axislabels[axis_index]);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
tune_neutral();
|
||||
|
||||
axis_deadline += calibration_interval / 3;
|
||||
}
|
||||
|
||||
if (!(axis_index < 3)) {
|
||||
break;
|
||||
}
|
||||
|
||||
// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
|
||||
|
||||
// if ((axis_left / 1000) == 0 && axis_left > 0) {
|
||||
// char buf[50];
|
||||
// sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
|
||||
// mavlink_log_info(mavlink_fd, buf);
|
||||
// }
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
|
||||
|
||||
x[calibration_counter] = mag.x;
|
||||
y[calibration_counter] = mag.y;
|
||||
z[calibration_counter] = mag.z;
|
||||
|
||||
/* get min/max values */
|
||||
|
||||
// if (mag.x < mag_min[0]) {
|
||||
// mag_min[0] = mag.x;
|
||||
// }
|
||||
// else if (mag.x > mag_max[0]) {
|
||||
// mag_max[0] = mag.x;
|
||||
// }
|
||||
|
||||
// if (raw.magnetometer_ga[1] < mag_min[1]) {
|
||||
// mag_min[1] = raw.magnetometer_ga[1];
|
||||
// }
|
||||
// else if (raw.magnetometer_ga[1] > mag_max[1]) {
|
||||
// mag_max[1] = raw.magnetometer_ga[1];
|
||||
// }
|
||||
|
||||
// if (raw.magnetometer_ga[2] < mag_min[2]) {
|
||||
// mag_min[2] = raw.magnetometer_ga[2];
|
||||
// }
|
||||
// else if (raw.magnetometer_ga[2] > mag_max[2]) {
|
||||
// mag_max[2] = raw.magnetometer_ga[2];
|
||||
// }
|
||||
|
||||
calibration_counter++;
|
||||
|
||||
} else if (poll_ret == 0) {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
float sphere_x;
|
||||
float sphere_y;
|
||||
float sphere_z;
|
||||
float sphere_radius;
|
||||
|
||||
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
|
||||
|
||||
free(x);
|
||||
free(y);
|
||||
free(z);
|
||||
|
||||
if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
|
||||
|
||||
fd = open(MAG_DEVICE_PATH, 0);
|
||||
|
||||
struct mag_scale mscale;
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
|
||||
warn("WARNING: failed to get scale / offsets for mag");
|
||||
|
||||
mscale.x_offset = sphere_x;
|
||||
mscale.y_offset = sphere_y;
|
||||
mscale.z_offset = sphere_z;
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
|
||||
warn("WARNING: failed to set scale / offsets for mag");
|
||||
|
||||
close(fd);
|
||||
|
||||
/* announce and set new offset */
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
|
||||
warnx("Setting X mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
|
||||
warnx("Setting Y mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
|
||||
warnx("Setting Z mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
|
||||
warnx("Setting X mag scale failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
|
||||
warnx("Setting Y mag scale failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
|
||||
warnx("Setting Z mag scale failed!\n");
|
||||
}
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
|
||||
}
|
||||
|
||||
warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
|
||||
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
|
||||
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
|
||||
|
||||
char buf[52];
|
||||
sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
|
||||
(double)mscale.y_offset, (double)mscale.z_offset);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
|
||||
sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
|
||||
(double)mscale.y_scale, (double)mscale.z_scale);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "mag calibration done");
|
||||
|
||||
tune_positive();
|
||||
/* third beep by cal end routine */
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
|
||||
}
|
||||
|
||||
close(sub_mag);
|
||||
}
|
||||
|
||||
void do_gyro_calibration()
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
|
||||
|
||||
const int calibration_count = 5000;
|
||||
|
||||
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
|
||||
struct sensor_combined_s raw;
|
||||
|
||||
int calibration_counter = 0;
|
||||
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
|
||||
|
||||
/* set offsets to zero */
|
||||
int fd = open(GYRO_DEVICE_PATH, 0);
|
||||
struct gyro_scale gscale_null = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
|
||||
warn("WARNING: failed to set scale / offsets for gyro");
|
||||
|
||||
close(fd);
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
|
||||
gyro_offset[0] += raw.gyro_rad_s[0];
|
||||
gyro_offset[1] += raw.gyro_rad_s[1];
|
||||
gyro_offset[2] += raw.gyro_rad_s[2];
|
||||
calibration_counter++;
|
||||
|
||||
} else if (poll_ret == 0) {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
gyro_offset[0] = gyro_offset[0] / calibration_count;
|
||||
gyro_offset[1] = gyro_offset[1] / calibration_count;
|
||||
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) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
}
|
||||
|
||||
// char buf[50];
|
||||
// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
|
||||
// mavlink_log_info(mavlink_fd, buf);
|
||||
mavlink_log_info(mavlink_fd, "gyro calibration done");
|
||||
|
||||
tune_positive();
|
||||
/* third beep by cal end routine */
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
|
||||
}
|
||||
|
||||
close(sub_sensor_combined);
|
||||
}
|
||||
|
||||
|
||||
void do_airspeed_calibration()
|
||||
{
|
||||
/* give directions */
|
||||
mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
|
||||
|
||||
const int calibration_count = 2500;
|
||||
|
||||
int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
|
||||
struct differential_pressure_s diff_pres;
|
||||
|
||||
int calibration_counter = 0;
|
||||
float diff_pres_offset = 0.0f;
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } };
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres);
|
||||
diff_pres_offset += diff_pres.differential_pressure_pa;
|
||||
calibration_counter++;
|
||||
|
||||
} else if (poll_ret == 0) {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
diff_pres_offset = diff_pres_offset / calibration_count;
|
||||
|
||||
if (isfinite(diff_pres_offset)) {
|
||||
|
||||
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
|
||||
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
|
||||
}
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
}
|
||||
|
||||
//char buf[50];
|
||||
//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
|
||||
//mavlink_log_info(mavlink_fd, buf);
|
||||
mavlink_log_info(mavlink_fd, "airspeed calibration done");
|
||||
|
||||
tune_positive();
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
|
||||
}
|
||||
|
||||
close(diff_pres_sub);
|
||||
}
|
||||
|
||||
void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety)
|
||||
{
|
||||
|
|
@ -1182,7 +718,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
warnx("ERROR: Failed to initialize leds");
|
||||
}
|
||||
|
||||
if (buzzer_init() != 0) {
|
||||
if (buzzer_init() != OK) {
|
||||
warnx("ERROR: Failed to initialize buzzer");
|
||||
}
|
||||
|
||||
|
|
@ -1246,7 +782,12 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* advertise to ORB */
|
||||
status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status);
|
||||
/* publish current state machine */
|
||||
state_machine_publish(status_pub, ¤t_status, mavlink_fd);
|
||||
|
||||
/* publish the new state */
|
||||
current_status.counter++;
|
||||
current_status.timestamp = hrt_absolute_time();
|
||||
orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status);
|
||||
|
||||
|
||||
safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety);
|
||||
|
||||
|
|
@ -1715,7 +1256,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
// state_changed = true;
|
||||
// }
|
||||
|
||||
orb_check(ORB_ID(vehicle_gps_position), &new_data);
|
||||
orb_check(gps_sub, &new_data);
|
||||
if (new_data) {
|
||||
|
||||
|
||||
|
|
@ -2222,18 +1763,18 @@ int commander_thread_main(int argc, char *argv[])
|
|||
/* play tone according to evaluation result */
|
||||
/* check if we recently armed */
|
||||
if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) {
|
||||
if (ioctl(buzzer, TONE_SET_ALARM, 12) == OK)
|
||||
if (tune_arm() == OK)
|
||||
arm_tune_played = true;
|
||||
|
||||
/* Trigger audio event for low battery */
|
||||
} else if (bat_remain < 0.1f && battery_voltage_valid) {
|
||||
if (ioctl(buzzer, TONE_SET_ALARM, 14) == OK)
|
||||
if (tune_critical_bat() == OK)
|
||||
battery_tune_played = true;
|
||||
} else if (bat_remain < 0.2f && battery_voltage_valid) {
|
||||
if (ioctl(buzzer, TONE_SET_ALARM, 13) == OK)
|
||||
if (tune_low_bat() == OK)
|
||||
battery_tune_played = true;
|
||||
} else if(battery_tune_played) {
|
||||
ioctl(buzzer, TONE_SET_ALARM, 0);
|
||||
tune_stop();
|
||||
battery_tune_played = false;
|
||||
}
|
||||
|
||||
|
|
@ -2305,25 +1846,25 @@ void *commander_low_prio_loop(void *arg)
|
|||
|
||||
case LOW_PRIO_TASK_GYRO_CALIBRATION:
|
||||
|
||||
do_gyro_calibration();
|
||||
do_gyro_calibration(mavlink_fd);
|
||||
|
||||
low_prio_task = LOW_PRIO_TASK_NONE;
|
||||
break;
|
||||
|
||||
case LOW_PRIO_TASK_MAG_CALIBRATION:
|
||||
|
||||
do_mag_calibration();
|
||||
do_mag_calibration(mavlink_fd);
|
||||
|
||||
low_prio_task = LOW_PRIO_TASK_NONE;
|
||||
break;
|
||||
|
||||
case LOW_PRIO_TASK_ALTITUDE_CALIBRATION:
|
||||
|
||||
// do_baro_calibration();
|
||||
// do_baro_calibration(mavlink_fd);
|
||||
|
||||
case LOW_PRIO_TASK_RC_CALIBRATION:
|
||||
|
||||
// do_rc_calibration();
|
||||
// do_rc_calibration(mavlink_fd);
|
||||
|
||||
low_prio_task = LOW_PRIO_TASK_NONE;
|
||||
break;
|
||||
|
|
@ -2337,7 +1878,7 @@ void *commander_low_prio_loop(void *arg)
|
|||
|
||||
case LOW_PRIO_TASK_AIRSPEED_CALIBRATION:
|
||||
|
||||
do_airspeed_calibration();
|
||||
do_airspeed_calibration(mavlink_fd);
|
||||
|
||||
low_prio_task = LOW_PRIO_TASK_NONE;
|
||||
break;
|
||||
|
|
|
|||
|
|
@ -49,10 +49,6 @@
|
|||
#ifndef COMMANDER_H_
|
||||
#define COMMANDER_H_
|
||||
|
||||
#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f
|
||||
#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f
|
||||
|
||||
void tune_confirm(void);
|
||||
void tune_error(void);
|
||||
|
||||
#endif /* COMMANDER_H_ */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,129 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file commander_helper.c
|
||||
* Commander helper functions implementations
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <unistd.h>
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <fcntl.h>
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_controls.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_tone_alarm.h>
|
||||
|
||||
#include "commander_helper.h"
|
||||
|
||||
bool is_multirotor(const struct vehicle_status_s *current_status)
|
||||
{
|
||||
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
|
||||
}
|
||||
|
||||
bool is_rotary_wing(const struct vehicle_status_s *current_status)
|
||||
{
|
||||
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|
||||
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
|
||||
}
|
||||
|
||||
static int buzzer;
|
||||
|
||||
int buzzer_init()
|
||||
{
|
||||
buzzer = open("/dev/tone_alarm", O_WRONLY);
|
||||
|
||||
if (buzzer < 0) {
|
||||
warnx("Buzzer: open fail\n");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void buzzer_deinit()
|
||||
{
|
||||
close(buzzer);
|
||||
}
|
||||
|
||||
void tune_error()
|
||||
{
|
||||
ioctl(buzzer, TONE_SET_ALARM, 2);
|
||||
}
|
||||
|
||||
void tune_positive()
|
||||
{
|
||||
ioctl(buzzer, TONE_SET_ALARM, 3);
|
||||
}
|
||||
|
||||
void tune_neutral()
|
||||
{
|
||||
ioctl(buzzer, TONE_SET_ALARM, 4);
|
||||
}
|
||||
|
||||
void tune_negative()
|
||||
{
|
||||
ioctl(buzzer, TONE_SET_ALARM, 5);
|
||||
}
|
||||
|
||||
int tune_arm()
|
||||
{
|
||||
return ioctl(buzzer, TONE_SET_ALARM, 12);
|
||||
}
|
||||
|
||||
int tune_critical_bat()
|
||||
{
|
||||
return ioctl(buzzer, TONE_SET_ALARM, 14);
|
||||
}
|
||||
|
||||
int tune_low_bat()
|
||||
{
|
||||
return ioctl(buzzer, TONE_SET_ALARM, 13);
|
||||
}
|
||||
|
||||
void tune_stop()
|
||||
{
|
||||
ioctl(buzzer, TONE_SET_ALARM, 0);
|
||||
}
|
||||
|
||||
|
|
@ -0,0 +1,66 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file commander_helper.h
|
||||
* Commander helper functions definitions
|
||||
*/
|
||||
|
||||
#ifndef COMMANDER_HELPER_H_
|
||||
#define COMMANDER_HELPER_H_
|
||||
|
||||
#include <uORB/uORB.h>
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/actuator_safety.h>
|
||||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
|
||||
|
||||
bool is_multirotor(const struct vehicle_status_s *current_status);
|
||||
bool is_rotary_wing(const struct vehicle_status_s *current_status);
|
||||
|
||||
int buzzer_init(void);
|
||||
void buzzer_deinit(void);
|
||||
|
||||
void tune_error(void);
|
||||
void tune_positive(void);
|
||||
void tune_neutral(void);
|
||||
void tune_negative(void);
|
||||
int tune_arm(void);
|
||||
int tune_critical_bat(void);
|
||||
int tune_low_bat(void);
|
||||
|
||||
void tune_stop(void);
|
||||
|
||||
#endif /* COMMANDER_HELPER_H_ */
|
||||
|
|
@ -0,0 +1,151 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file gyro_calibration.c
|
||||
* Gyroscope calibration routine
|
||||
*/
|
||||
|
||||
#include "gyro_calibration.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include <math.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
|
||||
void do_gyro_calibration(int mavlink_fd)
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
|
||||
|
||||
const int calibration_count = 5000;
|
||||
|
||||
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
|
||||
struct sensor_combined_s raw;
|
||||
|
||||
int calibration_counter = 0;
|
||||
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
|
||||
|
||||
/* set offsets to zero */
|
||||
int fd = open(GYRO_DEVICE_PATH, 0);
|
||||
struct gyro_scale gscale_null = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
|
||||
warn("WARNING: failed to set scale / offsets for gyro");
|
||||
|
||||
close(fd);
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
|
||||
gyro_offset[0] += raw.gyro_rad_s[0];
|
||||
gyro_offset[1] += raw.gyro_rad_s[1];
|
||||
gyro_offset[2] += raw.gyro_rad_s[2];
|
||||
calibration_counter++;
|
||||
|
||||
} else if (poll_ret == 0) {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
gyro_offset[0] = gyro_offset[0] / calibration_count;
|
||||
gyro_offset[1] = gyro_offset[1] / calibration_count;
|
||||
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) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
}
|
||||
|
||||
// char buf[50];
|
||||
// sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]);
|
||||
// mavlink_log_info(mavlink_fd, buf);
|
||||
mavlink_log_info(mavlink_fd, "gyro calibration done");
|
||||
|
||||
tune_positive();
|
||||
/* third beep by cal end routine */
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
|
||||
}
|
||||
|
||||
close(sub_sensor_combined);
|
||||
}
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file gyro_calibration.h
|
||||
* Gyroscope calibration routine
|
||||
*/
|
||||
|
||||
#ifndef GYRO_CALIBRATION_H_
|
||||
#define GYRO_CALIBRATION_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
void do_gyro_calibration(int mavlink_fd);
|
||||
|
||||
#endif /* GYRO_CALIBRATION_H_ */
|
||||
|
|
@ -0,0 +1,278 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mag_calibration.c
|
||||
* Magnetometer calibration routine
|
||||
*/
|
||||
|
||||
#include "mag_calibration.h"
|
||||
#include "commander_helper.h"
|
||||
#include "calibration_routines.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <poll.h>
|
||||
#include <math.h>
|
||||
#include <fcntl.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <drivers/drv_mag.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
|
||||
void do_mag_calibration(int mavlink_fd)
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
|
||||
|
||||
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
|
||||
struct mag_report mag;
|
||||
|
||||
/* 45 seconds */
|
||||
uint64_t calibration_interval = 45 * 1000 * 1000;
|
||||
|
||||
/* maximum 2000 values */
|
||||
const unsigned int calibration_maxcount = 500;
|
||||
unsigned int calibration_counter = 0;
|
||||
|
||||
/* limit update rate to get equally spaced measurements over time (in ms) */
|
||||
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
|
||||
|
||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
/* erase old calibration */
|
||||
struct mag_scale mscale_null = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
1.0f,
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
|
||||
warn("WARNING: failed to set scale / offsets for mag");
|
||||
mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
|
||||
}
|
||||
|
||||
/* calibrate range */
|
||||
if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
|
||||
warnx("failed to calibrate scale");
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
/* calibrate offsets */
|
||||
|
||||
// uint64_t calibration_start = hrt_absolute_time();
|
||||
|
||||
uint64_t axis_deadline = hrt_absolute_time();
|
||||
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
|
||||
|
||||
const char axislabels[3] = { 'X', 'Y', 'Z'};
|
||||
int axis_index = -1;
|
||||
|
||||
float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
|
||||
if (x == NULL || y == NULL || z == NULL) {
|
||||
warnx("mag cal failed: out of memory");
|
||||
mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
|
||||
warnx("x:%p y:%p z:%p\n", x, y, z);
|
||||
return;
|
||||
}
|
||||
|
||||
while (hrt_absolute_time() < calibration_deadline &&
|
||||
calibration_counter < calibration_maxcount) {
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } };
|
||||
|
||||
/* user guidance */
|
||||
if (hrt_absolute_time() >= axis_deadline &&
|
||||
axis_index < 3) {
|
||||
|
||||
axis_index++;
|
||||
|
||||
char buf[50];
|
||||
sprintf(buf, "please rotate around %c", axislabels[axis_index]);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
tune_neutral();
|
||||
|
||||
axis_deadline += calibration_interval / 3;
|
||||
}
|
||||
|
||||
if (!(axis_index < 3)) {
|
||||
break;
|
||||
}
|
||||
|
||||
// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
|
||||
|
||||
// if ((axis_left / 1000) == 0 && axis_left > 0) {
|
||||
// char buf[50];
|
||||
// sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
|
||||
// mavlink_log_info(mavlink_fd, buf);
|
||||
// }
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
|
||||
|
||||
x[calibration_counter] = mag.x;
|
||||
y[calibration_counter] = mag.y;
|
||||
z[calibration_counter] = mag.z;
|
||||
|
||||
/* get min/max values */
|
||||
|
||||
// if (mag.x < mag_min[0]) {
|
||||
// mag_min[0] = mag.x;
|
||||
// }
|
||||
// else if (mag.x > mag_max[0]) {
|
||||
// mag_max[0] = mag.x;
|
||||
// }
|
||||
|
||||
// if (raw.magnetometer_ga[1] < mag_min[1]) {
|
||||
// mag_min[1] = raw.magnetometer_ga[1];
|
||||
// }
|
||||
// else if (raw.magnetometer_ga[1] > mag_max[1]) {
|
||||
// mag_max[1] = raw.magnetometer_ga[1];
|
||||
// }
|
||||
|
||||
// if (raw.magnetometer_ga[2] < mag_min[2]) {
|
||||
// mag_min[2] = raw.magnetometer_ga[2];
|
||||
// }
|
||||
// else if (raw.magnetometer_ga[2] > mag_max[2]) {
|
||||
// mag_max[2] = raw.magnetometer_ga[2];
|
||||
// }
|
||||
|
||||
calibration_counter++;
|
||||
|
||||
} else if (poll_ret == 0) {
|
||||
/* any poll failure for 1s is a reason to abort */
|
||||
mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
float sphere_x;
|
||||
float sphere_y;
|
||||
float sphere_z;
|
||||
float sphere_radius;
|
||||
|
||||
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
|
||||
|
||||
free(x);
|
||||
free(y);
|
||||
free(z);
|
||||
|
||||
if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
|
||||
|
||||
fd = open(MAG_DEVICE_PATH, 0);
|
||||
|
||||
struct mag_scale mscale;
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
|
||||
warn("WARNING: failed to get scale / offsets for mag");
|
||||
|
||||
mscale.x_offset = sphere_x;
|
||||
mscale.y_offset = sphere_y;
|
||||
mscale.z_offset = sphere_z;
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
|
||||
warn("WARNING: failed to set scale / offsets for mag");
|
||||
|
||||
close(fd);
|
||||
|
||||
/* announce and set new offset */
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
|
||||
warnx("Setting X mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
|
||||
warnx("Setting Y mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
|
||||
warnx("Setting Z mag offset failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
|
||||
warnx("Setting X mag scale failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
|
||||
warnx("Setting Y mag scale failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
|
||||
warnx("Setting Z mag scale failed!\n");
|
||||
}
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
|
||||
}
|
||||
|
||||
warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
|
||||
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
|
||||
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
|
||||
|
||||
char buf[52];
|
||||
sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
|
||||
(double)mscale.y_offset, (double)mscale.z_offset);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
|
||||
sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
|
||||
(double)mscale.y_scale, (double)mscale.z_scale);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "mag calibration done");
|
||||
|
||||
tune_positive();
|
||||
/* third beep by cal end routine */
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
|
||||
}
|
||||
|
||||
close(sub_mag);
|
||||
}
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mag_calibration.h
|
||||
* Magnetometer calibration routine
|
||||
*/
|
||||
|
||||
#ifndef MAG_CALIBRATION_H_
|
||||
#define MAG_CALIBRATION_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
void do_mag_calibration(int mavlink_fd);
|
||||
|
||||
#endif /* MAG_CALIBRATION_H_ */
|
||||
|
|
@ -38,6 +38,12 @@
|
|||
MODULE_COMMAND = commander
|
||||
SRCS = commander.c \
|
||||
state_machine_helper.c \
|
||||
commander_helper.c \
|
||||
calibration_routines.c \
|
||||
accelerometer_calibration.c
|
||||
accelerometer_calibration.c \
|
||||
gyro_calibration.c \
|
||||
mag_calibration.c \
|
||||
baro_calibration.c \
|
||||
rc_calibration.c \
|
||||
airspeed_calibration.c
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,83 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file rc_calibration.c
|
||||
* Remote Control calibration routine
|
||||
*/
|
||||
|
||||
#include "rc_calibration.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <poll.h>
|
||||
#include <uORB/topics/sensor_combined.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
#include <systemlib/param/param.h>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
|
||||
void do_rc_calibration(int mavlink_fd)
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "trim calibration starting");
|
||||
|
||||
/* XXX fix this */
|
||||
// if (current_status.offboard_control_signal_lost) {
|
||||
// mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
|
||||
// return;
|
||||
// }
|
||||
|
||||
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
|
||||
struct manual_control_setpoint_s sp;
|
||||
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
|
||||
|
||||
/* set parameters */
|
||||
float p = sp.roll;
|
||||
param_set(param_find("TRIM_ROLL"), &p);
|
||||
p = sp.pitch;
|
||||
param_set(param_find("TRIM_PITCH"), &p);
|
||||
p = sp.yaw;
|
||||
param_set(param_find("TRIM_YAW"), &p);
|
||||
|
||||
/* store to permanent storage */
|
||||
/* auto-save */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
|
||||
}
|
||||
|
||||
tune_positive();
|
||||
|
||||
mavlink_log_info(mavlink_fd, "trim calibration done");
|
||||
}
|
||||
|
|
@ -0,0 +1,46 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file rc_calibration.h
|
||||
* Remote Control calibration routine
|
||||
*/
|
||||
|
||||
#ifndef RC_CALIBRATION_H_
|
||||
#define RC_CALIBRATION_H_
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
void do_rc_calibration(int mavlink_fd);
|
||||
|
||||
#endif /* RC_CALIBRATION_H_ */
|
||||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
|
|
@ -54,21 +54,8 @@
|
|||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
#include "state_machine_helper.h"
|
||||
#include "commander.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
bool is_multirotor(const struct vehicle_status_s *current_status)
|
||||
{
|
||||
return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_HEXAROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_OCTOROTOR) ||
|
||||
(current_status->system_type == VEHICLE_TYPE_TRICOPTER));
|
||||
}
|
||||
|
||||
bool is_rotary_wing(const struct vehicle_status_s *current_status)
|
||||
{
|
||||
return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER)
|
||||
|| (current_status->system_type == VEHICLE_TYPE_COAXIAL);
|
||||
}
|
||||
|
||||
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) {
|
||||
|
||||
|
|
@ -568,7 +555,11 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status
|
|||
|
||||
if (valid_transition) {
|
||||
current_status->hil_state = new_state;
|
||||
state_machine_publish(status_pub, current_status, mavlink_fd);
|
||||
|
||||
current_status->counter++;
|
||||
current_status->timestamp = hrt_absolute_time();
|
||||
|
||||
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
|
||||
ret = OK;
|
||||
} else {
|
||||
mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition");
|
||||
|
|
@ -579,50 +570,6 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status
|
|||
|
||||
|
||||
|
||||
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
|
||||
{
|
||||
/* publish the new state */
|
||||
current_status->counter++;
|
||||
current_status->timestamp = hrt_absolute_time();
|
||||
|
||||
/* assemble state vector based on flag values */
|
||||
// if (current_status->flag_control_rates_enabled) {
|
||||
// current_status->onboard_control_sensors_present |= 0x400;
|
||||
//
|
||||
// } else {
|
||||
// current_status->onboard_control_sensors_present &= ~0x400;
|
||||
// }
|
||||
|
||||
// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
|
||||
// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
|
||||
// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
|
||||
// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
|
||||
//
|
||||
// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0;
|
||||
// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0;
|
||||
// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0;
|
||||
// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0;
|
||||
// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0;
|
||||
|
||||
orb_publish(ORB_ID(vehicle_status), status_pub, current_status);
|
||||
}
|
||||
|
||||
// void publish_armed_status(const struct vehicle_status_s *current_status)
|
||||
// {
|
||||
// struct actuator_armed_s armed;
|
||||
// armed.armed = current_status->flag_system_armed;
|
||||
//
|
||||
// /* XXX allow arming by external components on multicopters only if not yet armed by RC */
|
||||
// /* XXX allow arming only if core sensors are ok */
|
||||
// armed.ready_to_arm = true;
|
||||
//
|
||||
// /* lock down actuators if required, only in HIL */
|
||||
// armed.lockdown = (current_status->flag_hil_enabled) ? true : false;
|
||||
// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
|
||||
// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
|
||||
// }
|
||||
|
||||
|
||||
// /*
|
||||
// * Wrapper functions (to be used in the commander), all functions assume lock on current_status
|
||||
// */
|
||||
|
|
@ -805,3 +752,4 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat
|
|||
//
|
||||
// return ret;
|
||||
//}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Thomas Gubler <thomasgubler@student.ethz.ch>
|
||||
* Julian Oes <joes@student.ethz.ch>
|
||||
*
|
||||
|
|
@ -50,15 +50,7 @@
|
|||
#include <uORB/topics/vehicle_control_mode.h>
|
||||
|
||||
|
||||
void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
|
||||
bool is_multirotor(const struct vehicle_status_s *current_status);
|
||||
|
||||
bool is_rotary_wing(const struct vehicle_status_s *current_status);
|
||||
|
||||
//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state);
|
||||
|
||||
void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd);
|
||||
int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd);
|
||||
|
||||
int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd);
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue