From cc499fcc2943b5db85e657a417d752c6a8a76ea4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jun 2015 23:02:09 +0200 Subject: [PATCH] Enable Q attitude estimator and INAV --- makefiles/posix/config_posix_sitl.mk | 2 ++ .../attitude_estimator_q_main.cpp | 35 +++++++++++-------- .../position_estimator_inav_main.c | 24 ++++++------- .../position_estimator_inav_params.c | 8 ++--- .../position_estimator_inav_params.h | 4 +-- 5 files changed, 39 insertions(+), 34 deletions(-) diff --git a/makefiles/posix/config_posix_sitl.mk b/makefiles/posix/config_posix_sitl.mk index d0ec4086bb..c317264143 100644 --- a/makefiles/posix/config_posix_sitl.mk +++ b/makefiles/posix/config_posix_sitl.mk @@ -31,6 +31,8 @@ MODULES += modules/mavlink # MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator +MODULES += modules/attitude_estimator_q +MODULES += modules/position_estimator_inav # # Vehicle Control diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index f999a8f51b..9b945de915 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -39,7 +39,7 @@ * @author Anton Babushkin */ -#include +#include #include #include #include @@ -47,8 +47,6 @@ #include #include #include -#include -#include #include #include #include @@ -189,7 +187,7 @@ AttitudeEstimatorQ::~AttitudeEstimatorQ() { /* if we have given up, kill it */ if (++i > 50) { - task_delete(_control_task); + px4_task_delete(_control_task); break; } } while (_control_task != -1); @@ -206,7 +204,7 @@ int AttitudeEstimatorQ::start() { SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2500, - (main_t)&AttitudeEstimatorQ::task_main_trampoline, + (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline, nullptr); if (_control_task < 0) { @@ -224,7 +222,7 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) { void AttitudeEstimatorQ::task_main() { warnx("started"); - _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); + _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -431,46 +429,53 @@ void AttitudeEstimatorQ::update(float dt) { int attitude_estimator_q_main(int argc, char *argv[]) { if (argc < 1) { - errx(1, "usage: attitude_estimator_q {start|stop|status}"); + warnx("usage: attitude_estimator_q {start|stop|status}"); + return 1; } if (!strcmp(argv[1], "start")) { if (attitude_estimator_q::instance != nullptr) { - errx(1, "already running"); + warnx("already running"); + return 1; } attitude_estimator_q::instance = new AttitudeEstimatorQ; if (attitude_estimator_q::instance == nullptr) { - errx(1, "alloc failed"); + warnx("alloc failed"); + return 1; } if (OK != attitude_estimator_q::instance->start()) { delete attitude_estimator_q::instance; attitude_estimator_q::instance = nullptr; - err(1, "start failed"); + warnx("start failed"); + return 1; } - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { if (attitude_estimator_q::instance == nullptr) { - errx(1, "not running"); + warnx("not running"); + return 1; } delete attitude_estimator_q::instance; attitude_estimator_q::instance = nullptr; - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { if (attitude_estimator_q::instance) { - errx(0, "running"); + warnx("running"); + return 0; } else { - errx(1, "not running"); + warnx("not running"); + return 1; } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index aedfe610b1..a048013c21 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 @@ -45,7 +45,6 @@ #include #include #include -#include #include #include #include @@ -91,7 +90,6 @@ static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5 static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss -static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss static const uint32_t updates_counter_len = 1000000; static const float max_flow = 1.0f; // max flow value that can be used, rad/s @@ -121,7 +119,7 @@ static void usage(const char *reason) } fprintf(stderr, "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); - exit(1); + return; } /** @@ -142,7 +140,7 @@ int position_estimator_inav_main(int argc, char *argv[]) if (thread_running) { warnx("already running"); /* this is not an error */ - exit(0); + return 0; } verbose_mode = false; @@ -157,7 +155,7 @@ int position_estimator_inav_main(int argc, char *argv[]) SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, (argv) ? (char * const *) &argv[2] : (char * const *) NULL); - exit(0); + return 0; } if (!strcmp(argv[1], "stop")) { @@ -169,7 +167,7 @@ int position_estimator_inav_main(int argc, char *argv[]) warnx("not started"); } - exit(0); + return 0; } if (!strcmp(argv[1], "status")) { @@ -180,11 +178,11 @@ int position_estimator_inav_main(int argc, char *argv[]) warnx("not started"); } - exit(0); + return 0; } usage("unrecognized command"); - exit(1); + return 1; } static void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) @@ -194,7 +192,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e if (f) { char *s = malloc(256); unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", - hrt_absolute_time(), msg, (double)dt, + (unsigned long long)hrt_absolute_time(), msg, (double)dt, (double)x_est[0], (double)x_est[1], (double)y_est[0], (double)y_est[1], (double)z_est[0], (double)z_est[1], (double)x_est_prev[0], (double)x_est_prev[1], (double)y_est_prev[0], (double)y_est_prev[1], (double)z_est_prev[0], (double)z_est_prev[1]); fwrite(s, 1, n, f); @@ -348,13 +346,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) struct position_estimator_inav_params params; struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter handles */ - parameters_init(&pos_inav_param_handles); + inav_parameters_init(&pos_inav_param_handles); /* first parameters read at start up */ struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param topic to clear updated flag */ /* first parameters update */ - parameters_update(&pos_inav_param_handles, ¶ms); + inav_parameters_update(&pos_inav_param_handles, ¶ms); struct pollfd fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, @@ -428,7 +426,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); - parameters_update(&pos_inav_param_handles, ¶ms); + inav_parameters_update(&pos_inav_param_handles, ¶ms); } /* actuator */ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 382e9e46d7..eaa0b0a99e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -301,7 +301,7 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0); */ PARAM_DEFINE_INT32(INAV_ENABLED, 1); -int parameters_init(struct position_estimator_inav_param_handles *h) +int inav_parameters_init(struct position_estimator_inav_param_handles *h) { h->w_z_baro = param_find("INAV_W_Z_BARO"); h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); @@ -326,10 +326,10 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->no_vision = param_find("CBRK_NO_VISION"); h->delay_gps = param_find("INAV_DELAY_GPS"); - return OK; + return 0; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +int inav_parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); @@ -353,5 +353,5 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->no_vision, &(p->no_vision)); param_get(h->delay_gps, &(p->delay_gps)); - return OK; + return 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 51bbda412a..7d348fb63c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -97,10 +97,10 @@ struct position_estimator_inav_param_handles { * Initialize all parameter handles and values * */ -int parameters_init(struct position_estimator_inav_param_handles *h); +int inav_parameters_init(struct position_estimator_inav_param_handles *h); /** * Update all parameters * */ -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p); +int inav_parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p);