Commit 5f100411 authored by Julian Oes's avatar Julian Oes
Browse files

Restore proper feedback (mavlink and tone) for calibration commands, etc

parent 6c3da5ae
......@@ -133,7 +133,7 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
int mat_invert3(float src[3][3], float dst[3][3]);
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
void do_accel_calibration(int mavlink_fd) {
int do_accel_calibration(int mavlink_fd) {
/* announce change */
mavlink_log_info(mavlink_fd, "accel calibration started");
......@@ -176,11 +176,11 @@ void do_accel_calibration(int mavlink_fd) {
}
mavlink_log_info(mavlink_fd, "accel calibration done");
tune_positive();
return OK;
} else {
/* measurements error */
mavlink_log_info(mavlink_fd, "accel calibration aborted");
tune_negative();
return ERROR;
}
/* exit accel calibration mode */
......
......@@ -45,6 +45,6 @@
#include <stdint.h>
void do_accel_calibration(int mavlink_fd);
int do_accel_calibration(int mavlink_fd);
#endif /* ACCELEROMETER_CALIBRATION_H_ */
......@@ -49,7 +49,13 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
void do_airspeed_calibration(int mavlink_fd)
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int do_airspeed_calibration(int mavlink_fd)
{
/* give directions */
mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
......@@ -79,7 +85,7 @@ void do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
return;
return ERROR;
}
}
......@@ -89,6 +95,7 @@ void do_airspeed_calibration(int mavlink_fd)
if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
return ERROR;
}
/* auto-save to EEPROM */
......@@ -96,6 +103,8 @@ void do_airspeed_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
return ERROR;
}
//char buf[50];
......@@ -103,11 +112,10 @@ void do_airspeed_calibration(int mavlink_fd)
//mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "airspeed calibration done");
tune_positive();
return OK;
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
return ERROR;
}
close(diff_pres_sub);
}
......@@ -41,6 +41,6 @@
#include <stdint.h>
void do_airspeed_calibration(int mavlink_fd);
int do_airspeed_calibration(int mavlink_fd);
#endif /* AIRSPEED_CALIBRATION_H_ */
\ No newline at end of file
#endif /* AIRSPEED_CALIBRATION_H_ */
......@@ -47,8 +47,14 @@
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
void do_baro_calibration(int mavlink_fd)
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
int do_baro_calibration(int mavlink_fd)
{
// TODO implement this
return;
return ERROR;
}
......@@ -41,6 +41,6 @@
#include <stdint.h>
void do_baro_calibration(int mavlink_fd);
int do_baro_calibration(int mavlink_fd);
#endif /* BARO_CALIBRATION_H_ */
\ No newline at end of file
#endif /* BARO_CALIBRATION_H_ */
......@@ -502,7 +502,13 @@ int commander_thread_main(int argc, char *argv[])
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.");
/* try again later */
usleep(20000);
mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
if (mavlink_fd < 0) {
warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
}
}
/* Main state machine */
......@@ -1628,6 +1634,33 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v
return res;
}
void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT result)
{
switch (result) {
case VEHICLE_CMD_RESULT_ACCEPTED:
tune_positive();
break;
case VEHICLE_CMD_RESULT_DENIED:
mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_FAILED:
mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
tune_negative();
break;
case VEHICLE_CMD_RESULT_UNSUPPORTED:
mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
tune_negative();
break;
default:
break;
}
}
void *commander_low_prio_loop(void *arg)
{
/* Set thread name */
......@@ -1668,125 +1701,110 @@ void *commander_low_prio_loop(void *arg)
cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM)
continue;
/* result of the command */
uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED;
/* only handle low-priority commands here */
switch (cmd.command) {
case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
if (is_safe(&status, &safety, &armed)) {
if (((int)(cmd.param1)) == 1) {
/* reboot */
systemreset(false);
} else if (((int)(cmd.param1)) == 3) {
/* reboot to bootloader */
systemreset(true);
} else {
result = VEHICLE_CMD_RESULT_DENIED;
}
if (is_safe(&status, &safety, &armed)) {
if (((int)(cmd.param1)) == 1) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
usleep(1000000);
/* reboot */
systemreset(false);
} else if (((int)(cmd.param1)) == 3) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
usleep(1000000);
/* reboot to bootloader */
systemreset(true);
} else {
result = VEHICLE_CMD_RESULT_DENIED;
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
}
break;
case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
} else {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
}
break;
/* try to go to INIT/PREFLIGHT arming state */
case VEHICLE_CMD_PREFLIGHT_CALIBRATION: {
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
result = VEHICLE_CMD_RESULT_DENIED;
break;
}
int calib_ret = ERROR;
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
do_gyro_calibration(mavlink_fd);
result = VEHICLE_CMD_RESULT_ACCEPTED;
/* try to go to INIT/PREFLIGHT arming state */
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
do_mag_calibration(mavlink_fd);
result = VEHICLE_CMD_RESULT_ACCEPTED;
// XXX disable interrupts in arming_state_transition
if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) {
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
break;
}
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
result = VEHICLE_CMD_RESULT_DENIED;
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
result = VEHICLE_CMD_RESULT_DENIED;
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
do_accel_calibration(mavlink_fd);
result = VEHICLE_CMD_RESULT_ACCEPTED;
if ((int)(cmd.param1) == 1) {
/* gyro calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_gyro_calibration(mavlink_fd);
} else if ((int)(cmd.param2) == 1) {
/* magnetometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_mag_calibration(mavlink_fd);
} else if ((int)(cmd.param3) == 1) {
/* zero-altitude pressure calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_accel_calibration(mavlink_fd);
} else if ((int)(cmd.param6) == 1) {
/* airspeed calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_airspeed_calibration(mavlink_fd);
}
} else if ((int)(cmd.param6) == 1) {
/* airspeed calibration */
do_airspeed_calibration(mavlink_fd);
result = VEHICLE_CMD_RESULT_ACCEPTED;
}
if (calib_ret == OK)
tune_positive();
else
tune_negative();
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
break;
}
break;
}
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
if (((int)(cmd.param1)) == 0) {
if (0 == param_load_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
result = VEHICLE_CMD_RESULT_ACCEPTED;
if (((int)(cmd.param1)) == 0) {
if (0 == param_load_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
tune_error();
result = VEHICLE_CMD_RESULT_FAILED;
}
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
} else if (((int)(cmd.param1)) == 1) {
if (0 == param_save_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else if (((int)(cmd.param1)) == 1) {
if (0 == param_save_default()) {
mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
tune_error();
result = VEHICLE_CMD_RESULT_FAILED;
}
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
break;
}
default:
break;
}
/* supported command handling stop */
if (result == VEHICLE_CMD_RESULT_ACCEPTED) {
tune_positive();
} else {
tune_negative();
if (result == VEHICLE_CMD_RESULT_DENIED) {
mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
} else if (result == VEHICLE_CMD_RESULT_FAILED) {
mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
} else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) {
mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
}
default:
answer_command(cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
break;
}
/* send any requested ACKs */
......
......@@ -50,8 +50,13 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
void do_gyro_calibration(int mavlink_fd)
int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
......@@ -98,7 +103,7 @@ void do_gyro_calibration(int mavlink_fd)
} 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;
return ERROR;
}
}
......@@ -137,18 +142,17 @@ void do_gyro_calibration(int mavlink_fd)
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;
return ERROR;
}
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
tune_neutral();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
return;
return ERROR;
}
......@@ -180,8 +184,7 @@ void do_gyro_calibration(int mavlink_fd)
&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
return;
return OK;
}
/* wait blocking for new data */
......@@ -221,7 +224,7 @@ void do_gyro_calibration(int mavlink_fd)
// operating near the 1e6/1e8 max sane resolution of float.
gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f;
warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral);
// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
// } else if (poll_ret == 0) {
// /* any poll failure for 1s is a reason to abort */
......@@ -232,8 +235,8 @@ 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);
warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
......@@ -272,12 +275,10 @@ void do_gyro_calibration(int mavlink_fd)
// mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "gyro calibration done");
tune_positive();
/* third beep by cal end routine */
return OK;
} else {
mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
return ERROR;
}
close(sub_sensor_combined);
}
......@@ -41,6 +41,6 @@
#include <stdint.h>
void do_gyro_calibration(int mavlink_fd);
int do_gyro_calibration(int mavlink_fd);
#endif /* GYRO_CALIBRATION_H_ */
\ No newline at end of file
#endif /* GYRO_CALIBRATION_H_ */
......@@ -53,8 +53,13 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
void do_mag_calibration(int mavlink_fd)
int do_mag_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
......@@ -113,7 +118,7 @@ void do_mag_calibration(int mavlink_fd)
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;
return ERROR;
}
while (hrt_absolute_time() < calibration_deadline &&
......@@ -252,6 +257,7 @@ void do_mag_calibration(int mavlink_fd)
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
return ERROR;
}
warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
......@@ -269,12 +275,11 @@ void do_mag_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, "mag calibration done");
tune_positive();
return OK;
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
return ERROR;
}
close(sub_mag);
}
\ No newline at end of file
}
......@@ -41,6 +41,6 @@
#include <stdint.h>
void do_mag_calibration(int mavlink_fd);
int do_mag_calibration(int mavlink_fd);
#endif /* MAG_CALIBRATION_H_ */
\ No newline at end of file
#endif /* MAG_CALIBRATION_H_ */
......@@ -46,8 +46,13 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;
void do_rc_calibration(int mavlink_fd)
int do_rc_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "trim calibration starting");
......@@ -75,9 +80,9 @@ void do_rc_calibration(int mavlink_fd)
if (save_ret != 0) {
mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
return ERROR;
}
tune_positive();
mavlink_log_info(mavlink_fd, "trim calibration done");
}
\ No newline at end of file
return OK;
}
......@@ -41,6 +41,6 @@
#include <stdint.h>
void do_rc_calibration(int mavlink_fd);
int do_rc_calibration(int mavlink_fd);
#endif /* RC_CALIBRATION_H_ */
\ No newline at end of file
#endif /* RC_CALIBRATION_H_ */
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