From 83e356fda41b0936d924e4e74673789bfdd0b29c Mon Sep 17 00:00:00 2001 From: Anton Date: Wed, 27 Mar 2013 16:12:29 +0400 Subject: [PATCH 01/70] Initial version of position_estimator_inav added --- apps/position_estimator_inav/Makefile | 47 +++ .../kalman_filter_inertial.c | 21 ++ .../kalman_filter_inertial.h | 5 + .../position_estimator_inav_main.c | 349 ++++++++++++++++++ .../position_estimator_inav_params.c | 59 +++ .../position_estimator_inav_params.h | 63 ++++ apps/position_estimator_inav/sounds.c | 40 ++ apps/position_estimator_inav/sounds.h | 16 + nuttx/configs/px4fmu/nsh/appconfig | 1 + 9 files changed, 601 insertions(+) create mode 100644 apps/position_estimator_inav/Makefile create mode 100644 apps/position_estimator_inav/kalman_filter_inertial.c create mode 100644 apps/position_estimator_inav/kalman_filter_inertial.h create mode 100644 apps/position_estimator_inav/position_estimator_inav_main.c create mode 100644 apps/position_estimator_inav/position_estimator_inav_params.c create mode 100644 apps/position_estimator_inav/position_estimator_inav_params.h create mode 100644 apps/position_estimator_inav/sounds.c create mode 100644 apps/position_estimator_inav/sounds.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile new file mode 100644 index 0000000000..4906f2d972 --- /dev/null +++ b/apps/position_estimator_inav/Makefile @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (C) 2012 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. +# +############################################################################ + +# +# Makefile to build the position estimator +# + +APPNAME = position_estimator_inav +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 4096 + +CSRCS = position_estimator_inav_main.c \ + position_estimator_inav_params.c \ + kalman_filter_inertial.c \ + sounds.c + +include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/kalman_filter_inertial.c b/apps/position_estimator_inav/kalman_filter_inertial.c new file mode 100644 index 0000000000..7de06cb44e --- /dev/null +++ b/apps/position_estimator_inav/kalman_filter_inertial.c @@ -0,0 +1,21 @@ +#include "kalman_filter_inertial.h" + +void kalman_filter_inertial_predict(float dt, float x[3]) { + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; +} + +void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { + float y[2]; + // y = z - x + for (int i = 0; i < 2; i++) { + y[i] = z[i] - x[i]; + } + // x = x + K * y + for (int i = 0; i < 3; i++) { // Row + for (int j = 0; j < 2; j++) { // Column + if (use[j]) + x[i] += k[i][j] * y[j]; + } + } +} diff --git a/apps/position_estimator_inav/kalman_filter_inertial.h b/apps/position_estimator_inav/kalman_filter_inertial.h new file mode 100644 index 0000000000..e6bbf13153 --- /dev/null +++ b/apps/position_estimator_inav/kalman_filter_inertial.h @@ -0,0 +1,5 @@ +#include + +void kalman_filter_inertial_predict(float dt, float x[3]); + +void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]); diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c new file mode 100644 index 0000000000..e28be5b51d --- /dev/null +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -0,0 +1,349 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli + * Lorenz Meier + * + * 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 position_estimator_inav_main.c + * Model-identification based position estimator for multirotors + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "position_estimator_inav_params.h" +//#include +#include "sounds.h" +#include +#include "kalman_filter_inertial.h" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int position_estimator_inav_task; /**< Handle of deamon task / thread */ + +__EXPORT int position_estimator_inav_main(int argc, char *argv[]); + +int position_estimator_inav_thread_main(int argc, char *argv[]); +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void usage(const char *reason) { + if (reason) + fprintf(stderr, "%s\n", reason); + fprintf(stderr,"usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); + exit(1); +} + + /** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int position_estimator_inav_main(int argc, char *argv[]) { + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("position_estimator_inav already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + position_estimator_inav_task = task_spawn("position_estimator_inav", + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + position_estimator_inav_thread_main, + (argv) ? (const char **) &argv[2] : (const char **) NULL ); + exit(0); + } + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tposition_estimator_inav is running\n"); + } else { + printf("\tposition_estimator_inav not started\n"); + } + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +/**************************************************************************** + * main + ****************************************************************************/ +int position_estimator_inav_thread_main(int argc, char *argv[]) { + /* welcome user */ + printf("[position_estimator_inav] started\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); + + /* initialize values */ + static float k[3][2] = { { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 0.0f, 0.0f } }; + static float x_est[3] = { 0.0f, 0.0f, 0.0f }; + static float y_est[3] = { 0.0f, 0.0f, 0.0f }; + static float z_est[3] = { 0.0f, 0.0f, 0.0f }; + const static float dT_const_120 = 1.0f / 120.0f; + const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f; + + int baro_loop_cnt = 0; + int baro_loop_end = 70; /* measurement for 1 second */ + float baro_alt0 = 0.0f; /* to determin while start up */ + float rho0 = 1.293f; /* standard pressure */ + const static float const_earth_gravity = 9.81f; + + static double lat_current = 0.0d; //[°]] --> 47.0 + static double lon_current = 0.0d; //[°]] -->8.5 + static double alt_current = 0.0d; //[m] above MSL + + /* declare and safely initialize all structs */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ + struct sensor_combined_s sensor; + memset(&sensor, 0, sizeof(sensor)); + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + struct vehicle_local_position_s local_pos_est; + memset(&local_pos_est, 0, sizeof(local_pos_est)); + + /* subscribe */ + int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); + int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + + /* advertise */ + orb_advert_t local_pos_est_pub = orb_advertise( + ORB_ID(vehicle_local_position), &local_pos_est); + + struct position_estimator_inav_params pos_inav_params; + struct position_estimator_inav_param_handles pos_inav_param_handles; + /* initialize parameter handles */ + parameters_init(&pos_inav_param_handles); + + bool local_flag_baroINITdone = false; /* in any case disable baroINITdone */ + /* FIRST PARAMETER READ at START UP*/ + struct parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param to clear updated flag */ + /* FIRST PARAMETER UPDATE */ + parameters_update(&pos_inav_param_handles, &pos_inav_params); + /* END FIRST PARAMETER UPDATE */ + /* wait until gps signal turns valid, only then can we initialize the projection */ + struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, .events = + POLLIN } }; + + while (gps.fix_type < 3) { + + /* wait for GPS updates, BUT READ VEHICLE STATUS (!) + * this choice is critical, since the vehicle status might not + * actually change, if this app is started after GPS lock was + * aquired. + */ + if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ + if (fds_init[0].revents & POLLIN) { + /* Wait for the GPS update to propagate (we have some time) */ + usleep(5000); + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, + &gps); + } + } + static int printcounter = 0; + if (printcounter == 100) { + printcounter = 0; + printf("[pos_est1D] wait for GPS fix type 3\n"); + } + printcounter++; + } + + /* get gps value for first initialization */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + lat_current = ((double) (gps.lat)) * 1e-7; + lon_current = ((double) (gps.lon)) * 1e-7; + alt_current = gps.alt * 1e-3; + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + /* publish global position messages only after first GPS message */ + printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", + lat_current, lon_current); + uint64_t last_time = 0; + thread_running = true; + + /** main loop */ + struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { + .fd = vehicle_status_sub, .events = POLLIN }, { .fd = + vehicle_attitude_sub, .events = POLLIN }, { .fd = + sensor_combined_sub, .events = POLLIN }, { .fd = + vehicle_gps_position_sub, .events = POLLIN }, }; + while (!thread_should_exit) { + int ret = poll(fds, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate + if (ret < 0) { + /* poll error */ + } else { + /* parameter update */ + if (fds[0].revents & POLLIN) { + /* read from param to clear updated flag */ + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), parameter_update_sub, + &update); + /* update parameters */ + parameters_update(&pos_inav_param_handles, &pos_inav_params); + //r = pos_inav_params.r; + } + /* vehicle status */ + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + } + /* vehicle attitude */ + if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + } + /* sensor combined */ + if (fds[3].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, + &sensor); + } + /* vehicle GPS position */ + if (fds[4].revents & POLLIN) { + /* new GPS value */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, + &gps); + static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ + /* Project gps lat lon (Geographic coordinate system) to plane */ + map_projection_project(((double) (gps.lat)) * 1e-7, + ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), + &(local_pos_gps[1])); + local_pos_gps[2] = (float) (gps.alt * 1e-3); + } + // barometric pressure estimation at start up + if (!local_flag_baroINITdone) { + // mean calculation over several measurements + if (baro_loop_cnt < baro_loop_end) { + baro_alt0 += sensor.baro_alt_meter; + baro_loop_cnt++; + } else { + baro_alt0 /= (float) (baro_loop_cnt); + local_flag_baroINITdone = true; + char *baro_m_start = "barometer initialized with alt0 = "; + char p0_char[15]; + sprintf(p0_char, "%8.2f", baro_alt0 / 100); + char *baro_m_end = " m"; + char str[80]; + strcpy(str, baro_m_start); + strcat(str, p0_char); + strcat(str, baro_m_end); + mavlink_log_info(mavlink_fd, str); + } + } + /* TODO convert accelerations from UAV frame to NED frame */ + float accel_NED[3]; + accel_NED[2] = sensor.accelerometer_m_s2[2] + const_earth_gravity; + /* prediction */ + kalman_filter_inertial_predict(dT_const_120, z_est); + /* prepare vectors for kalman filter correction */ + float z_meas[2]; // pos, accel + bool use_z[2] = { false, true }; + if (local_flag_baroINITdone) { + z_meas[0] = sensor.baro_alt_meter - baro_alt0; + use_z[0] = true; + } + z_meas[1] = accel_NED[2]; + /* correction */ + kalman_filter_inertial_update(z_est, z_meas, k, use_z); + printf("[pos_est_inav] att = %.3f, %.3f, %.3f, %.3f\n", att.q[0], att.q[1], att.q[2], att.q[3]); + printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", z_est[0], z_est[1], z_est[2]); + + local_pos_est.x = 0.0; + local_pos_est.vx = 0.0; + local_pos_est.y = 0.0; + local_pos_est.vy = 0.0; + local_pos_est.z = z_est[0]; + local_pos_est.vz = z_est[1]; + local_pos_est.timestamp = hrt_absolute_time(); + if ((isfinite(local_pos_est.x)) && (isfinite(local_pos_est.vx)) + && (isfinite(local_pos_est.y)) + && (isfinite(local_pos_est.vy)) + && (isfinite(local_pos_est.z)) + && (isfinite(local_pos_est.vz))) { + orb_publish(ORB_ID( + vehicle_local_position), local_pos_est_pub, + &local_pos_est); + } + } /* end of poll return value check */ + } + + printf("[pos_est_inav] exiting.\n"); + mavlink_log_info(mavlink_fd, "[pos_est_inav] exiting"); + thread_running = false; + return 0; +} diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c new file mode 100644 index 0000000000..5567fd2cf2 --- /dev/null +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -0,0 +1,59 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli +* Lorenz Meier + * + * 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 position_estimator_inav_params.c + * + * Parameters for position_estimator_inav + */ + +#include "position_estimator_inav_params.h" + +/* Kalman Filter covariances */ +/* gps measurement noise standard deviation */ +PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); + +int parameters_init(struct position_estimator_inav_param_handles *h) +{ + h->r = param_find("POS_EST_R"); + return OK; +} + +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +{ + param_get(h->r, &(p->r)); + return OK; +} diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h new file mode 100644 index 0000000000..c45ca81359 --- /dev/null +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -0,0 +1,63 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Damian Aregger + * Tobias Naegeli + * Lorenz Meier + * + * 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 position_estimator_inav_params.h + * + * Parameters for Position Estimator + */ + +#include + +struct position_estimator_inav_params { + float r; +}; + +struct position_estimator_inav_param_handles { + param_t r; +}; + +/** + * Initialize all parameter handles and values + * + */ +int 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); diff --git a/apps/position_estimator_inav/sounds.c b/apps/position_estimator_inav/sounds.c new file mode 100644 index 0000000000..4f7b05fea5 --- /dev/null +++ b/apps/position_estimator_inav/sounds.c @@ -0,0 +1,40 @@ +/* + * sounds.c + * + * Created on: Feb 26, 2013 + * Author: samuezih + */ + +#include +#include +#include + + +static int buzzer; + +int sounds_init(void) +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return 0; +} + +void sounds_deinit(void) +{ + close(buzzer); +} + +void tune_tetris(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 6); +} + +void tune_sonar(void) +{ + ioctl(buzzer, TONE_SET_ALARM, 7); +} diff --git a/apps/position_estimator_inav/sounds.h b/apps/position_estimator_inav/sounds.h new file mode 100644 index 0000000000..356e42169c --- /dev/null +++ b/apps/position_estimator_inav/sounds.h @@ -0,0 +1,16 @@ +/* + * sounds.h + * + * Created on: Feb 26, 2013 + * Author: samuezih + */ + +#ifndef SOUNDS_H_ +#define SOUNDS_H_ + +int sounds_init(void); +void sounds_deinit(void); +void tune_tetris(void); +void tune_sonar(void); + +#endif /* SOUNDS_H_ */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 80cf6f312c..c92e29930b 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -101,6 +101,7 @@ CONFIGURED_APPS += multirotor_pos_control CONFIGURED_APPS += fixedwing_att_control CONFIGURED_APPS += fixedwing_pos_control CONFIGURED_APPS += position_estimator +CONFIGURED_APPS += position_estimator_inav CONFIGURED_APPS += attitude_estimator_ekf CONFIGURED_APPS += hott_telemetry endif From b837692d48e6dcc568bdf7009f515504c0772cf8 Mon Sep 17 00:00:00 2001 From: Anton Date: Thu, 28 Mar 2013 21:35:38 +0400 Subject: [PATCH 02/70] Test rotation matrix --- .../position_estimator_inav_main.c | 110 ++++++++++-------- 1 file changed, 64 insertions(+), 46 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index e28be5b51d..774125e837 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -154,6 +154,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { const static float dT_const_120 = 1.0f / 120.0f; const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f; + bool use_gps = false; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ @@ -200,40 +201,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { parameters_update(&pos_inav_param_handles, &pos_inav_params); /* END FIRST PARAMETER UPDATE */ /* wait until gps signal turns valid, only then can we initialize the projection */ - struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, .events = - POLLIN } }; + if (use_gps) { + struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, + .events = POLLIN } }; - while (gps.fix_type < 3) { + while (gps.fix_type < 3) { - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ - if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ - if (fds_init[0].revents & POLLIN) { - /* Wait for the GPS update to propagate (we have some time) */ - usleep(5000); - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, - &gps); + /* wait for GPS updates, BUT READ VEHICLE STATUS (!) + * this choice is critical, since the vehicle status might not + * actually change, if this app is started after GPS lock was + * aquired. + */ + if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ + if (fds_init[0].revents & POLLIN) { + /* Wait for the GPS update to propagate (we have some time) */ + usleep(5000); + orb_copy(ORB_ID(vehicle_gps_position), + vehicle_gps_position_sub, &gps); + } } + static int printcounter = 0; + if (printcounter == 100) { + printcounter = 0; + printf("[pos_est1D] wait for GPS fix type 3\n"); + } + printcounter++; } - static int printcounter = 0; - if (printcounter == 100) { - printcounter = 0; - printf("[pos_est1D] wait for GPS fix type 3\n"); - } - printcounter++; - } - /* get gps value for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - lat_current = ((double) (gps.lat)) * 1e-7; - lon_current = ((double) (gps.lon)) * 1e-7; - alt_current = gps.alt * 1e-3; - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ + /* get gps value for first initialization */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + lat_current = ((double) (gps.lat)) * 1e-7; + lon_current = ((double) (gps.lon)) * 1e-7; + alt_current = gps.alt * 1e-3; + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + /* publish global position messages only after first GPS message */ + } printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); uint64_t last_time = 0; @@ -245,6 +248,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { vehicle_attitude_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN }, }; + printf("[position_estimator_inav] main loop started\n"); + static int printatt_counter = 0; while (!thread_should_exit) { int ret = poll(fds, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate if (ret < 0) { @@ -262,7 +267,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } /* vehicle status */ if (fds[1].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, + &vehicle_status); } /* vehicle attitude */ if (fds[2].revents & POLLIN) { @@ -270,20 +276,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } /* sensor combined */ if (fds[3].revents & POLLIN) { - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, - &sensor); + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); } - /* vehicle GPS position */ - if (fds[4].revents & POLLIN) { - /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, - &gps); - static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ - /* Project gps lat lon (Geographic coordinate system) to plane */ - map_projection_project(((double) (gps.lat)) * 1e-7, - ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), - &(local_pos_gps[1])); - local_pos_gps[2] = (float) (gps.alt * 1e-3); + if (use_gps) { + /* vehicle GPS position */ + if (fds[4].revents & POLLIN) { + /* new GPS value */ + orb_copy(ORB_ID(vehicle_gps_position), + vehicle_gps_position_sub, &gps); + static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ + /* Project gps lat lon (Geographic coordinate system) to plane */ + map_projection_project(((double) (gps.lat)) * 1e-7, + ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), + &(local_pos_gps[1])); + local_pos_gps[2] = (float) (gps.alt * 1e-3); + } } // barometric pressure estimation at start up if (!local_flag_baroINITdone) { @@ -320,9 +327,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { z_meas[1] = accel_NED[2]; /* correction */ kalman_filter_inertial_update(z_est, z_meas, k, use_z); - printf("[pos_est_inav] att = %.3f, %.3f, %.3f, %.3f\n", att.q[0], att.q[1], att.q[2], att.q[3]); - printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", z_est[0], z_est[1], z_est[2]); - + if (printatt_counter == 100) { + printatt_counter = 0; + printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", + att.pitch, att.roll, att.yaw); + printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", + att.R[0][0], att.R[0][1], att.R[0][2]); + printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", + att.R[1][0], att.R[1][1], att.R[1][2]); + printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", + att.R[2][0], att.R[2][1], att.R[2][2]); + printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", + z_est[0], z_est[1], z_est[2]); + } + printatt_counter++; local_pos_est.x = 0.0; local_pos_est.vx = 0.0; local_pos_est.y = 0.0; From fb663bbb0c02ab6d6b6e1b360f5833028cd25bdd Mon Sep 17 00:00:00 2001 From: Anton Date: Fri, 29 Mar 2013 13:08:56 +0400 Subject: [PATCH 03/70] Acceleration convertion from UAV frame to NED frame --- .../position_estimator_inav_main.c | 75 ++++++++++++------- 1 file changed, 46 insertions(+), 29 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 774125e837..4277fe19bf 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -87,18 +87,19 @@ static void usage(const char *reason); static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr,"usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); exit(1); } - /** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ +/** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ int position_estimator_inav_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -147,7 +148,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); /* initialize values */ - static float k[3][2] = { { 0.0f, 0.0f }, { 0.0f, 0.0f }, { 0.0f, 0.0f } }; + static float k[3][2] = { { 0.002f, 0.0f }, { 0.001f, 0.00002f }, { 0.0f, + 0.99f } }; static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -167,7 +169,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); /* make sure that baroINITdone = false */ + memset(&vehicle_status, 0, sizeof(vehicle_status)); + /* make sure that baroINITdone = false */ struct sensor_combined_s sensor; memset(&sensor, 0, sizeof(sensor)); struct vehicle_gps_position_s gps; @@ -239,19 +242,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); - uint64_t last_time = 0; + hrt_abstime last_time = 0; thread_running = true; /** main loop */ - struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { - .fd = vehicle_status_sub, .events = POLLIN }, { .fd = - vehicle_attitude_sub, .events = POLLIN }, { .fd = - sensor_combined_sub, .events = POLLIN }, { .fd = - vehicle_gps_position_sub, .events = POLLIN }, }; + struct pollfd fds[3] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN } }; printf("[position_estimator_inav] main loop started\n"); static int printatt_counter = 0; while (!thread_should_exit) { - int ret = poll(fds, 5, 20); //wait maximal this 20 ms = 50 Hz minimum rate + int ret = poll(fds, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate if (ret < 0) { /* poll error */ } else { @@ -271,16 +270,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { &vehicle_status); } /* vehicle attitude */ - if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - } + //if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + //} /* sensor combined */ - if (fds[3].revents & POLLIN) { + if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); } if (use_gps) { /* vehicle GPS position */ - if (fds[4].revents & POLLIN) { + //if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); @@ -290,7 +289,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); - } + //} } // barometric pressure estimation at start up if (!local_flag_baroINITdone) { @@ -312,16 +311,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, str); } } - /* TODO convert accelerations from UAV frame to NED frame */ - float accel_NED[3]; - accel_NED[2] = sensor.accelerometer_m_s2[2] + const_earth_gravity; + + hrt_abstime t = hrt_absolute_time(); + float dt = (t - last_time) / 1000000.0; + last_time = t; + /* convert accelerations from UAV frame to NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, 0.0f }; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + } + } + accel_NED[2] += const_earth_gravity; /* prediction */ - kalman_filter_inertial_predict(dT_const_120, z_est); + kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ float z_meas[2]; // pos, accel bool use_z[2] = { false, true }; if (local_flag_baroINITdone) { - z_meas[0] = sensor.baro_alt_meter - baro_alt0; + z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt use_z[0] = true; } z_meas[1] = accel_NED[2]; @@ -329,14 +337,23 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { kalman_filter_inertial_update(z_est, z_meas, k, use_z); if (printatt_counter == 100) { printatt_counter = 0; + printf("[pos_est_inav] dt = %.6f\n", dt); printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", att.pitch, att.roll, att.yaw); + printf("[pos_est_inav] accel_UAV = %.3f, %.3f, %.3f\n", + sensor.accelerometer_m_s2[0], + sensor.accelerometer_m_s2[1], + sensor.accelerometer_m_s2[2]); + printf("[pos_est_inav] accel_NED = %.3f, %.3f, %.3f\n", + accel_NED[0], accel_NED[1], accel_NED[2]); + /* printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", att.R[0][0], att.R[0][1], att.R[0][2]); printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", att.R[1][0], att.R[1][1], att.R[1][2]); printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", att.R[2][0], att.R[2][1], att.R[2][2]); + */ printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", z_est[0], z_est[1], z_est[2]); } From 382af2b52d98de82c5cc0953e2c2ba7acf65501f Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 Mar 2013 01:54:04 +0400 Subject: [PATCH 04/70] Acceleration vector transform implemented. Accelerometer calibration procedure defined but not implemented yet. --- apps/position_estimator_inav/Makefile | 1 + .../acceleration_transform.c | 93 ++++++++ .../acceleration_transform.h | 15 ++ .../position_estimator_inav_main.c | 216 +++++++++++------- 4 files changed, 242 insertions(+), 83 deletions(-) create mode 100644 apps/position_estimator_inav/acceleration_transform.c create mode 100644 apps/position_estimator_inav/acceleration_transform.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index 4906f2d972..e5f3b4b42b 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -42,6 +42,7 @@ STACKSIZE = 4096 CSRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ kalman_filter_inertial.c \ + acceleration_transform.c \ sounds.c include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c new file mode 100644 index 0000000000..9080c4e204 --- /dev/null +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -0,0 +1,93 @@ +/* + * acceleration_transform.c + * + * Created on: 30.03.2013 + * Author: Anton Babushkin + * + * Transform acceleration vector to true orientation and scale + * + * * * * Model * * * + * accel_corr = accel_T * (accel_raw - accel_offs) + * + * accel_corr[3] - fully corrected acceleration vector in UAV frame + * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform + * accel_raw[3] - raw acceleration vector + * accel_offs[3] - true acceleration offset vector, don't use sensors offset because + * it's caused not only by real offset but also by sensor rotation + * + * * * * Calibration * * * + * + * Tests + * + * accel_corr_x_p = [ g 0 0 ]^T // positive x + * accel_corr_x_n = [ -g 0 0 ]^T // negative x + * accel_corr_y_p = [ 0 g 0 ]^T // positive y + * accel_corr_y_n = [ 0 -g 0 ]^T // negative y + * accel_corr_z_p = [ 0 0 g ]^T // positive z + * accel_corr_z_n = [ 0 0 -g ]^T // negative z + * + * 6 tests * 3 axes = 18 equations + * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants + * + * Find accel_offs + * + * accel_offs = (accel_raw_x_p + accel_raw_x_n + + * accel_raw_y_p + accel_raw_y_n + + * accel_raw_z_p + accel_raw_z_n) / 6 + * + * + * Find accel_T + * + * 9 unknown constants + * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations + * + * accel_corr[i] = accel_T[i][0] * (accel_raw[0] - accel_offs[0]) + + * accel_T[i][1] * (accel_raw[1] - accel_offs[1]) + + * accel_T[i][2] * (accel_raw[2] - accel_offs[2]) + * A x = b + * + * x = [ accel_T[0][0] ] + * | accel_T[0][1] | + * | accel_T[0][2] | + * | accel_T[1][0] | + * | accel_T[1][1] | + * | accel_T[1][2] | + * | accel_T[2][0] | + * | accel_T[2][1] | + * [ accel_T[2][2] ] + * + * b = [ accel_corr_x_p[0] ] // One measurement per axis is enough + * | accel_corr_x_p[1] | + * | accel_corr_x_p[2] | + * | accel_corr_y_p[0] | + * | accel_corr_y_p[1] | + * | accel_corr_y_p[2] | + * | accel_corr_z_p[0] | + * | accel_corr_z_p[1] | + * [ accel_corr_z_p[2] ] + * + * a_x[i] = accel_raw_x_p[i] - accel_offs[i] // Measurement for X axis + * ... + * A = [ a_x[0] a_x[1] a_x[2] 0 0 0 0 0 0 ] + * | 0 0 0 a_x[0] a_x[1] a_x[2] 0 0 0 | + * | 0 0 0 0 0 0 a_x[0] a_x[1] a_x[2] | + * | a_y[0] a_y[1] a_y[2] 0 0 0 0 0 0 | + * | 0 0 0 a_y[0] a_y[1] a_y[2] 0 0 0 | + * | 0 0 0 0 0 0 a_y[0] a_y[1] a_y[2] | + * | a_z[0] a_z[1] a_z[2] 0 0 0 0 0 0 | + * | 0 0 0 a_z[0] a_z[1] a_z[2] 0 0 0 | + * [ 0 0 0 0 0 0 a_z[0] a_z[1] a_z[2] ] + * + * x = A^-1 b + */ + +#include "acceleration_transform.h" + +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]) { + for (int i = 0; i < 3; i++) { + accel_corr[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]); + } + } +} diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h new file mode 100644 index 0000000000..3692da77ec --- /dev/null +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -0,0 +1,15 @@ +/* + * acceleration_transform.h + * + * Created on: 30.03.2013 + * Author: ton + */ + +#ifndef ACCELERATION_TRANSFORM_H_ +#define ACCELERATION_TRANSFORM_H_ + +#include + +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]); + +#endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 4277fe19bf..1c6d21ded7 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -71,6 +71,7 @@ #include "sounds.h" #include #include "kalman_filter_inertial.h" +#include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -147,9 +148,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); + /* kalman filter K for simulation parameters: + * rate_accel = 200 Hz + * rate_baro = 100 Hz + * err_accel = 1.0 m/s^2 + * err_baro = 0.2 m + */ + static float k[3][2] = { + { 0.0262f, 0.00001f }, + { 0.0349f, 0.00191f }, + { 0.000259f, 0.618f } + }; /* initialize values */ - static float k[3][2] = { { 0.002f, 0.0f }, { 0.001f, 0.00002f }, { 0.0f, - 0.99f } }; static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -160,7 +170,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ - float rho0 = 1.293f; /* standard pressure */ const static float const_earth_gravity = 9.81f; static double lat_current = 0.0d; //[°]] --> 47.0 @@ -203,6 +212,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* FIRST PARAMETER UPDATE */ parameters_update(&pos_inav_param_handles, &pos_inav_params); /* END FIRST PARAMETER UPDATE */ + + // TODO implement calibration procedure, now put dummy values + int16_t accel_offs[3] = { 0, 0, 0 }; + float accel_T[3][3] = { + { 1.0f, 0.0f, 0.0f }, + { 0.0f, 1.0f, 0.0f }, + { 0.0f, 0.0f, 1.0f } + }; + /* wait until gps signal turns valid, only then can we initialize the projection */ if (use_gps) { struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, @@ -226,12 +244,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static int printcounter = 0; if (printcounter == 100) { printcounter = 0; - printf("[pos_est1D] wait for GPS fix type 3\n"); + printf("[position_estimator_inav] wait for GPS fix type 3\n"); } printcounter++; } - /* get gps value for first initialization */ + /* get GPS position for first initialization */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); lat_current = ((double) (gps.lat)) * 1e-7; lon_current = ((double) (gps.lon)) * 1e-7; @@ -240,20 +258,38 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - printf("[pos_est1D] initialized projection with: lat: %.10f, lon:%.10f\n", + printf("[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); + hrt_abstime last_time = 0; thread_running = true; - - /** main loop */ - struct pollfd fds[3] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN } }; - printf("[position_estimator_inav] main loop started\n"); static int printatt_counter = 0; + uint32_t accelerometer_counter = 0; + uint32_t baro_counter = 0; + uint16_t accelerometer_updates = 0; + uint16_t baro_updates = 0; + hrt_abstime updates_counter_start = hrt_absolute_time(); + uint32_t updates_counter_len = 1000000; + hrt_abstime pub_last = 0; + uint32_t pub_interval = 5000; // limit publish rate to 200 Hz + + /* main loop */ + struct pollfd fds[3] = { + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; + printf("[position_estimator_inav] main loop started\n"); while (!thread_should_exit) { - int ret = poll(fds, 3, 20); //wait maximal this 20 ms = 50 Hz minimum rate + bool accelerometer_updated = false; + bool baro_updated = false; + int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ - } else { + printf("[position_estimator_inav] subscriptions poll error\n", ret); + } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ @@ -262,7 +298,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { &update); /* update parameters */ parameters_update(&pos_inav_param_handles, &pos_inav_params); - //r = pos_inav_params.r; } /* vehicle status */ if (fds[1].revents & POLLIN) { @@ -270,16 +305,41 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { &vehicle_status); } /* vehicle attitude */ - //if (fds[2].revents & POLLIN) { - orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); - //} - /* sensor combined */ if (fds[2].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + } + /* sensor combined */ + if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + if (sensor.accelerometer_counter > accelerometer_counter) { + accelerometer_updated = true; + accelerometer_counter = sensor.accelerometer_counter; + accelerometer_updates++; + } + if (sensor.baro_counter > baro_counter) { + baro_updated = true; + baro_counter = sensor.baro_counter; + baro_updates++; + } + // barometric pressure estimation at start up + if (!local_flag_baroINITdone && baro_updated) { + // mean calculation over several measurements + if (baro_loop_cnt < baro_loop_end) { + baro_alt0 += sensor.baro_alt_meter; + baro_loop_cnt++; + } else { + baro_alt0 /= (float) (baro_loop_cnt); + local_flag_baroINITdone = true; + char str[80]; + sprintf(str, "[position_estimator_inav] barometer initialized with alt0 = %.2f", baro_alt0); + printf("%s\n", str); + mavlink_log_info(mavlink_fd, str); + } + } } if (use_gps) { /* vehicle GPS position */ - //if (fds[4].revents & POLLIN) { + if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); @@ -289,75 +349,65 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); - //} - } - // barometric pressure estimation at start up - if (!local_flag_baroINITdone) { - // mean calculation over several measurements - if (baro_loop_cnt < baro_loop_end) { - baro_alt0 += sensor.baro_alt_meter; - baro_loop_cnt++; - } else { - baro_alt0 /= (float) (baro_loop_cnt); - local_flag_baroINITdone = true; - char *baro_m_start = "barometer initialized with alt0 = "; - char p0_char[15]; - sprintf(p0_char, "%8.2f", baro_alt0 / 100); - char *baro_m_end = " m"; - char str[80]; - strcpy(str, baro_m_start); - strcat(str, p0_char); - strcat(str, baro_m_end); - mavlink_log_info(mavlink_fd, str); } } - hrt_abstime t = hrt_absolute_time(); - float dt = (t - last_time) / 1000000.0; - last_time = t; - /* convert accelerations from UAV frame to NED frame */ - float accel_NED[3] = { 0.0f, 0.0f, 0.0f }; - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; - } - } - accel_NED[2] += const_earth_gravity; - /* prediction */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // pos, accel - bool use_z[2] = { false, true }; - if (local_flag_baroINITdone) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; + } /* end of poll return value check */ + + hrt_abstime t = hrt_absolute_time(); + float dt = (t - last_time) / 1000000.0; + last_time = t; + /* calculate corrected acceleration vector in UAV frame */ + float accel_corr[3]; + acceleration_correct(accel_corr, sensor.accelerometer_raw, accel_T, accel_offs); + /* transform acceleration vector from UAV frame to NED frame */ + float accel_NED[3]; + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * accel_corr[j]; } + } + accel_NED[2] += const_earth_gravity; + /* kalman filter prediction */ + kalman_filter_inertial_predict(dt, z_est); + /* prepare vectors for kalman filter correction */ + float z_meas[2]; // position, acceleration + bool use_z[2] = { false, false }; + if (local_flag_baroINITdone && baro_updated) { + z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt + use_z[0] = true; + } + if (accelerometer_updated) { z_meas[1] = accel_NED[2]; + use_z[1] = true; + } + if (use_z[0] || use_z[1]) { /* correction */ kalman_filter_inertial_update(z_est, z_meas, k, use_z); - if (printatt_counter == 100) { - printatt_counter = 0; - printf("[pos_est_inav] dt = %.6f\n", dt); - printf("[pos_est_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", - att.pitch, att.roll, att.yaw); - printf("[pos_est_inav] accel_UAV = %.3f, %.3f, %.3f\n", - sensor.accelerometer_m_s2[0], - sensor.accelerometer_m_s2[1], - sensor.accelerometer_m_s2[2]); - printf("[pos_est_inav] accel_NED = %.3f, %.3f, %.3f\n", - accel_NED[0], accel_NED[1], accel_NED[2]); - /* - printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", - att.R[0][0], att.R[0][1], att.R[0][2]); - printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", - att.R[1][0], att.R[1][1], att.R[1][2]); - printf("[pos_est_inav] Rot_matrix: %.3f %.3f %.3f\n", - att.R[2][0], att.R[2][1], att.R[2][2]); - */ - printf("[pos_est_inav] z = %.2f, vz = %.2f, az = %.2f\n", - z_est[0], z_est[1], z_est[2]); - } - printatt_counter++; + } + if (t - updates_counter_start > updates_counter_len) { + float updates_dt = (t - updates_counter_start) * 0.000001f; + printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); + updates_counter_start = t; + } + if (printatt_counter == 100) { + printatt_counter = 0; + printf("[position_estimator_inav] dt = %.6f\n", dt); + printf("[position_estimator_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", + att.pitch, att.roll, att.yaw); + printf("[position_estimator_inav] accel_UAV = %.3f, %.3f, %.3f\n", + sensor.accelerometer_m_s2[0], + sensor.accelerometer_m_s2[1], + sensor.accelerometer_m_s2[2]); + printf("[position_estimator_inav] accel_NED = %.3f, %.3f, %.3f\n", + accel_NED[0], accel_NED[1], accel_NED[2]); + printf("[position_estimator_inav] z = %.2f, vz = %.2f, az = %.2f\n", + z_est[0], z_est[1], z_est[2]); + } + printatt_counter++; + if (t - pub_last > pub_interval) { + pub_last = t; local_pos_est.x = 0.0; local_pos_est.vx = 0.0; local_pos_est.y = 0.0; @@ -374,11 +424,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { vehicle_local_position), local_pos_est_pub, &local_pos_est); } - } /* end of poll return value check */ + } } - printf("[pos_est_inav] exiting.\n"); - mavlink_log_info(mavlink_fd, "[pos_est_inav] exiting"); + printf("[position_estimator_inav] exiting.\n"); + mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); thread_running = false; return 0; } From 114685a40bf38dc4281224ea18f898b6159df037 Mon Sep 17 00:00:00 2001 From: Anton Date: Sat, 30 Mar 2013 21:30:58 +0400 Subject: [PATCH 05/70] position_estimator_inav - first working version --- apps/position_estimator_inav/Makefile | 3 +- .../acceleration_transform.c | 4 +- .../acceleration_transform.h | 4 +- .../kalman_filter_inertial.c | 14 +++- .../kalman_filter_inertial.h | 7 ++ .../position_estimator_inav_main.c | 82 ++++++------------- .../position_estimator_inav_params.c | 69 ++++++++++++++-- .../position_estimator_inav_params.h | 31 +++++-- apps/position_estimator_inav/sounds.c | 40 --------- apps/position_estimator_inav/sounds.h | 16 ---- 10 files changed, 135 insertions(+), 135 deletions(-) delete mode 100644 apps/position_estimator_inav/sounds.c delete mode 100644 apps/position_estimator_inav/sounds.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index e5f3b4b42b..192ec18255 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -42,7 +42,6 @@ STACKSIZE = 4096 CSRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ kalman_filter_inertial.c \ - acceleration_transform.c \ - sounds.c + acceleration_transform.c include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index 9080c4e204..bd933cab4b 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -1,8 +1,8 @@ /* * acceleration_transform.c * - * Created on: 30.03.2013 - * Author: Anton Babushkin + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Transform acceleration vector to true orientation and scale * diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h index 3692da77ec..26bf0d68fc 100644 --- a/apps/position_estimator_inav/acceleration_transform.h +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -1,8 +1,8 @@ /* * acceleration_transform.h * - * Created on: 30.03.2013 - * Author: ton + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin */ #ifndef ACCELERATION_TRANSFORM_H_ diff --git a/apps/position_estimator_inav/kalman_filter_inertial.c b/apps/position_estimator_inav/kalman_filter_inertial.c index 7de06cb44e..64031ee7bb 100644 --- a/apps/position_estimator_inav/kalman_filter_inertial.c +++ b/apps/position_estimator_inav/kalman_filter_inertial.c @@ -1,3 +1,10 @@ +/* + * kalman_filter_inertial.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + #include "kalman_filter_inertial.h" void kalman_filter_inertial_predict(float dt, float x[3]) { @@ -7,10 +14,9 @@ void kalman_filter_inertial_predict(float dt, float x[3]) { void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { float y[2]; - // y = z - x - for (int i = 0; i < 2; i++) { - y[i] = z[i] - x[i]; - } + // y = z - H x + y[0] = z[0] - x[0]; + y[1] = z[1] - x[2]; // x = x + K * y for (int i = 0; i < 3; i++) { // Row for (int j = 0; j < 2; j++) { // Column diff --git a/apps/position_estimator_inav/kalman_filter_inertial.h b/apps/position_estimator_inav/kalman_filter_inertial.h index e6bbf13153..3e5134c33e 100644 --- a/apps/position_estimator_inav/kalman_filter_inertial.h +++ b/apps/position_estimator_inav/kalman_filter_inertial.h @@ -1,3 +1,10 @@ +/* + * kalman_filter_inertial.h + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + #include void kalman_filter_inertial_predict(float dt, float x[3]); diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 1c6d21ded7..b8ef1f76a4 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -67,15 +65,13 @@ #include #include "position_estimator_inav_params.h" -//#include -#include "sounds.h" -#include #include "kalman_filter_inertial.h" #include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ +static bool verbose_mode = false; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -89,7 +85,7 @@ static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-p ]\n\n"); + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } @@ -106,12 +102,14 @@ int position_estimator_inav_main(int argc, char *argv[]) { usage("missing command"); if (!strcmp(argv[1], "start")) { - if (thread_running) { printf("position_estimator_inav already running\n"); /* this is not an error */ exit(0); } + if (argc > 1) + if (!strcmp(argv[2], "-v")) + verbose_mode = true; thread_should_exit = false; position_estimator_inav_task = task_spawn("position_estimator_inav", @@ -148,17 +146,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); - /* kalman filter K for simulation parameters: - * rate_accel = 200 Hz - * rate_baro = 100 Hz - * err_accel = 1.0 m/s^2 - * err_baro = 0.2 m - */ - static float k[3][2] = { - { 0.0262f, 0.00001f }, - { 0.0349f, 0.00191f }, - { 0.000259f, 0.618f } - }; /* initialize values */ static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -213,14 +200,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { parameters_update(&pos_inav_param_handles, &pos_inav_params); /* END FIRST PARAMETER UPDATE */ - // TODO implement calibration procedure, now put dummy values - int16_t accel_offs[3] = { 0, 0, 0 }; - float accel_T[3][3] = { - { 1.0f, 0.0f, 0.0f }, - { 0.0f, 1.0f, 0.0f }, - { 0.0f, 0.0f, 1.0f } - }; - /* wait until gps signal turns valid, only then can we initialize the projection */ if (use_gps) { struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, @@ -270,11 +249,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint16_t baro_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; - hrt_abstime pub_last = 0; + hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 5000; // limit publish rate to 200 Hz /* main loop */ - struct pollfd fds[3] = { + struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, @@ -288,7 +267,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ - printf("[position_estimator_inav] subscriptions poll error\n", ret); + if (verbose_mode) + printf("[position_estimator_inav] subscriptions poll error\n"); } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { @@ -359,7 +339,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { last_time = t; /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, accel_T, accel_offs); + acceleration_correct(accel_corr, sensor.accelerometer_raw, pos_inav_params.acc_T, pos_inav_params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -384,34 +364,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, k, use_z); + kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, use_z); } - if (t - updates_counter_start > updates_counter_len) { - float updates_dt = (t - updates_counter_start) * 0.000001f; - printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); - updates_counter_start = t; + if (verbose_mode) { + /* print updates rate */ + if (t - updates_counter_start > updates_counter_len) { + float updates_dt = (t - updates_counter_start) * 0.000001f; + printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); + updates_counter_start = t; + accelerometer_updates = 0; + baro_updates = 0; + } } - if (printatt_counter == 100) { - printatt_counter = 0; - printf("[position_estimator_inav] dt = %.6f\n", dt); - printf("[position_estimator_inav] pitch = %.3f, roll = %.3f, yaw = %.3f\n", - att.pitch, att.roll, att.yaw); - printf("[position_estimator_inav] accel_UAV = %.3f, %.3f, %.3f\n", - sensor.accelerometer_m_s2[0], - sensor.accelerometer_m_s2[1], - sensor.accelerometer_m_s2[2]); - printf("[position_estimator_inav] accel_NED = %.3f, %.3f, %.3f\n", - accel_NED[0], accel_NED[1], accel_NED[2]); - printf("[position_estimator_inav] z = %.2f, vz = %.2f, az = %.2f\n", - z_est[0], z_est[1], z_est[2]); - } - printatt_counter++; if (t - pub_last > pub_interval) { pub_last = t; - local_pos_est.x = 0.0; - local_pos_est.vx = 0.0; - local_pos_est.y = 0.0; - local_pos_est.vy = 0.0; + local_pos_est.x = 0.0f; + local_pos_est.vx = 0.0f; + local_pos_est.y = 0.0f; + local_pos_est.vy = 0.0f; local_pos_est.z = z_est[0]; local_pos_est.vz = z_est[1]; local_pos_est.timestamp = hrt_absolute_time(); diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index 5567fd2cf2..fb082fbcb2 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli -* Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,16 +42,73 @@ /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ -PARAM_DEFINE_FLOAT(POS_EST_R, 1.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); + +PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); +PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); +PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); + +PARAM_DEFINE_FLOAT(INAV_ACC_T_00, 0.0021f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_02, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_11, 0.0021f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_12, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->r = param_find("POS_EST_R"); + h->k_alt_00 = param_find("INAV_K_ALT_00"); + h->k_alt_01 = param_find("INAV_K_ALT_01"); + h->k_alt_10 = param_find("INAV_K_ALT_10"); + h->k_alt_11 = param_find("INAV_K_ALT_11"); + h->k_alt_20 = param_find("INAV_K_ALT_20"); + h->k_alt_21 = param_find("INAV_K_ALT_21"); + + h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); + h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); + h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); + + h->acc_t_00 = param_find("INAV_ACC_T_00"); + h->acc_t_01 = param_find("INAV_ACC_T_01"); + h->acc_t_02 = param_find("INAV_ACC_T_02"); + h->acc_t_10 = param_find("INAV_ACC_T_10"); + h->acc_t_11 = param_find("INAV_ACC_T_11"); + h->acc_t_12 = param_find("INAV_ACC_T_12"); + h->acc_t_20 = param_find("INAV_ACC_T_20"); + h->acc_t_21 = param_find("INAV_ACC_T_21"); + h->acc_t_22 = param_find("INAV_ACC_T_22"); return OK; } int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->r, &(p->r)); + param_get(h->k_alt_00, &(p->k[0][0])); + param_get(h->k_alt_01, &(p->k[0][1])); + param_get(h->k_alt_10, &(p->k[1][0])); + param_get(h->k_alt_11, &(p->k[1][1])); + param_get(h->k_alt_20, &(p->k[2][0])); + param_get(h->k_alt_21, &(p->k[2][1])); + + param_get(h->acc_offs_0, &(p->acc_offs[0])); + param_get(h->acc_offs_1, &(p->acc_offs[1])); + param_get(h->acc_offs_2, &(p->acc_offs[2])); + + param_get(h->acc_t_00, &(p->acc_T[0][0])); + param_get(h->acc_t_01, &(p->acc_T[0][1])); + param_get(h->acc_t_02, &(p->acc_T[0][2])); + param_get(h->acc_t_10, &(p->acc_T[1][0])); + param_get(h->acc_t_11, &(p->acc_T[1][1])); + param_get(h->acc_t_12, &(p->acc_T[1][2])); + param_get(h->acc_t_20, &(p->acc_T[2][0])); + param_get(h->acc_t_21, &(p->acc_T[2][1])); + param_get(h->acc_t_22, &(p->acc_T[2][2])); return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index c45ca81359..694bf015bd 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -1,9 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Damian Aregger - * Tobias Naegeli - * Lorenz Meier + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -43,11 +41,32 @@ #include struct position_estimator_inav_params { - float r; + float k[3][2]; + int16_t acc_offs[3]; + float acc_T[3][3]; }; struct position_estimator_inav_param_handles { - param_t r; + param_t k_alt_00; + param_t k_alt_01; + param_t k_alt_10; + param_t k_alt_11; + param_t k_alt_20; + param_t k_alt_21; + + param_t acc_offs_0; + param_t acc_offs_1; + param_t acc_offs_2; + + param_t acc_t_00; + param_t acc_t_01; + param_t acc_t_02; + param_t acc_t_10; + param_t acc_t_11; + param_t acc_t_12; + param_t acc_t_20; + param_t acc_t_21; + param_t acc_t_22; }; /** diff --git a/apps/position_estimator_inav/sounds.c b/apps/position_estimator_inav/sounds.c deleted file mode 100644 index 4f7b05fea5..0000000000 --- a/apps/position_estimator_inav/sounds.c +++ /dev/null @@ -1,40 +0,0 @@ -/* - * sounds.c - * - * Created on: Feb 26, 2013 - * Author: samuezih - */ - -#include -#include -#include - - -static int buzzer; - -int sounds_init(void) -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -void sounds_deinit(void) -{ - close(buzzer); -} - -void tune_tetris(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 6); -} - -void tune_sonar(void) -{ - ioctl(buzzer, TONE_SET_ALARM, 7); -} diff --git a/apps/position_estimator_inav/sounds.h b/apps/position_estimator_inav/sounds.h deleted file mode 100644 index 356e42169c..0000000000 --- a/apps/position_estimator_inav/sounds.h +++ /dev/null @@ -1,16 +0,0 @@ -/* - * sounds.h - * - * Created on: Feb 26, 2013 - * Author: samuezih - */ - -#ifndef SOUNDS_H_ -#define SOUNDS_H_ - -int sounds_init(void); -void sounds_deinit(void); -void tune_tetris(void); -void tune_sonar(void); - -#endif /* SOUNDS_H_ */ From 1be74a4716444e08c47ad3eda1f56db6f2893615 Mon Sep 17 00:00:00 2001 From: Anton Date: Sun, 31 Mar 2013 12:24:37 +0400 Subject: [PATCH 06/70] Accelerometers offsets calibration implemented --- .../acceleration_transform.c | 78 +++++---- .../acceleration_transform.h | 3 + .../position_estimator_inav_main.c | 158 +++++++++++++++--- 3 files changed, 179 insertions(+), 60 deletions(-) diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index bd933cab4b..812920878c 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -17,23 +17,23 @@ * * * * * Calibration * * * * - * Tests + * Reference vectors + * accel_corr_ref[6][3] = [ g 0 0 ] // positive x + * | -g 0 0 | // negative x + * | 0 g 0 | // positive y + * | 0 -g 0 | // negative y + * | 0 0 g | // positive z + * [ 0 0 -g ] // negative z + * accel_raw_ref[6][3] * - * accel_corr_x_p = [ g 0 0 ]^T // positive x - * accel_corr_x_n = [ -g 0 0 ]^T // negative x - * accel_corr_y_p = [ 0 g 0 ]^T // positive y - * accel_corr_y_n = [ 0 -g 0 ]^T // negative y - * accel_corr_z_p = [ 0 0 g ]^T // positive z - * accel_corr_z_n = [ 0 0 -g ]^T // negative z + * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 * - * 6 tests * 3 axes = 18 equations + * 6 reference vectors * 3 axes = 18 equations * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants * * Find accel_offs * - * accel_offs = (accel_raw_x_p + accel_raw_x_n + - * accel_raw_y_p + accel_raw_y_n + - * accel_raw_z_p + accel_raw_z_n) / 6 + * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 * * * Find accel_T @@ -41,9 +41,8 @@ * 9 unknown constants * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations * - * accel_corr[i] = accel_T[i][0] * (accel_raw[0] - accel_offs[0]) + - * accel_T[i][1] * (accel_raw[1] - accel_offs[1]) + - * accel_T[i][2] * (accel_raw[2] - accel_offs[2]) + * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 + * * A x = b * * x = [ accel_T[0][0] ] @@ -56,34 +55,35 @@ * | accel_T[2][1] | * [ accel_T[2][2] ] * - * b = [ accel_corr_x_p[0] ] // One measurement per axis is enough - * | accel_corr_x_p[1] | - * | accel_corr_x_p[2] | - * | accel_corr_y_p[0] | - * | accel_corr_y_p[1] | - * | accel_corr_y_p[2] | - * | accel_corr_z_p[0] | - * | accel_corr_z_p[1] | - * [ accel_corr_z_p[2] ] + * b = [ accel_corr_ref[0][0] ] // One measurement per axis is enough + * | accel_corr_ref[0][1] | + * | accel_corr_ref[0][2] | + * | accel_corr_ref[2][0] | + * | accel_corr_ref[2][1] | + * | accel_corr_ref[2][2] | + * | accel_corr_ref[4][0] | + * | accel_corr_ref[4][1] | + * [ accel_corr_ref[4][2] ] * - * a_x[i] = accel_raw_x_p[i] - accel_offs[i] // Measurement for X axis - * ... - * A = [ a_x[0] a_x[1] a_x[2] 0 0 0 0 0 0 ] - * | 0 0 0 a_x[0] a_x[1] a_x[2] 0 0 0 | - * | 0 0 0 0 0 0 a_x[0] a_x[1] a_x[2] | - * | a_y[0] a_y[1] a_y[2] 0 0 0 0 0 0 | - * | 0 0 0 a_y[0] a_y[1] a_y[2] 0 0 0 | - * | 0 0 0 0 0 0 a_y[0] a_y[1] a_y[2] | - * | a_z[0] a_z[1] a_z[2] 0 0 0 0 0 0 | - * | 0 0 0 a_z[0] a_z[1] a_z[2] 0 0 0 | - * [ 0 0 0 0 0 0 a_z[0] a_z[1] a_z[2] ] + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0...5, j = 0...2 + * + * A = [ a[0][0] a[0][1] a[0][2] 0 0 0 0 0 0 ] + * | 0 0 0 a[0][0] a[0][1] a[0][2] 0 0 0 | + * | 0 0 0 0 0 0 a[0][0] a[0][1] a[0][2] | + * | a[1][0] a[1][1] a[1][2] 0 0 0 0 0 0 | + * | 0 0 0 a[1][0] a[1][1] a[1][2] 0 0 0 | + * | 0 0 0 0 0 0 a[1][0] a[1][1] a[1][2] | + * | a[2][0] a[2][1] a[2][2] 0 0 0 0 0 0 | + * | 0 0 0 a[2][0] a[2][1] a[2][2] 0 0 0 | + * [ 0 0 0 0 0 0 a[2][0] a[2][1] a[2][2] ] * * x = A^-1 b */ #include "acceleration_transform.h" -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]) { +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], + float accel_T[3][3], int16_t accel_offs[3]) { for (int i = 0; i < 3; i++) { accel_corr[i] = 0.0f; for (int j = 0; j < 3; j++) { @@ -91,3 +91,11 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel } } } + +void calculate_calibration_values(int16_t accel_raw_ref[6][3], + float accel_T[3][3], int16_t accel_offs[3], float g) { + for (int i = 0; i < 3; i++) { + accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) + + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); + } +} diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h index 26bf0d68fc..e21aebe73c 100644 --- a/apps/position_estimator_inav/acceleration_transform.h +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -12,4 +12,7 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]); +void calculate_calibration_values(int16_t accel_raw_ref[6][3], + float accel_T[3][3], int16_t accel_offs[3], float g); + #endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index b8ef1f76a4..552046568b 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -72,6 +72,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +const static float const_earth_gravity = 9.81f; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -84,19 +85,19 @@ static void usage(const char *reason); static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); - exit(1); -} + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status|calibrate} [-v]\n\n"); + exit(1); + } -/** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ + /** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ int position_estimator_inav_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -132,10 +133,112 @@ int position_estimator_inav_main(int argc, char *argv[]) { exit(0); } + if (!strcmp(argv[1], "calibrate")) { + do_accelerometer_calibration(); + exit(0); + } + usage("unrecognized command"); exit(1); } +void wait_for_input() { + printf( + "press any key to continue, 'Q' to abort\n"); + while (true ) { + int c = getchar(); + if (c == 'q' || c == 'Q') { + exit(0); + } else { + return; + } + } +} + +void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], + int samples) { + printf("[position_estimator_inav] measuring...\n"); + struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; + int count = 0; + int32_t accel_sum[3] = { 0, 0, 0 }; + while (count < samples) { + int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { + struct sensor_combined_s sensor; + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + accel_sum[0] += sensor.accelerometer_raw[0]; + accel_sum[1] += sensor.accelerometer_raw[1]; + accel_sum[2] += sensor.accelerometer_raw[2]; + count++; + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + printf("[position_estimator_inav] no accelerometer data for 1s\n"); + exit(1); + } else { + printf("[position_estimator_inav] poll error\n"); + exit(1); + } + } + for (int i = 0; i < 3; i++) + accel_avg[i] = (accel_sum[i] + count / 2) / count; + printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], accel_avg[1], accel_avg[2]); +} + +void do_accelerometer_calibration() { + printf("[position_estimator_inav] calibration started\n"); + const int calibration_samples = 1000; + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + int16_t accel_raw_ref[6][3]; // Reference measurements + printf("[position_estimator_inav] place vehicle level, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle on it's back, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle on right side, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle on left side, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle nose down, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), + calibration_samples); + printf("[position_estimator_inav] place vehicle nose up, "); + wait_for_input(); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), + calibration_samples); + close(sensor_combined_sub); + printf("[position_estimator_inav] reference data collection done\n"); + + int16_t accel_offs[3]; + float accel_T[3][3]; + calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, + const_earth_gravity); + printf("[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", + accel_offs[0], accel_offs[1], accel_offs[2]); + int32_t accel_offs_int32[3] = { accel_offs[0], accel_offs[1], accel_offs[2] }; + + if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0])) + || param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1])) + || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2]))) { + printf("[position_estimator_inav] setting parameters failed\n"); + exit(1); + } + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + if (save_ret != 0) { + printf("[position_estimator_inav] auto-save of parameters to storage failed\n"); + exit(1); + } + printf("[position_estimator_inav] calibration done\n"); +} + /**************************************************************************** * main ****************************************************************************/ @@ -157,7 +260,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ - const static float const_earth_gravity = 9.81f; static double lat_current = 0.0d; //[°]] --> 47.0 static double lon_current = 0.0d; //[°]] -->8.5 @@ -237,7 +339,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - printf("[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", + printf( + "[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); hrt_abstime last_time = 0; @@ -253,13 +356,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint32_t pub_interval = 5000; // limit publish rate to 200 Hz /* main loop */ - struct pollfd fds[5] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = vehicle_status_sub, .events = POLLIN }, - { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } - }; + struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { + .fd = vehicle_status_sub, .events = POLLIN }, { .fd = + vehicle_attitude_sub, .events = POLLIN }, { .fd = + sensor_combined_sub, .events = POLLIN }, { .fd = + vehicle_gps_position_sub, .events = POLLIN } }; printf("[position_estimator_inav] main loop started\n"); while (!thread_should_exit) { bool accelerometer_updated = false; @@ -311,7 +412,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { baro_alt0 /= (float) (baro_loop_cnt); local_flag_baroINITdone = true; char str[80]; - sprintf(str, "[position_estimator_inav] barometer initialized with alt0 = %.2f", baro_alt0); + sprintf(str, + "[position_estimator_inav] baro_alt0 = %.2f", + baro_alt0); printf("%s\n", str); mavlink_log_info(mavlink_fd, str); } @@ -339,7 +442,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { last_time = t; /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, pos_inav_params.acc_T, pos_inav_params.acc_offs); + acceleration_correct(accel_corr, sensor.accelerometer_raw, + pos_inav_params.acc_T, pos_inav_params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -364,13 +468,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, use_z); + kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, + use_z); } if (verbose_mode) { /* print updates rate */ if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; - printf("[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", accelerometer_updates / updates_dt, baro_updates / updates_dt); + printf( + "[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", + accelerometer_updates / updates_dt, + baro_updates / updates_dt); updates_counter_start = t; accelerometer_updates = 0; baro_updates = 0; From d536e3d5f9b0fc3187cdcd8fc20bb8bb600b9e98 Mon Sep 17 00:00:00 2001 From: Anton Date: Sun, 31 Mar 2013 20:42:15 +0400 Subject: [PATCH 07/70] Complete calibration implemented --- .../acceleration_transform.c | 87 ++++++++++++------ .../acceleration_transform.h | 5 +- .../position_estimator_inav_main.c | 89 ++++++++++++------- 3 files changed, 119 insertions(+), 62 deletions(-) diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index 812920878c..3aba9b403d 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -43,41 +43,31 @@ * * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 * + * Solve separate system for each row of accel_T: + * + * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 + * * A x = b * - * x = [ accel_T[0][0] ] - * | accel_T[0][1] | - * | accel_T[0][2] | - * | accel_T[1][0] | - * | accel_T[1][1] | - * | accel_T[1][2] | - * | accel_T[2][0] | - * | accel_T[2][1] | - * [ accel_T[2][2] ] + * x = [ accel_T[0][i] ] + * | accel_T[1][i] | + * [ accel_T[2][i] ] * - * b = [ accel_corr_ref[0][0] ] // One measurement per axis is enough - * | accel_corr_ref[0][1] | - * | accel_corr_ref[0][2] | - * | accel_corr_ref[2][0] | - * | accel_corr_ref[2][1] | - * | accel_corr_ref[2][2] | - * | accel_corr_ref[4][0] | - * | accel_corr_ref[4][1] | - * [ accel_corr_ref[4][2] ] + * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * | accel_corr_ref[2][i] | + * [ accel_corr_ref[4][i] ] * - * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0...5, j = 0...2 + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 * - * A = [ a[0][0] a[0][1] a[0][2] 0 0 0 0 0 0 ] - * | 0 0 0 a[0][0] a[0][1] a[0][2] 0 0 0 | - * | 0 0 0 0 0 0 a[0][0] a[0][1] a[0][2] | - * | a[1][0] a[1][1] a[1][2] 0 0 0 0 0 0 | - * | 0 0 0 a[1][0] a[1][1] a[1][2] 0 0 0 | - * | 0 0 0 0 0 0 a[1][0] a[1][1] a[1][2] | - * | a[2][0] a[2][1] a[2][2] 0 0 0 0 0 0 | - * | 0 0 0 a[2][0] a[2][1] a[2][2] 0 0 0 | - * [ 0 0 0 0 0 0 a[2][0] a[2][1] a[2][2] ] + * Matrix A is common for all three systems: + * A = [ a[0][0] a[0][1] a[0][2] ] + * | a[2][0] a[2][1] a[2][2] | + * [ a[4][0] a[4][1] a[4][2] ] * * x = A^-1 b + * + * accel_T = A^-1 * g + * g = 9.81 */ #include "acceleration_transform.h" @@ -92,10 +82,49 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], } } -void calculate_calibration_values(int16_t accel_raw_ref[6][3], +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) + return -1; // Singular matrix + dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; + dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; + dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; + dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; + dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; + dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; + dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; + dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; + dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; + return 0; +} + +int calculate_calibration_values(int16_t accel_raw_ref[6][3], float accel_T[3][3], int16_t accel_offs[3], float g) { + /* calculate raw offsets */ for (int i = 0; i < 3; i++) { accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); } + /* fill matrix A for linear equations system*/ + float mat_A[3][3]; + memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + float a = (accel_raw_ref[i * 2][j] - accel_offs[j]); + mat_A[i][j] = a; + } + } + /* calculate inverse matrix for A */ + float mat_A_inv[3][3]; + mat_invert3(mat_A, mat_A_inv); + for (int i = 0; i < 3; i++) { + /* copy results to accel_T */ + for (int j = 0; j < 3; j++) { + /* simplify matrices mult because b has only one non-zero element == g at index i */ + accel_T[j][i] = mat_A_inv[j][i] * g; + } + } + return 0; } diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h index e21aebe73c..4d1299db5d 100644 --- a/apps/position_estimator_inav/acceleration_transform.h +++ b/apps/position_estimator_inav/acceleration_transform.h @@ -10,9 +10,10 @@ #include -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel_T[3][3], int16_t accel_offs[3]); +void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], + float accel_T[3][3], int16_t accel_offs[3]); -void calculate_calibration_values(int16_t accel_raw_ref[6][3], +int calculate_calibration_values(int16_t accel_raw_ref[6][3], float accel_T[3][3], int16_t accel_offs[3], float g); #endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 552046568b..292cf7f215 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -76,12 +76,15 @@ const static float const_earth_gravity = 9.81f; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); +void do_accelerometer_calibration(); + int position_estimator_inav_thread_main(int argc, char *argv[]); + +static void usage(const char *reason); + /** * Print the correct usage. */ -static void usage(const char *reason); - static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -143,8 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[]) { } void wait_for_input() { - printf( - "press any key to continue, 'Q' to abort\n"); + printf("press any key to continue, 'Q' to abort\n"); while (true ) { int c = getchar(); if (c == 'q' || c == 'Q') { @@ -179,9 +181,11 @@ void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], exit(1); } } - for (int i = 0; i < 3; i++) + for (int i = 0; i < 3; i++) { accel_avg[i] = (accel_sum[i] + count / 2) / count; - printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], accel_avg[1], accel_avg[2]); + } + printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], + accel_avg[1], accel_avg[2]); } void do_accelerometer_calibration() { @@ -191,49 +195,72 @@ void do_accelerometer_calibration() { int16_t accel_raw_ref[6][3]; // Reference measurements printf("[position_estimator_inav] place vehicle level, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), calibration_samples); printf("[position_estimator_inav] place vehicle on it's back, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), calibration_samples); printf("[position_estimator_inav] place vehicle on right side, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), calibration_samples); printf("[position_estimator_inav] place vehicle on left side, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), calibration_samples); printf("[position_estimator_inav] place vehicle nose down, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), calibration_samples); printf("[position_estimator_inav] place vehicle nose up, "); wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), calibration_samples); close(sensor_combined_sub); printf("[position_estimator_inav] reference data collection done\n"); int16_t accel_offs[3]; float accel_T[3][3]; - calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, + int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, const_earth_gravity); - printf("[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", + if (res != 0) { + printf( + "[position_estimator_inav] calibration parameters calculation error\n"); + exit(1); + + } + printf( + "[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", accel_offs[0], accel_offs[1], accel_offs[2]); - int32_t accel_offs_int32[3] = { accel_offs[0], accel_offs[1], accel_offs[2] }; + printf( + "[position_estimator_inav] accelerometers transform matrix:\n [ %0.6f %0.6f %0.6f ]\n | %0.6f %0.6f %0.6f |\n [ %0.6f %0.6f %0.6f ]\n", + accel_T[0][0], accel_T[0][1], accel_T[0][2], accel_T[1][0], + accel_T[1][1], accel_T[1][2], accel_T[2][0], accel_T[2][1], + accel_T[2][2]); + int32_t accel_offs_int32[3] = + { accel_offs[0], accel_offs[1], accel_offs[2] }; if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0])) || param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1])) - || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2]))) { + || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2])) + || param_set(param_find("INAV_ACC_T_00"), &(accel_T[0][0])) + || param_set(param_find("INAV_ACC_T_01"), &(accel_T[0][1])) + || param_set(param_find("INAV_ACC_T_02"), &(accel_T[0][2])) + || param_set(param_find("INAV_ACC_T_10"), &(accel_T[1][0])) + || param_set(param_find("INAV_ACC_T_11"), &(accel_T[1][1])) + || param_set(param_find("INAV_ACC_T_12"), &(accel_T[1][2])) + || param_set(param_find("INAV_ACC_T_20"), &(accel_T[2][0])) + || param_set(param_find("INAV_ACC_T_21"), &(accel_T[2][1])) + || param_set(param_find("INAV_ACC_T_22"), &(accel_T[2][2]))) { printf("[position_estimator_inav] setting parameters failed\n"); exit(1); } /* auto-save to EEPROM */ int save_ret = param_save_default(); if (save_ret != 0) { - printf("[position_estimator_inav] auto-save of parameters to storage failed\n"); + printf( + "[position_estimator_inav] auto-save of parameters to storage failed\n"); exit(1); } printf("[position_estimator_inav] calibration done\n"); @@ -253,8 +280,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - const static float dT_const_120 = 1.0f / 120.0f; - const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f; bool use_gps = false; int baro_loop_cnt = 0; @@ -308,12 +333,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .events = POLLIN } }; while (gps.fix_type < 3) { - - /* wait for GPS updates, BUT READ VEHICLE STATUS (!) - * this choice is critical, since the vehicle status might not - * actually change, if this app is started after GPS lock was - * aquired. - */ if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ @@ -345,15 +364,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { hrt_abstime last_time = 0; thread_running = true; - static int printatt_counter = 0; uint32_t accelerometer_counter = 0; uint32_t baro_counter = 0; uint16_t accelerometer_updates = 0; uint16_t baro_updates = 0; + uint16_t gps_updates = 0; + uint16_t attitude_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; hrt_abstime pub_last = hrt_absolute_time(); - uint32_t pub_interval = 5000; // limit publish rate to 200 Hz + uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* main loop */ struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { @@ -368,8 +388,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ - if (verbose_mode) - printf("[position_estimator_inav] subscriptions poll error\n"); + printf("[position_estimator_inav] subscriptions poll error\n"); + thread_should_exit = true; + continue; } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { @@ -388,6 +409,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* vehicle attitude */ if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); + attitude_updates++; } /* sensor combined */ if (fds[3].revents & POLLIN) { @@ -432,6 +454,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + gps_updates++; } } @@ -476,12 +499,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; printf( - "[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n", + "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", accelerometer_updates / updates_dt, - baro_updates / updates_dt); + baro_updates / updates_dt, + gps_updates / updates_dt, + attitude_updates / updates_dt); updates_counter_start = t; accelerometer_updates = 0; baro_updates = 0; + gps_updates = 0; + attitude_updates = 0; } } if (t - pub_last > pub_interval) { From 4a062086aefd4fcd25f40d0cfa65ab3a1c3c3984 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 8 Apr 2013 17:04:04 +0400 Subject: [PATCH 08/70] position_estimator_inav: bugfixes, some refactoring --- .../position_estimator_inav_main.c | 80 ++++++++++--------- .../position_estimator_inav_params.c | 23 ++++-- .../position_estimator_inav_params.h | 3 + 3 files changed, 63 insertions(+), 43 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 292cf7f215..97d669612b 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -281,7 +281,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - bool use_gps = false; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determin while start up */ @@ -300,8 +299,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_local_position_s local_pos_est; - memset(&local_pos_est, 0, sizeof(local_pos_est)); + struct vehicle_local_position_s pos; + memset(&pos, 0, sizeof(pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -311,10 +310,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); /* advertise */ - orb_advert_t local_pos_est_pub = orb_advertise( - ORB_ID(vehicle_local_position), &local_pos_est); + orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos); - struct position_estimator_inav_params pos_inav_params; + 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); @@ -324,21 +322,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { struct parameter_update_s param_update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_update); /* read from param to clear updated flag */ /* FIRST PARAMETER UPDATE */ - parameters_update(&pos_inav_param_handles, &pos_inav_params); + parameters_update(&pos_inav_param_handles, ¶ms); /* END FIRST PARAMETER UPDATE */ /* wait until gps signal turns valid, only then can we initialize the projection */ - if (use_gps) { - struct pollfd fds_init[1] = { { .fd = vehicle_gps_position_sub, - .events = POLLIN } }; + if (params.use_gps) { + struct pollfd fds_init[1] = { + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; while (gps.fix_type < 3) { if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ usleep(5000); - orb_copy(ORB_ID(vehicle_gps_position), - vehicle_gps_position_sub, &gps); + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); } } static int printcounter = 0; @@ -354,6 +352,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { lat_current = ((double) (gps.lat)) * 1e-7; lon_current = ((double) (gps.lon)) * 1e-7; alt_current = gps.alt * 1e-3; + + pos.home_lat = lat_current * 1e7; + pos.home_lon = lon_current * 1e7; + pos.home_timestamp = hrt_absolute_time(); + /* initialize coordinates */ map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ @@ -376,16 +379,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* main loop */ - struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, { - .fd = vehicle_status_sub, .events = POLLIN }, { .fd = - vehicle_attitude_sub, .events = POLLIN }, { .fd = - sensor_combined_sub, .events = POLLIN }, { .fd = - vehicle_gps_position_sub, .events = POLLIN } }; + struct pollfd fds[5] = { + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; printf("[position_estimator_inav] main loop started\n"); while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; - int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ printf("[position_estimator_inav] subscriptions poll error\n"); @@ -399,7 +404,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update); /* update parameters */ - parameters_update(&pos_inav_param_handles, &pos_inav_params); + parameters_update(&pos_inav_param_handles, ¶ms); } /* vehicle status */ if (fds[1].revents & POLLIN) { @@ -442,7 +447,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } } } - if (use_gps) { + if (params.use_gps) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ @@ -454,8 +459,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + pos.valid = gps.fix_type >= 3; gps_updates++; } + } else { + pos.valid = true; } } /* end of poll return value check */ @@ -466,7 +474,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* calculate corrected acceleration vector in UAV frame */ float accel_corr[3]; acceleration_correct(accel_corr, sensor.accelerometer_raw, - pos_inav_params.acc_T, pos_inav_params.acc_offs); + params.acc_T, params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { @@ -491,7 +499,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, pos_inav_params.k, + kalman_filter_inertial_update(z_est, z_meas, params.k, use_z); } if (verbose_mode) { @@ -513,21 +521,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - local_pos_est.x = 0.0f; - local_pos_est.vx = 0.0f; - local_pos_est.y = 0.0f; - local_pos_est.vy = 0.0f; - local_pos_est.z = z_est[0]; - local_pos_est.vz = z_est[1]; - local_pos_est.timestamp = hrt_absolute_time(); - if ((isfinite(local_pos_est.x)) && (isfinite(local_pos_est.vx)) - && (isfinite(local_pos_est.y)) - && (isfinite(local_pos_est.vy)) - && (isfinite(local_pos_est.z)) - && (isfinite(local_pos_est.vz))) { + pos.x = 0.0f; + pos.vx = 0.0f; + pos.y = 0.0f; + pos.vy = 0.0f; + pos.z = z_est[0]; + pos.vz = z_est[1]; + pos.timestamp = hrt_absolute_time(); + if ((isfinite(pos.x)) && (isfinite(pos.vx)) + && (isfinite(pos.y)) + && (isfinite(pos.vy)) + && (isfinite(pos.z)) + && (isfinite(pos.vz))) { orb_publish(ORB_ID( - vehicle_local_position), local_pos_est_pub, - &local_pos_est); + vehicle_local_position), vehicle_local_position_pub, + &pos); } } } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index fb082fbcb2..d3a165947c 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -42,6 +42,8 @@ /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ +PARAM_DEFINE_INT32(INAV_USE_GPS, 0); + PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); @@ -63,8 +65,9 @@ PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); -int parameters_init(struct position_estimator_inav_param_handles *h) -{ +int parameters_init(struct position_estimator_inav_param_handles *h) { + h->use_gps = param_find("INAV_USE_GPS"); + h->k_alt_00 = param_find("INAV_K_ALT_00"); h->k_alt_01 = param_find("INAV_K_ALT_01"); h->k_alt_10 = param_find("INAV_K_ALT_10"); @@ -88,8 +91,9 @@ int parameters_init(struct position_estimator_inav_param_handles *h) return OK; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) -{ +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { + param_get(h->use_gps, &(p->use_gps)); + param_get(h->k_alt_00, &(p->k[0][0])); param_get(h->k_alt_01, &(p->k[0][1])); param_get(h->k_alt_10, &(p->k[1][0])); @@ -97,9 +101,14 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); - param_get(h->acc_offs_0, &(p->acc_offs[0])); - param_get(h->acc_offs_1, &(p->acc_offs[1])); - param_get(h->acc_offs_2, &(p->acc_offs[2])); + /* read int32 and cast to int16 */ + int32_t t; + param_get(h->acc_offs_0, &t); + p->acc_offs[0] = t; + param_get(h->acc_offs_1, &t); + p->acc_offs[1] = t; + param_get(h->acc_offs_2, &t); + p->acc_offs[2] = t; param_get(h->acc_t_00, &(p->acc_T[0][0])); param_get(h->acc_t_01, &(p->acc_T[0][1])); diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 694bf015bd..51e57a9146 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -41,12 +41,15 @@ #include struct position_estimator_inav_params { + int use_gps; float k[3][2]; int16_t acc_offs[3]; float acc_T[3][3]; }; struct position_estimator_inav_param_handles { + param_t use_gps; + param_t k_alt_00; param_t k_alt_01; param_t k_alt_10; From 825957cde0e441ce89be2c44fccacd90c4514b59 Mon Sep 17 00:00:00 2001 From: Anton Date: Mon, 8 Apr 2013 17:08:16 +0400 Subject: [PATCH 09/70] multirotor_pos_control: cleanup, altitude hold mode implemented --- .../multirotor_att_control_main.c | 190 +++++++------- .../multirotor_pos_control.c | 203 ++++++++++----- .../multirotor_pos_control_params.c | 29 ++- .../multirotor_pos_control_params.h | 18 +- .../multirotor_pos_control/position_control.c | 235 ------------------ .../multirotor_pos_control/position_control.h | 50 ---- 6 files changed, 262 insertions(+), 463 deletions(-) delete mode 100644 apps/multirotor_pos_control/position_control.c delete mode 100644 apps/multirotor_pos_control/position_control.h diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c9..09710f0fcb 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -242,124 +242,124 @@ mc_thread_main(int argc, char *argv[]) } else if (state.flag_control_manual_enabled) { - if (state.flag_control_attitude_enabled) { + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS) { + /* direct manual input */ + if (state.flag_control_attitude_enabled) { - /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { - att_sp.yaw_body = att.yaw; - } - - static bool rc_loss_first_time = true; - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; - - } else { - att_sp.thrust = 0.0f; - } - - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - - rc_loss_first_time = false; - - } else { - rc_loss_first_time = true; - - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { att_sp.yaw_body = att.yaw; } - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + static bool rc_loss_first_time = true; - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (state.rc_signal_lost) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + + /* + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + att_sp.thrust = failsafe_throttle; } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ + att_sp.thrust = 0.0f; + } - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; + + rc_loss_first_time = false; + + } else { + rc_loss_first_time = true; + + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + + /* set attitude if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { + att_sp.yaw_body = att.yaw; + } + + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { rates_sp.yaw = manual.yaw; control_yaw_position = false; - first_time_after_yaw_speed_control = true; } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ - control_yaw_position = true; + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; + } } } + + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); } - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); - } + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + } - /* STEP 2: publish the controller output */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); - /* STEP 2: publish the result to the vehicle actuators */ + /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - } - } else { - /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); + } } } - } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 7b8d83aa8a..d2b6685aca 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -35,7 +35,7 @@ /** * @file multirotor_pos_control.c * - * Skeleton for multirotor position controller + * Multirotor position controller */ #include @@ -52,13 +52,16 @@ #include #include #include +#include #include #include #include #include +#include #include #include #include +#include #include "multirotor_pos_control_params.h" @@ -79,9 +82,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); -static void -usage(const char *reason) -{ +static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); @@ -96,21 +97,20 @@ usage(const char *reason) * The actual stack size should be set in the call * to task_spawn(). */ -int multirotor_pos_control_main(int argc, char *argv[]) -{ +int multirotor_pos_control_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor pos control already running\n"); + printf("multirotor_pos_control already running\n"); /* this is not an error */ exit(0); } thread_should_exit = false; - deamon_task = task_spawn("multirotor pos control", + deamon_task = task_spawn("multirotor_pos_control", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 60, 4096, @@ -126,9 +126,9 @@ int multirotor_pos_control_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor pos control app is running\n"); + printf("\tmultirotor_pos_control app is running\n"); } else { - printf("\tmultirotor pos control app not started\n"); + printf("\tmultirotor_pos_control app not started\n"); } exit(0); } @@ -137,98 +137,165 @@ int multirotor_pos_control_main(int argc, char *argv[]) exit(1); } -static int -multirotor_pos_control_thread_main(int argc, char *argv[]) -{ +static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor pos control] Control started, taking over position control\n"); + printf("[multirotor_pos_control] started\n"); + static int mavlink_fd; + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); /* structures */ - struct vehicle_status_s state; + struct vehicle_status_s status; + memset(&status, 0, sizeof(status)); struct vehicle_attitude_s att; - //struct vehicle_global_position_setpoint_s global_pos_sp; - struct vehicle_local_position_setpoint_s local_pos_sp; - struct vehicle_vicon_position_s local_pos; - struct manual_control_setpoint_s manual; + memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; + memset(&att_sp, 0, sizeof(att_sp)); + struct manual_control_setpoint_s manual; + memset(&manual, 0, sizeof(manual)); + struct vehicle_local_position_s local_pos; + memset(&local_pos, 0, sizeof(local_pos)); + struct vehicle_local_position_setpoint_s local_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); /* subscribe to attitude, motor setpoints and system state */ - int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int param_sub = orb_subscribe(ORB_ID(parameter_update)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int local_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); - //int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - /* publish attitude setpoint */ + /* publish setpoint */ + orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + bool reset_sp_alt = true; + bool reset_sp_pos = true; + hrt_abstime t_prev = 0; + float alt_integral = 0.0f; + float alt_ctl_dz = 0.2f; + thread_running = true; - int loopcounter = 0; + struct multirotor_position_control_params params; + struct multirotor_position_control_param_handles params_h; + parameters_init(¶ms_h); + parameters_update(¶ms_h, ¶ms); - struct multirotor_position_control_params p; - struct multirotor_position_control_param_handles h; - parameters_init(&h); - parameters_update(&h, &p); + int paramcheck_counter = 0; - - while (1) { + while (!thread_should_exit) { /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + orb_copy(ORB_ID(vehicle_status), state_sub, &status); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_vicon_position), local_pos_sub, &local_pos); - /* get a local copy of local position setpoint */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - - if (loopcounter == 500) { - parameters_update(&h, &p); - loopcounter = 0; + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { + /* get a local copy of local position setpoint */ + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); } - // if (state.state_machine == SYSTEM_STATE_AUTO) { - - // XXX IMPLEMENT POSITION CONTROL HERE + /* check parameters at 1 Hz*/ + paramcheck_counter++; + if (paramcheck_counter == 50) { + bool param_updated; + orb_check(param_sub, ¶m_updated); + if (param_updated) { + parameters_update(¶ms_h, ¶ms); + } + paramcheck_counter = 0; + } - float dT = 1.0f / 50.0f; + if (status.state_machine == SYSTEM_STATE_MANUAL) { + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + hrt_abstime t = hrt_absolute_time(); + if (reset_sp_alt) { + reset_sp_alt = false; + local_pos_sp.z = local_pos.z; + alt_integral = manual.throttle; + char str[80]; + sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); + mavlink_log_info(mavlink_fd, str); + } - float x_setpoint = 0.0f; + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + /* move altitude setpoint by manual controls */ + float alt_ctl = manual.throttle - 0.5f; + if (fabs(alt_ctl) < alt_ctl_dz) { + alt_ctl = 0.0f; + } else { + if (alt_ctl > 0.0f) + alt_ctl = (alt_ctl - alt_ctl_dz) / (0.5f - alt_ctl_dz); + else + alt_ctl = (alt_ctl + alt_ctl_dz) / (0.5f - alt_ctl_dz); + local_pos_sp.z -= alt_ctl * params.alt_rate_max * dt; + if (local_pos_sp.z > local_pos.z + err_linear_limit) + local_pos_sp.z = local_pos.z + err_linear_limit; + else if (local_pos_sp.z < local_pos.z - err_linear_limit) + local_pos_sp.z = local_pos.z - err_linear_limit; + } - // XXX enable switching between Vicon and local position estimate - /* local pos is the Vicon position */ - - // XXX just an example, lacks rotation around world-body transformation - att_sp.pitch_body = (local_pos.x - x_setpoint) * p.p; - att_sp.roll_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.3f; - att_sp.timestamp = hrt_absolute_time(); - - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - // } else if (state.state_machine == SYSTEM_STATE_STABILIZED) { - /* set setpoint to current position */ - // XXX select pos reset channel on remote - /* reset setpoint to current position (position hold) */ - // if (1 == 2) { - // local_pos_sp.x = local_pos.x; - // local_pos_sp.y = local_pos.y; - // local_pos_sp.z = local_pos.z; - // local_pos_sp.yaw = att.yaw; - // } - // } + /* PID for altitude */ + float alt_err = local_pos.z - local_pos_sp.z; + /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ + if (alt_err > err_linear_limit) { + alt_err = err_linear_limit; + } else if (alt_err < -err_linear_limit) { + alt_err = -err_linear_limit; + } + /* PID for altitude rate */ + float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; + float thrust_ctl = thrust_ctl_pd + alt_integral; + if (thrust_ctl < params.thr_min) { + thrust_ctl = params.thr_min; + } else if (thrust_ctl > params.thr_max) { + thrust_ctl = params.thr_max; + } else { + /* integrate only in linear area (with 20% gap) and not on min/max throttle */ + if (fabs(thrust_ctl_pd) < err_linear_limit * params.alt_p * 0.8f) + alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + } + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + // TODO add position controller + att_sp.pitch_body = manual.pitch; + att_sp.roll_body = manual.roll; + att_sp.yaw_body = manual.yaw; + } else { + att_sp.pitch_body = manual.pitch; + att_sp.roll_body = manual.roll; + att_sp.yaw_body = manual.yaw; + } + att_sp.thrust = thrust_ctl; + att_sp.timestamp = hrt_absolute_time(); + /* publish local position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + t_prev = t; + } else { + reset_sp_alt = true; + } + } else { + reset_sp_alt = true; + } /* run at approximately 50 Hz */ usleep(20000); - loopcounter++; } - printf("[multirotor pos control] ending now...\n"); + printf("[multirotor_pos_control] exiting\n"); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] exiting"); thread_running = false; diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.c b/apps/multirotor_pos_control/multirotor_pos_control_params.c index 6b73dc4052..90dd820f43 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control_params.c +++ b/apps/multirotor_pos_control/multirotor_pos_control_params.c @@ -34,29 +34,40 @@ ****************************************************************************/ /* - * @file multirotor_position_control_params.c + * @file multirotor_pos_control_params.c * - * Parameters for EKF filter + * Parameters for multirotor_pos_control */ #include "multirotor_pos_control_params.h" -/* Extended Kalman Filter covariances */ - /* controller parameters */ -PARAM_DEFINE_FLOAT(MC_POS_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); +PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); +PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.01f); +PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); int parameters_init(struct multirotor_position_control_param_handles *h) { + h->thr_min = param_find("MPC_THR_MIN"); + h->thr_max = param_find("MPC_THR_MAX"); /* PID parameters */ - h->p = param_find("MC_POS_P"); - + h->alt_p = param_find("MPC_ALT_P"); + h->alt_i = param_find("MPC_ALT_I"); + h->alt_d = param_find("MPC_ALT_D"); + h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); return OK; } int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p) { - param_get(h->p, &(p->p)); - + param_get(h->thr_min, &(p->thr_min)); + param_get(h->thr_max, &(p->thr_max)); + param_get(h->alt_p, &(p->alt_p)); + param_get(h->alt_i, &(p->alt_i)); + param_get(h->alt_d, &(p->alt_d)); + param_get(h->alt_rate_max, &(p->alt_rate_max)); return OK; } diff --git a/apps/multirotor_pos_control/multirotor_pos_control_params.h b/apps/multirotor_pos_control/multirotor_pos_control_params.h index 274f6c22a9..849055cd16 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control_params.h +++ b/apps/multirotor_pos_control/multirotor_pos_control_params.h @@ -42,15 +42,21 @@ #include struct multirotor_position_control_params { - float p; - float i; - float d; + float thr_min; + float thr_max; + float alt_p; + float alt_i; + float alt_d; + float alt_rate_max; }; struct multirotor_position_control_param_handles { - param_t p; - param_t i; - param_t d; + param_t thr_min; + param_t thr_max; + param_t alt_p; + param_t alt_i; + param_t alt_d; + param_t alt_rate_max; }; /** diff --git a/apps/multirotor_pos_control/position_control.c b/apps/multirotor_pos_control/position_control.c deleted file mode 100644 index 9c53a5bf6c..0000000000 --- a/apps/multirotor_pos_control/position_control.c +++ /dev/null @@ -1,235 +0,0 @@ -// /**************************************************************************** -// * -// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. -// * Author: @author Lorenz Meier -// * @author Laurens Mackay -// * @author Tobias Naegeli -// * @author Martin Rutschmann -// * -// * 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 multirotor_position_control.c -// * Implementation of the position control for a multirotor VTOL -// */ - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include "multirotor_position_control.h" - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) -// { -// static PID_t distance_controller; - -// static int read_ret; -// static global_data_position_t position_estimated; - -// static uint16_t counter; - -// static bool initialized; -// static uint16_t pm_counter; - -// static float lat_next; -// static float lon_next; - -// static float pitch_current; - -// static float thrust_total; - - -// if (initialized == false) { - -// pid_init(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], -// PID_MODE_DERIVATIV_CALC, 150);//150 - -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// thrust_total = 0.0f; - -// /* Position initialization */ -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// lat_next = position_estimated.lat; -// lon_next = position_estimated.lon; - -// /* attitude initialization */ -// global_data_lock(&global_data_attitude->access_conf); -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); - -// initialized = true; -// } - -// /* load new parameters with 10Hz */ -// if (counter % 50 == 0) { -// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { -// /* check whether new parameters are available */ -// if (global_data_parameter_storage->counter > pm_counter) { -// pid_set_parameters(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); - -// // -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// pm_counter = global_data_parameter_storage->counter; -// printf("Position controller changed pid parameters\n"); -// } -// } - -// global_data_unlock(&global_data_parameter_storage->access_conf); -// } - - -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// /* Get next waypoint */ //TODO: add local copy - -// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { -// lat_next = global_data_position_setpoint->x; -// lon_next = global_data_position_setpoint->y; -// global_data_unlock(&global_data_position_setpoint->access_conf); -// } - -// /* Get distance to waypoint */ -// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); -// // if(counter % 5 == 0) -// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); - -// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ -// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); - -// if (counter % 5 == 0) -// printf("bearing: %.4f\n", bearing); - -// /* Calculate speed in direction of bearing (needed for controller) */ -// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); -// // if(counter % 5 == 0) -// // printf("speed_norm: %.4f\n", speed_norm); -// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller - -// /* Control Thrust in bearing direction */ -// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, -// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else - -// // if(counter % 5 == 0) -// // printf("horizontal thrust: %.4f\n", horizontal_thrust); - -// /* Get total thrust (from remote for now) */ -// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { -// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? -// global_data_unlock(&global_data_rc_channels->access_conf); -// } - -// const float max_gas = 500.0f; -// thrust_total *= max_gas / 20000.0f; //TODO: check this -// thrust_total += max_gas / 2.0f; - - -// if (horizontal_thrust > thrust_total) { -// horizontal_thrust = thrust_total; - -// } else if (horizontal_thrust < -thrust_total) { -// horizontal_thrust = -thrust_total; -// } - - - -// //TODO: maybe we want to add a speed controller later... - -// /* Calclulate thrust in east and north direction */ -// float thrust_north = cosf(bearing) * horizontal_thrust; -// float thrust_east = sinf(bearing) * horizontal_thrust; - -// if (counter % 10 == 0) { -// printf("thrust north: %.4f\n", thrust_north); -// printf("thrust east: %.4f\n", thrust_east); -// fflush(stdout); -// } - -// /* Get current attitude */ -// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); -// } - -// /* Get desired pitch & roll */ -// float pitch_desired = 0.0f; -// float roll_desired = 0.0f; - -// if (thrust_total != 0) { -// float pitch_fraction = -thrust_north / thrust_total; -// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); - -// if (roll_fraction < -1) { -// roll_fraction = -1; - -// } else if (roll_fraction > 1) { -// roll_fraction = 1; -// } - -// // if(counter % 5 == 0) -// // { -// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); -// // fflush(stdout); -// // } - -// pitch_desired = asinf(pitch_fraction); -// roll_desired = asinf(roll_fraction); -// } - -// att_sp.roll = roll_desired; -// att_sp.pitch = pitch_desired; - -// counter++; -// } diff --git a/apps/multirotor_pos_control/position_control.h b/apps/multirotor_pos_control/position_control.h deleted file mode 100644 index 2144ebc343..0000000000 --- a/apps/multirotor_pos_control/position_control.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * - * 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 multirotor_position_control.h - * Definition of the position control for a multirotor VTOL - */ - -// #ifndef POSITION_CONTROL_H_ -// #define POSITION_CONTROL_H_ - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); - -// #endif /* POSITION_CONTROL_H_ */ From 5abae2c78d0f7f41e1340fe5e396936da2c7a580 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 20 Apr 2013 09:14:26 +0400 Subject: [PATCH 10/70] Publish manual_sas_mode immediately, SAS modes switch order changed to more safe --- apps/commander/commander.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7c0a25f862..3c74c15efa 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1510,6 +1510,7 @@ int commander_thread_main(int argc, char *argv[]) uint64_t start_time = hrt_absolute_time(); uint64_t failsave_ll_start_time = 0; + enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; bool state_changed = true; bool param_init_forced = true; @@ -1931,8 +1932,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { - /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + /* bottom stick position, set default */ + /* this MUST be mapped to extremal position to switch easy in case of emergency */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { @@ -1940,8 +1942,14 @@ int commander_thread_main(int argc, char *argv[]) current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; } else { - /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + /* center stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + } + + if (current_status.manual_sas_mode != manual_sas_mode) { + /* publish SAS mode changes immediately */ + manual_sas_mode = current_status.manual_sas_mode; + state_changed = true; } /* From 57cca9dbb41a1803c3cdb61151947e99625c59cb Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 20 Apr 2013 12:33:02 +0400 Subject: [PATCH 11/70] Abs yaw and other bugs fixed --- .../multirotor_att_control_main.c | 207 ++++++++++-------- .../multirotor_pos_control.c | 154 +++++++------ 2 files changed, 199 insertions(+), 162 deletions(-) diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 09710f0fcb..2582998285 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -241,123 +241,140 @@ mc_thread_main(int argc, char *argv[]) } else if (state.flag_control_manual_enabled) { + /* direct manual input */ + if (state.flag_control_attitude_enabled) { + /* Control attitude, update attitude setpoint depending on SAS mode: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS: roll, pitch, yaw, thrust + * VEHICLE_MANUAL_SAS_MODE_ALTITUDE: roll, pitch, yaw + * VEHICLE_MANUAL_SAS_MODE_SIMPLE: yaw + * */ - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS) { - /* direct manual input */ - if (state.flag_control_attitude_enabled) { + /* initialize to current yaw if switching to manual or att control */ + if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || + state.flag_control_manual_enabled != flag_control_manual_enabled || + state.flag_system_armed != flag_system_armed) { + att_sp.yaw_body = att.yaw; + } - /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { + static bool rc_loss_first_time = true; + + /* if the RC signal is lost, try to stay level and go slowly back down to ground */ + if (state.rc_signal_lost) { + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + /* Don't reset attitude setpoint in SIMPLE SAS mode, it's handled by position controller. */ + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ALTITUDE) { + /* Don't touch throttle in modes with altitude hold, it's handled by position controller. + * + * Only go to failsafe throttle if last known throttle was + * high enough to create some lift to make hovering state likely. + * + * This is to prevent that someone landing, but not disarming his + * multicopter (throttle = 0) does not make it jump up in the air + * if shutting down his remote. + */ + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ + param_get(failsafe_throttle_handle, &failsafe_throttle); + att_sp.thrust = failsafe_throttle; + + } else { + att_sp.thrust = 0.0f; + } + } + } + + /* keep current yaw, do not attempt to go to north orientation, + * since if the pilot regains RC control, he will be lost regarding + * the current orientation. + */ + if (rc_loss_first_time) + att_sp.yaw_body = att.yaw; + + rc_loss_first_time = false; + + } else { + rc_loss_first_time = true; + + /* control yaw in all SAS modes */ + /* set yaw if arming */ + if (!flag_control_attitude_enabled && state.flag_system_armed) { att_sp.yaw_body = att.yaw; } - static bool rc_loss_first_time = true; + /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ + if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; + if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; } else { - att_sp.thrust = 0.0f; - } + /* + * This mode SHOULD be the default mode, which is: + * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS + * + * However, we fall back to this setting for all other (nonsense) + * settings as well. + */ - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; - - rc_loss_first_time = false; - - } else { - rc_loss_first_time = true; - - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { - att_sp.yaw_body = att.yaw; - } - - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && ( + state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS || + manual.throttle > 0.3f)) { rates_sp.yaw = manual.yaw; control_yaw_position = false; + first_time_after_yaw_speed_control = true; } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ - - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; } + + control_yaw_position = true; } } - - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); } - if (motor_test_mode) { - printf("testmode"); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.1f; - att_sp.timestamp = hrt_absolute_time(); + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_SIMPLE) { + /* don't update attitude setpoint in SIMPLE SAS mode */ + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; + if (state.manual_sas_mode != VEHICLE_MANUAL_SAS_MODE_ALTITUDE) { + /* don't set throttle in alt hold modes */ + att_sp.thrust = manual.throttle; + } } - /* STEP 2: publish the controller output */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + att_sp.timestamp = hrt_absolute_time(); + } - } else { - /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; + if (motor_test_mode) { + printf("testmode"); + att_sp.roll_body = 0.0f; + att_sp.pitch_body = 0.0f; + att_sp.yaw_body = 0.0f; + att_sp.thrust = 0.1f; + att_sp.timestamp = hrt_absolute_time(); + } - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } + /* STEP 2: publish the controller output */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + + } else { + /* manual rate inputs, from RC control or joystick */ + if (state.flag_control_rates_enabled && + state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { + rates_sp.roll = manual.roll; + + rates_sp.pitch = manual.pitch; + rates_sp.yaw = manual.yaw; + rates_sp.thrust = manual.throttle; + rates_sp.timestamp = hrt_absolute_time(); } } } diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index d2b6685aca..7757a78c11 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -162,6 +162,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int param_sub = orb_subscribe(ORB_ID(parameter_update)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -186,18 +187,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int paramcheck_counter = 0; while (!thread_should_exit) { - /* get a local copy of the vehicle state */ orb_copy(ORB_ID(vehicle_status), state_sub, &status); - /* get a local copy of manual setpoint */ - orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); - /* get a local copy of attitude */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); - /* get a local copy of local position */ - orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); - if (status.state_machine == SYSTEM_STATE_AUTO) { - /* get a local copy of local position setpoint */ - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - } /* check parameters at 1 Hz*/ paramcheck_counter++; @@ -210,26 +200,58 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { paramcheck_counter = 0; } - if (status.state_machine == SYSTEM_STATE_MANUAL) { - if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - hrt_abstime t = hrt_absolute_time(); - if (reset_sp_alt) { - reset_sp_alt = false; - local_pos_sp.z = local_pos.z; - alt_integral = manual.throttle; - char str[80]; - sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - mavlink_log_info(mavlink_fd, str); - } + /* Check if controller should act */ + bool act = status.flag_system_armed && ( + /* SAS modes */ + ( + status.flag_control_manual_enabled && + status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && ( + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE + ) + ) || + /* AUTO mode */ + status.state_machine == SYSTEM_STATE_AUTO + ); - float dt; - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - } else { - dt = 0.0f; - } - float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; - /* move altitude setpoint by manual controls */ + if (act) { + orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); + orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { + orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + } + + hrt_abstime t = hrt_absolute_time(); + if (reset_sp_alt) { + reset_sp_alt = false; + local_pos_sp.z = local_pos.z; + alt_integral = manual.throttle; + char str[80]; + sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); + mavlink_log_info(mavlink_fd, str); + } + + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { + reset_sp_pos = false; + local_pos_sp.x = local_pos.x; + local_pos_sp.y = local_pos.y; + char str[80]; + sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, str); + } + + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + + if (status.flag_control_manual_enabled) { + /* move altitude setpoint with manual controls */ float alt_ctl = manual.throttle - 0.5f; if (fabs(alt_ctl) < alt_ctl_dz) { alt_ctl = 0.0f; @@ -245,48 +267,46 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { local_pos_sp.z = local_pos.z - err_linear_limit; } - /* PID for altitude */ - float alt_err = local_pos.z - local_pos_sp.z; - /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - if (alt_err > err_linear_limit) { - alt_err = err_linear_limit; - } else if (alt_err < -err_linear_limit) { - alt_err = -err_linear_limit; - } - /* PID for altitude rate */ - float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; - float thrust_ctl = thrust_ctl_pd + alt_integral; - if (thrust_ctl < params.thr_min) { - thrust_ctl = params.thr_min; - } else if (thrust_ctl > params.thr_max) { - thrust_ctl = params.thr_max; - } else { - /* integrate only in linear area (with 20% gap) and not on min/max throttle */ - if (fabs(thrust_ctl_pd) < err_linear_limit * params.alt_p * 0.8f) - alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; - } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - // TODO add position controller - att_sp.pitch_body = manual.pitch; - att_sp.roll_body = manual.roll; - att_sp.yaw_body = manual.yaw; - } else { - att_sp.pitch_body = manual.pitch; - att_sp.roll_body = manual.roll; - att_sp.yaw_body = manual.yaw; + // TODO move position setpoint with manual controls } - att_sp.thrust = thrust_ctl; - att_sp.timestamp = hrt_absolute_time(); - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); - /* publish new attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - t_prev = t; - } else { - reset_sp_alt = true; } + + /* PID for altitude */ + float alt_err = local_pos.z - local_pos_sp.z; + /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ + if (alt_err > err_linear_limit) { + alt_err = err_linear_limit; + } else if (alt_err < -err_linear_limit) { + alt_err = -err_linear_limit; + } + /* P and D components */ + float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; + /* add I component */ + float thrust_ctl = thrust_ctl_pd + alt_integral; + alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + if (thrust_ctl < params.thr_min) { + thrust_ctl = params.thr_min; + } else if (thrust_ctl > params.thr_max) { + thrust_ctl = params.thr_max; + } + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + // TODO add position controller + } else { + reset_sp_pos = true; + } + att_sp.thrust = thrust_ctl; + att_sp.timestamp = hrt_absolute_time(); + if (status.flag_control_manual_enabled) { + /* publish local position setpoint in manual mode */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } + /* publish new attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + t_prev = t; } else { reset_sp_alt = true; + reset_sp_pos = true; } /* run at approximately 50 Hz */ From 10dfd2ba393089e5008978eb0469e8c1289d5917 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 20 Apr 2013 22:38:38 +0400 Subject: [PATCH 12/70] dt calculation bug fixed --- .../multirotor_pos_control.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index 7757a78c11..ac7645764c 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -214,6 +214,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { status.state_machine == SYSTEM_STATE_AUTO ); + hrt_abstime t = hrt_absolute_time(); + float dt; + if (t_prev != 0) { + dt = (t - t_prev) * 0.000001f; + } else { + dt = 0.0f; + } + t_prev = t; if (act) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -223,7 +231,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); } - hrt_abstime t = hrt_absolute_time(); if (reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; @@ -242,12 +249,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, str); } - float dt; - if (t_prev != 0) { - dt = (t - t_prev) * 0.000001f; - } else { - dt = 0.0f; - } float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; if (status.flag_control_manual_enabled) { @@ -303,7 +304,6 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); - t_prev = t; } else { reset_sp_alt = true; reset_sp_pos = true; From 0c6eaae8632f3a6520afdaf60adf13a8b6e55566 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 24 Apr 2013 18:42:34 +0400 Subject: [PATCH 13/70] Estimate accelerometers offset in position_estimator_inav --- .../position_estimator_inav_main.c | 74 +++++++++++-------- .../position_estimator_inav_params.c | 8 ++ .../position_estimator_inav_params.h | 3 + 3 files changed, 53 insertions(+), 32 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 97d669612b..6e8c0ab5fe 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -280,6 +280,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { static float x_est[3] = { 0.0f, 0.0f, 0.0f }; static float y_est[3] = { 0.0f, 0.0f, 0.0f }; static float z_est[3] = { 0.0f, 0.0f, 0.0f }; + float accel_offs_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ @@ -471,36 +472,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { hrt_abstime t = hrt_absolute_time(); float dt = (t - last_time) / 1000000.0; last_time = t; - /* calculate corrected acceleration vector in UAV frame */ - float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, - params.acc_T, params.acc_offs); - /* transform acceleration vector from UAV frame to NED frame */ - float accel_NED[3]; - for (int i = 0; i < 3; i++) { - accel_NED[i] = 0.0f; - for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * accel_corr[j]; + if (att.R_valid) { + /* calculate corrected acceleration vector in UAV frame */ + float accel_corr[3]; + acceleration_correct(accel_corr, sensor.accelerometer_raw, + params.acc_T, params.acc_offs); + /* transform acceleration vector from UAV frame to NED frame */ + float accel_NED[3]; + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * (accel_corr[j] - accel_offs_est[j]); + } + } + accel_NED[2] += const_earth_gravity; + /* accelerometers offset drift correction: rotate acceleration error back to UAV frame and integrate */ + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + /* the inverse of a rotation matrix is its transpose, just swap i and j */ + accel_offs_est[i] += att.R[j][i] * accel_NED[j] * params.acc_offs_w * dt; + } + } + /* kalman filter prediction */ + kalman_filter_inertial_predict(dt, z_est); + /* prepare vectors for kalman filter correction */ + float z_meas[2]; // position, acceleration + bool use_z[2] = { false, false }; + if (local_flag_baroINITdone && baro_updated) { + z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt + use_z[0] = true; + } + if (accelerometer_updated) { + z_meas[1] = accel_NED[2]; + use_z[1] = true; + } + if (use_z[0] || use_z[1]) { + /* correction */ + kalman_filter_inertial_update(z_est, z_meas, params.k, + use_z); } - } - accel_NED[2] += const_earth_gravity; - /* kalman filter prediction */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // position, acceleration - bool use_z[2] = { false, false }; - if (local_flag_baroINITdone && baro_updated) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; - } - if (accelerometer_updated) { - z_meas[1] = accel_NED[2]; - use_z[1] = true; - } - if (use_z[0] || use_z[1]) { - /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k, - use_z); } if (verbose_mode) { /* print updates rate */ @@ -521,9 +531,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = 0.0f; - pos.vx = 0.0f; - pos.y = 0.0f; + pos.x = accel_offs_est[0]; + pos.vx = accel_offs_est[1]; + pos.y = accel_offs_est[2]; pos.vy = 0.0f; pos.z = z_est[0]; pos.vz = z_est[1]; diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index d3a165947c..8cd9844c7d 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -39,6 +39,7 @@ */ #include "position_estimator_inav_params.h" +#include "acceleration_transform.h" /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ @@ -51,6 +52,8 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_ACC_OFFS_W, 0.0f); + PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); @@ -75,6 +78,8 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); + h->acc_offs_w = param_find("INAV_ACC_OFFS_W"); + h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); @@ -101,6 +106,8 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); + param_get(h->acc_offs_w, &(p->acc_offs_w)); + /* read int32 and cast to int16 */ int32_t t; param_get(h->acc_offs_0, &t); @@ -119,5 +126,6 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->acc_t_20, &(p->acc_T[2][0])); param_get(h->acc_t_21, &(p->acc_T[2][1])); param_get(h->acc_t_22, &(p->acc_T[2][2])); + return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 51e57a9146..2c6fb3df2e 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -43,6 +43,7 @@ struct position_estimator_inav_params { int use_gps; float k[3][2]; + float acc_offs_w; int16_t acc_offs[3]; float acc_T[3][3]; }; @@ -57,6 +58,8 @@ struct position_estimator_inav_param_handles { param_t k_alt_20; param_t k_alt_21; + param_t acc_offs_w; + param_t acc_offs_0; param_t acc_offs_1; param_t acc_offs_2; From 7570d9082ca4291b598f3e34be291189678685ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 May 2013 17:41:25 +0400 Subject: [PATCH 14/70] Use common accel calibration. Cleanup. --- apps/position_estimator_inav/Makefile | 5 - .../acceleration_transform.c | 130 ------------ .../acceleration_transform.h | 19 -- .../position_estimator_inav_main.c | 185 +++--------------- .../position_estimator_inav_params.c | 53 ----- .../position_estimator_inav_params.h | 19 -- 6 files changed, 22 insertions(+), 389 deletions(-) delete mode 100644 apps/position_estimator_inav/acceleration_transform.c delete mode 100644 apps/position_estimator_inav/acceleration_transform.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index 192ec18255..566bb9f431 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -39,9 +39,4 @@ APPNAME = position_estimator_inav PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 -CSRCS = position_estimator_inav_main.c \ - position_estimator_inav_params.c \ - kalman_filter_inertial.c \ - acceleration_transform.c - include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c deleted file mode 100644 index 3aba9b403d..0000000000 --- a/apps/position_estimator_inav/acceleration_transform.c +++ /dev/null @@ -1,130 +0,0 @@ -/* - * acceleration_transform.c - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - * - * Transform acceleration vector to true orientation and scale - * - * * * * Model * * * - * accel_corr = accel_T * (accel_raw - accel_offs) - * - * accel_corr[3] - fully corrected acceleration vector in UAV frame - * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform - * accel_raw[3] - raw acceleration vector - * accel_offs[3] - true acceleration offset vector, don't use sensors offset because - * it's caused not only by real offset but also by sensor rotation - * - * * * * Calibration * * * - * - * Reference vectors - * accel_corr_ref[6][3] = [ g 0 0 ] // positive x - * | -g 0 0 | // negative x - * | 0 g 0 | // positive y - * | 0 -g 0 | // negative y - * | 0 0 g | // positive z - * [ 0 0 -g ] // negative z - * accel_raw_ref[6][3] - * - * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 - * - * 6 reference vectors * 3 axes = 18 equations - * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants - * - * Find accel_offs - * - * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 - * - * - * Find accel_T - * - * 9 unknown constants - * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations - * - * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 - * - * Solve separate system for each row of accel_T: - * - * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 - * - * A x = b - * - * x = [ accel_T[0][i] ] - * | accel_T[1][i] | - * [ accel_T[2][i] ] - * - * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough - * | accel_corr_ref[2][i] | - * [ accel_corr_ref[4][i] ] - * - * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 - * - * Matrix A is common for all three systems: - * A = [ a[0][0] a[0][1] a[0][2] ] - * | a[2][0] a[2][1] a[2][2] | - * [ a[4][0] a[4][1] a[4][2] ] - * - * x = A^-1 b - * - * accel_T = A^-1 * g - * g = 9.81 - */ - -#include "acceleration_transform.h" - -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], - float accel_T[3][3], int16_t accel_offs[3]) { - for (int i = 0; i < 3; i++) { - accel_corr[i] = 0.0f; - for (int j = 0; j < 3; j++) { - accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]); - } - } -} - -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) - return -1; // Singular matrix - dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; - dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; - dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; - dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; - dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; - dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; - dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; - dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; - dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; - return 0; -} - -int calculate_calibration_values(int16_t accel_raw_ref[6][3], - float accel_T[3][3], int16_t accel_offs[3], float g) { - /* calculate raw offsets */ - for (int i = 0; i < 3; i++) { - accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) - + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); - } - /* fill matrix A for linear equations system*/ - float mat_A[3][3]; - memset(mat_A, 0, sizeof(mat_A)); - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - float a = (accel_raw_ref[i * 2][j] - accel_offs[j]); - mat_A[i][j] = a; - } - } - /* calculate inverse matrix for A */ - float mat_A_inv[3][3]; - mat_invert3(mat_A, mat_A_inv); - for (int i = 0; i < 3; i++) { - /* copy results to accel_T */ - for (int j = 0; j < 3; j++) { - /* simplify matrices mult because b has only one non-zero element == g at index i */ - accel_T[j][i] = mat_A_inv[j][i] * g; - } - } - return 0; -} diff --git a/apps/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h deleted file mode 100644 index 4d1299db5d..0000000000 --- a/apps/position_estimator_inav/acceleration_transform.h +++ /dev/null @@ -1,19 +0,0 @@ -/* - * acceleration_transform.h - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#ifndef ACCELERATION_TRANSFORM_H_ -#define ACCELERATION_TRANSFORM_H_ - -#include - -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], - float accel_T[3][3], int16_t accel_offs[3]); - -int calculate_calibration_values(int16_t accel_raw_ref[6][3], - float accel_T[3][3], int16_t accel_offs[3], float g); - -#endif /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 6e8c0ab5fe..071ec6ad4d 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -62,22 +62,19 @@ #include #include #include +#include #include #include "position_estimator_inav_params.h" #include "kalman_filter_inertial.h" -#include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -const static float const_earth_gravity = 9.81f; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); -void do_accelerometer_calibration(); - int position_estimator_inav_thread_main(int argc, char *argv[]); static void usage(const char *reason); @@ -89,18 +86,18 @@ static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, - "usage: position_estimator_inav {start|stop|status|calibrate} [-v]\n\n"); + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } - /** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ +/** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ int position_estimator_inav_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -136,136 +133,10 @@ int position_estimator_inav_main(int argc, char *argv[]) { exit(0); } - if (!strcmp(argv[1], "calibrate")) { - do_accelerometer_calibration(); - exit(0); - } - usage("unrecognized command"); exit(1); } -void wait_for_input() { - printf("press any key to continue, 'Q' to abort\n"); - while (true ) { - int c = getchar(); - if (c == 'q' || c == 'Q') { - exit(0); - } else { - return; - } - } -} - -void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], - int samples) { - printf("[position_estimator_inav] measuring...\n"); - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; - int count = 0; - int32_t accel_sum[3] = { 0, 0, 0 }; - while (count < samples) { - int poll_ret = poll(fds, 1, 1000); - if (poll_ret == 1) { - struct sensor_combined_s sensor; - orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - accel_sum[0] += sensor.accelerometer_raw[0]; - accel_sum[1] += sensor.accelerometer_raw[1]; - accel_sum[2] += sensor.accelerometer_raw[2]; - count++; - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - printf("[position_estimator_inav] no accelerometer data for 1s\n"); - exit(1); - } else { - printf("[position_estimator_inav] poll error\n"); - exit(1); - } - } - for (int i = 0; i < 3; i++) { - accel_avg[i] = (accel_sum[i] + count / 2) / count; - } - printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], - accel_avg[1], accel_avg[2]); -} - -void do_accelerometer_calibration() { - printf("[position_estimator_inav] calibration started\n"); - const int calibration_samples = 1000; - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int16_t accel_raw_ref[6][3]; // Reference measurements - printf("[position_estimator_inav] place vehicle level, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on it's back, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on right side, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on left side, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle nose down, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle nose up, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), - calibration_samples); - close(sensor_combined_sub); - printf("[position_estimator_inav] reference data collection done\n"); - - int16_t accel_offs[3]; - float accel_T[3][3]; - int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, - const_earth_gravity); - if (res != 0) { - printf( - "[position_estimator_inav] calibration parameters calculation error\n"); - exit(1); - - } - printf( - "[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", - accel_offs[0], accel_offs[1], accel_offs[2]); - printf( - "[position_estimator_inav] accelerometers transform matrix:\n [ %0.6f %0.6f %0.6f ]\n | %0.6f %0.6f %0.6f |\n [ %0.6f %0.6f %0.6f ]\n", - accel_T[0][0], accel_T[0][1], accel_T[0][2], accel_T[1][0], - accel_T[1][1], accel_T[1][2], accel_T[2][0], accel_T[2][1], - accel_T[2][2]); - int32_t accel_offs_int32[3] = - { accel_offs[0], accel_offs[1], accel_offs[2] }; - - if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0])) - || param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1])) - || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2])) - || param_set(param_find("INAV_ACC_T_00"), &(accel_T[0][0])) - || param_set(param_find("INAV_ACC_T_01"), &(accel_T[0][1])) - || param_set(param_find("INAV_ACC_T_02"), &(accel_T[0][2])) - || param_set(param_find("INAV_ACC_T_10"), &(accel_T[1][0])) - || param_set(param_find("INAV_ACC_T_11"), &(accel_T[1][1])) - || param_set(param_find("INAV_ACC_T_12"), &(accel_T[1][2])) - || param_set(param_find("INAV_ACC_T_20"), &(accel_T[2][0])) - || param_set(param_find("INAV_ACC_T_21"), &(accel_T[2][1])) - || param_set(param_find("INAV_ACC_T_22"), &(accel_T[2][2]))) { - printf("[position_estimator_inav] setting parameters failed\n"); - exit(1); - } - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - if (save_ret != 0) { - printf( - "[position_estimator_inav] auto-save of parameters to storage failed\n"); - exit(1); - } - printf("[position_estimator_inav] calibration done\n"); -} - /**************************************************************************** * main ****************************************************************************/ @@ -277,18 +148,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); /* initialize values */ - static float x_est[3] = { 0.0f, 0.0f, 0.0f }; - static float y_est[3] = { 0.0f, 0.0f, 0.0f }; - static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - float accel_offs_est[3] = { 0.0f, 0.0f, 0.0f }; + float x_est[3] = { 0.0f, 0.0f, 0.0f }; + float y_est[3] = { 0.0f, 0.0f, 0.0f }; + float z_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ - float baro_alt0 = 0.0f; /* to determin while start up */ + float baro_alt0 = 0.0f; /* to determine while start up */ - static double lat_current = 0.0d; //[°]] --> 47.0 - static double lon_current = 0.0d; //[°]] -->8.5 - static double alt_current = 0.0d; //[m] above MSL + static double lat_current = 0.0; //[°]] --> 47.0 + static double lon_current = 0.0; //[°]] -->8.5 + static double alt_current = 0.0; //[m] above MSL /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; @@ -473,26 +343,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { float dt = (t - last_time) / 1000000.0; last_time = t; if (att.R_valid) { - /* calculate corrected acceleration vector in UAV frame */ - float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, - params.acc_T, params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * (accel_corr[j] - accel_offs_est[j]); - } - } - accel_NED[2] += const_earth_gravity; - /* accelerometers offset drift correction: rotate acceleration error back to UAV frame and integrate */ - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - /* the inverse of a rotation matrix is its transpose, just swap i and j */ - accel_offs_est[i] += att.R[j][i] * accel_NED[j] * params.acc_offs_w * dt; + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; /* kalman filter prediction */ kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ @@ -531,9 +390,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = accel_offs_est[0]; - pos.vx = accel_offs_est[1]; - pos.y = accel_offs_est[2]; + pos.x = 0.0f; + pos.vx = 0.0f; + pos.y = 0.0f; pos.vy = 0.0f; pos.z = z_est[0]; pos.vz = z_est[1]; diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index 8cd9844c7d..bb2b014813 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -39,7 +39,6 @@ */ #include "position_estimator_inav_params.h" -#include "acceleration_transform.h" /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ @@ -52,22 +51,6 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_OFFS_W, 0.0f); - -PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); -PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); -PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); - -PARAM_DEFINE_FLOAT(INAV_ACC_T_00, 0.0021f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_01, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_02, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_10, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_11, 0.0021f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_12, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); - int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -78,21 +61,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); - h->acc_offs_w = param_find("INAV_ACC_OFFS_W"); - - h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); - h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); - h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); - - h->acc_t_00 = param_find("INAV_ACC_T_00"); - h->acc_t_01 = param_find("INAV_ACC_T_01"); - h->acc_t_02 = param_find("INAV_ACC_T_02"); - h->acc_t_10 = param_find("INAV_ACC_T_10"); - h->acc_t_11 = param_find("INAV_ACC_T_11"); - h->acc_t_12 = param_find("INAV_ACC_T_12"); - h->acc_t_20 = param_find("INAV_ACC_T_20"); - h->acc_t_21 = param_find("INAV_ACC_T_21"); - h->acc_t_22 = param_find("INAV_ACC_T_22"); return OK; } @@ -106,26 +74,5 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); - param_get(h->acc_offs_w, &(p->acc_offs_w)); - - /* read int32 and cast to int16 */ - int32_t t; - param_get(h->acc_offs_0, &t); - p->acc_offs[0] = t; - param_get(h->acc_offs_1, &t); - p->acc_offs[1] = t; - param_get(h->acc_offs_2, &t); - p->acc_offs[2] = t; - - param_get(h->acc_t_00, &(p->acc_T[0][0])); - param_get(h->acc_t_01, &(p->acc_T[0][1])); - param_get(h->acc_t_02, &(p->acc_T[0][2])); - param_get(h->acc_t_10, &(p->acc_T[1][0])); - param_get(h->acc_t_11, &(p->acc_T[1][1])); - param_get(h->acc_t_12, &(p->acc_T[1][2])); - param_get(h->acc_t_20, &(p->acc_T[2][0])); - param_get(h->acc_t_21, &(p->acc_T[2][1])); - param_get(h->acc_t_22, &(p->acc_T[2][2])); - return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 2c6fb3df2e..eaa1bbc953 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -43,9 +43,6 @@ struct position_estimator_inav_params { int use_gps; float k[3][2]; - float acc_offs_w; - int16_t acc_offs[3]; - float acc_T[3][3]; }; struct position_estimator_inav_param_handles { @@ -57,22 +54,6 @@ struct position_estimator_inav_param_handles { param_t k_alt_11; param_t k_alt_20; param_t k_alt_21; - - param_t acc_offs_w; - - param_t acc_offs_0; - param_t acc_offs_1; - param_t acc_offs_2; - - param_t acc_t_00; - param_t acc_t_01; - param_t acc_t_02; - param_t acc_t_10; - param_t acc_t_11; - param_t acc_t_12; - param_t acc_t_20; - param_t acc_t_21; - param_t acc_t_22; }; /** From 861a21ef7559386d8301c6b8860a13ac2fc0ef64 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 May 2013 19:02:32 +0400 Subject: [PATCH 15/70] Position estimation using accel+GPS implemented --- .../position_estimator_inav_main.c | 51 ++++++++++++++----- .../position_estimator_inav_params.c | 35 ++++++++++--- .../position_estimator_inav_params.h | 10 +++- 3 files changed, 74 insertions(+), 22 deletions(-) diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 071ec6ad4d..2b485f8950 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -261,6 +261,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; + bool gps_updated = false; + float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; + int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate if (ret < 0) { /* poll error */ @@ -322,14 +325,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), - vehicle_gps_position_sub, &gps); - static float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; /* output variables from tangent plane mapping */ + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); /* Project gps lat lon (Geographic coordinate system) to plane */ map_projection_project(((double) (gps.lat)) * 1e-7, ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), &(local_pos_gps[1])); local_pos_gps[2] = (float) (gps.alt * 1e-3); + gps_updated = true; pos.valid = gps.fix_type >= 3; gps_updates++; } @@ -352,7 +354,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } } accel_NED[2] += CONSTANTS_ONE_G; - /* kalman filter prediction */ + + /* kalman filter for altitude */ kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ float z_meas[2]; // position, acceleration @@ -367,8 +370,32 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k, - use_z); + kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z); + } + + if (params.use_gps) { + /* kalman filter for position */ + kalman_filter_inertial_predict(dt, x_est); + kalman_filter_inertial_predict(dt, y_est); + /* prepare vectors for kalman filter correction */ + float x_meas[2]; // position, acceleration + float y_meas[2]; // position, acceleration + bool use_xy[2] = { false, false }; + if (gps_updated) { + x_meas[0] = local_pos_gps[0]; + y_meas[0] = local_pos_gps[1]; + use_xy[0] = true; + } + if (accelerometer_updated) { + x_meas[1] = accel_NED[0]; + y_meas[1] = accel_NED[1]; + use_xy[1] = true; + } + if (use_xy[0] || use_xy[1]) { + /* correction */ + kalman_filter_inertial_update(x_est, x_meas, params.k_pos, use_xy); + kalman_filter_inertial_update(y_est, y_meas, params.k_pos, use_xy); + } } } if (verbose_mode) { @@ -390,10 +417,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = 0.0f; - pos.vx = 0.0f; - pos.y = 0.0f; - pos.vy = 0.0f; + pos.x = x_est[0]; + pos.vx = x_est[1]; + pos.y = y_est[0]; + pos.vy = y_est[1]; pos.z = z_est[0]; pos.vz = z_est[1]; pos.timestamp = hrt_absolute_time(); @@ -402,9 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { && (isfinite(pos.vy)) && (isfinite(pos.z)) && (isfinite(pos.vz))) { - orb_publish(ORB_ID( - vehicle_local_position), vehicle_local_position_pub, - &pos); + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); } } } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index bb2b014813..8466bcd0a3 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -40,8 +40,6 @@ #include "position_estimator_inav_params.h" -/* Kalman Filter covariances */ -/* gps measurement noise standard deviation */ PARAM_DEFINE_INT32(INAV_USE_GPS, 0); PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); @@ -51,6 +49,13 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); + int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -61,18 +66,32 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); + h->k_pos_00 = param_find("INAV_K_POS_00"); + h->k_pos_01 = param_find("INAV_K_POS_01"); + h->k_pos_10 = param_find("INAV_K_POS_10"); + h->k_pos_11 = param_find("INAV_K_POS_11"); + h->k_pos_20 = param_find("INAV_K_POS_20"); + h->k_pos_21 = param_find("INAV_K_POS_21"); + return OK; } int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { param_get(h->use_gps, &(p->use_gps)); - param_get(h->k_alt_00, &(p->k[0][0])); - param_get(h->k_alt_01, &(p->k[0][1])); - param_get(h->k_alt_10, &(p->k[1][0])); - param_get(h->k_alt_11, &(p->k[1][1])); - param_get(h->k_alt_20, &(p->k[2][0])); - param_get(h->k_alt_21, &(p->k[2][1])); + param_get(h->k_alt_00, &(p->k_alt[0][0])); + param_get(h->k_alt_01, &(p->k_alt[0][1])); + param_get(h->k_alt_10, &(p->k_alt[1][0])); + param_get(h->k_alt_11, &(p->k_alt[1][1])); + param_get(h->k_alt_20, &(p->k_alt[2][0])); + param_get(h->k_alt_21, &(p->k_alt[2][1])); + + param_get(h->k_pos_00, &(p->k_pos[0][0])); + param_get(h->k_pos_01, &(p->k_pos[0][1])); + param_get(h->k_pos_10, &(p->k_pos[1][0])); + param_get(h->k_pos_11, &(p->k_pos[1][1])); + param_get(h->k_pos_20, &(p->k_pos[2][0])); + param_get(h->k_pos_21, &(p->k_pos[2][1])); return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index eaa1bbc953..8cdc2e10f3 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -42,7 +42,8 @@ struct position_estimator_inav_params { int use_gps; - float k[3][2]; + float k_alt[3][2]; + float k_pos[3][2]; }; struct position_estimator_inav_param_handles { @@ -54,6 +55,13 @@ struct position_estimator_inav_param_handles { param_t k_alt_11; param_t k_alt_20; param_t k_alt_21; + + param_t k_pos_00; + param_t k_pos_01; + param_t k_pos_10; + param_t k_pos_11; + param_t k_pos_20; + param_t k_pos_21; }; /** From c21ba6c5007530f22db704924e79e9bb1ea79130 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 17 May 2013 12:32:07 +0400 Subject: [PATCH 16/70] position_estimator_inav added to config_px4fmu_default.mk --- makefiles/config_px4fmu_default.mk | 1 + 1 file changed, 1 insertion(+) diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index ae62b70347..23700b0ac7 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -64,6 +64,7 @@ MODULES += modules/attitude_estimator_ekf MODULES += modules/position_estimator_mc MODULES += modules/position_estimator MODULES += modules/att_pos_estimator_ekf +MODULES += modules/position_estimator_inav # # Vehicle Control From 5842c2212334876979f329b9b6bceba9609d91af Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 20 May 2013 19:47:38 +0400 Subject: [PATCH 17/70] Use GPS velocity in position estimator --- .../kalman_filter_inertial.c | 17 ++++++++++++- .../kalman_filter_inertial.h | 4 ++- .../position_estimator_inav_main.c | 25 +++++++++++-------- .../position_estimator_inav_params.c | 9 +++++++ .../position_estimator_inav_params.h | 5 +++- 5 files changed, 46 insertions(+), 14 deletions(-) diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c index 64031ee7bb..390e1b7208 100644 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.c +++ b/src/modules/position_estimator_inav/kalman_filter_inertial.c @@ -12,7 +12,7 @@ void kalman_filter_inertial_predict(float dt, float x[3]) { x[1] += x[2] * dt; } -void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { +void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]) { float y[2]; // y = z - H x y[0] = z[0] - x[0]; @@ -25,3 +25,18 @@ void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool u } } } + +void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) { + float y[2]; + // y = z - H x + y[0] = z[0] - x[0]; + y[1] = z[1] - x[1]; + y[2] = z[2] - x[2]; + // x = x + K * y + for (int i = 0; i < 3; i++) { // Row + for (int j = 0; j < 3; j++) { // Column + if (use[j]) + x[i] += k[i][j] * y[j]; + } + } +} diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h index 3e5134c33e..d34f582980 100644 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.h +++ b/src/modules/position_estimator_inav/kalman_filter_inertial.h @@ -9,4 +9,6 @@ void kalman_filter_inertial_predict(float dt, float x[3]); -void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]); +void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]); + +void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]); 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 2b485f8950..6ecdfe01d9 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -370,7 +370,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (use_z[0] || use_z[1]) { /* correction */ - kalman_filter_inertial_update(z_est, z_meas, params.k_alt, use_z); + kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z); } if (params.use_gps) { @@ -378,23 +378,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { kalman_filter_inertial_predict(dt, x_est); kalman_filter_inertial_predict(dt, y_est); /* prepare vectors for kalman filter correction */ - float x_meas[2]; // position, acceleration - float y_meas[2]; // position, acceleration - bool use_xy[2] = { false, false }; + float x_meas[3]; // position, velocity, acceleration + float y_meas[3]; // position, velocity, acceleration + bool use_xy[3] = { false, false, false }; if (gps_updated) { x_meas[0] = local_pos_gps[0]; y_meas[0] = local_pos_gps[1]; + x_meas[1] = gps.vel_n_m_s; + y_meas[1] = gps.vel_e_m_s; use_xy[0] = true; - } - if (accelerometer_updated) { - x_meas[1] = accel_NED[0]; - y_meas[1] = accel_NED[1]; use_xy[1] = true; } - if (use_xy[0] || use_xy[1]) { + if (accelerometer_updated) { + x_meas[2] = accel_NED[0]; + y_meas[2] = accel_NED[1]; + use_xy[2] = true; + } + if (use_xy[0] || use_xy[2]) { /* correction */ - kalman_filter_inertial_update(x_est, x_meas, params.k_pos, use_xy); - kalman_filter_inertial_update(y_est, y_meas, params.k_pos, use_xy); + kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy); + kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy); } } } 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 8466bcd0a3..20142b70c5 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -51,10 +51,13 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_02, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_12, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); +PARAM_DEFINE_FLOAT(INAV_K_POS_22, 0.0f); int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -68,10 +71,13 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_pos_00 = param_find("INAV_K_POS_00"); h->k_pos_01 = param_find("INAV_K_POS_01"); + h->k_pos_02 = param_find("INAV_K_POS_02"); h->k_pos_10 = param_find("INAV_K_POS_10"); h->k_pos_11 = param_find("INAV_K_POS_11"); + h->k_pos_12 = param_find("INAV_K_POS_12"); h->k_pos_20 = param_find("INAV_K_POS_20"); h->k_pos_21 = param_find("INAV_K_POS_21"); + h->k_pos_22 = param_find("INAV_K_POS_22"); return OK; } @@ -88,10 +94,13 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_pos_00, &(p->k_pos[0][0])); param_get(h->k_pos_01, &(p->k_pos[0][1])); + param_get(h->k_pos_02, &(p->k_pos[0][2])); param_get(h->k_pos_10, &(p->k_pos[1][0])); param_get(h->k_pos_11, &(p->k_pos[1][1])); + param_get(h->k_pos_12, &(p->k_pos[1][2])); param_get(h->k_pos_20, &(p->k_pos[2][0])); param_get(h->k_pos_21, &(p->k_pos[2][1])); + param_get(h->k_pos_22, &(p->k_pos[2][2])); return OK; } 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 8cdc2e10f3..c0e0042b6e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -43,7 +43,7 @@ struct position_estimator_inav_params { int use_gps; float k_alt[3][2]; - float k_pos[3][2]; + float k_pos[3][3]; }; struct position_estimator_inav_param_handles { @@ -58,10 +58,13 @@ struct position_estimator_inav_param_handles { param_t k_pos_00; param_t k_pos_01; + param_t k_pos_02; param_t k_pos_10; param_t k_pos_11; + param_t k_pos_12; param_t k_pos_20; param_t k_pos_21; + param_t k_pos_22; }; /** From 4bf49cfc35e86528a7b321dc0b8fb55c36fad510 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Jun 2013 19:28:25 +0400 Subject: [PATCH 18/70] multirotor_pos_control cleanup --- .../multirotor_pos_control/position_control.c | 235 ------------------ .../multirotor_pos_control/position_control.h | 50 ---- 2 files changed, 285 deletions(-) delete mode 100644 src/modules/multirotor_pos_control/position_control.c delete mode 100644 src/modules/multirotor_pos_control/position_control.h diff --git a/src/modules/multirotor_pos_control/position_control.c b/src/modules/multirotor_pos_control/position_control.c deleted file mode 100644 index 9c53a5bf6c..0000000000 --- a/src/modules/multirotor_pos_control/position_control.c +++ /dev/null @@ -1,235 +0,0 @@ -// /**************************************************************************** -// * -// * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. -// * Author: @author Lorenz Meier -// * @author Laurens Mackay -// * @author Tobias Naegeli -// * @author Martin Rutschmann -// * -// * 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 multirotor_position_control.c -// * Implementation of the position control for a multirotor VTOL -// */ - -// #include -// #include -// #include -// #include -// #include -// #include -// #include -// #include - -// #include "multirotor_position_control.h" - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp) -// { -// static PID_t distance_controller; - -// static int read_ret; -// static global_data_position_t position_estimated; - -// static uint16_t counter; - -// static bool initialized; -// static uint16_t pm_counter; - -// static float lat_next; -// static float lon_next; - -// static float pitch_current; - -// static float thrust_total; - - -// if (initialized == false) { - -// pid_init(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU], -// PID_MODE_DERIVATIV_CALC, 150);//150 - -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// thrust_total = 0.0f; - -// /* Position initialization */ -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// lat_next = position_estimated.lat; -// lon_next = position_estimated.lon; - -// /* attitude initialization */ -// global_data_lock(&global_data_attitude->access_conf); -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); - -// initialized = true; -// } - -// /* load new parameters with 10Hz */ -// if (counter % 50 == 0) { -// if (global_data_trylock(&global_data_parameter_storage->access_conf) == 0) { -// /* check whether new parameters are available */ -// if (global_data_parameter_storage->counter > pm_counter) { -// pid_set_parameters(&distance_controller, -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_P], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_I], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_D], -// global_data_parameter_storage->pm.param_values[PARAM_PID_POS_AWU]); - -// // -// // pid_pos_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_LIM]; -// // pid_pos_z_lim = global_data_parameter_storage->pm.param_values[PARAM_PID_POS_Z_LIM]; - -// pm_counter = global_data_parameter_storage->counter; -// printf("Position controller changed pid parameters\n"); -// } -// } - -// global_data_unlock(&global_data_parameter_storage->access_conf); -// } - - -// /* Wait for new position estimate */ -// do { -// read_ret = read_lock_position(&position_estimated); -// } while (read_ret != 0); - -// /* Get next waypoint */ //TODO: add local copy - -// if (0 == global_data_trylock(&global_data_position_setpoint->access_conf)) { -// lat_next = global_data_position_setpoint->x; -// lon_next = global_data_position_setpoint->y; -// global_data_unlock(&global_data_position_setpoint->access_conf); -// } - -// /* Get distance to waypoint */ -// float distance_to_waypoint = get_distance_to_next_waypoint(position_estimated.lat , position_estimated.lon, lat_next, lon_next); -// // if(counter % 5 == 0) -// // printf("distance_to_waypoint: %.4f\n", distance_to_waypoint); - -// /* Get bearing to waypoint (direction on earth surface to next waypoint) */ -// float bearing = get_bearing_to_next_waypoint(position_estimated.lat, position_estimated.lon, lat_next, lon_next); - -// if (counter % 5 == 0) -// printf("bearing: %.4f\n", bearing); - -// /* Calculate speed in direction of bearing (needed for controller) */ -// float speed_norm = sqrtf(position_estimated.vx * position_estimated.vx + position_estimated.vy * position_estimated.vy); -// // if(counter % 5 == 0) -// // printf("speed_norm: %.4f\n", speed_norm); -// float speed_to_waypoint = 0; //(position_estimated.vx * cosf(bearing) + position_estimated.vy * sinf(bearing))/speed_norm; //FIXME, TODO: re-enable this once we have a full estimate of the speed, then we can do a PID for the distance controller - -// /* Control Thrust in bearing direction */ -// float horizontal_thrust = -pid_calculate(&distance_controller, 0, distance_to_waypoint, speed_to_waypoint, -// CONTROL_PID_POSITION_INTERVAL); //TODO: maybe this "-" sign is an error somewhere else - -// // if(counter % 5 == 0) -// // printf("horizontal thrust: %.4f\n", horizontal_thrust); - -// /* Get total thrust (from remote for now) */ -// if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) { -// thrust_total = (float)global_data_rc_channels->chan[THROTTLE].scale; //TODO: how should we use the RC_CHANNELS_FUNCTION enum? -// global_data_unlock(&global_data_rc_channels->access_conf); -// } - -// const float max_gas = 500.0f; -// thrust_total *= max_gas / 20000.0f; //TODO: check this -// thrust_total += max_gas / 2.0f; - - -// if (horizontal_thrust > thrust_total) { -// horizontal_thrust = thrust_total; - -// } else if (horizontal_thrust < -thrust_total) { -// horizontal_thrust = -thrust_total; -// } - - - -// //TODO: maybe we want to add a speed controller later... - -// /* Calclulate thrust in east and north direction */ -// float thrust_north = cosf(bearing) * horizontal_thrust; -// float thrust_east = sinf(bearing) * horizontal_thrust; - -// if (counter % 10 == 0) { -// printf("thrust north: %.4f\n", thrust_north); -// printf("thrust east: %.4f\n", thrust_east); -// fflush(stdout); -// } - -// /* Get current attitude */ -// if (0 == global_data_trylock(&global_data_attitude->access_conf)) { -// pitch_current = global_data_attitude->pitch; -// global_data_unlock(&global_data_attitude->access_conf); -// } - -// /* Get desired pitch & roll */ -// float pitch_desired = 0.0f; -// float roll_desired = 0.0f; - -// if (thrust_total != 0) { -// float pitch_fraction = -thrust_north / thrust_total; -// float roll_fraction = thrust_east / (cosf(pitch_current) * thrust_total); - -// if (roll_fraction < -1) { -// roll_fraction = -1; - -// } else if (roll_fraction > 1) { -// roll_fraction = 1; -// } - -// // if(counter % 5 == 0) -// // { -// // printf("pitch_fraction: %.4f, roll_fraction: %.4f\n",pitch_fraction, roll_fraction); -// // fflush(stdout); -// // } - -// pitch_desired = asinf(pitch_fraction); -// roll_desired = asinf(roll_fraction); -// } - -// att_sp.roll = roll_desired; -// att_sp.pitch = pitch_desired; - -// counter++; -// } diff --git a/src/modules/multirotor_pos_control/position_control.h b/src/modules/multirotor_pos_control/position_control.h deleted file mode 100644 index 2144ebc343..0000000000 --- a/src/modules/multirotor_pos_control/position_control.h +++ /dev/null @@ -1,50 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann - * - * 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 multirotor_position_control.h - * Definition of the position control for a multirotor VTOL - */ - -// #ifndef POSITION_CONTROL_H_ -// #define POSITION_CONTROL_H_ - -// void control_multirotor_position(const struct vehicle_state_s *vstatus, const struct vehicle_manual_control_s *manual, -// const struct vehicle_attitude_s *att, const struct vehicle_local_position_s *local_pos, -// const struct vehicle_local_position_setpoint_s *local_pos_sp, struct vehicle_attitude_setpoint_s *att_sp); - -// #endif /* POSITION_CONTROL_H_ */ From 4256e43de7ea4c9cad98e8bfc9a811310bfb3d77 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 10 Jun 2013 23:16:04 +0400 Subject: [PATCH 19/70] Complete position estimator implemented (GPS + Baro + Accel) --- .../position_estimator_inav/inertial_filter.c | 28 ++ .../position_estimator_inav/inertial_filter.h | 13 + .../kalman_filter_inertial.c | 42 --- .../kalman_filter_inertial.h | 14 - src/modules/position_estimator_inav/module.mk | 2 +- .../position_estimator_inav_main.c | 242 ++++++++++-------- .../position_estimator_inav_params.c | 76 ++---- .../position_estimator_inav_params.h | 31 +-- 8 files changed, 207 insertions(+), 241 deletions(-) create mode 100644 src/modules/position_estimator_inav/inertial_filter.c create mode 100644 src/modules/position_estimator_inav/inertial_filter.h delete mode 100644 src/modules/position_estimator_inav/kalman_filter_inertial.c delete mode 100644 src/modules/position_estimator_inav/kalman_filter_inertial.h diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c new file mode 100644 index 0000000000..b70d3504e6 --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -0,0 +1,28 @@ +/* + * inertial_filter.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + +#include "inertial_filter.h" + +void inertial_filter_predict(float dt, float x[3]) +{ + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; +} + +void inertial_filter_correct(float dt, float x[3], int i, float z, float w) +{ + float e = z - x[i]; + x[i] += e * w * dt; + + if (i == 0) { + x[1] += e * w * w * dt; + //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 + + } else if (i == 1) { + x[2] += e * w * w * dt; + } +} diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h new file mode 100644 index 0000000000..18c194abf2 --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -0,0 +1,13 @@ +/* + * inertial_filter.h + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + */ + +#include +#include + +void inertial_filter_predict(float dt, float x[3]); + +void inertial_filter_correct(float dt, float x[3], int i, float z, float w); diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.c b/src/modules/position_estimator_inav/kalman_filter_inertial.c deleted file mode 100644 index 390e1b7208..0000000000 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.c +++ /dev/null @@ -1,42 +0,0 @@ -/* - * kalman_filter_inertial.c - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#include "kalman_filter_inertial.h" - -void kalman_filter_inertial_predict(float dt, float x[3]) { - x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; - x[1] += x[2] * dt; -} - -void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]) { - float y[2]; - // y = z - H x - y[0] = z[0] - x[0]; - y[1] = z[1] - x[2]; - // x = x + K * y - for (int i = 0; i < 3; i++) { // Row - for (int j = 0; j < 2; j++) { // Column - if (use[j]) - x[i] += k[i][j] * y[j]; - } - } -} - -void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]) { - float y[2]; - // y = z - H x - y[0] = z[0] - x[0]; - y[1] = z[1] - x[1]; - y[2] = z[2] - x[2]; - // x = x + K * y - for (int i = 0; i < 3; i++) { // Row - for (int j = 0; j < 3; j++) { // Column - if (use[j]) - x[i] += k[i][j] * y[j]; - } - } -} diff --git a/src/modules/position_estimator_inav/kalman_filter_inertial.h b/src/modules/position_estimator_inav/kalman_filter_inertial.h deleted file mode 100644 index d34f582980..0000000000 --- a/src/modules/position_estimator_inav/kalman_filter_inertial.h +++ /dev/null @@ -1,14 +0,0 @@ -/* - * kalman_filter_inertial.h - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#include - -void kalman_filter_inertial_predict(float dt, float x[3]); - -void kalman_filter_inertial2_update(float x[3], float z[2], float k[3][2], bool use[2]); - -void kalman_filter_inertial3_update(float x[3], float z[3], float k[3][3], bool use[3]); diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 8ac701c535..939d768495 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -38,4 +38,4 @@ MODULE_COMMAND = position_estimator_inav SRCS = position_estimator_inav_main.c \ position_estimator_inav_params.c \ - kalman_filter_inertial.c + inertial_filter.c 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 6ecdfe01d9..306ebe5ccc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -66,7 +66,7 @@ #include #include "position_estimator_inav_params.h" -#include "kalman_filter_inertial.h" +#include "inertial_filter.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -82,13 +82,15 @@ static void usage(const char *reason); /** * Print the correct usage. */ -static void usage(const char *reason) { +static void usage(const char *reason) +{ if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, - "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); - exit(1); - } + + fprintf(stderr, + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + exit(1); +} /** * The position_estimator_inav_thread only briefly exists to start @@ -98,7 +100,8 @@ static void usage(const char *reason) { * The actual stack size should be set in the call * to task_create(). */ -int position_estimator_inav_main(int argc, char *argv[]) { +int position_estimator_inav_main(int argc, char *argv[]) +{ if (argc < 1) usage("missing command"); @@ -108,17 +111,19 @@ int position_estimator_inav_main(int argc, char *argv[]) { /* this is not an error */ exit(0); } + if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; thread_should_exit = false; position_estimator_inav_task = task_spawn("position_estimator_inav", - SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, - position_estimator_inav_thread_main, - (argv) ? (const char **) &argv[2] : (const char **) NULL ); + SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, + position_estimator_inav_thread_main, + (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); } + if (!strcmp(argv[1], "stop")) { thread_should_exit = true; exit(0); @@ -127,9 +132,11 @@ int position_estimator_inav_main(int argc, char *argv[]) { if (!strcmp(argv[1], "status")) { if (thread_running) { printf("\tposition_estimator_inav is running\n"); + } else { printf("\tposition_estimator_inav not started\n"); } + exit(0); } @@ -140,10 +147,10 @@ int position_estimator_inav_main(int argc, char *argv[]) { /**************************************************************************** * main ****************************************************************************/ -int position_estimator_inav_thread_main(int argc, char *argv[]) { - /* welcome user */ - printf("[position_estimator_inav] started\n"); - static int mavlink_fd; +int position_estimator_inav_thread_main(int argc, char *argv[]) +{ + warnx("started."); + int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); @@ -156,9 +163,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { int baro_loop_end = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determine while start up */ - static double lat_current = 0.0; //[°]] --> 47.0 - static double lon_current = 0.0; //[°]] -->8.5 - static double alt_current = 0.0; //[m] above MSL + double lat_current = 0.0; //[°]] --> 47.0 + double lon_current = 0.0; //[°]] -->8.5 + double alt_current = 0.0; //[m] above MSL /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; @@ -188,40 +195,42 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* initialize parameter handles */ parameters_init(&pos_inav_param_handles); - bool local_flag_baroINITdone = false; /* in any case disable baroINITdone */ - /* FIRST PARAMETER READ at START UP*/ + bool local_flag_baroINITdone = false; + /* 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 to clear updated flag */ - /* FIRST PARAMETER 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); - /* END FIRST PARAMETER UPDATE */ - /* wait until gps signal turns valid, only then can we initialize the projection */ + /* wait for GPS fix, only then can we initialize the projection */ if (params.use_gps) { struct pollfd fds_init[1] = { - { .fd = vehicle_gps_position_sub, .events = POLLIN } + { .fd = vehicle_gps_position_sub, .events = POLLIN } }; while (gps.fix_type < 3) { - if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */ + if (poll(fds_init, 1, 5000)) { if (fds_init[0].revents & POLLIN) { /* Wait for the GPS update to propagate (we have some time) */ usleep(5000); orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); } } + static int printcounter = 0; + if (printcounter == 100) { printcounter = 0; - printf("[position_estimator_inav] wait for GPS fix type 3\n"); + warnx("waiting for GPS fix type 3..."); } + printcounter++; } /* get GPS position for first initialization */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - lat_current = ((double) (gps.lat)) * 1e-7; - lon_current = ((double) (gps.lon)) * 1e-7; + lat_current = ((double)(gps.lat)) * 1e-7; + lon_current = ((double)(gps.lon)) * 1e-7; alt_current = gps.alt * 1e-3; pos.home_lat = lat_current * 1e7; @@ -232,15 +241,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - printf( - "[position_estimator_inav] initialized projection with: lat: %.10f, lon:%.10f\n", - lat_current, lon_current); - hrt_abstime last_time = 0; + warnx("initialized projection with: lat: %.10f, lon:%.10f", lat_current, lon_current); + + hrt_abstime t_prev = 0; thread_running = true; - uint32_t accelerometer_counter = 0; + uint32_t accel_counter = 0; + hrt_abstime accel_t = 0; + float accel_dt = 0.0f; uint32_t baro_counter = 0; - uint16_t accelerometer_updates = 0; + hrt_abstime baro_t = 0; + hrt_abstime gps_t = 0; + uint16_t accel_updates = 0; uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; @@ -251,173 +263,184 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { /* main loop */ struct pollfd fds[5] = { - { .fd = parameter_update_sub, .events = POLLIN }, - { .fd = vehicle_status_sub, .events = POLLIN }, - { .fd = vehicle_attitude_sub, .events = POLLIN }, - { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } + { .fd = parameter_update_sub, .events = POLLIN }, + { .fd = vehicle_status_sub, .events = POLLIN }, + { .fd = vehicle_attitude_sub, .events = POLLIN }, + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } }; printf("[position_estimator_inav] main loop started\n"); + while (!thread_should_exit) { bool accelerometer_updated = false; bool baro_updated = false; bool gps_updated = false; - float local_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; + float proj_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate + hrt_abstime t = hrt_absolute_time(); + if (ret < 0) { /* poll error */ - printf("[position_estimator_inav] subscriptions poll error\n"); + warnx("subscriptions poll error."); thread_should_exit = true; continue; + } else if (ret > 0) { /* parameter update */ if (fds[0].revents & POLLIN) { /* read from param to clear updated flag */ struct parameter_update_s update; orb_copy(ORB_ID(parameter_update), parameter_update_sub, - &update); + &update); /* update parameters */ parameters_update(&pos_inav_param_handles, ¶ms); } + /* vehicle status */ if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, - &vehicle_status); + &vehicle_status); } + /* vehicle attitude */ if (fds[2].revents & POLLIN) { orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); attitude_updates++; } + /* sensor combined */ if (fds[3].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - if (sensor.accelerometer_counter > accelerometer_counter) { + + if (sensor.accelerometer_counter > accel_counter) { accelerometer_updated = true; - accelerometer_counter = sensor.accelerometer_counter; - accelerometer_updates++; + accel_counter = sensor.accelerometer_counter; + accel_updates++; + accel_dt = accel_t > 0 ? (t - accel_t) / 1000000.0f : 0.0f; + accel_t = t; } + if (sensor.baro_counter > baro_counter) { baro_updated = true; baro_counter = sensor.baro_counter; baro_updates++; } - // barometric pressure estimation at start up + + /* barometric pressure estimation at start up */ if (!local_flag_baroINITdone && baro_updated) { - // mean calculation over several measurements + /* mean calculation over several measurements */ if (baro_loop_cnt < baro_loop_end) { baro_alt0 += sensor.baro_alt_meter; baro_loop_cnt++; + } else { - baro_alt0 /= (float) (baro_loop_cnt); + baro_alt0 /= (float)(baro_loop_cnt); local_flag_baroINITdone = true; - char str[80]; - sprintf(str, - "[position_estimator_inav] baro_alt0 = %.2f", - baro_alt0); - printf("%s\n", str); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "[position_estimator_inav] baro_alt0 = %.2f", baro_alt0); } } } + if (params.use_gps) { /* vehicle GPS position */ if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - /* Project gps lat lon (Geographic coordinate system) to plane */ - map_projection_project(((double) (gps.lat)) * 1e-7, - ((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]), - &(local_pos_gps[1])); - local_pos_gps[2] = (float) (gps.alt * 1e-3); + /* project GPS lat lon (Geographic coordinate system) to plane */ + map_projection_project(((double)(gps.lat)) * 1e-7, + ((double)(gps.lon)) * 1e-7, &(proj_pos_gps[0]), + &(proj_pos_gps[1])); + proj_pos_gps[2] = (float)(gps.alt * 1e-3); gps_updated = true; pos.valid = gps.fix_type >= 3; gps_updates++; } + } else { pos.valid = true; } - } /* end of poll return value check */ + } + + /* end of poll return value check */ + + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0 : 0.0f; + t_prev = t; - hrt_abstime t = hrt_absolute_time(); - float dt = (t - last_time) / 1000000.0; - last_time = t; if (att.R_valid) { /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; + for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; - /* kalman filter for altitude */ - kalman_filter_inertial_predict(dt, z_est); - /* prepare vectors for kalman filter correction */ - float z_meas[2]; // position, acceleration - bool use_z[2] = { false, false }; + /* inertial filter prediction for altitude */ + inertial_filter_predict(dt, z_est); + + /* inertial filter correction for altitude */ if (local_flag_baroINITdone && baro_updated) { - z_meas[0] = baro_alt0 - sensor.baro_alt_meter; // Z = -alt - use_z[0] = true; + if (baro_t > 0) { + inertial_filter_correct((t - baro_t) / 1000000.0f, z_est, 0, baro_alt0 - sensor.baro_alt_meter, params.w_alt_baro); + } + + baro_t = t; } + if (accelerometer_updated) { - z_meas[1] = accel_NED[2]; - use_z[1] = true; - } - if (use_z[0] || use_z[1]) { - /* correction */ - kalman_filter_inertial2_update(z_est, z_meas, params.k_alt, use_z); + inertial_filter_correct(accel_dt, z_est, 2, accel_NED[2], params.w_alt_acc); } if (params.use_gps) { - /* kalman filter for position */ - kalman_filter_inertial_predict(dt, x_est); - kalman_filter_inertial_predict(dt, y_est); - /* prepare vectors for kalman filter correction */ - float x_meas[3]; // position, velocity, acceleration - float y_meas[3]; // position, velocity, acceleration - bool use_xy[3] = { false, false, false }; + /* inertial filter prediction for position */ + inertial_filter_predict(dt, x_est); + inertial_filter_predict(dt, y_est); + + /* inertial filter correction for position */ if (gps_updated) { - x_meas[0] = local_pos_gps[0]; - y_meas[0] = local_pos_gps[1]; - x_meas[1] = gps.vel_n_m_s; - y_meas[1] = gps.vel_e_m_s; - use_xy[0] = true; - use_xy[1] = true; + if (gps_t > 0) { + float gps_dt = (t - gps_t) / 1000000.0f; + inertial_filter_correct(gps_dt, x_est, 0, proj_pos_gps[0], params.w_pos_gps_p); + inertial_filter_correct(gps_dt, x_est, 1, gps.vel_n_m_s, params.w_pos_gps_v); + inertial_filter_correct(gps_dt, y_est, 0, proj_pos_gps[1], params.w_pos_gps_p); + inertial_filter_correct(gps_dt, y_est, 1, gps.vel_e_m_s, params.w_pos_gps_v); + } + + gps_t = t; } + if (accelerometer_updated) { - x_meas[2] = accel_NED[0]; - y_meas[2] = accel_NED[1]; - use_xy[2] = true; - } - if (use_xy[0] || use_xy[2]) { - /* correction */ - kalman_filter_inertial3_update(x_est, x_meas, params.k_pos, use_xy); - kalman_filter_inertial3_update(y_est, y_meas, params.k_pos, use_xy); + inertial_filter_correct(accel_dt, x_est, 2, accel_NED[0], params.w_pos_acc); + inertial_filter_correct(accel_dt, y_est, 2, accel_NED[1], params.w_pos_acc); } } } + if (verbose_mode) { /* print updates rate */ if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; printf( - "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", - accelerometer_updates / updates_dt, - baro_updates / updates_dt, - gps_updates / updates_dt, - attitude_updates / updates_dt); + "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", + accel_updates / updates_dt, + baro_updates / updates_dt, + gps_updates / updates_dt, + attitude_updates / updates_dt); updates_counter_start = t; - accelerometer_updates = 0; + accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; } } + if (t - pub_last > pub_interval) { pub_last = t; pos.x = x_est[0]; @@ -427,17 +450,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { pos.z = z_est[0]; pos.vz = z_est[1]; pos.timestamp = hrt_absolute_time(); + if ((isfinite(pos.x)) && (isfinite(pos.vx)) - && (isfinite(pos.y)) - && (isfinite(pos.vy)) - && (isfinite(pos.z)) - && (isfinite(pos.vz))) { + && (isfinite(pos.y)) + && (isfinite(pos.vy)) + && (isfinite(pos.z)) + && (isfinite(pos.vz))) { orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); } } } - printf("[position_estimator_inav] exiting.\n"); + warnx("exiting."); mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); thread_running = false; return 0; 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 20142b70c5..8dd0ceff82 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -34,73 +34,39 @@ /* * @file position_estimator_inav_params.c - * + * * Parameters for position_estimator_inav */ #include "position_estimator_inav_params.h" -PARAM_DEFINE_INT32(INAV_USE_GPS, 0); +PARAM_DEFINE_INT32(INAV_USE_GPS, 1); +PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 50.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_00, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_01, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_10, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); - -PARAM_DEFINE_FLOAT(INAV_K_POS_00, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_01, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_02, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_10, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_11, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_12, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_20, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_K_POS_22, 0.0f); - -int parameters_init(struct position_estimator_inav_param_handles *h) { +int parameters_init(struct position_estimator_inav_param_handles *h) +{ h->use_gps = param_find("INAV_USE_GPS"); - - h->k_alt_00 = param_find("INAV_K_ALT_00"); - h->k_alt_01 = param_find("INAV_K_ALT_01"); - h->k_alt_10 = param_find("INAV_K_ALT_10"); - h->k_alt_11 = param_find("INAV_K_ALT_11"); - h->k_alt_20 = param_find("INAV_K_ALT_20"); - h->k_alt_21 = param_find("INAV_K_ALT_21"); - - h->k_pos_00 = param_find("INAV_K_POS_00"); - h->k_pos_01 = param_find("INAV_K_POS_01"); - h->k_pos_02 = param_find("INAV_K_POS_02"); - h->k_pos_10 = param_find("INAV_K_POS_10"); - h->k_pos_11 = param_find("INAV_K_POS_11"); - h->k_pos_12 = param_find("INAV_K_POS_12"); - h->k_pos_20 = param_find("INAV_K_POS_20"); - h->k_pos_21 = param_find("INAV_K_POS_21"); - h->k_pos_22 = param_find("INAV_K_POS_22"); + h->w_alt_baro = param_find("INAV_W_ALT_BARO"); + h->w_alt_acc = param_find("INAV_W_ALT_ACC"); + h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); + h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); + h->w_pos_acc = param_find("INAV_W_POS_ACC"); return OK; } -int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { +int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) +{ param_get(h->use_gps, &(p->use_gps)); - - param_get(h->k_alt_00, &(p->k_alt[0][0])); - param_get(h->k_alt_01, &(p->k_alt[0][1])); - param_get(h->k_alt_10, &(p->k_alt[1][0])); - param_get(h->k_alt_11, &(p->k_alt[1][1])); - param_get(h->k_alt_20, &(p->k_alt[2][0])); - param_get(h->k_alt_21, &(p->k_alt[2][1])); - - param_get(h->k_pos_00, &(p->k_pos[0][0])); - param_get(h->k_pos_01, &(p->k_pos[0][1])); - param_get(h->k_pos_02, &(p->k_pos[0][2])); - param_get(h->k_pos_10, &(p->k_pos[1][0])); - param_get(h->k_pos_11, &(p->k_pos[1][1])); - param_get(h->k_pos_12, &(p->k_pos[1][2])); - param_get(h->k_pos_20, &(p->k_pos[2][0])); - param_get(h->k_pos_21, &(p->k_pos[2][1])); - param_get(h->k_pos_22, &(p->k_pos[2][2])); + param_get(h->w_alt_baro, &(p->w_alt_baro)); + param_get(h->w_alt_acc, &(p->w_alt_acc)); + param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); + param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); + param_get(h->w_pos_acc, &(p->w_pos_acc)); return OK; } 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 c0e0042b6e..870227fefc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -34,7 +34,7 @@ /* * @file position_estimator_inav_params.h - * + * * Parameters for Position Estimator */ @@ -42,29 +42,20 @@ struct position_estimator_inav_params { int use_gps; - float k_alt[3][2]; - float k_pos[3][3]; + float w_alt_baro; + float w_alt_acc; + float w_pos_gps_p; + float w_pos_gps_v; + float w_pos_acc; }; struct position_estimator_inav_param_handles { param_t use_gps; - - param_t k_alt_00; - param_t k_alt_01; - param_t k_alt_10; - param_t k_alt_11; - param_t k_alt_20; - param_t k_alt_21; - - param_t k_pos_00; - param_t k_pos_01; - param_t k_pos_02; - param_t k_pos_10; - param_t k_pos_11; - param_t k_pos_12; - param_t k_pos_20; - param_t k_pos_21; - param_t k_pos_22; + param_t w_alt_baro; + param_t w_alt_acc; + param_t w_pos_gps_p; + param_t w_pos_gps_v; + param_t w_pos_acc; }; /** From 4860c73008c0bdfe69503fbbfa4e717a144fc3e0 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Jun 2013 06:48:24 +0400 Subject: [PATCH 20/70] multirotor_pos_control: position controller implemented --- .../multirotor_pos_control.c | 128 +++++++++++++----- .../multirotor_pos_control_params.c | 22 ++- .../multirotor_pos_control_params.h | 10 ++ 3 files changed, 126 insertions(+), 34 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 6fe129d849..ad5670edca 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -59,7 +60,6 @@ #include #include #include -#include #include #include @@ -82,6 +82,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]); */ static void usage(const char *reason); +static float scale_control(float ctl, float end, float dz); + +static float limit_value(float v, float limit); + +static float norm(float x, float y); + static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -137,9 +143,32 @@ int multirotor_pos_control_main(int argc, char *argv[]) { exit(1); } +static float scale_control(float ctl, float end, float dz) { + if (ctl > dz) { + return (ctl - dz) / (end - dz); + } else if (ctl < -dz) { + return (ctl + dz) / (end - dz); + } else { + return 0.0f; + } +} + +static float limit_value(float v, float limit) { + if (v > limit) { + v = limit; + } else if (v < -limit) { + v = -limit; + } + return v; +} + +static float norm(float x, float y) { + return sqrtf(x * x + y * y); +} + static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - printf("[multirotor_pos_control] started\n"); + warnx("started."); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); @@ -175,7 +204,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { bool reset_sp_pos = true; hrt_abstime t_prev = 0; float alt_integral = 0.0f; - float alt_ctl_dz = 0.2f; + /* integrate in NED frame to estimate wind but not attitude offset */ + float pos_x_integral = 0.0f; + float pos_y_integral = 0.0f; + const float alt_ctl_dz = 0.2f; + const float pos_ctl_dz = 0.05f; thread_running = true; @@ -235,64 +268,97 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; alt_integral = manual.throttle; - char str[80]; - sprintf(str, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); - mavlink_log_info(mavlink_fd, str); + mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - char str[80]; - sprintf(str, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); - mavlink_log_info(mavlink_fd, str); + pos_x_integral = 0.0f; + pos_y_integral = 0.0f; + mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } - float err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; + float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max; if (status.flag_control_manual_enabled) { /* move altitude setpoint with manual controls */ - float alt_ctl = manual.throttle - 0.5f; - if (fabs(alt_ctl) < alt_ctl_dz) { - alt_ctl = 0.0f; - } else { - if (alt_ctl > 0.0f) - alt_ctl = (alt_ctl - alt_ctl_dz) / (0.5f - alt_ctl_dz); - else - alt_ctl = (alt_ctl + alt_ctl_dz) / (0.5f - alt_ctl_dz); - local_pos_sp.z -= alt_ctl * params.alt_rate_max * dt; - if (local_pos_sp.z > local_pos.z + err_linear_limit) - local_pos_sp.z = local_pos.z + err_linear_limit; - else if (local_pos_sp.z < local_pos.z - err_linear_limit) - local_pos_sp.z = local_pos.z - err_linear_limit; + float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + if (alt_sp_ctl != 0.0f) { + local_pos_sp.z -= alt_sp_ctl * params.alt_rate_max * dt; + if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) { + local_pos_sp.z = local_pos.z + alt_err_linear_limit; + } else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) { + local_pos_sp.z = local_pos.z - alt_err_linear_limit; + } } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { - // TODO move position setpoint with manual controls + /* move position setpoint with manual controls */ + float pos_x_sp_ctl = scale_control(-manual.pitch, 1.0f, pos_ctl_dz); + float pos_y_sp_ctl = scale_control(manual.roll, 1.0f, pos_ctl_dz); + if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) { + /* calculate direction and increment of control in NED frame */ + float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl); + float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max * dt; + local_pos_sp.x += cosf(dir_sp_ctl) * d_sp_ctl; + local_pos_sp.y += sinf(dir_sp_ctl) * d_sp_ctl; + /* limit maximum setpoint from position offset and preserve direction */ + float pos_vec_x = local_pos_sp.x - local_pos.x; + float pos_vec_y = local_pos_sp.y - local_pos.y; + float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / pos_err_linear_limit; + if (pos_vec_norm > 1.0f) { + local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; + local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; + } + } } } /* PID for altitude */ - float alt_err = local_pos.z - local_pos_sp.z; /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - if (alt_err > err_linear_limit) { - alt_err = err_linear_limit; - } else if (alt_err < -err_linear_limit) { - alt_err = -err_linear_limit; - } + float alt_err = limit_value(local_pos.z - local_pos_sp.z, alt_err_linear_limit); /* P and D components */ float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; /* add I component */ float thrust_ctl = thrust_ctl_pd + alt_integral; + /* integrate */ alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; + if (alt_integral < params.thr_min) { + alt_integral = params.thr_min; + } else if (alt_integral > params.thr_max) { + alt_integral = params.thr_max; + } if (thrust_ctl < params.thr_min) { thrust_ctl = params.thr_min; } else if (thrust_ctl > params.thr_max) { thrust_ctl = params.thr_max; } if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { - // TODO add position controller + /* PID for position */ + /* don't accelerate more than POS_RATE_MAX, limit error to corresponding value */ + float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit); + float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit); + /* P and D components */ + float pos_x_ctl_pd = - pos_x_err * params.pos_p - local_pos.vx * params.pos_d; + float pos_y_ctl_pd = - pos_y_err * params.pos_p - local_pos.vy * params.pos_d; + /* add I component */ + float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; + float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; + /* integrate */ + pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); + pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); + /* calculate direction and slope in NED frame */ + float dir = atan2f(pos_y_ctl, pos_x_ctl); + /* use approximation: slope ~ sin(slope) = force */ + float slope = limit_value(sqrtf(pos_x_ctl * pos_x_ctl + pos_y_ctl * pos_y_ctl), params.slope_max); + /* convert direction to body frame */ + dir -= att.yaw; + /* calculate roll and pitch */ + att_sp.pitch_body = -cosf(dir) * slope; // reverse pitch + att_sp.roll_body = sinf(dir) * slope; } else { reset_sp_pos = true; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 90dd820f43..9610ef9f78 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -45,19 +45,29 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.01f); -PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.1f); +PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.1f); PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); +PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.3f); int parameters_init(struct multirotor_position_control_param_handles *h) { h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); - /* PID parameters */ h->alt_p = param_find("MPC_ALT_P"); h->alt_i = param_find("MPC_ALT_I"); h->alt_d = param_find("MPC_ALT_D"); h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); + h->pos_p = param_find("MPC_POS_P"); + h->pos_i = param_find("MPC_POS_I"); + h->pos_d = param_find("MPC_POS_D"); + h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); + h->slope_max = param_find("MPC_SLOPE_MAX"); + return OK; } @@ -69,5 +79,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->alt_i, &(p->alt_i)); param_get(h->alt_d, &(p->alt_d)); param_get(h->alt_rate_max, &(p->alt_rate_max)); + param_get(h->pos_p, &(p->pos_p)); + param_get(h->pos_i, &(p->pos_i)); + param_get(h->pos_d, &(p->pos_d)); + param_get(h->pos_rate_max, &(p->pos_rate_max)); + param_get(h->slope_max, &(p->slope_max)); + return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 849055cd16..589ee92a1d 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -48,6 +48,11 @@ struct multirotor_position_control_params { float alt_i; float alt_d; float alt_rate_max; + float pos_p; + float pos_i; + float pos_d; + float pos_rate_max; + float slope_max; }; struct multirotor_position_control_param_handles { @@ -57,6 +62,11 @@ struct multirotor_position_control_param_handles { param_t alt_i; param_t alt_d; param_t alt_rate_max; + param_t pos_p; + param_t pos_i; + param_t pos_d; + param_t pos_rate_max; + param_t slope_max; }; /** From 4cdee2be03b693b843c4dec93e67eadec903f5d8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Jun 2013 06:49:17 +0400 Subject: [PATCH 21/70] position_estimator_inav cosmetic changes --- .../position_estimator_inav_main.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) 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 306ebe5ccc..07ea7cd5cc 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -152,7 +152,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) warnx("started."); int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); + mavlink_log_info(mavlink_fd, "[inav] started"); /* initialize values */ float x_est[3] = { 0.0f, 0.0f, 0.0f }; @@ -241,8 +241,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) map_projection_init(lat_current, lon_current); /* publish global position messages only after first GPS message */ } - - warnx("initialized projection with: lat: %.10f, lon:%.10f", lat_current, lon_current); + warnx("initialized projection with: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] home: lat = %.10f, lon = %.10f", lat_current, lon_current); hrt_abstime t_prev = 0; thread_running = true; @@ -269,7 +269,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - printf("[position_estimator_inav] main loop started\n"); + warnx("main loop started."); while (!thread_should_exit) { bool accelerometer_updated = false; @@ -337,7 +337,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { baro_alt0 /= (float)(baro_loop_cnt); local_flag_baroINITdone = true; - mavlink_log_info(mavlink_fd, "[position_estimator_inav] baro_alt0 = %.2f", baro_alt0); + warnx("baro_alt0 = %.2f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] baro_alt0 = %.2f", baro_alt0); + pos.home_alt = baro_alt0; } } } @@ -428,7 +430,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; printf( - "[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", + "[inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", accel_updates / updates_dt, baro_updates / updates_dt, gps_updates / updates_dt, @@ -462,7 +464,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } warnx("exiting."); - mavlink_log_info(mavlink_fd, "[position_estimator_inav] exiting"); + mavlink_log_info(mavlink_fd, "[inav] exiting"); thread_running = false; return 0; } From e4b25f857016302fa254d41a964e586da49dd4b3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 13 Jun 2013 17:12:13 +0400 Subject: [PATCH 22/70] Default parameters updated for position_estimator_inav and multirotor_pos_control --- .../multirotor_pos_control/multirotor_pos_control_params.c | 4 ++-- .../position_estimator_inav_params.c | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 9610ef9f78..7f2ba3db8a 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -51,8 +51,8 @@ PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f); PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); -PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 3.0f); -PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.3f); +PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f); +PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { 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 8dd0ceff82..49dc7f51ff 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -43,9 +43,9 @@ PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 1.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); int parameters_init(struct position_estimator_inav_param_handles *h) { From 38ca3bd78a9b415df4a260a8cba43e2c13703972 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 15 Jun 2013 11:36:26 +0400 Subject: [PATCH 23/70] multirotor_pos_control fixes, introduced HARD control mode (disabled by default) --- .../multirotor_pos_control.c | 43 ++++++++++++------- .../multirotor_pos_control_params.c | 11 +++++ .../multirotor_pos_control_params.h | 10 +++++ 3 files changed, 49 insertions(+), 15 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index ad5670edca..c94972e7df 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -283,11 +283,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max; + float pos_sp_speed_x = 0.0f; + float pos_sp_speed_y = 0.0f; + float pos_sp_speed_z = 0.0f; + if (status.flag_control_manual_enabled) { /* move altitude setpoint with manual controls */ float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (alt_sp_ctl != 0.0f) { - local_pos_sp.z -= alt_sp_ctl * params.alt_rate_max * dt; + pos_sp_speed_z = -alt_sp_ctl * params.alt_rate_max; + local_pos_sp.z += pos_sp_speed_z * dt; if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) { local_pos_sp.z = local_pos.z + alt_err_linear_limit; } else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) { @@ -297,14 +302,16 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { /* move position setpoint with manual controls */ - float pos_x_sp_ctl = scale_control(-manual.pitch, 1.0f, pos_ctl_dz); - float pos_y_sp_ctl = scale_control(manual.roll, 1.0f, pos_ctl_dz); + float pos_x_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); + float pos_y_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl); - float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max * dt; - local_pos_sp.x += cosf(dir_sp_ctl) * d_sp_ctl; - local_pos_sp.y += sinf(dir_sp_ctl) * d_sp_ctl; + float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max; + pos_sp_speed_x = cosf(dir_sp_ctl) * d_sp_ctl; + pos_sp_speed_x = sinf(dir_sp_ctl) * d_sp_ctl; + local_pos_sp.x += pos_sp_speed_x * dt; + local_pos_sp.y += pos_sp_speed_y * dt; /* limit maximum setpoint from position offset and preserve direction */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; @@ -315,15 +322,19 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } } } + + if (params.hard == 0) { + pos_sp_speed_x = 0.0f; + pos_sp_speed_y = 0.0f; + pos_sp_speed_z = 0.0f; + } } /* PID for altitude */ /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - float alt_err = limit_value(local_pos.z - local_pos_sp.z, alt_err_linear_limit); + float alt_err = limit_value(local_pos_sp.z - local_pos.z, alt_err_linear_limit); /* P and D components */ - float thrust_ctl_pd = alt_err * params.alt_p + local_pos.vz * params.alt_d; - /* add I component */ - float thrust_ctl = thrust_ctl_pd + alt_integral; + float thrust_ctl_pd = -(alt_err * params.alt_p + (pos_sp_speed_z - local_pos.vz) * params.alt_d); // altitude = -z /* integrate */ alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; if (alt_integral < params.thr_min) { @@ -331,6 +342,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } else if (alt_integral > params.thr_max) { alt_integral = params.thr_max; } + /* add I component */ + float thrust_ctl = thrust_ctl_pd + alt_integral; if (thrust_ctl < params.thr_min) { thrust_ctl = params.thr_min; } else if (thrust_ctl > params.thr_max) { @@ -342,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit); float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit); /* P and D components */ - float pos_x_ctl_pd = - pos_x_err * params.pos_p - local_pos.vx * params.pos_d; - float pos_y_ctl_pd = - pos_y_err * params.pos_p - local_pos.vy * params.pos_d; - /* add I component */ - float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; - float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; + float pos_x_ctl_pd = - pos_x_err * params.pos_p + (pos_sp_speed_x - local_pos.vx) * params.pos_d; + float pos_y_ctl_pd = - pos_y_err * params.pos_p + (pos_sp_speed_y - local_pos.vy) * params.pos_d; /* integrate */ pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); + /* add I component */ + float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; + float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; /* calculate direction and slope in NED frame */ float dir = atan2f(pos_y_ctl, pos_x_ctl); /* use approximation: slope ~ sin(slope) = force */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 7f2ba3db8a..7c3296cae3 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -53,6 +53,7 @@ PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f); PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); +PARAM_DEFINE_INT32(MPC_HARD, 0); int parameters_init(struct multirotor_position_control_param_handles *h) { @@ -67,6 +68,11 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->pos_d = param_find("MPC_POS_D"); h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); h->slope_max = param_find("MPC_SLOPE_MAX"); + h->slope_max = param_find("MPC_HARD"); + + h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); + h->rc_scale_roll = param_find("RC_SCALE_ROLL"); + h->rc_scale_yaw = param_find("RC_SCALE_YAW"); return OK; } @@ -84,6 +90,11 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->pos_d, &(p->pos_d)); param_get(h->pos_rate_max, &(p->pos_rate_max)); param_get(h->slope_max, &(p->slope_max)); + param_get(h->hard, &(p->hard)); + + param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); + param_get(h->rc_scale_roll, &(p->rc_scale_roll)); + param_get(h->rc_scale_yaw, &(p->rc_scale_yaw)); return OK; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 589ee92a1d..13b667ad30 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -53,6 +53,11 @@ struct multirotor_position_control_params { float pos_d; float pos_rate_max; float slope_max; + int hard; + + float rc_scale_pitch; + float rc_scale_roll; + float rc_scale_yaw; }; struct multirotor_position_control_param_handles { @@ -67,6 +72,11 @@ struct multirotor_position_control_param_handles { param_t pos_d; param_t pos_rate_max; param_t slope_max; + param_t hard; + + param_t rc_scale_pitch; + param_t rc_scale_roll; + param_t rc_scale_yaw; }; /** From 650de90a904368eb93689bbb8a3be63888bf244e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 14:40:55 +0400 Subject: [PATCH 24/70] sdlog2: ARSP, GPOS messages added --- src/modules/sdlog2/sdlog2.c | 18 ++++++++++------ src/modules/sdlog2/sdlog2_messages.h | 31 +++++++++++++++++++++------- 2 files changed, 35 insertions(+), 14 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index e93b934c8b..65cfdc1884 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -608,12 +608,9 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* file descriptors to wait for */ - struct pollfd fds_control[2]; - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 16; + const ssize_t fdsc = 17; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -681,8 +678,9 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_STAT_s log_STAT; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; - struct log_ARSP_s log_ARSP; struct log_AIRS_s log_AIRS; + struct log_ARSP_s log_ARSP; + struct log_GPOS_s log_GPOS; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1053,7 +1051,15 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos); - // TODO not implemented yet + log_msg.msg_type = LOG_GPOS_MSG; + log_msg.body.log_GPOS.lat = buf.global_pos.lat; + log_msg.body.log_GPOS.lon = buf.global_pos.lon; + log_msg.body.log_GPOS.alt = buf.global_pos.alt; + log_msg.body.log_GPOS.vel_n = buf.global_pos.vx; + log_msg.body.log_GPOS.vel_e = buf.global_pos.vy; + log_msg.body.log_GPOS.vel_d = buf.global_pos.vz; + log_msg.body.log_GPOS.hdg = buf.global_pos.hdg; + LOGBUFFER_WRITE_AND_COUNT(GPOS); } /* --- VICON POSITION --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 5eeebcd95b..c47c388c4c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -171,20 +171,33 @@ struct log_OUT0_s { float output[8]; }; -/* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 13 -struct log_ARSP_s { - float roll_rate_sp; - float pitch_rate_sp; - float yaw_rate_sp; -}; - /* --- AIRS - AIRSPEED --- */ #define LOG_AIRS_MSG 13 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; }; + +/* --- ARSP - ATTITUDE RATE SET POINT --- */ +#define LOG_ARSP_MSG 14 +struct log_ARSP_s { + float roll_rate_sp; + float pitch_rate_sp; + float yaw_rate_sp; +}; + +/* --- GPOS - GLOBAL POSITION --- */ +#define LOG_GPOS_MSG 15 +struct log_GPOS_s { + int32_t lat; + int32_t lon; + float alt; + float vel_n; + float vel_e; + float vel_d; + float hdg; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -203,6 +216,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), + LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), + LOG_FORMAT(GPOS, "LLfffff", "Lat,Lon,Alt,VelN,VelE,VelD"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From a83aca753c2a5c8680c8a3b7258a1b2c25fbbce8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 14:41:35 +0400 Subject: [PATCH 25/70] position_estimator_inav rewrite, publishes vehicle_global_position now --- .../position_estimator_inav/inertial_filter.c | 10 +- .../position_estimator_inav/inertial_filter.h | 2 +- .../position_estimator_inav_main.c | 297 +++++++++--------- 3 files changed, 160 insertions(+), 149 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index b70d3504e6..8cdde5e1a5 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -13,16 +13,16 @@ void inertial_filter_predict(float dt, float x[3]) x[1] += x[2] * dt; } -void inertial_filter_correct(float dt, float x[3], int i, float z, float w) +void inertial_filter_correct(float edt, float x[3], int i, float w) { - float e = z - x[i]; - x[i] += e * w * dt; + float ewdt = w * edt; + x[i] += ewdt; if (i == 0) { - x[1] += e * w * w * dt; + x[1] += w * ewdt; //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 } else if (i == 1) { - x[2] += e * w * w * dt; + x[2] += w * ewdt; } } diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h index 18c194abf2..dca6375dce 100644 --- a/src/modules/position_estimator_inav/inertial_filter.h +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -10,4 +10,4 @@ void inertial_filter_predict(float dt, float x[3]); -void inertial_filter_correct(float dt, float x[3], int i, float z, float w); +void inertial_filter_correct(float edt, float x[3], int i, float w); 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 07ea7cd5cc..ac51a70106 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -57,6 +57,7 @@ #include #include #include +#include #include #include #include @@ -159,14 +160,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f }; - int baro_loop_cnt = 0; - int baro_loop_end = 70; /* measurement for 1 second */ + int baro_init_cnt = 0; + int baro_init_num = 70; /* measurement for 1 second */ float baro_alt0 = 0.0f; /* to determine while start up */ double lat_current = 0.0; //[°]] --> 47.0 double lon_current = 0.0; //[°]] -->8.5 double alt_current = 0.0; //[m] above MSL + uint32_t accel_counter = 0; + uint32_t baro_counter = 0; + /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); @@ -177,8 +181,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&gps, 0, sizeof(gps)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_local_position_s pos; - memset(&pos, 0, sizeof(pos)); + struct vehicle_local_position_s local_pos; + memset(&local_pos, 0, sizeof(local_pos)); + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -188,79 +194,96 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); /* advertise */ - orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &pos); + orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); + orb_advert_t vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); 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); - bool local_flag_baroINITdone = false; /* 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); - /* wait for GPS fix, only then can we initialize the projection */ - if (params.use_gps) { - struct pollfd fds_init[1] = { - { .fd = vehicle_gps_position_sub, .events = POLLIN } - }; + struct pollfd fds_init[2] = { + { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = vehicle_gps_position_sub, .events = POLLIN } + }; - while (gps.fix_type < 3) { - if (poll(fds_init, 1, 5000)) { - if (fds_init[0].revents & POLLIN) { - /* Wait for the GPS update to propagate (we have some time) */ - usleep(5000); - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + /* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */ + bool wait_gps = params.use_gps; + bool wait_baro = true; + + while (wait_gps || wait_baro) { + if (poll(fds_init, 2, 1000)) { + if (fds_init[0].revents & POLLIN) { + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + if (wait_baro && sensor.baro_counter > baro_counter) { + /* mean calculation over several measurements */ + if (baro_init_cnt < baro_init_num) { + baro_alt0 += sensor.baro_alt_meter; + baro_init_cnt++; + + } else { + wait_baro = false; + baro_alt0 /= (float) baro_init_cnt; + warnx("init baro: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); + local_pos.home_alt = baro_alt0; + local_pos.home_timestamp = hrt_absolute_time(); + } } } + if (fds_init[1].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (wait_gps && gps.fix_type >= 3) { + wait_gps = false; + /* get GPS position for first initialization */ + lat_current = gps.lat * 1e-7; + lon_current = gps.lon * 1e-7; + alt_current = gps.alt * 1e-3; - static int printcounter = 0; + local_pos.home_lat = lat_current * 1e7; + local_pos.home_lon = lon_current * 1e7; + local_pos.home_hdg = 0.0f; + local_pos.home_timestamp = hrt_absolute_time(); - if (printcounter == 100) { - printcounter = 0; - warnx("waiting for GPS fix type 3..."); + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + } } - - printcounter++; } - - /* get GPS position for first initialization */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - lat_current = ((double)(gps.lat)) * 1e-7; - lon_current = ((double)(gps.lon)) * 1e-7; - alt_current = gps.alt * 1e-3; - - pos.home_lat = lat_current * 1e7; - pos.home_lon = lon_current * 1e7; - pos.home_timestamp = hrt_absolute_time(); - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - /* publish global position messages only after first GPS message */ } - warnx("initialized projection with: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] home: lat = %.10f, lon = %.10f", lat_current, lon_current); hrt_abstime t_prev = 0; - thread_running = true; - uint32_t accel_counter = 0; + + uint16_t accel_updates = 0; hrt_abstime accel_t = 0; - float accel_dt = 0.0f; - uint32_t baro_counter = 0; + uint16_t baro_updates = 0; hrt_abstime baro_t = 0; hrt_abstime gps_t = 0; - uint16_t accel_updates = 0; - uint16_t baro_updates = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; + hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; + hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 4000; // limit publish rate to 250 Hz + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ + float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D + float baro_corr = 0.0f; // D + float gps_corr[2][2] = { + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) + }; + /* main loop */ struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, @@ -269,14 +292,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = sensor_combined_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN } }; + + thread_running = true; warnx("main loop started."); while (!thread_should_exit) { - bool accelerometer_updated = false; - bool baro_updated = false; - bool gps_updated = false; - float proj_pos_gps[3] = { 0.0f, 0.0f, 0.0f }; - int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); @@ -314,34 +334,31 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); if (sensor.accelerometer_counter > accel_counter) { - accelerometer_updated = true; + if (att.R_valid) { + /* transform acceleration vector from body frame to NED frame */ + float accel_NED[3]; + for (int i = 0; i < 3; i++) { + accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; + } + } + accel_NED[2] += CONSTANTS_ONE_G; + accel_corr[0] = accel_NED[0] - x_est[2]; + accel_corr[1] = accel_NED[1] - y_est[2]; + accel_corr[2] = accel_NED[2] - z_est[2]; + } else { + memset(accel_corr, 0, sizeof(accel_corr)); + } accel_counter = sensor.accelerometer_counter; accel_updates++; - accel_dt = accel_t > 0 ? (t - accel_t) / 1000000.0f : 0.0f; - accel_t = t; } if (sensor.baro_counter > baro_counter) { - baro_updated = true; + baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[2]; baro_counter = sensor.baro_counter; baro_updates++; } - - /* barometric pressure estimation at start up */ - if (!local_flag_baroINITdone && baro_updated) { - /* mean calculation over several measurements */ - if (baro_loop_cnt < baro_loop_end) { - baro_alt0 += sensor.baro_alt_meter; - baro_loop_cnt++; - - } else { - baro_alt0 /= (float)(baro_loop_cnt); - local_flag_baroINITdone = true; - warnx("baro_alt0 = %.2f", baro_alt0); - mavlink_log_info(mavlink_fd, "[inav] baro_alt0 = %.2f", baro_alt0); - pos.home_alt = baro_alt0; - } - } } if (params.use_gps) { @@ -349,80 +366,56 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[4].revents & POLLIN) { /* new GPS value */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - /* project GPS lat lon (Geographic coordinate system) to plane */ - map_projection_project(((double)(gps.lat)) * 1e-7, - ((double)(gps.lon)) * 1e-7, &(proj_pos_gps[0]), - &(proj_pos_gps[1])); - proj_pos_gps[2] = (float)(gps.alt * 1e-3); - gps_updated = true; - pos.valid = gps.fix_type >= 3; - gps_updates++; + if (gps.fix_type >= 3) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s; + gps_corr[1][1] = gps.vel_e_m_s; + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } + local_pos.valid = true; + gps_updates++; + } else { + local_pos.valid = false; + } } - } else { - pos.valid = true; + local_pos.valid = true; } } /* end of poll return value check */ - float dt = t_prev > 0 ? (t - t_prev) / 1000000.0 : 0.0f; + float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; - if (att.R_valid) { - /* transform acceleration vector from UAV frame to NED frame */ - float accel_NED[3]; + /* inertial filter prediction for altitude */ + inertial_filter_predict(dt, z_est); - for (int i = 0; i < 3; i++) { - accel_NED[i] = 0.0f; + /* inertial filter correction for altitude */ + inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc); - for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; - } - } + if (params.use_gps) { + /* inertial filter prediction for position */ + inertial_filter_predict(dt, x_est); + inertial_filter_predict(dt, y_est); - accel_NED[2] += CONSTANTS_ONE_G; + /* inertial filter correction for position */ + inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); - /* inertial filter prediction for altitude */ - inertial_filter_predict(dt, z_est); - - /* inertial filter correction for altitude */ - if (local_flag_baroINITdone && baro_updated) { - if (baro_t > 0) { - inertial_filter_correct((t - baro_t) / 1000000.0f, z_est, 0, baro_alt0 - sensor.baro_alt_meter, params.w_alt_baro); - } - - baro_t = t; - } - - if (accelerometer_updated) { - inertial_filter_correct(accel_dt, z_est, 2, accel_NED[2], params.w_alt_acc); - } - - if (params.use_gps) { - /* inertial filter prediction for position */ - inertial_filter_predict(dt, x_est); - inertial_filter_predict(dt, y_est); - - /* inertial filter correction for position */ - if (gps_updated) { - if (gps_t > 0) { - float gps_dt = (t - gps_t) / 1000000.0f; - inertial_filter_correct(gps_dt, x_est, 0, proj_pos_gps[0], params.w_pos_gps_p); - inertial_filter_correct(gps_dt, x_est, 1, gps.vel_n_m_s, params.w_pos_gps_v); - inertial_filter_correct(gps_dt, y_est, 0, proj_pos_gps[1], params.w_pos_gps_p); - inertial_filter_correct(gps_dt, y_est, 1, gps.vel_e_m_s, params.w_pos_gps_v); - } - - gps_t = t; - } - - if (accelerometer_updated) { - inertial_filter_correct(accel_dt, x_est, 2, accel_NED[0], params.w_pos_acc); - inertial_filter_correct(accel_dt, y_est, 2, accel_NED[1], params.w_pos_acc); - } - } + inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc); } if (verbose_mode) { @@ -445,20 +438,38 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - pub_last > pub_interval) { pub_last = t; - pos.x = x_est[0]; - pos.vx = x_est[1]; - pos.y = y_est[0]; - pos.vy = y_est[1]; - pos.z = z_est[0]; - pos.vz = z_est[1]; - pos.timestamp = hrt_absolute_time(); + local_pos.timestamp = t; + local_pos.x = x_est[0]; + local_pos.vx = x_est[1]; + local_pos.y = y_est[0]; + local_pos.vy = y_est[1]; + local_pos.z = z_est[0]; + local_pos.vz = z_est[1]; + local_pos.absolute_alt = local_pos.home_alt - local_pos.z; + local_pos.hdg = att.yaw; - if ((isfinite(pos.x)) && (isfinite(pos.vx)) - && (isfinite(pos.y)) - && (isfinite(pos.vy)) - && (isfinite(pos.z)) - && (isfinite(pos.vz))) { - orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &pos); + if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx)) + && (isfinite(local_pos.y)) + && (isfinite(local_pos.vy)) + && (isfinite(local_pos.z)) + && (isfinite(local_pos.vz))) { + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); + + if (params.use_gps) { + global_pos.valid = local_pos.valid; + double est_lat, est_lon; + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = (int32_t) (est_lat * 1e7); + global_pos.lon = (int32_t) (est_lon * 1e7); + global_pos.alt = -local_pos.z - local_pos.home_alt; + global_pos.relative_alt = -local_pos.z; + global_pos.vx = local_pos.vx; + global_pos.vy = local_pos.vy; + global_pos.vz = local_pos.vz; + global_pos.hdg = local_pos.hdg; + + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); + } } } } From 2124c52cff152e696a73888a7b16556d3c882ec1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 22:06:45 +0400 Subject: [PATCH 26/70] position_estimator_inav bugfixes --- src/modules/position_estimator_inav/inertial_filter.c | 2 +- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index 8cdde5e1a5..acaf693f17 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -20,7 +20,7 @@ void inertial_filter_correct(float edt, float x[3], int i, float w) if (i == 0) { x[1] += w * ewdt; - //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 + x[2] += w * w * ewdt / 3.0; } else if (i == 1) { x[2] += w * ewdt; 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 ac51a70106..a8a66e93d8 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -355,7 +355,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[2]; + baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } From effce6edfa3b4258a855342d8f7a265f2f18f521 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Jun 2013 22:07:05 +0400 Subject: [PATCH 27/70] sdlog2 GPOS message bug fix --- src/modules/sdlog2/sdlog2_messages.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index b42f11e61d..55bc367174 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -218,7 +218,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), - LOG_FORMAT(GPOS, "LLfffff", "Lat,Lon,Alt,VelN,VelE,VelD"), + LOG_FORMAT(GPOS, "LLfffff", "Lat,Lon,Alt,VelN,VelE,VelD,Heading"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From eb38ea17896e9970e9bdf532557ebcd87f81e34a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 19 Jun 2013 12:17:10 +0400 Subject: [PATCH 28/70] position_estimator_inav: better handling of lost GPS, minor fixes --- .../position_estimator_inav_main.c | 60 +++++++++---------- 1 file changed, 30 insertions(+), 30 deletions(-) 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 a8a66e93d8..278a319b51 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -254,7 +254,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* initialize coordinates */ map_projection_init(lat_current, lon_current); warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); } } } @@ -361,34 +361,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (params.use_gps) { + if (params.use_gps && fds[4].revents & POLLIN) { /* vehicle GPS position */ - if (fds[4].revents & POLLIN) { - /* new GPS value */ - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s; - gps_corr[1][1] = gps.vel_e_m_s; - } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; - } - local_pos.valid = true; - gps_updates++; + orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (gps.fix_type >= 3) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s; + gps_corr[1][1] = gps.vel_e_m_s; } else { - local_pos.valid = false; + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; } + gps_updates++; + } else { + memset(gps_corr, 0, sizeof(gps_corr)); } - } else { - local_pos.valid = true; } - } /* end of poll return value check */ @@ -403,19 +396,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro); inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc); - if (params.use_gps) { + /* dont't try to estimate position when no any position source available */ + bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + if (can_estimate_pos) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); /* inertial filter correction for position */ - inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); - inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc); + + if (params.use_gps && gps.fix_type >= 3) { + inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid) { + inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); + } + } } if (verbose_mode) { @@ -439,6 +438,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - pub_last > pub_interval) { pub_last = t; local_pos.timestamp = t; + local_pos.valid = can_estimate_pos; local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; From 7d37c2a8b36309f27d7001ba035d30c72620ba05 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 17:32:29 +0400 Subject: [PATCH 29/70] multirotor_pos_control: bugs fixed --- .../multirotor_pos_control.c | 14 +++++++------- .../multirotor_pos_control_params.c | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index c94972e7df..508879ae2e 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -302,14 +302,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE) { /* move position setpoint with manual controls */ - float pos_x_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); - float pos_y_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); - if (pos_x_sp_ctl != 0.0f || pos_y_sp_ctl != 0.0f) { + float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); + float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); + if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ - float dir_sp_ctl = att.yaw + atan2f(pos_y_sp_ctl, pos_x_sp_ctl); - float d_sp_ctl = norm(pos_x_sp_ctl, pos_y_sp_ctl) * params.pos_rate_max; - pos_sp_speed_x = cosf(dir_sp_ctl) * d_sp_ctl; - pos_sp_speed_x = sinf(dir_sp_ctl) * d_sp_ctl; + float pos_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float pos_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.pos_rate_max; + pos_sp_speed_x = cosf(pos_sp_ctl_dir) * pos_sp_ctl_speed; + pos_sp_speed_y = sinf(pos_sp_ctl_dir) * pos_sp_ctl_speed; local_pos_sp.x += pos_sp_speed_x * dt; local_pos_sp.y += pos_sp_speed_y * dt; /* limit maximum setpoint from position offset and preserve direction */ diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index 7c3296cae3..d284de433c 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -68,7 +68,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->pos_d = param_find("MPC_POS_D"); h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); h->slope_max = param_find("MPC_SLOPE_MAX"); - h->slope_max = param_find("MPC_HARD"); + h->hard = param_find("MPC_HARD"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); From 1759f30e3aa4b2da25dcd0c836480f069d917a88 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Jun 2013 11:10:46 +0400 Subject: [PATCH 30/70] Sonar added to position_estimator_inav --- .../position_estimator_inav/inertial_filter.c | 7 +- .../position_estimator_inav/inertial_filter.h | 2 +- .../position_estimator_inav_main.c | 79 +++++++++++++++---- .../position_estimator_inav_params.c | 15 ++++ .../position_estimator_inav_params.h | 10 +++ 5 files changed, 93 insertions(+), 20 deletions(-) diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index acaf693f17..13328edb4b 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -13,9 +13,12 @@ void inertial_filter_predict(float dt, float x[3]) x[1] += x[2] * dt; } -void inertial_filter_correct(float edt, float x[3], int i, float w) +void inertial_filter_correct(float e, float dt, float x[3], int i, float w) { - float ewdt = w * edt; + float ewdt = w * dt; + if (ewdt > 1.0f) + ewdt = 1.0f; // prevent over-correcting + ewdt *= e; x[i] += ewdt; if (i == 0) { diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h index dca6375dce..761c17097d 100644 --- a/src/modules/position_estimator_inav/inertial_filter.h +++ b/src/modules/position_estimator_inav/inertial_filter.h @@ -10,4 +10,4 @@ void inertial_filter_predict(float dt, float x[3]); -void inertial_filter_correct(float edt, float x[3], int i, float w); +void inertial_filter_correct(float e, float dt, float x[3], int i, float w); 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 278a319b51..fb09948ec4 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -59,6 +59,7 @@ #include #include #include +#include #include #include #include @@ -183,6 +184,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&att, 0, sizeof(att)); struct vehicle_local_position_s local_pos; memset(&local_pos, 0, sizeof(local_pos)); + struct optical_flow_s flow; + memset(&flow, 0, sizeof(flow)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); @@ -191,6 +194,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow)); int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); /* advertise */ @@ -263,12 +267,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime t_prev = 0; uint16_t accel_updates = 0; - hrt_abstime accel_t = 0; uint16_t baro_updates = 0; - hrt_abstime baro_t = 0; - hrt_abstime gps_t = 0; uint16_t gps_updates = 0; uint16_t attitude_updates = 0; + uint16_t flow_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); uint32_t updates_counter_len = 1000000; @@ -283,6 +285,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { 0.0f, 0.0f }, // N (pos, vel) { 0.0f, 0.0f }, // E (pos, vel) }; + float sonar_corr = 0.0f; + float sonar_corr_filtered = 0.0f; + float flow_corr[] = { 0.0f, 0.0f }; // X, Y + + float sonar_prev = 0.0f; + hrt_abstime sonar_time = 0; /* main loop */ struct pollfd fds[5] = { @@ -290,6 +298,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, { .fd = sensor_combined_sub, .events = POLLIN }, + { .fd = optical_flow_sub, .events = POLLIN }, { .fd = vehicle_gps_position_sub, .events = POLLIN } }; @@ -297,7 +306,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) warnx("main loop started."); while (!thread_should_exit) { - int ret = poll(fds, params.use_gps ? 5 : 4, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -355,13 +364,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (sensor.baro_counter > baro_counter) { - baro_corr = baro_alt0 - sensor.baro_alt_meter - z_est[0]; + baro_corr = - sensor.baro_alt_meter - z_est[0]; baro_counter = sensor.baro_counter; baro_updates++; } } - if (params.use_gps && fds[4].revents & POLLIN) { + /* optical flow */ + if (fds[4].revents & POLLIN) { + orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { + if (flow.ground_distance_m != sonar_prev) { + sonar_time = t; + sonar_prev = flow.ground_distance_m; + sonar_corr = -flow.ground_distance_m - z_est[0]; + sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + if (fabsf(sonar_corr) > params.sonar_err) { + // correction is too large: spike or new ground level? + if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { + // spike detected, ignore + sonar_corr = 0.0f; + } else { + // new ground level + baro_alt0 += sonar_corr; + warnx("new home: alt = %.3f", baro_alt0); + mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); + local_pos.home_alt = baro_alt0; + local_pos.home_timestamp = hrt_absolute_time(); + sonar_corr = 0.0f; + sonar_corr_filtered = 0.0; + } + } + } + } else { + sonar_corr = 0.0f; + } + flow_updates++; + } + + if (params.use_gps && fds[5].revents & POLLIN) { /* vehicle GPS position */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); if (gps.fix_type >= 3) { @@ -393,8 +434,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, z_est); /* inertial filter correction for altitude */ - inertial_filter_correct(baro_corr * dt, z_est, 0, params.w_alt_baro); - inertial_filter_correct(accel_corr[2] * dt, z_est, 2, params.w_alt_acc); + baro_alt0 += sonar_corr * params.w_alt_sonar * dt; + inertial_filter_correct(baro_corr + baro_alt0, dt, z_est, 0, params.w_alt_baro); + inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); + inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); /* dont't try to estimate position when no any position source available */ bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; @@ -404,15 +447,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_predict(dt, y_est); /* inertial filter correction for position */ - inertial_filter_correct(accel_corr[0] * dt, x_est, 2, params.w_pos_acc); - inertial_filter_correct(accel_corr[1] * dt, y_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); + inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); if (params.use_gps && gps.fix_type >= 3) { - inertial_filter_correct(gps_corr[0][0] * dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0] * dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); if (gps.vel_ned_valid) { - inertial_filter_correct(gps_corr[0][1] * dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1] * dt, y_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); } } } @@ -421,17 +464,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* print updates rate */ if (t - updates_counter_start > updates_counter_len) { float updates_dt = (t - updates_counter_start) * 0.000001f; - printf( - "[inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n", + warnx( + "updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s, flow = %.1f/s", accel_updates / updates_dt, baro_updates / updates_dt, gps_updates / updates_dt, - attitude_updates / updates_dt); + attitude_updates / updates_dt, + flow_updates / updates_dt); updates_counter_start = t; accel_updates = 0; baro_updates = 0; gps_updates = 0; attitude_updates = 0; + flow_updates = 0; } } 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 49dc7f51ff..eac2fc1ce9 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -43,18 +43,28 @@ PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); +PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f); +PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.5f); +PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); h->w_alt_baro = param_find("INAV_W_ALT_BARO"); h->w_alt_acc = param_find("INAV_W_ALT_ACC"); + h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P"); h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); h->w_pos_acc = param_find("INAV_W_POS_ACC"); + h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->flow_k = param_find("INAV_FLOW_K"); + h->sonar_filt = param_find("INAV_SONAR_FILT"); + h->sonar_err = param_find("INAV_SONAR_ERR"); return OK; } @@ -64,9 +74,14 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->use_gps, &(p->use_gps)); param_get(h->w_alt_baro, &(p->w_alt_baro)); param_get(h->w_alt_acc, &(p->w_alt_acc)); + param_get(h->w_alt_sonar, &(p->w_alt_sonar)); param_get(h->w_pos_gps_p, &(p->w_pos_gps_p)); param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); param_get(h->w_pos_acc, &(p->w_pos_acc)); + param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->flow_k, &(p->flow_k)); + param_get(h->sonar_filt, &(p->sonar_filt)); + param_get(h->sonar_err, &(p->sonar_err)); return OK; } 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 870227fefc..cca172b5d6 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -44,18 +44,28 @@ struct position_estimator_inav_params { int use_gps; float w_alt_baro; float w_alt_acc; + float w_alt_sonar; float w_pos_gps_p; float w_pos_gps_v; float w_pos_acc; + float w_pos_flow; + float flow_k; + float sonar_filt; + float sonar_err; }; struct position_estimator_inav_param_handles { param_t use_gps; param_t w_alt_baro; param_t w_alt_acc; + param_t w_alt_sonar; param_t w_pos_gps_p; param_t w_pos_gps_v; param_t w_pos_acc; + param_t w_pos_flow; + param_t flow_k; + param_t sonar_filt; + param_t sonar_err; }; /** From 8e732fc52763a05739e4f13caf0dff84817839cd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Jun 2013 12:58:12 +0400 Subject: [PATCH 31/70] position_estimator_inav default parameters changed, some fixes --- .../multirotor_pos_control/multirotor_pos_control.c | 10 ++++++++++ .../position_estimator_inav_main.c | 1 + .../position_estimator_inav_params.c | 2 +- 3 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 508879ae2e..3a14d516a6 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -209,6 +209,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_y_integral = 0.0f; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; + float home_alt = 0.0f; + hrt_abstime home_alt_t = 0; thread_running = true; @@ -288,6 +290,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_sp_speed_z = 0.0f; if (status.flag_control_manual_enabled) { + if (local_pos.home_timestamp != home_alt_t) { + if (home_alt_t != 0) { + /* home alt changed, don't follow large ground level changes in manual flight */ + local_pos_sp.z -= local_pos.home_alt - home_alt; + } + home_alt_t = local_pos.home_timestamp; + home_alt = local_pos.home_alt; + } /* move altitude setpoint with manual controls */ float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); if (alt_sp_ctl != 0.0f) { 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 fb09948ec4..428269e4aa 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -391,6 +391,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); local_pos.home_alt = baro_alt0; local_pos.home_timestamp = hrt_absolute_time(); + z_est[0] += sonar_corr; sonar_corr = 0.0f; sonar_corr_filtered = 0.0; } 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 eac2fc1ce9..c90c611a77 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -49,7 +49,7 @@ PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.5f); +PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); int parameters_init(struct position_estimator_inav_param_handles *h) From 7dfaed3ee7718c15731070f3e3bd54d725f72029 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 28 Jun 2013 23:35:48 +0400 Subject: [PATCH 32/70] multirotor_pos_control: new ground level -> altitude setpoint correction fixed --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3a14d516a6..cb6afa1cb4 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -293,7 +293,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (local_pos.home_timestamp != home_alt_t) { if (home_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z -= local_pos.home_alt - home_alt; + local_pos_sp.z += local_pos.home_alt - home_alt; } home_alt_t = local_pos.home_timestamp; home_alt = local_pos.home_alt; From 843fb2d37179b82601a51e2d210052318f3301ab Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Jul 2013 09:29:06 +0400 Subject: [PATCH 33/70] position_estimator_inav init bugs fixed --- .../position_estimator_inav_main.c | 91 ++++++++++++++----- 1 file changed, 66 insertions(+), 25 deletions(-) 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 428269e4aa..fb5a779bc0 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -62,6 +62,7 @@ #include #include #include +#include #include #include #include @@ -162,7 +163,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float z_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_init_cnt = 0; - int baro_init_num = 70; /* measurement for 1 second */ + int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ double lat_current = 0.0; //[°]] --> 47.0 @@ -220,12 +221,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */ bool wait_gps = params.use_gps; bool wait_baro = true; + hrt_abstime wait_gps_start = 0; + const hrt_abstime wait_gps_delay = 5000000; // wait for 5s after 3D fix - while (wait_gps || wait_baro) { - if (poll(fds_init, 2, 1000)) { + thread_running = true; + + while ((wait_gps || wait_baro) && !thread_should_exit) { + int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000); + + if (ret < 0) { + /* poll error */ + errx(1, "subscriptions poll error on init."); + + } else if (ret > 0) { if (fds_init[0].revents & POLLIN) { orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + if (wait_baro && sensor.baro_counter > baro_counter) { + baro_counter = sensor.baro_counter; + /* mean calculation over several measurements */ if (baro_init_cnt < baro_init_num) { baro_alt0 += sensor.baro_alt_meter; @@ -241,24 +255,33 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } } - if (fds_init[1].revents & POLLIN) { + + if (params.use_gps && (fds_init[1].revents & POLLIN)) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (wait_gps && gps.fix_type >= 3) { - wait_gps = false; - /* get GPS position for first initialization */ - lat_current = gps.lat * 1e-7; - lon_current = gps.lon * 1e-7; - alt_current = gps.alt * 1e-3; + hrt_abstime t = hrt_absolute_time(); - local_pos.home_lat = lat_current * 1e7; - local_pos.home_lon = lon_current * 1e7; - local_pos.home_hdg = 0.0f; - local_pos.home_timestamp = hrt_absolute_time(); + if (wait_gps_start == 0) { + wait_gps_start = t; - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); + } else if (t - wait_gps_start > wait_gps_delay) { + wait_gps = false; + /* get GPS position for first initialization */ + lat_current = gps.lat * 1e-7; + lon_current = gps.lon * 1e-7; + alt_current = gps.alt * 1e-3; + + local_pos.home_lat = lat_current * 1e7; + local_pos.home_lon = lon_current * 1e7; + local_pos.home_hdg = 0.0f; + local_pos.home_timestamp = t; + + /* initialize coordinates */ + map_projection_init(lat_current, lon_current); + warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); + } } } } @@ -282,8 +305,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D float baro_corr = 0.0f; // D float gps_corr[2][2] = { - { 0.0f, 0.0f }, // N (pos, vel) - { 0.0f, 0.0f }, // E (pos, vel) + { 0.0f, 0.0f }, // N (pos, vel) + { 0.0f, 0.0f }, // E (pos, vel) }; float sonar_corr = 0.0f; float sonar_corr_filtered = 0.0f; @@ -293,7 +316,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime sonar_time = 0; /* main loop */ - struct pollfd fds[5] = { + struct pollfd fds[6] = { { .fd = parameter_update_sub, .events = POLLIN }, { .fd = vehicle_status_sub, .events = POLLIN }, { .fd = vehicle_attitude_sub, .events = POLLIN }, @@ -302,8 +325,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - thread_running = true; - warnx("main loop started."); + if (!thread_should_exit) { + warnx("main loop started."); + } while (!thread_should_exit) { int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate @@ -346,19 +370,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (att.R_valid) { /* transform acceleration vector from body frame to NED frame */ float accel_NED[3]; + for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; + for (int j = 0; j < 3; j++) { accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; accel_corr[0] = accel_NED[0] - x_est[2]; accel_corr[1] = accel_NED[1] - y_est[2]; accel_corr[2] = accel_NED[2] - z_est[2]; + } else { memset(accel_corr, 0, sizeof(accel_corr)); } + accel_counter = sensor.accelerometer_counter; accel_updates++; } @@ -373,17 +402,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* optical flow */ if (fds[4].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && (flow.ground_distance_m != sonar_prev || t - sonar_time < 150000)) { if (flow.ground_distance_m != sonar_prev) { sonar_time = t; sonar_prev = flow.ground_distance_m; sonar_corr = -flow.ground_distance_m - z_est[0]; sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt; + if (fabsf(sonar_corr) > params.sonar_err) { // correction is too large: spike or new ground level? if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) { // spike detected, ignore sonar_corr = 0.0f; + } else { // new ground level baro_alt0 += sonar_corr; @@ -397,29 +429,36 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } } + } else { sonar_corr = 0.0f; } + flow_updates++; } - if (params.use_gps && fds[5].revents & POLLIN) { + if (params.use_gps && (fds[5].revents & POLLIN)) { /* vehicle GPS position */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); + if (gps.fix_type >= 3) { /* project GPS lat lon to plane */ float gps_proj[2]; map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); gps_corr[0][0] = gps_proj[0] - x_est[0]; gps_corr[1][0] = gps_proj[1] - y_est[0]; + if (gps.vel_ned_valid) { gps_corr[0][1] = gps.vel_n_m_s; gps_corr[1][1] = gps.vel_e_m_s; + } else { gps_corr[0][1] = 0.0f; gps_corr[1][1] = 0.0f; } + gps_updates++; + } else { memset(gps_corr, 0, sizeof(gps_corr)); } @@ -442,6 +481,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* dont't try to estimate position when no any position source available */ bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + if (can_estimate_pos) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); @@ -454,6 +494,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (params.use_gps && gps.fix_type >= 3) { inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid) { inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); @@ -505,8 +546,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) global_pos.valid = local_pos.valid; double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - global_pos.lat = (int32_t) (est_lat * 1e7); - global_pos.lon = (int32_t) (est_lon * 1e7); + global_pos.lat = (int32_t)(est_lat * 1e7); + global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.alt = -local_pos.z - local_pos.home_alt; global_pos.relative_alt = -local_pos.z; global_pos.vx = local_pos.vx; From 04fbed493aab1dede6c2200aaab2b990d24a66e7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 9 Jul 2013 14:09:48 +0400 Subject: [PATCH 34/70] multirotor_pos_control: use separate PID controllers for position and velocity --- src/modules/multirotor_pos_control/module.mk | 3 +- .../multirotor_pos_control.c | 197 ++++++++++-------- .../multirotor_pos_control_params.c | 63 +++--- .../multirotor_pos_control_params.h | 42 ++-- .../multirotor_pos_control/thrust_pid.c | 179 ++++++++++++++++ .../multirotor_pos_control/thrust_pid.h | 75 +++++++ .../position_estimator_inav_main.c | 2 +- 7 files changed, 423 insertions(+), 138 deletions(-) create mode 100644 src/modules/multirotor_pos_control/thrust_pid.c create mode 100644 src/modules/multirotor_pos_control/thrust_pid.h diff --git a/src/modules/multirotor_pos_control/module.mk b/src/modules/multirotor_pos_control/module.mk index d048477455..bc4b48fb40 100644 --- a/src/modules/multirotor_pos_control/module.mk +++ b/src/modules/multirotor_pos_control/module.mk @@ -38,4 +38,5 @@ MODULE_COMMAND = multirotor_pos_control SRCS = multirotor_pos_control.c \ - multirotor_pos_control_params.c + multirotor_pos_control_params.c \ + thrust_pid.c diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index acae03fae0..d56c3d58f8 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -61,9 +61,11 @@ #include #include #include +#include #include #include "multirotor_pos_control_params.h" +#include "thrust_pid.h" static bool thread_should_exit = false; /**< Deamon exit flag */ @@ -84,8 +86,6 @@ static void usage(const char *reason); static float scale_control(float ctl, float end, float dz); -static float limit_value(float v, float limit); - static float norm(float x, float y); static void usage(const char *reason) { @@ -110,11 +110,12 @@ int multirotor_pos_control_main(int argc, char *argv[]) { if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("multirotor_pos_control already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } + warnx("start"); thread_should_exit = false; deamon_task = task_spawn_cmd("multirotor_pos_control", SCHED_DEFAULT, @@ -126,15 +127,16 @@ int multirotor_pos_control_main(int argc, char *argv[]) { } if (!strcmp(argv[1], "stop")) { + warnx("stop"); thread_should_exit = true; exit(0); } if (!strcmp(argv[1], "status")) { if (thread_running) { - printf("\tmultirotor_pos_control app is running\n"); + warnx("app is running"); } else { - printf("\tmultirotor_pos_control app not started\n"); + warnx("app not started"); } exit(0); } @@ -153,22 +155,13 @@ static float scale_control(float ctl, float end, float dz) { } } -static float limit_value(float v, float limit) { - if (v > limit) { - v = limit; - } else if (v < -limit) { - v = -limit; - } - return v; -} - static float norm(float x, float y) { return sqrtf(x * x + y * y); } static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* welcome user */ - warnx("started."); + warnx("started"); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); @@ -203,15 +196,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { bool reset_sp_alt = true; bool reset_sp_pos = true; hrt_abstime t_prev = 0; - float alt_integral = 0.0f; /* integrate in NED frame to estimate wind but not attitude offset */ - float pos_x_integral = 0.0f; - float pos_y_integral = 0.0f; const float alt_ctl_dz = 0.2f; const float pos_ctl_dz = 0.05f; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; + static PID_t xy_pos_pids[2]; + static PID_t xy_vel_pids[2]; + static PID_t z_pos_pid; + static thrust_pid_t z_vel_pid; + thread_running = true; struct multirotor_position_control_params params; @@ -219,6 +214,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { parameters_init(¶ms_h); parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + } + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + int paramcheck_counter = 0; while (!thread_should_exit) { @@ -231,6 +233,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { orb_check(param_sub, ¶m_updated); if (param_updated) { parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max); + } + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); + thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } paramcheck_counter = 0; } @@ -269,7 +277,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { if (reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; - alt_integral = manual.throttle; + z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } @@ -277,18 +285,17 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - pos_x_integral = 0.0f; - pos_y_integral = 0.0f; + xy_vel_pids[0].integral = 0.0f; + xy_vel_pids[1].integral = 0.0f; mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } - float alt_err_linear_limit = params.alt_d / params.alt_p * params.alt_rate_max; - float pos_err_linear_limit = params.pos_d / params.pos_p * params.pos_rate_max; + float z_sp_offs_max = params.z_vel_max / params.z_p * 2.0f; + float xy_sp_offs_max = params.xy_vel_max / params.xy_p * 2.0f; - float pos_sp_speed_x = 0.0f; - float pos_sp_speed_y = 0.0f; - float pos_sp_speed_z = 0.0f; + float sp_move_rate[3] = { 0.0f, 0.0f, 0.0f }; + /* manual control */ if (status.flag_control_manual_enabled) { if (local_pos.home_timestamp != home_alt_t) { if (home_alt_t != 0) { @@ -299,14 +306,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { home_alt = local_pos.home_alt; } /* move altitude setpoint with manual controls */ - float alt_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); - if (alt_sp_ctl != 0.0f) { - pos_sp_speed_z = -alt_sp_ctl * params.alt_rate_max; - local_pos_sp.z += pos_sp_speed_z * dt; - if (local_pos_sp.z > local_pos.z + alt_err_linear_limit) { - local_pos_sp.z = local_pos.z + alt_err_linear_limit; - } else if (local_pos_sp.z < local_pos.z - alt_err_linear_limit) { - local_pos_sp.z = local_pos.z - alt_err_linear_limit; + float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + if (z_sp_ctl != 0.0f) { + sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; + local_pos_sp.z += sp_move_rate[2] * dt; + if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { + local_pos_sp.z = local_pos.z + z_sp_offs_max; + } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { + local_pos_sp.z = local_pos.z - z_sp_offs_max; } } @@ -316,76 +323,84 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ - float pos_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); - float pos_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.pos_rate_max; - pos_sp_speed_x = cosf(pos_sp_ctl_dir) * pos_sp_ctl_speed; - pos_sp_speed_y = sinf(pos_sp_ctl_dir) * pos_sp_ctl_speed; - local_pos_sp.x += pos_sp_speed_x * dt; - local_pos_sp.y += pos_sp_speed_y * dt; + float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); + float xy_sp_ctl_speed = norm(pos_pitch_sp_ctl, pos_roll_sp_ctl) * params.xy_vel_max; + sp_move_rate[0] = cosf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; + local_pos_sp.x += sp_move_rate[0] * dt; + local_pos_sp.y += sp_move_rate[1] * dt; /* limit maximum setpoint from position offset and preserve direction */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; - float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / pos_err_linear_limit; + float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; if (pos_vec_norm > 1.0f) { local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; } } } - - if (params.hard == 0) { - pos_sp_speed_x = 0.0f; - pos_sp_speed_y = 0.0f; - pos_sp_speed_z = 0.0f; - } } - /* PID for altitude */ - /* don't accelerate more than ALT_RATE_MAX, limit error to corresponding value */ - float alt_err = limit_value(local_pos_sp.z - local_pos.z, alt_err_linear_limit); - /* P and D components */ - float thrust_ctl_pd = -(alt_err * params.alt_p + (pos_sp_speed_z - local_pos.vz) * params.alt_d); // altitude = -z - /* integrate */ - alt_integral += thrust_ctl_pd / params.alt_p * params.alt_i * dt; - if (alt_integral < params.thr_min) { - alt_integral = params.thr_min; - } else if (alt_integral > params.thr_max) { - alt_integral = params.thr_max; - } - /* add I component */ - float thrust_ctl = thrust_ctl_pd + alt_integral; - if (thrust_ctl < params.thr_min) { - thrust_ctl = params.thr_min; - } else if (thrust_ctl > params.thr_max) { - thrust_ctl = params.thr_max; - } + /* run position & altitude controllers, calculate velocity setpoint */ + float vel_sp[3] = { 0.0f, 0.0f, 0.0f }; + vel_sp[2] = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { - /* PID for position */ - /* don't accelerate more than POS_RATE_MAX, limit error to corresponding value */ - float pos_x_err = limit_value(local_pos.x - local_pos_sp.x, pos_err_linear_limit); - float pos_y_err = limit_value(local_pos.y - local_pos_sp.y, pos_err_linear_limit); - /* P and D components */ - float pos_x_ctl_pd = - pos_x_err * params.pos_p + (pos_sp_speed_x - local_pos.vx) * params.pos_d; - float pos_y_ctl_pd = - pos_y_err * params.pos_p + (pos_sp_speed_y - local_pos.vy) * params.pos_d; - /* integrate */ - pos_x_integral = limit_value(pos_x_integral + pos_x_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); - pos_y_integral = limit_value(pos_y_integral + pos_y_ctl_pd / params.pos_p * params.pos_i * dt, params.slope_max); - /* add I component */ - float pos_x_ctl = pos_x_ctl_pd + pos_x_integral; - float pos_y_ctl = pos_y_ctl_pd + pos_y_integral; - /* calculate direction and slope in NED frame */ - float dir = atan2f(pos_y_ctl, pos_x_ctl); - /* use approximation: slope ~ sin(slope) = force */ - float slope = limit_value(sqrtf(pos_x_ctl * pos_x_ctl + pos_y_ctl * pos_y_ctl), params.slope_max); - /* convert direction to body frame */ - dir -= att.yaw; - /* calculate roll and pitch */ - att_sp.pitch_body = -cosf(dir) * slope; // reverse pitch - att_sp.roll_body = sinf(dir) * slope; + /* calculate velocity set point in NED frame */ + vel_sp[0] = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); + vel_sp[1] = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); } else { reset_sp_pos = true; } - att_sp.thrust = thrust_ctl; + /* calculate direction and norm of thrust in NED frame + * limit 3D speed by ellipsoid: + * (vx/xy_vel_max)^2 + (vy/xy_vel_max)^2 + (vz/z_vel_max)^2 = 1 */ + float v; + float vel_sp_norm = 0.0f; + v = vel_sp[0] / params.xy_vel_max; + vel_sp_norm += v * v; + v = vel_sp[1] / params.xy_vel_max; + vel_sp_norm += v * v; + v = vel_sp[2] / params.z_vel_max; + vel_sp_norm += v * v; + vel_sp_norm = sqrtf(vel_sp_norm); + if (vel_sp_norm > 1.0f) { + vel_sp[0] /= vel_sp_norm; + vel_sp[1] /= vel_sp_norm; + vel_sp[2] /= vel_sp_norm; + } + + /* run velocity controllers, calculate thrust vector */ + float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, vel_sp[2], local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + /* calculate velocity set point in NED frame */ + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], vel_sp[0], local_pos.vx, 0.0f, dt); + thrust_sp[1] = pid_calculate(&xy_vel_pids[1], vel_sp[1], local_pos.vy, 0.0f, dt); + } + /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ + /* limit horizontal part of thrust */ + float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); + float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + if (thrust_xy_norm > params.slope_max) { + thrust_xy_norm = params.slope_max; + } + /* use approximation: slope ~ sin(slope) = force */ + /* convert direction to body frame */ + thrust_xy_dir -= att.yaw; + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { + /* calculate roll and pitch */ + att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; + att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch + } + /* attitude-thrust compensation */ + float att_comp; + if (att.R[2][2] > 0.8f) + att_comp = 1.0f / att.R[2][2]; + else if (att.R[2][2] > 0.0f) + att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; + else + att_comp = 1.0f; + att_sp.thrust = -thrust_sp[2] * att_comp; att_sp.timestamp = hrt_absolute_time(); if (status.flag_control_manual_enabled) { /* publish local position setpoint in manual mode */ @@ -403,8 +418,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } - printf("[multirotor_pos_control] exiting\n"); - mavlink_log_info(mavlink_fd, "[multirotor_pos_control] exiting"); + warnx("stopped"); + mavlink_log_info(mavlink_fd, "[multirotor_pos_control] stopped"); thread_running = false; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index d284de433c..f8a982c6c0 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -44,31 +44,37 @@ /* controller parameters */ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.3f); PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.7f); -PARAM_DEFINE_FLOAT(MPC_ALT_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_I, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_D, 0.1f); -PARAM_DEFINE_FLOAT(MPC_ALT_RATE_MAX, 3.0f); -PARAM_DEFINE_FLOAT(MPC_POS_P, 0.1f); -PARAM_DEFINE_FLOAT(MPC_POS_I, 0.0f); -PARAM_DEFINE_FLOAT(MPC_POS_D, 0.2f); -PARAM_DEFINE_FLOAT(MPC_POS_RATE_MAX, 10.0f); +PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); +PARAM_DEFINE_FLOAT(MPC_Z_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); +PARAM_DEFINE_FLOAT(MPC_XY_P, 0.5f); +PARAM_DEFINE_FLOAT(MPC_XY_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); +PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f); PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); -PARAM_DEFINE_INT32(MPC_HARD, 0); int parameters_init(struct multirotor_position_control_param_handles *h) { h->thr_min = param_find("MPC_THR_MIN"); h->thr_max = param_find("MPC_THR_MAX"); - h->alt_p = param_find("MPC_ALT_P"); - h->alt_i = param_find("MPC_ALT_I"); - h->alt_d = param_find("MPC_ALT_D"); - h->alt_rate_max = param_find("MPC_ALT_RATE_MAX"); - h->pos_p = param_find("MPC_POS_P"); - h->pos_i = param_find("MPC_POS_I"); - h->pos_d = param_find("MPC_POS_D"); - h->pos_rate_max = param_find("MPC_POS_RATE_MAX"); + h->z_p = param_find("MPC_Z_P"); + h->z_d = param_find("MPC_Z_D"); + h->z_vel_p = param_find("MPC_Z_VEL_P"); + h->z_vel_i = param_find("MPC_Z_VEL_I"); + h->z_vel_d = param_find("MPC_Z_VEL_D"); + h->z_vel_max = param_find("MPC_Z_VEL_MAX"); + h->xy_p = param_find("MPC_XY_P"); + h->xy_d = param_find("MPC_XY_D"); + h->xy_vel_p = param_find("MPC_XY_VEL_P"); + h->xy_vel_i = param_find("MPC_XY_VEL_I"); + h->xy_vel_d = param_find("MPC_XY_VEL_D"); + h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); h->slope_max = param_find("MPC_SLOPE_MAX"); - h->hard = param_find("MPC_HARD"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); @@ -81,16 +87,19 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, { param_get(h->thr_min, &(p->thr_min)); param_get(h->thr_max, &(p->thr_max)); - param_get(h->alt_p, &(p->alt_p)); - param_get(h->alt_i, &(p->alt_i)); - param_get(h->alt_d, &(p->alt_d)); - param_get(h->alt_rate_max, &(p->alt_rate_max)); - param_get(h->pos_p, &(p->pos_p)); - param_get(h->pos_i, &(p->pos_i)); - param_get(h->pos_d, &(p->pos_d)); - param_get(h->pos_rate_max, &(p->pos_rate_max)); + param_get(h->z_p, &(p->z_p)); + param_get(h->z_d, &(p->z_d)); + param_get(h->z_vel_p, &(p->z_vel_p)); + param_get(h->z_vel_i, &(p->z_vel_i)); + param_get(h->z_vel_d, &(p->z_vel_d)); + param_get(h->z_vel_max, &(p->z_vel_max)); + param_get(h->xy_p, &(p->xy_p)); + param_get(h->xy_d, &(p->xy_d)); + param_get(h->xy_vel_p, &(p->xy_vel_p)); + param_get(h->xy_vel_i, &(p->xy_vel_i)); + param_get(h->xy_vel_d, &(p->xy_vel_d)); + param_get(h->xy_vel_max, &(p->xy_vel_max)); param_get(h->slope_max, &(p->slope_max)); - param_get(h->hard, &(p->hard)); param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); param_get(h->rc_scale_roll, &(p->rc_scale_roll)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index 13b667ad30..e9b1f5c390 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -44,16 +44,19 @@ struct multirotor_position_control_params { float thr_min; float thr_max; - float alt_p; - float alt_i; - float alt_d; - float alt_rate_max; - float pos_p; - float pos_i; - float pos_d; - float pos_rate_max; + float z_p; + float z_d; + float z_vel_p; + float z_vel_i; + float z_vel_d; + float z_vel_max; + float xy_p; + float xy_d; + float xy_vel_p; + float xy_vel_i; + float xy_vel_d; + float xy_vel_max; float slope_max; - int hard; float rc_scale_pitch; float rc_scale_roll; @@ -63,16 +66,19 @@ struct multirotor_position_control_params { struct multirotor_position_control_param_handles { param_t thr_min; param_t thr_max; - param_t alt_p; - param_t alt_i; - param_t alt_d; - param_t alt_rate_max; - param_t pos_p; - param_t pos_i; - param_t pos_d; - param_t pos_rate_max; + param_t z_p; + param_t z_d; + param_t z_vel_p; + param_t z_vel_i; + param_t z_vel_d; + param_t z_vel_max; + param_t xy_p; + param_t xy_d; + param_t xy_vel_p; + param_t xy_vel_i; + param_t xy_vel_d; + param_t xy_vel_max; param_t slope_max; - param_t hard; param_t rc_scale_pitch; param_t rc_scale_roll; diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c new file mode 100644 index 0000000000..89efe1334e --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -0,0 +1,179 @@ +/**************************************************************************** + * + * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. + * Author: Laurens Mackay + * Tobias Naegeli + * Martin Rutschmann + * Anton Babushkin + * Julian Oes + * + * 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 thrust_pid.c + * + * Implementation of generic PID control interface. + * + * @author Laurens Mackay + * @author Tobias Naegeli + * @author Martin Rutschmann + * @author Anton Babushkin + * @author Julian Oes + */ + +#include "thrust_pid.h" +#include + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min) +{ + pid->kp = kp; + pid->ki = ki; + pid->kd = kd; + pid->limit_min = limit_min; + pid->limit_max = limit_max; + pid->mode = mode; + pid->dt_min = dt_min; + pid->last_output = 0.0f; + pid->sp = 0.0f; + pid->error_previous = 0.0f; + pid->integral = 0.0f; +} + +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max) +{ + int ret = 0; + + if (isfinite(kp)) { + pid->kp = kp; + + } else { + ret = 1; + } + + if (isfinite(ki)) { + pid->ki = ki; + + } else { + ret = 1; + } + + if (isfinite(kd)) { + pid->kd = kd; + + } else { + ret = 1; + } + + if (isfinite(limit_min)) { + pid->limit_min = limit_min; + + } else { + ret = 1; + } + + if (isfinite(limit_max)) { + pid->limit_max = limit_max; + + } else { + ret = 1; + } + + return ret; +} + +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt) +{ + /* Alternative integral component calculation + error = setpoint - actual_position + integral = integral + (Ki*error*dt) + derivative = (error - previous_error)/dt + output = (Kp*error) + integral + (Kd*derivative) + previous_error = error + wait(dt) + goto start + */ + + if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { + return pid->last_output; + } + + float i, d; + pid->sp = sp; + + // Calculated current error value + float error = pid->sp - val; + + // Calculate or measured current error derivative + if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC) { + d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = error; + + } else if (pid->mode == THRUST_PID_MODE_DERIVATIV_CALC_NO_SP) { + d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min); + pid->error_previous = -val; + + } else { + d = 0.0f; + } + + if (!isfinite(d)) { + d = 0.0f; + } + + // Calculate the error integral and check for saturation + i = pid->integral + (pid->ki * error * dt); + + float output = (error * pid->kp) + i + (d * pid->kd); + if (output < pid->limit_min || output > pid->limit_max) { + i = pid->integral; // If saturated then do not update integral value + // recalculate output with old integral + output = (error * pid->kp) + i + (d * pid->kd); + + } else { + if (!isfinite(i)) { + i = 0.0f; + } + + pid->integral = i; + } + + if (isfinite(output)) { + if (output > pid->limit_max) { + output = pid->limit_max; + + } else if (output < pid->limit_min) { + output = pid->limit_min; + } + + pid->last_output = output; + } + + return pid->last_output; +} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h new file mode 100644 index 0000000000..65ee33c51c --- /dev/null +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -0,0 +1,75 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin + * + * 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 thrust_pid.h + * + * Definition of thrust control PID interface. + * + * @author Anton Babushkin + */ + +#ifndef THRUST_PID_H_ +#define THRUST_PID_H_ + +#include + +__BEGIN_DECLS + +/* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error */ +#define THRUST_PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored */ +#define THRUST_PID_MODE_DERIVATIV_CALC_NO_SP 1 + +typedef struct { + float kp; + float ki; + float kd; + float sp; + float integral; + float error_previous; + float last_output; + float limit_min; + float limit_max; + float dt_min; + uint8_t mode; +} thrust_pid_t; + +__EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); +__EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt); + +__END_DECLS + +#endif /* THRUST_PID_H_ */ 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 fb5a779bc0..9db2633c68 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -120,7 +120,7 @@ int position_estimator_inav_main(int argc, char *argv[]) verbose_mode = true; thread_should_exit = false; - position_estimator_inav_task = task_spawn("position_estimator_inav", + position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", SCHED_RR, SCHED_PRIORITY_MAX - 5, 4096, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); From c51c211bbaef70defe73c91f6acd6ab99f0f3a04 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 08:45:49 +0400 Subject: [PATCH 35/70] position_estimator_inav: global position altitude fixed --- .../position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 9db2633c68..3b0fbd7823 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -548,7 +548,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); global_pos.lat = (int32_t)(est_lat * 1e7); global_pos.lon = (int32_t)(est_lon * 1e7); - global_pos.alt = -local_pos.z - local_pos.home_alt; + global_pos.alt = local_pos.home_alt - local_pos.z; global_pos.relative_alt = -local_pos.z; global_pos.vx = local_pos.vx; global_pos.vy = local_pos.vy; From d4c6ebde338c8aa1b0d2c9574d3bbdeead525cea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 13:17:42 +0400 Subject: [PATCH 36/70] multirotor_pos_control: global_position_setpoint support in AUTO mode --- .../multirotor_pos_control.c | 39 ++++++++++++++++++- 1 file changed, 38 insertions(+), 1 deletion(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index d56c3d58f8..3e5857c265 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -60,6 +60,7 @@ #include #include #include +#include #include #include #include @@ -179,6 +180,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { memset(&local_pos, 0, sizeof(local_pos)); struct vehicle_local_position_setpoint_s local_pos_sp; memset(&local_pos_sp, 0, sizeof(local_pos_sp)); + struct vehicle_global_position_setpoint_s global_pos_sp; + memset(&local_pos_sp, 0, sizeof(local_pos_sp)); /* subscribe to attitude, motor setpoints and system state */ int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -188,6 +191,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); int local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); + int global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); @@ -201,6 +205,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { const float pos_ctl_dz = 0.05f; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; + uint64_t local_home_timestamp = 0; static PID_t xy_pos_pids[2]; static PID_t xy_vel_pids[2]; @@ -222,6 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; + bool global_pos_sp_updated = false; while (!thread_should_exit) { orb_copy(ORB_ID(vehicle_status), state_sub, &status); @@ -243,6 +249,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { paramcheck_counter = 0; } + /* only check global position setpoint updates but not read */ + if (!global_pos_sp_updated) { + orb_check(global_pos_sp_sub, &global_pos_sp_updated); + } + /* Check if controller should act */ bool act = status.flag_system_armed && ( /* SAS modes */ @@ -271,7 +282,33 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); if (status.state_machine == SYSTEM_STATE_AUTO) { - orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); + if (local_pos.home_timestamp != local_home_timestamp) { + local_home_timestamp = local_pos.home_timestamp; + /* init local projection using local position home */ + double lat_home = local_pos.home_lat * 1e-7; + double lon_home = local_pos.home_lon * 1e-7; + map_projection_init(lat_home, lon_home); + warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); + mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); + } + if (global_pos_sp_updated) { + global_pos_sp_updated = false; + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); + double sp_lat = global_pos_sp.lat * 1e-7; + double sp_lon = global_pos_sp.lon * 1e-7; + /* project global setpoint to local setpoint */ + map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + if (global_pos_sp.altitude_is_relative) { + local_pos_sp.z = -global_pos_sp.altitude; + } else { + local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; + } + warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + /* publish local position setpoint as projection of global position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + } } if (reset_sp_alt) { From 5937c3a31b39d2fe5b70c2eef6327562208f1042 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 16:30:11 +0400 Subject: [PATCH 37/70] uORB topic vehicle_global_velocity_setpoint added --- src/modules/uORB/objects_common.cpp | 3 + .../topics/vehicle_global_velocity_setpoint.h | 64 +++++++++++++++++++ 2 files changed, 67 insertions(+) create mode 100644 src/modules/uORB/topics/vehicle_global_velocity_setpoint.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index ae5fc6c61c..1d5f174059 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -110,6 +110,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp #include "topics/vehicle_global_position_set_triplet.h" ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s); +#include "topics/vehicle_global_velocity_setpoint.h" +ORB_DEFINE(vehicle_global_velocity_setpoint, struct vehicle_global_velocity_setpoint_s); + #include "topics/mission.h" ORB_DEFINE(mission, struct mission_s); diff --git a/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h new file mode 100644 index 0000000000..73961cdfed --- /dev/null +++ b/src/modules/uORB/topics/vehicle_global_velocity_setpoint.h @@ -0,0 +1,64 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: @author Anton Babushkin + * + * 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 vehicle_global_velocity_setpoint.h + * Definition of the global velocity setpoint uORB topic. + */ + +#ifndef TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ +#define TOPIC_VEHICLE_GLOBAL_VELOCITY_SETPOINT_H_ + +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +struct vehicle_global_velocity_setpoint_s +{ + float vx; /**< in m/s NED */ + float vy; /**< in m/s NED */ + float vz; /**< in m/s NED */ +}; /**< Velocity setpoint in NED frame */ + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_global_velocity_setpoint); + +#endif From eb5af244b9281e8d654d5f87feedde3e652b5fc5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 21:57:46 +0400 Subject: [PATCH 38/70] sdlog2: GVSP (global velocity setpoint) message added, cleanup --- src/modules/sdlog2/sdlog2.c | 70 ++++++++++++---------------- src/modules/sdlog2/sdlog2_messages.h | 9 ++++ 2 files changed, 40 insertions(+), 39 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index ec1b7628d7..6641d88f32 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -75,6 +75,7 @@ #include #include #include +#include #include #include #include @@ -94,7 +95,6 @@ log_msgs_written++; \ } else { \ log_msgs_skipped++; \ - /*printf("skip\n");*/ \ } #define LOG_ORB_SUBSCRIBE(_var, _topic) subs.##_var##_sub = orb_subscribe(ORB_ID(##_topic##)); \ @@ -102,9 +102,6 @@ fds[fdsc_count].events = POLLIN; \ fdsc_count++; - -//#define SDLOG2_DEBUG - static bool main_thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -233,7 +230,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (thread_running) { - printf("sdlog2 already running\n"); + warnx("already running"); /* this is not an error */ exit(0); } @@ -250,7 +247,7 @@ int sdlog2_main(int argc, char *argv[]) if (!strcmp(argv[1], "stop")) { if (!thread_running) { - printf("\tsdlog2 is not started\n"); + warnx("not started"); } main_thread_should_exit = true; @@ -262,7 +259,7 @@ int sdlog2_main(int argc, char *argv[]) sdlog2_status(); } else { - printf("\tsdlog2 not started\n"); + warnx("not started\n"); } exit(0); @@ -387,11 +384,6 @@ static void *logwriter_thread(void *arg) /* only get pointer to thread-safe data, do heavy I/O a few lines down */ int available = logbuffer_get_ptr(logbuf, &read_ptr, &is_part); -#ifdef SDLOG2_DEBUG - int rp = logbuf->read_ptr; - int wp = logbuf->write_ptr; -#endif - /* continue */ pthread_mutex_unlock(&logbuffer_mutex); @@ -407,9 +399,6 @@ static void *logwriter_thread(void *arg) n = write(log_file, read_ptr, n); should_wait = (n == available) && !is_part; -#ifdef SDLOG2_DEBUG - printf("write %i %i of %i rp=%i wp=%i, is_part=%i, should_wait=%i\n", log_bytes_written, n, available, rp, wp, (int)is_part, (int)should_wait); -#endif if (n < 0) { main_thread_should_exit = true; @@ -422,14 +411,8 @@ static void *logwriter_thread(void *arg) } else { n = 0; -#ifdef SDLOG2_DEBUG - printf("no data available, main_thread_should_exit=%i, logwriter_should_exit=%i\n", (int)main_thread_should_exit, (int)logwriter_should_exit); -#endif /* exit only with empty buffer */ if (main_thread_should_exit || logwriter_should_exit) { -#ifdef SDLOG2_DEBUG - printf("break logwriter thread\n"); -#endif break; } should_wait = true; @@ -444,10 +427,6 @@ static void *logwriter_thread(void *arg) fsync(log_file); close(log_file); -#ifdef SDLOG2_DEBUG - printf("logwriter thread exit\n"); -#endif - return OK; } @@ -614,14 +593,6 @@ int sdlog2_thread_main(int argc, char *argv[]) errx(1, "can't allocate log buffer, exiting."); } - /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ - /* number of messages */ - const ssize_t fdsc = 19; - /* Sanity check variable and index */ - ssize_t fdsc_count = 0; - /* file descriptors to wait for */ - struct pollfd fds[fdsc]; - struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); @@ -646,6 +617,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct differential_pressure_s diff_pres; struct airspeed_s airspeed; struct esc_status_s esc; + struct vehicle_global_velocity_setpoint_s global_vel_sp; } buf; memset(&buf, 0, sizeof(buf)); @@ -669,6 +641,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int rc_sub; int airspeed_sub; int esc_sub; + int global_vel_sp_sub; } subs; /* log message buffer: header + body */ @@ -694,6 +667,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPOS_s log_GPOS; struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; + struct log_GVSP_s log_GVSP; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -701,6 +675,14 @@ int sdlog2_thread_main(int argc, char *argv[]) #pragma pack(pop) memset(&log_msg.body, 0, sizeof(log_msg.body)); + /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ + /* number of messages */ + const ssize_t fdsc = 20; + /* Sanity check variable and index */ + ssize_t fdsc_count = 0; + /* file descriptors to wait for */ + struct pollfd fds[fdsc]; + /* --- VEHICLE COMMAND --- */ subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); fds[fdsc_count].fd = subs.cmd_sub; @@ -815,6 +797,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GLOBAL VELOCITY SETPOINT --- */ + subs.global_vel_sp_sub = orb_subscribe(ORB_ID(vehicle_global_velocity_setpoint)); + fds[fdsc_count].fd = subs.global_vel_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* WARNING: If you get the error message below, * then the number of registered messages (fdsc) * differs from the number of messages in the above list. @@ -1166,14 +1154,18 @@ int sdlog2_thread_main(int argc, char *argv[]) } } -#ifdef SDLOG2_DEBUG - printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif + /* --- GLOBAL VELOCITY SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_global_velocity_setpoint), subs.global_vel_sp_sub, &buf.global_vel_sp); + log_msg.msg_type = LOG_GVSP_MSG; + log_msg.body.log_GVSP.vx = buf.global_vel_sp.vx; + log_msg.body.log_GVSP.vy = buf.global_vel_sp.vy; + log_msg.body.log_GVSP.vz = buf.global_vel_sp.vz; + LOGBUFFER_WRITE_AND_COUNT(GVSP); + } + /* signal the other thread new data, but not yet unlock */ if (logbuffer_count(&lb) > MIN_BYTES_TO_WRITE) { -#ifdef SDLOG2_DEBUG - printf("signal rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb)); -#endif /* only request write if several packets can be written at once */ pthread_cond_signal(&logbuffer_cond); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 0e479b524b..06e640b5b3 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -244,6 +244,14 @@ struct log_ESC_s { uint16_t esc_setpoint_raw; }; +/* --- GVSP - GLOBAL VELOCITY SETPOINT --- */ +#define LOG_GVSP_MSG 19 +struct log_GVSP_s { + float vx; + float vy; + float vz; +}; + #pragma pack(pop) /* construct list of all message formats */ @@ -267,6 +275,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), + LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"), }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From 87782300509572ea593f309cbbf0a1ca64a53775 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 21:59:49 +0400 Subject: [PATCH 39/70] multirotor_pos_control fixes, systemlib/pid now accepts limit = 0.0, means no limit --- .../multirotor_pos_control.c | 147 +++++++++++------- src/modules/systemlib/pid/pid.c | 14 +- 2 files changed, 103 insertions(+), 58 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 3e5857c265..1d2dd384ac 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -61,6 +61,7 @@ #include #include #include +#include #include #include #include @@ -89,9 +90,11 @@ static float scale_control(float ctl, float end, float dz); static float norm(float x, float y); -static void usage(const char *reason) { +static void usage(const char *reason) +{ if (reason) fprintf(stderr, "%s\n", reason); + fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); exit(1); } @@ -100,11 +103,12 @@ static void usage(const char *reason) { * The deamon app only briefly exists to start * the background job. The stack size assigned in the * Makefile does only apply to this management task. - * + * * The actual stack size should be set in the call * to task_spawn(). */ -int multirotor_pos_control_main(int argc, char *argv[]) { +int multirotor_pos_control_main(int argc, char *argv[]) +{ if (argc < 1) usage("missing command"); @@ -119,11 +123,11 @@ int multirotor_pos_control_main(int argc, char *argv[]) { warnx("start"); thread_should_exit = false; deamon_task = task_spawn_cmd("multirotor_pos_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 60, - 4096, - multirotor_pos_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 60, + 4096, + multirotor_pos_control_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -136,9 +140,11 @@ int multirotor_pos_control_main(int argc, char *argv[]) { if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("app is running"); + } else { warnx("app not started"); } + exit(0); } @@ -146,26 +152,31 @@ int multirotor_pos_control_main(int argc, char *argv[]) { exit(1); } -static float scale_control(float ctl, float end, float dz) { +static float scale_control(float ctl, float end, float dz) +{ if (ctl > dz) { return (ctl - dz) / (end - dz); + } else if (ctl < -dz) { return (ctl + dz) / (end - dz); + } else { return 0.0f; } } -static float norm(float x, float y) { +static float norm(float x, float y) +{ return sqrtf(x * x + y * y); } -static int multirotor_pos_control_thread_main(int argc, char *argv[]) { +static int multirotor_pos_control_thread_main(int argc, char *argv[]) +{ /* welcome user */ warnx("started"); static int mavlink_fd; mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(mavlink_fd, "[multirotor_pos_control] started"); + mavlink_log_info(mavlink_fd, "[mpc] started"); /* structures */ struct vehicle_status_s status; @@ -181,7 +192,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { struct vehicle_local_position_setpoint_s local_pos_sp; memset(&local_pos_sp, 0, sizeof(local_pos_sp)); struct vehicle_global_position_setpoint_s global_pos_sp; - memset(&local_pos_sp, 0, sizeof(local_pos_sp)); + memset(&global_pos_sp, 0, sizeof(local_pos_sp)); + struct vehicle_global_velocity_setpoint_s global_vel_sp; + memset(&global_vel_sp, 0, sizeof(global_vel_sp)); /* subscribe to attitude, motor setpoints and system state */ int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -195,6 +208,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* publish setpoint */ orb_advert_t local_pos_sp_pub = orb_advertise(ORB_ID(vehicle_local_position_setpoint), &local_pos_sp); + orb_advert_t global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &global_vel_sp); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); bool reset_sp_alt = true; @@ -220,10 +234,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { parameters_update(¶ms_h, ¶ms); for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -234,18 +249,23 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* check parameters at 1 Hz*/ paramcheck_counter++; + if (paramcheck_counter == 50) { bool param_updated; orb_check(param_sub, ¶m_updated); + if (param_updated) { parameters_update(¶ms_h, ¶ms); + for (int i = 0; i < 2; i++) { - pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, params.xy_vel_max); + pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max); } + pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } + paramcheck_counter = 0; } @@ -256,31 +276,36 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* Check if controller should act */ bool act = status.flag_system_armed && ( - /* SAS modes */ - ( - status.flag_control_manual_enabled && - status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && ( - status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || - status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE - ) - ) || - /* AUTO mode */ - status.state_machine == SYSTEM_STATE_AUTO - ); + /* SAS modes */ + ( + status.flag_control_manual_enabled && + status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS && ( + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ALTITUDE || + status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE + ) + ) || + /* AUTO mode */ + status.state_machine == SYSTEM_STATE_AUTO + ); hrt_abstime t = hrt_absolute_time(); float dt; + if (t_prev != 0) { dt = (t - t_prev) * 0.000001f; + } else { dt = 0.0f; } + t_prev = t; + if (act) { orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), att_sp_sub, &att_sp); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); + if (status.state_machine == SYSTEM_STATE_AUTO) { //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); if (local_pos.home_timestamp != local_home_timestamp) { @@ -292,6 +317,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); } + if (global_pos_sp_updated) { global_pos_sp_updated = false; orb_copy(ORB_ID(vehicle_global_position_setpoint), global_pos_sp_sub, &global_pos_sp); @@ -299,11 +325,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { double sp_lon = global_pos_sp.lon * 1e-7; /* project global setpoint to local setpoint */ map_projection_project(sp_lat, sp_lon, &(local_pos_sp.x), &(local_pos_sp.y)); + if (global_pos_sp.altitude_is_relative) { local_pos_sp.z = -global_pos_sp.altitude; + } else { local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; } + warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); /* publish local position setpoint as projection of global position setpoint */ @@ -339,16 +368,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* home alt changed, don't follow large ground level changes in manual flight */ local_pos_sp.z += local_pos.home_alt - home_alt; } + home_alt_t = local_pos.home_timestamp; home_alt = local_pos.home_alt; } + /* move altitude setpoint with manual controls */ float z_sp_ctl = scale_control(manual.throttle - 0.5f, 0.5f, alt_ctl_dz); + if (z_sp_ctl != 0.0f) { sp_move_rate[2] = -z_sp_ctl * params.z_vel_max; local_pos_sp.z += sp_move_rate[2] * dt; + if (local_pos_sp.z > local_pos.z + z_sp_offs_max) { local_pos_sp.z = local_pos.z + z_sp_offs_max; + } else if (local_pos_sp.z < local_pos.z - z_sp_offs_max) { local_pos_sp.z = local_pos.z - z_sp_offs_max; } @@ -358,6 +392,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { /* move position setpoint with manual controls */ float pos_pitch_sp_ctl = scale_control(-manual.pitch / params.rc_scale_pitch, 1.0f, pos_ctl_dz); float pos_roll_sp_ctl = scale_control(manual.roll / params.rc_scale_roll, 1.0f, pos_ctl_dz); + if (pos_pitch_sp_ctl != 0.0f || pos_roll_sp_ctl != 0.0f) { /* calculate direction and increment of control in NED frame */ float xy_sp_ctl_dir = att.yaw + atan2f(pos_roll_sp_ctl, pos_pitch_sp_ctl); @@ -370,6 +405,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; + if (pos_vec_norm > 1.0f) { local_pos_sp.x = local_pos.x + pos_vec_x / pos_vec_norm; local_pos_sp.y = local_pos.y + pos_vec_y / pos_vec_norm; @@ -379,72 +415,79 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } /* run position & altitude controllers, calculate velocity setpoint */ - float vel_sp[3] = { 0.0f, 0.0f, 0.0f }; - vel_sp[2] = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ - vel_sp[0] = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); - vel_sp[1] = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); + global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + + /* limit horizontal speed */ + float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; + if (xy_vel_sp_norm > 1.0f) { + global_vel_sp.vx /= xy_vel_sp_norm; + global_vel_sp.vy /= xy_vel_sp_norm; + } + } else { reset_sp_pos = true; + global_vel_sp.vx = 0.0f; + global_vel_sp.vy = 0.0f; } - /* calculate direction and norm of thrust in NED frame - * limit 3D speed by ellipsoid: - * (vx/xy_vel_max)^2 + (vy/xy_vel_max)^2 + (vz/z_vel_max)^2 = 1 */ - float v; - float vel_sp_norm = 0.0f; - v = vel_sp[0] / params.xy_vel_max; - vel_sp_norm += v * v; - v = vel_sp[1] / params.xy_vel_max; - vel_sp_norm += v * v; - v = vel_sp[2] / params.z_vel_max; - vel_sp_norm += v * v; - vel_sp_norm = sqrtf(vel_sp_norm); - if (vel_sp_norm > 1.0f) { - vel_sp[0] /= vel_sp_norm; - vel_sp[1] /= vel_sp_norm; - vel_sp[2] /= vel_sp_norm; - } + + /* publish new velocity setpoint */ + orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); /* run velocity controllers, calculate thrust vector */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, vel_sp[2], local_pos.vz, dt); + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt); + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ - thrust_sp[0] = pid_calculate(&xy_vel_pids[0], vel_sp[0], local_pos.vx, 0.0f, dt); - thrust_sp[1] = pid_calculate(&xy_vel_pids[1], vel_sp[1], local_pos.vy, 0.0f, dt); + thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); + thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); } + /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ /* limit horizontal part of thrust */ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + if (thrust_xy_norm > params.slope_max) { thrust_xy_norm = params.slope_max; } + /* use approximation: slope ~ sin(slope) = force */ /* convert direction to body frame */ thrust_xy_dir -= att.yaw; + if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate roll and pitch */ att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch } + /* attitude-thrust compensation */ float att_comp; + if (att.R[2][2] > 0.8f) att_comp = 1.0f / att.R[2][2]; else if (att.R[2][2] > 0.0f) att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; else att_comp = 1.0f; + att_sp.thrust = -thrust_sp[2] * att_comp; att_sp.timestamp = hrt_absolute_time(); + if (status.flag_control_manual_enabled) { /* publish local position setpoint in manual mode */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } + /* publish new attitude setpoint */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } else { reset_sp_alt = true; reset_sp_pos = true; @@ -456,7 +499,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) { } warnx("stopped"); - mavlink_log_info(mavlink_fd, "[multirotor_pos_control] stopped"); + mavlink_log_info(mavlink_fd, "[mpc] stopped"); thread_running = false; diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 4996a8f661..562f3ac04b 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -168,8 +168,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculate the error integral and check for saturation i = pid->integral + (error * dt); - if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit || - fabsf(i) > pid->intmax) { + if ((pid->limit != 0.0f && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) || + fabsf(i) > pid->intmax) { i = pid->integral; // If saturated then do not update integral value pid->saturated = 1; @@ -186,11 +186,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (isfinite(output)) { - if (output > pid->limit) { - output = pid->limit; + if (pid->limit != 0.0f) { + if (output > pid->limit) { + output = pid->limit; - } else if (output < -pid->limit) { - output = -pid->limit; + } else if (output < -pid->limit) { + output = -pid->limit; + } } pid->last_output = output; From b174a6051527dacc858c6ed54b5d113888c5d4de Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 15 Jul 2013 09:11:52 +0400 Subject: [PATCH 40/70] multirotor_pos_control: PID ontroller derivative mode fixed --- .../multirotor_pos_control/multirotor_pos_control.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 1d2dd384ac..4bae7efa4a 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -234,11 +234,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) parameters_update(¶ms_h, ¶ms); for (int i = 0; i < 2; i++) { - pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -401,7 +401,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) sp_move_rate[1] = sinf(xy_sp_ctl_dir) * xy_sp_ctl_speed; local_pos_sp.x += sp_move_rate[0] * dt; local_pos_sp.y += sp_move_rate[1] * dt; - /* limit maximum setpoint from position offset and preserve direction */ + /* limit maximum setpoint from position offset and preserve direction + * fail safe, should not happen in normal operation */ float pos_vec_x = local_pos_sp.x - local_pos.x; float pos_vec_y = local_pos_sp.y - local_pos.y; float pos_vec_norm = norm(pos_vec_x, pos_vec_y) / xy_sp_offs_max; From c4248c055f098e775e57a7f0ea6ffda49e930b01 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 17 Jul 2013 19:54:47 +0400 Subject: [PATCH 41/70] position_estimator_inav: minor fixes --- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 3b0fbd7823..a9bc09e0d2 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -115,6 +115,7 @@ int position_estimator_inav_main(int argc, char *argv[]) exit(0); } + verbose_mode = false; if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; @@ -379,10 +380,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - accel_NED[2] += CONSTANTS_ONE_G; accel_corr[0] = accel_NED[0] - x_est[2]; accel_corr[1] = accel_NED[1] - y_est[2]; - accel_corr[2] = accel_NED[2] - z_est[2]; + accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2]; } else { memset(accel_corr, 0, sizeof(accel_corr)); From 17366c4b0d2bbcb3d0705bac1a7e4bc737e0bf40 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 17 Jul 2013 22:22:51 +0400 Subject: [PATCH 42/70] multirotor_pos_control: fixes for AUTO mode, minor cleanup --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 4bae7efa4a..e5a3f3e548 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -333,21 +333,21 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; } - warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); + warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); mavlink_log_info(mavlink_fd, "new setpoint: %.7f, %.7f, %.2f, %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); /* publish local position setpoint as projection of global position setpoint */ orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } } - if (reset_sp_alt) { + if (status.flag_control_manual_enabled && reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } - if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { + if (status.flag_control_manual_enabled && status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE && reset_sp_pos) { reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; From 68b7e0315568efc91a067e161d134b00c1c7e132 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 17 Jul 2013 22:27:22 +0400 Subject: [PATCH 43/70] sdlog2: copyright fix --- src/modules/sdlog2/sdlog2.c | 4 ++-- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6641d88f32..1d34daf588 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -2,7 +2,7 @@ * * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * Author: Lorenz Meier - * Anton Babushkin + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,7 +40,7 @@ * does the heavy SD I/O in a low-priority worker thread. * * @author Lorenz Meier - * @author Anton Babushkin + * @author Anton Babushkin */ #include diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 06e640b5b3..95d19d955e 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -1,7 +1,7 @@ /**************************************************************************** * * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Anton Babushkin + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,7 +37,7 @@ * * Log messages and structures definition. * - * @author Anton Babushkin + * @author Anton Babushkin */ #ifndef SDLOG2_MESSAGES_H_ From 1dac58571eadf528e949c4e170de1edf61be2982 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 19 Jul 2013 22:40:45 +0400 Subject: [PATCH 44/70] multirotor_pos_control: minor cleanup --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index e5a3f3e548..b49186ee92 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -95,7 +95,7 @@ static void usage(const char *reason) if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: deamon {start|stop|status} [-p ]\n\n"); + fprintf(stderr, "usage: multirotor_pos_control {start|stop|status}\n\n"); exit(1); } @@ -314,7 +314,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) double lat_home = local_pos.home_lat * 1e-7; double lon_home = local_pos.home_lon * 1e-7; map_projection_init(lat_home, lon_home); - warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); + warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); } From 98a4345410e91a1e3966b3364f487652af210d03 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 21 Jul 2013 22:42:45 +0400 Subject: [PATCH 45/70] multirotor_pos_control: some refactoring and cleanup, attitude-thrust correction moved to thrust_pid --- .../multirotor_pos_control.c | 39 +++++------- .../multirotor_pos_control_params.c | 6 +- .../multirotor_pos_control_params.h | 4 +- .../multirotor_pos_control/thrust_pid.c | 61 ++++++++++--------- .../multirotor_pos_control/thrust_pid.h | 2 +- 5 files changed, 54 insertions(+), 58 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index b49186ee92..6252178d11 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -235,7 +235,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); - pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); + pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); @@ -259,7 +259,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.slope_max); + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); @@ -416,12 +416,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } /* run position & altitude controllers, calculate velocity setpoint */ - global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz, dt); + global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ - global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx, dt); - global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy, dt); + global_vel_sp.vx = pid_calculate(&xy_pos_pids[0], local_pos_sp.x, local_pos.x, local_pos.vx - sp_move_rate[0], dt) + sp_move_rate[0]; + global_vel_sp.vy = pid_calculate(&xy_pos_pids[1], local_pos_sp.y, local_pos.y, local_pos.vy - sp_move_rate[1], dt) + sp_move_rate[1]; /* limit horizontal speed */ float xy_vel_sp_norm = norm(global_vel_sp.vx, global_vel_sp.vy) / params.xy_vel_max; @@ -439,9 +439,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - /* run velocity controllers, calculate thrust vector */ + /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt); + thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate velocity set point in NED frame */ @@ -452,33 +452,24 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* thrust_vector now contains desired acceleration (but not in m/s^2) in NED frame */ /* limit horizontal part of thrust */ float thrust_xy_dir = atan2f(thrust_sp[1], thrust_sp[0]); - float thrust_xy_norm = norm(thrust_sp[0], thrust_sp[1]); + /* assuming that vertical component of thrust is g, + * horizontal component = g * tan(alpha) */ + float tilt = atanf(norm(thrust_sp[0], thrust_sp[1])); - if (thrust_xy_norm > params.slope_max) { - thrust_xy_norm = params.slope_max; + if (tilt > params.tilt_max) { + tilt = params.tilt_max; } - /* use approximation: slope ~ sin(slope) = force */ /* convert direction to body frame */ thrust_xy_dir -= att.yaw; if (status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_SIMPLE || status.state_machine == SYSTEM_STATE_AUTO) { /* calculate roll and pitch */ - att_sp.roll_body = sinf(thrust_xy_dir) * thrust_xy_norm; - att_sp.pitch_body = -cosf(thrust_xy_dir) * thrust_xy_norm / cosf(att_sp.roll_body); // reverse pitch + att_sp.roll_body = sinf(thrust_xy_dir) * tilt; + att_sp.pitch_body = -cosf(thrust_xy_dir) * tilt / cosf(att_sp.roll_body); } - /* attitude-thrust compensation */ - float att_comp; - - if (att.R[2][2] > 0.8f) - att_comp = 1.0f / att.R[2][2]; - else if (att.R[2][2] > 0.0f) - att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * att.R[2][2] + 1.0f; - else - att_comp = 1.0f; - - att_sp.thrust = -thrust_sp[2] * att_comp; + att_sp.thrust = -thrust_sp[2]; att_sp.timestamp = hrt_absolute_time(); if (status.flag_control_manual_enabled) { diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c index f8a982c6c0..1094fd23b7 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c @@ -56,7 +56,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.2f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.0f); PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 10.0f); -PARAM_DEFINE_FLOAT(MPC_SLOPE_MAX, 0.5f); +PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f); int parameters_init(struct multirotor_position_control_param_handles *h) { @@ -74,7 +74,7 @@ int parameters_init(struct multirotor_position_control_param_handles *h) h->xy_vel_i = param_find("MPC_XY_VEL_I"); h->xy_vel_d = param_find("MPC_XY_VEL_D"); h->xy_vel_max = param_find("MPC_XY_VEL_MAX"); - h->slope_max = param_find("MPC_SLOPE_MAX"); + h->tilt_max = param_find("MPC_TILT_MAX"); h->rc_scale_pitch = param_find("RC_SCALE_PITCH"); h->rc_scale_roll = param_find("RC_SCALE_ROLL"); @@ -99,7 +99,7 @@ int parameters_update(const struct multirotor_position_control_param_handles *h, param_get(h->xy_vel_i, &(p->xy_vel_i)); param_get(h->xy_vel_d, &(p->xy_vel_d)); param_get(h->xy_vel_max, &(p->xy_vel_max)); - param_get(h->slope_max, &(p->slope_max)); + param_get(h->tilt_max, &(p->tilt_max)); param_get(h->rc_scale_pitch, &(p->rc_scale_pitch)); param_get(h->rc_scale_roll, &(p->rc_scale_roll)); diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h index e9b1f5c390..14a3de2e59 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h +++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h @@ -56,7 +56,7 @@ struct multirotor_position_control_params { float xy_vel_i; float xy_vel_d; float xy_vel_max; - float slope_max; + float tilt_max; float rc_scale_pitch; float rc_scale_roll; @@ -78,7 +78,7 @@ struct multirotor_position_control_param_handles { param_t xy_vel_i; param_t xy_vel_d; param_t xy_vel_max; - param_t slope_max; + param_t tilt_max; param_t rc_scale_pitch; param_t rc_scale_roll; diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c index 89efe1334e..51dcd7df24 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -1,11 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Laurens Mackay - * Tobias Naegeli - * Martin Rutschmann - * Anton Babushkin - * Julian Oes + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,13 +35,9 @@ /** * @file thrust_pid.c * - * Implementation of generic PID control interface. + * Implementation of thrust control PID. * - * @author Laurens Mackay - * @author Tobias Naegeli - * @author Martin Rutschmann * @author Anton Babushkin - * @author Julian Oes */ #include "thrust_pid.h" @@ -108,16 +100,18 @@ __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, fl return ret; } -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt) +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22) { /* Alternative integral component calculation - error = setpoint - actual_position - integral = integral + (Ki*error*dt) - derivative = (error - previous_error)/dt - output = (Kp*error) + integral + (Kd*derivative) - previous_error = error - wait(dt) - goto start + * + * start: + * error = setpoint - current_value + * integral = integral + (Ki * error * dt) + * derivative = (error - previous_error) / dt + * previous_error = error + * output = (Kp * error) + integral + (Kd * derivative) + * wait(dt) + * goto start */ if (!isfinite(sp) || !isfinite(val) || !isfinite(dt)) { @@ -147,21 +141,32 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa d = 0.0f; } - // Calculate the error integral and check for saturation + /* calculate the error integral */ i = pid->integral + (pid->ki * error * dt); - float output = (error * pid->kp) + i + (d * pid->kd); + /* attitude-thrust compensation + * r22 is (2, 2) componet of rotation matrix for current attitude */ + float att_comp; + + if (r22 > 0.8f) + att_comp = 1.0f / r22; + else if (r22 > 0.0f) + att_comp = ((1.0f / 0.8f - 1.0f) / 0.8f) * r22 + 1.0f; + else + att_comp = 1.0f; + + /* calculate output */ + float output = ((error * pid->kp) + i + (d * pid->kd)) * att_comp; + + /* check for saturation */ if (output < pid->limit_min || output > pid->limit_max) { - i = pid->integral; // If saturated then do not update integral value - // recalculate output with old integral - output = (error * pid->kp) + i + (d * pid->kd); + /* saturated, recalculate output with old integral */ + output = (error * pid->kp) + pid->integral + (d * pid->kd); } else { - if (!isfinite(i)) { - i = 0.0f; + if (isfinite(i)) { + pid->integral = i; } - - pid->integral = i; } if (isfinite(output)) { diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h index 65ee33c51c..551c032fe2 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -68,7 +68,7 @@ typedef struct { __EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); -__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt); +__EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); __END_DECLS From bd50092dd29a83dd5044fc3027e20be91b3275e5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 21 Jul 2013 22:44:46 +0400 Subject: [PATCH 46/70] position_estimator_inav: accelerometer bias estimation for Z, default weights for GPS updated --- .../position_estimator_inav_main.c | 20 +++++++++++++------ .../position_estimator_inav_params.c | 9 ++++++--- .../position_estimator_inav_params.h | 2 ++ 3 files changed, 22 insertions(+), 9 deletions(-) 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 a9bc09e0d2..42b5fcb615 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -167,9 +167,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ - double lat_current = 0.0; //[°]] --> 47.0 - double lon_current = 0.0; //[°]] -->8.5 - double alt_current = 0.0; //[m] above MSL + double lat_current = 0.0f; //[°] --> 47.0 + double lon_current = 0.0f; //[°] --> 8.5 + double alt_current = 0.0f; //[m] above MSL uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -302,8 +302,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime pub_last = hrt_absolute_time(); uint32_t pub_interval = 4000; // limit publish rate to 250 Hz + /* acceleration in NED frame */ + float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; + /* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */ float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D + float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame float baro_corr = 0.0f; // D float gps_corr[2][2] = { { 0.0f, 0.0f }, // N (pos, vel) @@ -369,9 +373,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (sensor.accelerometer_counter > accel_counter) { if (att.R_valid) { + /* correct accel bias, now only for Z */ + sensor.accelerometer_m_s2[2] -= accel_bias[2]; /* transform acceleration vector from body frame to NED frame */ - float accel_NED[3]; - for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; @@ -425,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) local_pos.home_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; sonar_corr = 0.0f; - sonar_corr_filtered = 0.0; + sonar_corr_filtered = 0.0f; } } } @@ -470,6 +474,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; + /* accel bias correction, now only for Z + * not strictly correct, but stable and works */ + accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; + /* inertial filter prediction for altitude */ inertial_filter_predict(dt, z_est); 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 c90c611a77..70248b3b72 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -44,10 +44,11 @@ PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 4.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f); PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f); -PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 10.0f); +PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 0.0f); +PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f); PARAM_DEFINE_FLOAT(INAV_FLOW_K, 1.0f); PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.02f); PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); @@ -62,6 +63,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V"); h->w_pos_acc = param_find("INAV_W_POS_ACC"); h->w_pos_flow = param_find("INAV_W_POS_FLOW"); + h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); h->sonar_filt = param_find("INAV_SONAR_FILT"); h->sonar_err = param_find("INAV_SONAR_ERR"); @@ -79,6 +81,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->w_pos_gps_v, &(p->w_pos_gps_v)); param_get(h->w_pos_acc, &(p->w_pos_acc)); param_get(h->w_pos_flow, &(p->w_pos_flow)); + param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); param_get(h->sonar_filt, &(p->sonar_filt)); param_get(h->sonar_err, &(p->sonar_err)); 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 cca172b5d6..1e70a3c48a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -49,6 +49,7 @@ struct position_estimator_inav_params { float w_pos_gps_v; float w_pos_acc; float w_pos_flow; + float w_acc_bias; float flow_k; float sonar_filt; float sonar_err; @@ -63,6 +64,7 @@ struct position_estimator_inav_param_handles { param_t w_pos_gps_v; param_t w_pos_acc; param_t w_pos_flow; + param_t w_acc_bias; param_t flow_k; param_t sonar_filt; param_t sonar_err; From 8dd5130d998f7609c8a5d457093e31320b3668fd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 24 Jul 2013 18:20:04 +0400 Subject: [PATCH 47/70] position_estimator_inav, multirotor_pos_control: bugs fixed --- src/modules/multirotor_pos_control/multirotor_pos_control.c | 3 ++- .../position_estimator_inav/position_estimator_inav_main.c | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 6252178d11..10f21c55de 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -259,7 +259,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max); + // TODO 1000.0 is hotfix + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1000.0f, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); 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 42b5fcb615..d4b2f0e43c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -453,8 +453,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) gps_corr[1][0] = gps_proj[1] - y_est[0]; if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s; - gps_corr[1][1] = gps.vel_e_m_s; + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; } else { gps_corr[0][1] = 0.0f; From a013fc5d0b28cd72f41a28c8229c2d7ab99965f4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 25 Jul 2013 20:05:45 +0400 Subject: [PATCH 48/70] multirotor_pos_control: limit xy velocity integral output to tilt_max / 2 --- .../multirotor_pos_control.c | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 10f21c55de..44d478a9e7 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -248,9 +248,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), state_sub, &status); /* check parameters at 1 Hz*/ - paramcheck_counter++; - - if (paramcheck_counter == 50) { + if (--paramcheck_counter <= 0) { + paramcheck_counter = 50; bool param_updated; orb_check(param_sub, ¶m_updated); @@ -259,15 +258,19 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) for (int i = 0; i < 2; i++) { pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f); - // TODO 1000.0 is hotfix - pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1000.0f, params.tilt_max); + /* use integral_limit_out = tilt_max / 2 */ + float i_limit; + if (params.xy_vel_i == 0.0) { + i_limit = params.tilt_max / params.xy_vel_i / 2.0; + } else { + i_limit = 1.0f; // not used really + } + pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max); } pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max); thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min); } - - paramcheck_counter = 0; } /* only check global position setpoint updates but not read */ From 98f403002f72e7fb3e38398de9d87746f7918347 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 15 Aug 2013 17:27:29 +0200 Subject: [PATCH 49/70] Lov voltage warning should work again --- src/modules/commander/commander.cpp | 23 ++++++++++------------- 1 file changed, 10 insertions(+), 13 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5de99040cc..1ae88d17ab 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -550,6 +550,7 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + status.condition_battery_voltage_valid = false; // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -606,8 +607,8 @@ int commander_thread_main(int argc, char *argv[]) enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; bool armed_previous = false; - bool low_battery_voltage_actions_done; - bool critical_battery_voltage_actions_done; + bool low_battery_voltage_actions_done = false; + bool critical_battery_voltage_actions_done = false; uint64_t last_idle_time = 0; @@ -810,19 +811,15 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - status.battery_voltage = battery.voltage_v; - status.condition_battery_voltage_valid = true; - } - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { - status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + warnx("bat v: %2.2f", battery.voltage_v); - } else { - status.battery_voltage = 0.0f; + /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ + if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } } /* update subsystem */ From 3f9f1d60e03f501209deb6c7532c37dfb786f343 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 16 Aug 2013 09:23:39 +0200 Subject: [PATCH 50/70] Added audio and text warning if arming is refused due to mode switch --- src/modules/commander/commander.cpp | 25 +++++++++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1ae88d17ab..6181dafaba 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -204,6 +204,7 @@ void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicl transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); void print_reject_mode(const char *msg); +void print_reject_arm(const char *msg); transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); @@ -1082,6 +1083,16 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates safety switch not pressed */ + + if (!(!safety.safety_switch_available || safety.safety_off)) { + print_reject_arm("NOT ARMING: Press safety switch first."); + + } else { + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } } /* fill current_status according to mode switches */ @@ -1490,6 +1501,20 @@ print_reject_mode(const char *msg) } } +void +print_reject_arm(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "[cmd] %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) { From 63af0a80ee19a73a94a3b46bbddffe1b80610a3c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 15:08:10 +0200 Subject: [PATCH 51/70] multirotor_att_control: run rate controller only if vehicle_control_mode flag set, codestyle fixed --- .../multirotor_att_control_main.c | 43 ++++++++++--------- 1 file changed, 23 insertions(+), 20 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 7014d22c4a..65b19c8e9c 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -242,7 +242,7 @@ mc_thread_main(int argc, char *argv[]) /* control attitude, update attitude setpoint depending on mode */ /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || - control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { + control_mode.flag_control_manual_enabled != flag_control_manual_enabled) { att_sp.yaw_body = att.yaw; } @@ -305,6 +305,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; reset_yaw_sp = false; } + control_yaw_position = true; } @@ -312,6 +313,7 @@ mc_thread_main(int argc, char *argv[]) /* don't update attitude setpoint in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; + if (!control_mode.flag_control_position_enabled) { /* don't set throttle in altitude hold modes */ att_sp.thrust = manual.throttle; @@ -355,24 +357,25 @@ mc_thread_main(int argc, char *argv[]) /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; + if (control_mode.flag_control_rates_enabled) { + /* get current rate setpoint */ + bool rates_sp_valid = false; + orb_check(rates_sp_sub, &rates_sp_valid); - /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); + if (rates_sp_valid) { + orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + } - if (rates_sp_valid) { - orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); + /* apply controller */ + float gyro[3]; + gyro[0] = att.rollspeed; + gyro[1] = att.pitchspeed; + gyro[2] = att.yawspeed; + + multirotor_control_rates(&rates_sp, gyro, &actuators); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); } - /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - - multirotor_control_rates(&rates_sp, gyro, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; flag_control_manual_enabled = control_mode.flag_control_manual_enabled; @@ -449,11 +452,11 @@ int multirotor_att_control_main(int argc, char *argv[]) thread_should_exit = false; mc_task = task_spawn_cmd("multirotor_att_control", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 2048, - mc_thread_main, - NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 2048, + mc_thread_main, + NULL); exit(0); } From 4882bc0f1c74e9ddebbeaa73bc3e753ac43bdf9c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 21:24:19 +0200 Subject: [PATCH 52/70] position_estimator_inav: fixed global_pos topic publishing --- .../position_estimator_inav/position_estimator_inav_main.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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 d4b2f0e43c..0530c2deac 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -116,6 +116,7 @@ int position_estimator_inav_main(int argc, char *argv[]) } verbose_mode = false; + if (argc > 1) if (!strcmp(argv[2], "-v")) verbose_mode = true; @@ -375,6 +376,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (att.R_valid) { /* correct accel bias, now only for Z */ sensor.accelerometer_m_s2[2] -= accel_bias[2]; + /* transform acceleration vector from body frame to NED frame */ for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; @@ -551,9 +553,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); if (params.use_gps) { - global_pos.valid = local_pos.valid; double est_lat, est_lon; map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + + global_pos.valid = local_pos.valid; + global_pos.timestamp = t; + global_pos.time_gps_usec = gps.time_gps_usec; global_pos.lat = (int32_t)(est_lat * 1e7); global_pos.lon = (int32_t)(est_lon * 1e7); global_pos.alt = local_pos.home_alt - local_pos.z; From c543f89ec17048c1b5264623a885a9247a05304c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 16 Aug 2013 23:36:49 +0200 Subject: [PATCH 53/70] commander, multirotor_pos_control, multirotor_att_control: bugfixes --- src/modules/commander/commander.cpp | 132 ++++++++++-------- .../commander/state_machine_helper.cpp | 89 ++++++------ .../multirotor_att_control_main.c | 8 +- .../multirotor_pos_control.c | 22 +-- .../uORB/topics/vehicle_control_mode.h | 3 +- .../uORB/topics/vehicle_local_position.h | 1 + 6 files changed, 134 insertions(+), 121 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6181dafaba..872939d6dc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -122,7 +122,7 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -353,27 +353,42 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - case VEHICLE_CMD_COMPONENT_ARM_DISARM: + case VEHICLE_CMD_NAV_TAKEOFF: { + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } + + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } break; + } case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: - if (is_safe(status, safety, armed)) { + if (is_safe(status, safety, armed)) { - if (((int)(cmd->param1)) == 1) { - /* reboot */ - up_systemreset(); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ + if (((int)(cmd->param1)) == 1) { + /* reboot */ + up_systemreset(); + + } else if (((int)(cmd->param1)) == 3) { + /* reboot to bootloader */ + + // XXX implement + result = VEHICLE_CMD_RESULT_UNSUPPORTED; - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { result = VEHICLE_CMD_RESULT_DENIED; } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { @@ -519,6 +534,7 @@ int commander_thread_main(int argc, char *argv[]) orb_advert_t status_pub; /* make sure we are in preflight state */ memset(&status, 0, sizeof(status)); + status.condition_landed = true; // initialize to safe value /* armed topic */ struct actuator_armed_s armed; @@ -760,6 +776,14 @@ int commander_thread_main(int argc, char *argv[]) last_diff_pres_time = diff_pres.timestamp; } + /* Check for valid airspeed/differential pressure measurements */ + if (t - last_diff_pres_time < 2000000 && t > 2000000) { + status.condition_airspeed_valid = true; + + } else { + status.condition_airspeed_valid = false; + } + orb_check(cmd_sub, &updated); if (updated) { @@ -785,6 +809,20 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* set the condition to valid if there has recently been a global position estimate */ + if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) { + if (!status.condition_global_position_valid) { + status.condition_global_position_valid = true; + status_changed = true; + } + + } else { + if (status.condition_global_position_valid) { + status.condition_global_position_valid = false; + status_changed = true; + } + } + /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -794,7 +832,7 @@ int commander_thread_main(int argc, char *argv[]) } /* set the condition to valid if there has recently been a local position estimate */ - if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) { + if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) { if (!status.condition_local_position_valid) { status.condition_local_position_valid = true; status_changed = true; @@ -920,36 +958,6 @@ int commander_thread_main(int argc, char *argv[]) * set of position measurements is available. */ - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = status.condition_global_position_valid; - bool local_pos_valid = status.condition_local_position_valid; - bool airspeed_valid = status.condition_airspeed_valid; - - - /* check for global or local position updates, set a timeout of 2s */ - if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { - status.condition_global_position_valid = true; - - } else { - status.condition_global_position_valid = false; - } - - if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { - status.condition_local_position_valid = true; - - } else { - status.condition_local_position_valid = false; - } - - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; - - } else { - status.condition_airspeed_valid = false; - } - orb_check(gps_sub, &updated); if (updated) { @@ -1083,6 +1091,7 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + } else if (res == TRANSITION_DENIED) { /* DENIED here indicates safety switch not pressed */ @@ -1536,8 +1545,10 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; case MAIN_STATE_AUTO: - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; + } else { if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); @@ -1545,20 +1556,27 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v break; } else { - /* if not landed: act depending on switches */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 163aceed2e..f313adce47 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -109,9 +109,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ - { + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -182,6 +181,7 @@ bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s // 3) Safety switch is present AND engaged -> actuators locked if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { return true; + } else { return false; } @@ -284,6 +284,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; @@ -294,6 +295,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -304,6 +306,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = true; break; @@ -314,6 +317,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; @@ -324,80 +328,68 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = true; break; case NAVIGATION_STATE_AUTO_READY: ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_climb_rate_enabled = false; control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } break; case NAVIGATION_STATE_AUTO_LOITER: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_MISSION: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_RTL: - - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; + control_mode->flag_control_manual_enabled = false; break; case NAVIGATION_STATE_AUTO_LAND: @@ -411,6 +403,7 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_climb_rate_enabled = true; control_mode->flag_control_manual_enabled = false; } diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 65b19c8e9c..1aa24a4fc8 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -250,12 +250,12 @@ mc_thread_main(int argc, char *argv[]) /* if the RC signal is lost, try to stay level and go slowly back down to ground */ if (control_mode.failsave_highlevel) { - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_velocity_enabled) { /* Don't reset attitude setpoint in position control mode, it's handled by position controller. */ att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; - if (!control_mode.flag_control_altitude_enabled) { + if (!control_mode.flag_control_climb_rate_enabled) { /* Don't touch throttle in modes with altitude hold, it's handled by position controller. * * Only go to failsafe throttle if last known throttle was @@ -309,12 +309,12 @@ mc_thread_main(int argc, char *argv[]) control_yaw_position = true; } - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_velocity_enabled) { /* don't update attitude setpoint in position control mode */ att_sp.roll_body = manual.roll; att_sp.pitch_body = manual.pitch; - if (!control_mode.flag_control_position_enabled) { + if (!control_mode.flag_control_climb_rate_enabled) { /* don't set throttle in altitude hold modes */ att_sp.thrust = manual.throttle; } diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 0120fa61c7..1cb4702409 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -221,10 +221,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) hrt_abstime home_alt_t = 0; uint64_t local_home_timestamp = 0; - static PID_t xy_pos_pids[2]; - static PID_t xy_vel_pids[2]; - static PID_t z_pos_pid; - static thrust_pid_t z_vel_pid; + PID_t xy_pos_pids[2]; + PID_t xy_vel_pids[2]; + PID_t z_pos_pid; + thrust_pid_t z_vel_pid; thread_running = true; @@ -238,7 +238,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); } - pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f); + pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f); thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f); int paramcheck_counter = 0; @@ -247,9 +247,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); - /* check parameters at 1 Hz*/ - if (--paramcheck_counter <= 0) { - paramcheck_counter = 50; + /* check parameters at 1 Hz */ + if (++paramcheck_counter >= 50) { + paramcheck_counter = 0; bool param_updated; orb_check(param_sub, ¶m_updated); @@ -441,14 +441,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); // TODO subcrive to velocity setpoint if altitude/position control disabled - if (control_mode.flag_control_velocity_enabled) { + if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; - if (control_mode.flag_control_altitude_enabled) { + if (control_mode.flag_control_climb_rate_enabled) { thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } - if (control_mode.flag_control_position_enabled) { + if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index d83eb45d9a..fe169c6e6b 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -75,9 +75,10 @@ struct vehicle_control_mode_s //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_velocity_enabled; /**< true if horisontal speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ + bool flag_control_climb_rate_enabled; /**< true if climb rate is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 76eddeacd2..9d3b4468c6 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -75,6 +75,7 @@ struct vehicle_local_position_s float home_alt; /**< Altitude in meters LOGME */ float home_hdg; /**< Compass heading in radians -PI..+PI. */ + bool landed; /**< true if vehicle is landed */ }; /** From 2be5240b9f70803417c9648133490409ba40ba55 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 17 Aug 2013 12:37:41 +0200 Subject: [PATCH 54/70] commander, multirotor_att_control, position_estimator_inav: position valid flag fixed, other fixes and cleaunup --- src/modules/commander/commander.cpp | 124 ++++++++---------- .../multirotor_att_control_main.c | 20 ++- .../position_estimator_inav_main.c | 15 +-- 3 files changed, 74 insertions(+), 85 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 872939d6dc..d40f6675b5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,12 +117,9 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define GPS_FIX_TYPE_2D 2 -#define GPS_FIX_TYPE_3D 3 -#define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 -#define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) - -#define POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ +#define RC_TIMEOUT 100000 +#define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 @@ -197,6 +194,8 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode */ int commander_thread_main(int argc, char *argv[]); +void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed); + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); @@ -354,19 +353,21 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } case VEHICLE_CMD_NAV_TAKEOFF: { - transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res == TRANSITION_CHANGED) { - mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); - } + transition_result_t nav_res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode); - if (nav_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (nav_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] TAKEOFF on command"); + } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + if (nav_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; } - break; - } case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_safe(status, safety, armed)) { @@ -713,8 +714,6 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { - hrt_abstime t = hrt_absolute_time(); - /* update parameters */ orb_check(param_changed_sub, &updated); @@ -773,16 +772,9 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - last_diff_pres_time = diff_pres.timestamp; } - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; - - } else { - status.condition_airspeed_valid = false; - } + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); orb_check(cmd_sub, &updated); @@ -809,19 +801,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } - /* set the condition to valid if there has recently been a global position estimate */ - if (t - global_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && global_position.valid) { - if (!status.condition_global_position_valid) { - status.condition_global_position_valid = true; - status_changed = true; - } - - } else { - if (status.condition_global_position_valid) { - status.condition_global_position_valid = false; - status_changed = true; - } - } + /* update condition_global_position_valid */ + check_valid(global_position.timestamp, POSITION_TIMEOUT, global_position.valid, &(status.condition_global_position_valid), &status_changed); /* update local position estimate */ orb_check(local_position_sub, &updated); @@ -831,19 +812,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } - /* set the condition to valid if there has recently been a local position estimate */ - if (t - local_position.timestamp < POSITION_TIMEOUT && t > POSITION_TIMEOUT && local_position.valid) { - if (!status.condition_local_position_valid) { - status.condition_local_position_valid = true; - status_changed = true; - } - - } else { - if (status.condition_local_position_valid) { - status.condition_local_position_valid = false; - status_changed = true; - } - } + /* update condition_local_position_valid */ + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed); /* update battery status */ orb_check(battery_sub, &updated); @@ -854,7 +824,7 @@ int commander_thread_main(int argc, char *argv[]) warnx("bat v: %2.2f", battery.voltage_v); /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -981,10 +951,9 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D && + if (!home_position_set && gps_position.fix_type >= 3 && (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk - (t - gps_position.timestamp_position < 2000000) - && !armed.armed) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ // TODO use global position estimate home.lat = gps_position.lat; @@ -1019,7 +988,7 @@ int commander_thread_main(int argc, char *argv[]) /* ignore RC signals if in offboard control mode */ if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { /* start RC input check */ - if ((t - sp_man.timestamp) < 100000) { + if (hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1123,7 +1092,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* print error message for first RC glitch and then every 5s */ - if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) { + if (!status.rc_signal_cutting_off || (hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL) { // TODO remove debug code if (!status.rc_signal_cutting_off) { warnx("Reason: not rc_signal_cutting_off\n"); @@ -1136,11 +1105,11 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); - last_print_control_time = t; + last_print_control_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = t - sp_man.timestamp; + status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ if (status.rc_signal_lost_interval > 1000000) { @@ -1157,7 +1126,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO check this /* state machine update for offboard control */ if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((t - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? + if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? // /* decide about attitude control flag, enable in att/pos/vel */ // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || @@ -1213,23 +1182,23 @@ int commander_thread_main(int argc, char *argv[]) } else { /* print error message for first offboard signal glitch and then every 5s */ - if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) { + if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_control_time) > PRINT_INTERVAL)) { status.offboard_control_signal_weak = true; mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); - last_print_control_time = t; + last_print_control_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ if (status.offboard_control_signal_lost_interval > 100000) { status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = t; + status.failsave_lowlevel_start_time = hrt_absolute_time(); tune_positive(); /* kill motors after timeout */ - if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + if (hrt_absolute_time() > status.failsave_lowlevel_start_time + failsafe_lowlevel_timeout_ms * 1000) { status.failsave_lowlevel = true; status_changed = true; } @@ -1256,9 +1225,11 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } + hrt_abstime t1 = hrt_absolute_time(); + /* publish arming state */ if (arming_state_changed) { - armed.timestamp = t; + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1266,14 +1237,14 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_changed) { /* publish new navigation state */ control_mode.counter++; - control_mode.timestamp = t; + control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); } /* publish vehicle status at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { status.counter++; - status.timestamp = t; + status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); status_changed = false; } @@ -1340,6 +1311,18 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +{ + hrt_abstime t = hrt_absolute_time(); + bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); + + if (*valid_out != valid_new) { + *valid_out = valid_new; + *changed = true; + } +} + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) { @@ -1367,7 +1350,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp /* position estimated, solid */ led_on(LED_BLUE); - } else if (leds_counter == 0) { + } else if (leds_counter == 6) { /* waiting for position estimate, short blink at 1.25Hz */ led_on(LED_BLUE); @@ -1548,6 +1531,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { /* don't act while taking off */ res = TRANSITION_NOT_CHANGED; + } else { if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 1aa24a4fc8..5cad667f61 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -367,14 +367,20 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - float gyro[3]; - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; - - multirotor_control_rates(&rates_sp, gyro, &actuators); - orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + float rates[3]; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; + multirotor_control_rates(&rates_sp, rates, &actuators); + } else { + /* rates controller disabled, set actuators to zero for safety */ + actuators.control[0] = 0.0f; + actuators.control[1] = 0.0f; + actuators.control[2] = 0.0f; + actuators.control[3] = 0.0f; } + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); /* update state */ flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; 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 0530c2deac..0e7e0ef5d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -75,6 +75,7 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; +static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -490,7 +491,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); /* dont't try to estimate position when no any position source available */ - bool can_estimate_pos = params.use_gps && gps.fix_type >= 3; + bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout; if (can_estimate_pos) { /* inertial filter prediction for position */ @@ -501,14 +502,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); - if (params.use_gps && gps.fix_type >= 3) { - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); - if (gps.vel_ned_valid) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); - } + if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); } } From 408eaf0ad1c8aa87c74f83281de3a7c25fc4b4e6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 18 Aug 2013 09:22:40 +0200 Subject: [PATCH 55/70] Add ioctl to find out if mag is external or onboard --- src/drivers/drv_mag.h | 3 +++ src/drivers/hmc5883/hmc5883.cpp | 24 +++++++++++++++++++----- src/drivers/lsm303d/lsm303d.cpp | 15 ++++++++++++--- 3 files changed, 34 insertions(+), 8 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 9aab995a17..e95034e8e3 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -111,4 +111,7 @@ ORB_DECLARE(sensor_mag); /** perform self test and report status */ #define MAGIOCSELFTEST _MAGIOC(7) +/** determine if mag is external or onboard */ +#define MAGIOCGEXTERNAL _MAGIOC(8) + #endif /* _DRV_MAG_H */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 039b496f4b..9e9c067d53 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -167,6 +167,8 @@ private: bool _sensor_ok; /**< sensor was found and reports ok */ bool _calibrated; /**< the calibration is valid */ + int _bus; /**< the bus the device is connected to */ + /** * Test whether the device supported by the driver is present at a * specific address. @@ -326,7 +328,8 @@ HMC5883::HMC5883(int bus) : _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), _sensor_ok(false), - _calibrated(false) + _calibrated(false), + _bus(bus) { // enable debug() calls _debug_enabled = false; @@ -665,6 +668,12 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) case MAGIOCSELFTEST: return check_calibration(); + case MAGIOCGEXTERNAL: + if (_bus == PX4_I2C_BUS_EXPANSION) + return 1; + else + return 0; + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -851,12 +860,12 @@ HMC5883::collect() _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif - /* XXX axis assignment of external sensor is yet unknown */ - _reports[_next_report].x = ((report.y * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* the standard external mag seems to be rolled 180deg, therefore y and z inverted */ + _reports[_next_report].x = ((report.x * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; + _reports[_next_report].y = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; + _reports[_next_report].z = ((((report.z == -32768) ? 32767 : -report.z) * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif @@ -1293,6 +1302,11 @@ test() warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z); warnx("time: %lld", report.timestamp); + /* check if mag is onboard or external */ + if ((ret = ioctl(fd, MAGIOCGEXTERNAL, 0)) < 0) + errx(1, "failed to get if mag is onboard or external"); + warnx("device active: %s", ret ? "external" : "onboard"); + /* set the queue depth to 10 */ if (OK != ioctl(fd, SENSORIOCSQUEUEDEPTH, 10)) errx(1, "failed to set queue depth"); diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 117583fafe..3e6ce64b81 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -869,6 +869,10 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) // return self_test(); return -EINVAL; + case MAGIOCGEXTERNAL: + /* no external mag board yet */ + return 0; + default: /* give it to the superclass */ return SPI::ioctl(filp, cmd, arg); @@ -1422,7 +1426,7 @@ test() int fd_accel = -1; struct accel_report accel_report; ssize_t sz; - int filter_bandwidth; + int ret; /* get the driver */ fd_accel = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -1445,10 +1449,10 @@ test() warnx("accel z: \t%d\traw", (int)accel_report.z_raw); warnx("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); - if (ERROR == (filter_bandwidth = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) + if (ERROR == (ret = ioctl(fd_accel, ACCELIOCGLOWPASS, 0))) warnx("accel antialias filter bandwidth: fail"); else - warnx("accel antialias filter bandwidth: %d Hz", filter_bandwidth); + warnx("accel antialias filter bandwidth: %d Hz", ret); int fd_mag = -1; struct mag_report m_report; @@ -1459,6 +1463,11 @@ test() if (fd_mag < 0) err(1, "%s open failed", MAG_DEVICE_PATH); + /* check if mag is onboard or external */ + if ((ret = ioctl(fd_mag, MAGIOCGEXTERNAL, 0)) < 0) + errx(1, "failed to get if mag is onboard or external"); + warnx("device active: %s", ret ? "external" : "onboard"); + /* do a simple demand read */ sz = read(fd_mag, &m_report, sizeof(m_report)); From bc717f1715590a6915c223dde53fe6e1139f92d2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 18 Aug 2013 09:33:59 +0200 Subject: [PATCH 56/70] Sensors should now take into account the orientation of an external mag --- src/modules/sensors/sensors.cpp | 20 ++++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c47f6bb7d9..198da9f0ae 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -284,6 +284,7 @@ private: math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ + bool _mag_is_external; /**< true if the active mag is on an external board */ struct { float min[_rc_max_chan_count]; @@ -537,7 +538,8 @@ Sensors::Sensors() : _loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")), _board_rotation(3,3), - _external_mag_rotation(3,3) + _external_mag_rotation(3,3), + _mag_is_external(false) { /* basic r/c parameters */ @@ -948,6 +950,14 @@ Sensors::mag_init() /* set the driver to poll at 150Hz */ ioctl(fd, SENSORIOCSPOLLRATE, 150); + int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + if (ret < 0) + errx(1, "FATAL: unknown if magnetometer is external or onboard"); + else if (ret == 1) + _mag_is_external = true; + else + _mag_is_external = false; + close(fd); } @@ -1044,10 +1054,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); - // XXX TODO add support for external mag orientation - math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z}; - vect = _board_rotation*vect; + + if (_mag_is_external) + vect = _external_mag_rotation*vect; + else + vect = _board_rotation*vect; raw.magnetometer_ga[0] = vect(0); raw.magnetometer_ga[1] = vect(1); From 80f38e3dea6b707314b407c7c511c19aa4eade39 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 18 Aug 2013 21:00:47 +0200 Subject: [PATCH 57/70] Put console and syslog on UART8, added support to nshterm for proper serial port config --- ROMFS/px4fmu_common/init.d/rcS | 9 ++++++++ makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 2 ++ nuttx-configs/px4fmu-v2/nsh/defconfig | 14 ++++++------ src/systemcmds/nshterm/nshterm.c | 31 ++++++++++++++++++++++++--- 5 files changed, 47 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index f0ee1a0c6e..e06d90d1c9 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -58,6 +58,15 @@ fi if [ $MODE == autostart ] then +# +# Start terminal +# +if sercon +then + echo "USB connected" + nshterm /dev/ttyACM0 & +fi + # # Start the ORB (first app to start) # diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index db13cc1972..8f2ade8dc8 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -54,6 +54,7 @@ MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config +MODULES += systemcmds/nshterm # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index cc182e6af5..101b497125 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -52,6 +52,8 @@ MODULES += systemcmds/pwm MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests +MODULES += systemcmds/config +MODULES += systemcmds/nshterm # # General system control diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 85bf6dd2fc..3e2a481087 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -420,7 +420,7 @@ CONFIG_TASK_NAME_SIZE=24 CONFIG_START_YEAR=1970 CONFIG_START_MONTH=1 CONFIG_START_DAY=1 -# CONFIG_DEV_CONSOLE is not set +CONFIG_DEV_CONSOLE=y # CONFIG_MUTEX_TYPES is not set CONFIG_PRIORITY_INHERITANCE=y CONFIG_SEM_PREALLOCHOLDERS=8 @@ -523,7 +523,7 @@ CONFIG_PIPES=y # CONFIG_POWER is not set # CONFIG_SENSORS is not set CONFIG_SERIAL=y -# CONFIG_DEV_LOWCONSOLE is not set +CONFIG_DEV_LOWCONSOLE=y CONFIG_SERIAL_REMOVABLE=y # CONFIG_16550_UART is not set CONFIG_ARCH_HAVE_UART4=y @@ -542,8 +542,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2 # CONFIG_UART4_SERIAL_CONSOLE is not set # CONFIG_USART6_SERIAL_CONSOLE is not set # CONFIG_UART7_SERIAL_CONSOLE is not set -# CONFIG_UART8_SERIAL_CONSOLE is not set -CONFIG_NO_SERIAL_CONSOLE=y +CONFIG_UART8_SERIAL_CONSOLE=y +# CONFIG_NO_SERIAL_CONSOLE is not set # # USART1 Configuration @@ -650,7 +650,7 @@ CONFIG_USBDEV_MAXPOWER=500 # CONFIG_USBDEV_COMPOSITE is not set # CONFIG_PL2303 is not set CONFIG_CDCACM=y -CONFIG_CDCACM_CONSOLE=y +CONFIG_CDCACM_CONSOLE=n CONFIG_CDCACM_EP0MAXPACKET=64 CONFIG_CDCACM_EPINTIN=1 CONFIG_CDCACM_EPINTIN_FSSIZE=64 @@ -716,10 +716,10 @@ CONFIG_FS_BINFS=y # # System Logging # -# CONFIG_SYSLOG_ENABLE is not set +CONFIG_SYSLOG_ENABLE=y CONFIG_SYSLOG=y CONFIG_SYSLOG_CHAR=y -CONFIG_SYSLOG_DEVPATH="/dev/ttyS0" +CONFIG_SYSLOG_DEVPATH="/dev/ttyS6" # # Graphics Support diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index bde0e78411..2341068a22 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -40,6 +40,7 @@ */ #include +#include #include #include #include @@ -48,6 +49,7 @@ #include #include #include +#include __EXPORT int nshterm_main(int argc, char *argv[]); @@ -61,8 +63,8 @@ nshterm_main(int argc, char *argv[]) uint8_t retries = 0; int fd = -1; while (retries < 5) { - // the retries are to cope with the behaviour of /dev/ttyACM0 - // which may not be ready immediately. + /* the retries are to cope with the behaviour of /dev/ttyACM0 */ + /* which may not be ready immediately. */ fd = open(argv[1], O_RDWR); if (fd != -1) { break; @@ -74,7 +76,30 @@ nshterm_main(int argc, char *argv[]) perror(argv[1]); exit(1); } - // setup standard file descriptors + + /* set up the serial port with output processing */ + + /* Try to set baud rate */ + struct termios uart_config; + int termios_state; + + /* Back up the original uart configuration to restore it after exit */ + if ((termios_state = tcgetattr(fd, &uart_config)) < 0) { + warnx("ERROR get termios config %s: %d\n", argv[1], termios_state); + close(fd); + return -1; + } + + /* Set ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag |= (ONLCR | OPOST | OCRNL); + + if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); + close(fd); + return -1; + } + + /* setup standard file descriptors */ close(0); close(1); close(2); From ffc2a8b7a358a2003e5ed548c41878b33e7aa726 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 18 Aug 2013 23:05:26 +0200 Subject: [PATCH 58/70] vehicle_local_position topic updated, position_estimator_inav and commander fixes, only altitude estimate required for SETBELT now. --- src/modules/commander/commander.cpp | 100 +++++---- .../commander/state_machine_helper.cpp | 27 +-- .../multirotor_pos_control.c | 20 +- .../position_estimator_inav_main.c | 195 +++++++++--------- .../position_estimator_inav_params.c | 3 - .../position_estimator_inav_params.h | 2 - src/modules/sdlog2/sdlog2.c | 7 +- src/modules/sdlog2/sdlog2_messages.h | 9 +- .../uORB/topics/vehicle_local_position.h | 39 ++-- src/modules/uORB/topics/vehicle_status.h | 4 +- 10 files changed, 194 insertions(+), 212 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d40f6675b5..ab7d2e6cf6 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -551,7 +551,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&control_mode, 0, sizeof(control_mode)); status.main_state = MAIN_STATE_MANUAL; - status.navigation_state = NAVIGATION_STATE_STANDBY; + status.navigation_state = NAVIGATION_STATE_DIRECT; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; @@ -812,8 +812,9 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } - /* update condition_local_position_valid */ - check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.valid, &(status.condition_local_position_valid), &status_changed); + /* update condition_local_position_valid and condition_local_altitude_valid */ + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); + check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); /* update battery status */ orb_check(battery_sub, &updated); @@ -1512,68 +1513,61 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v { transition_result_t res = TRANSITION_DENIED; - if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { - /* ARMED */ - switch (current_status->main_state) { - case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); - break; + switch (current_status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); + break; - case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); - break; + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); + break; - case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); - break; + case MAIN_STATE_EASY: + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); + break; - case MAIN_STATE_AUTO: - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* don't act while taking off */ - res = TRANSITION_NOT_CHANGED; + case MAIN_STATE_AUTO: + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + res = TRANSITION_NOT_CHANGED; + + } else { + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; - - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - - } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } - } + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); + } } + + } else { + /* always switch to loiter in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } - - break; - - default: - break; } - } else { - /* DISARMED */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode); + break; + + default: + break; } return res; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f313adce47..76c7eaf92f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -217,9 +217,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_SEATBELT: - /* need position estimate */ - // TODO only need altitude estimate really - if (current_state->condition_local_position_valid) { + /* need altitude estimate */ + if (current_state->condition_local_altitude_valid) { ret = TRANSITION_CHANGED; } @@ -227,7 +226,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: - /* need position estimate */ + /* need local position estimate */ if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -236,8 +235,8 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: - /* need position estimate */ - if (current_state->condition_local_position_valid) { + /* need global position estimate */ + if (current_state->condition_global_position_valid) { ret = TRANSITION_CHANGED; } @@ -277,17 +276,6 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ } else { switch (new_navigation_state) { - case NAVIGATION_STATE_STANDBY: - ret = TRANSITION_CHANGED; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_climb_rate_enabled = false; - control_mode->flag_control_manual_enabled = false; - break; - case NAVIGATION_STATE_DIRECT: ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; @@ -394,9 +382,8 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ case NAVIGATION_STATE_AUTO_LAND: - /* deny transitions from landed states */ - if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && - current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + /* deny transitions from landed state */ + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { ret = TRANSITION_CHANGED; control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 1cb4702409..80ce33cda5 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -219,7 +219,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) const float pos_ctl_dz = 0.05f; float home_alt = 0.0f; hrt_abstime home_alt_t = 0; - uint64_t local_home_timestamp = 0; + uint64_t local_ref_timestamp = 0; PID_t xy_pos_pids[2]; PID_t xy_vel_pids[2]; @@ -316,11 +316,11 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { /* non-manual mode, project global setpoints to local frame */ //orb_copy(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_sub, &local_pos_sp); - if (local_pos.home_timestamp != local_home_timestamp) { - local_home_timestamp = local_pos.home_timestamp; + if (local_pos.ref_timestamp != local_ref_timestamp) { + local_ref_timestamp = local_pos.ref_timestamp; /* init local projection using local position home */ - double lat_home = local_pos.home_lat * 1e-7; - double lon_home = local_pos.home_lon * 1e-7; + double lat_home = local_pos.ref_lat * 1e-7; + double lon_home = local_pos.ref_lon * 1e-7; map_projection_init(lat_home, lon_home); warnx("local pos home: lat = %.10f, lon = %.10f", lat_home, lon_home); mavlink_log_info(mavlink_fd, "local pos home: %.7f, %.7f", lat_home, lon_home); @@ -338,7 +338,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.z = -global_pos_sp.altitude; } else { - local_pos_sp.z = local_pos.home_alt - global_pos_sp.altitude; + local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } warnx("new setpoint: lat = %.10f, lon = %.10f, x = %.2f, y = %.2f", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); @@ -355,14 +355,14 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* manual control - move setpoints */ if (control_mode.flag_control_manual_enabled) { - if (local_pos.home_timestamp != home_alt_t) { + if (local_pos.ref_timestamp != home_alt_t) { if (home_alt_t != 0) { /* home alt changed, don't follow large ground level changes in manual flight */ - local_pos_sp.z += local_pos.home_alt - home_alt; + local_pos_sp.z += local_pos.ref_alt - home_alt; } - home_alt_t = local_pos.home_timestamp; - home_alt = local_pos.home_alt; + home_alt_t = local_pos.ref_timestamp; + home_alt = local_pos.ref_alt; } if (control_mode.flag_control_altitude_enabled) { 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 0e7e0ef5d7..81f938719c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -76,6 +76,7 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s +static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -169,10 +170,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int baro_init_num = 200; float baro_alt0 = 0.0f; /* to determine while start up */ - double lat_current = 0.0f; //[°] --> 47.0 - double lon_current = 0.0f; //[°] --> 8.5 - double alt_current = 0.0f; //[m] above MSL - uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -216,21 +213,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* first parameters update */ parameters_update(&pos_inav_param_handles, ¶ms); - struct pollfd fds_init[2] = { + struct pollfd fds_init[1] = { { .fd = sensor_combined_sub, .events = POLLIN }, - { .fd = vehicle_gps_position_sub, .events = POLLIN } }; - /* wait for initial sensors values: baro, GPS fix, only then can we initialize the projection */ - bool wait_gps = params.use_gps; + /* wait for initial baro value */ bool wait_baro = true; - hrt_abstime wait_gps_start = 0; - const hrt_abstime wait_gps_delay = 5000000; // wait for 5s after 3D fix thread_running = true; - while ((wait_gps || wait_baro) && !thread_should_exit) { - int ret = poll(fds_init, params.use_gps ? 2 : 1, 1000); + while (wait_baro && !thread_should_exit) { + int ret = poll(fds_init, 1, 1000); if (ret < 0) { /* poll error */ @@ -253,43 +246,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) baro_alt0 /= (float) baro_init_cnt; warnx("init baro: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] init baro: alt = %.3f", baro_alt0); - local_pos.home_alt = baro_alt0; - local_pos.home_timestamp = hrt_absolute_time(); - } - } - } - - if (params.use_gps && (fds_init[1].revents & POLLIN)) { - orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - - if (wait_gps && gps.fix_type >= 3) { - hrt_abstime t = hrt_absolute_time(); - - if (wait_gps_start == 0) { - wait_gps_start = t; - - } else if (t - wait_gps_start > wait_gps_delay) { - wait_gps = false; - /* get GPS position for first initialization */ - lat_current = gps.lat * 1e-7; - lon_current = gps.lon * 1e-7; - alt_current = gps.alt * 1e-3; - - local_pos.home_lat = lat_current * 1e7; - local_pos.home_lon = lon_current * 1e7; - local_pos.home_hdg = 0.0f; - local_pos.home_timestamp = t; - - /* initialize coordinates */ - map_projection_init(lat_current, lon_current); - warnx("init GPS: lat = %.10f, lon = %.10f", lat_current, lon_current); - mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat_current, lon_current); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + local_pos.z_valid = true; + local_pos.v_z_valid = true; + local_pos.global_z = true; } } } } } + bool ref_xy_inited = false; + hrt_abstime ref_xy_init_start = 0; + const hrt_abstime ref_xy_init_delay = 5000000; // wait for 5s after 3D fix + hrt_abstime t_prev = 0; uint16_t accel_updates = 0; @@ -337,7 +308,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } while (!thread_should_exit) { - int ret = poll(fds, params.use_gps ? 6 : 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate + int ret = poll(fds, 6, 10); // wait maximal this 10 ms = 100 Hz minimum rate hrt_abstime t = hrt_absolute_time(); if (ret < 0) { @@ -428,8 +399,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) baro_alt0 += sonar_corr; warnx("new home: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); - local_pos.home_alt = baro_alt0; - local_pos.home_timestamp = hrt_absolute_time(); + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); z_est[0] += sonar_corr; sonar_corr = 0.0f; sonar_corr_filtered = 0.0f; @@ -444,29 +415,57 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) flow_updates++; } - if (params.use_gps && (fds[5].revents & POLLIN)) { + if (fds[5].revents & POLLIN) { /* vehicle GPS position */ orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); if (gps.fix_type >= 3) { - /* project GPS lat lon to plane */ - float gps_proj[2]; - map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); - gps_corr[0][0] = gps_proj[0] - x_est[0]; - gps_corr[1][0] = gps_proj[1] - y_est[0]; + /* initialize reference position if needed */ + if (ref_xy_inited) { + /* project GPS lat lon to plane */ + float gps_proj[2]; + map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1])); + /* calculate correction for position */ + gps_corr[0][0] = gps_proj[0] - x_est[0]; + gps_corr[1][0] = gps_proj[1] - y_est[0]; - if (gps.vel_ned_valid) { - gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; - gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + /* calculate correction for velocity */ + if (gps.vel_ned_valid) { + gps_corr[0][1] = gps.vel_n_m_s - x_est[1]; + gps_corr[1][1] = gps.vel_e_m_s - y_est[1]; + + } else { + gps_corr[0][1] = 0.0f; + gps_corr[1][1] = 0.0f; + } } else { - gps_corr[0][1] = 0.0f; - gps_corr[1][1] = 0.0f; + hrt_abstime t = hrt_absolute_time(); + + if (ref_xy_init_start == 0) { + ref_xy_init_start = t; + + } else if (t > ref_xy_init_start + ref_xy_init_delay) { + ref_xy_inited = true; + /* reference GPS position */ + double lat = gps.lat * 1e-7; + double lon = gps.lon * 1e-7; + + local_pos.ref_lat = gps.lat; + local_pos.ref_lon = gps.lon; + local_pos.ref_timestamp = t; + + /* initialize projection */ + map_projection_init(lat, lon); + warnx("init GPS: lat = %.10f, lon = %.10f", lat, lon); + mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon); + } } gps_updates++; } else { + /* no GPS lock */ memset(gps_corr, 0, sizeof(gps_corr)); } } @@ -490,10 +489,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(sonar_corr, dt, z_est, 0, params.w_alt_sonar); inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc); - /* dont't try to estimate position when no any position source available */ - bool can_estimate_pos = params.use_gps && gps.fix_type >= 3 && t - gps.timestamp_position < gps_timeout; + bool gps_valid = ref_xy_inited && gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout; + bool flow_valid = false; // TODO implement opt flow - if (can_estimate_pos) { + /* try to estimate xy even if no absolute position source available, + * if using optical flow velocity will be correct in this case */ + bool can_estimate_xy = gps_valid || flow_valid; + + if (can_estimate_xy) { /* inertial filter prediction for position */ inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, y_est); @@ -502,12 +505,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc); inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc); - inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); - inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); - - if (gps.vel_ned_valid && t - gps.timestamp_velocity < gps_timeout) { - inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); - inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + if (gps_valid) { + inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, params.w_pos_gps_p); + inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, params.w_pos_gps_p); + if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_timeout) { + inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, params.w_pos_gps_v); + inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, params.w_pos_gps_v); + } } } @@ -533,43 +537,48 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (t - pub_last > pub_interval) { pub_last = t; + /* publish local position */ local_pos.timestamp = t; - local_pos.valid = can_estimate_pos; + local_pos.xy_valid = can_estimate_xy && gps_valid; + local_pos.v_xy_valid = can_estimate_xy; + local_pos.global_xy = local_pos.xy_valid && gps_valid; // will make sense when local position sources (e.g. vicon) will be implemented local_pos.x = x_est[0]; local_pos.vx = x_est[1]; local_pos.y = y_est[0]; local_pos.vy = y_est[1]; local_pos.z = z_est[0]; local_pos.vz = z_est[1]; - local_pos.absolute_alt = local_pos.home_alt - local_pos.z; - local_pos.hdg = att.yaw; + local_pos.landed = false; // TODO - if ((isfinite(local_pos.x)) && (isfinite(local_pos.vx)) - && (isfinite(local_pos.y)) - && (isfinite(local_pos.vy)) - && (isfinite(local_pos.z)) - && (isfinite(local_pos.vz))) { - orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); + orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos); - if (params.use_gps) { - double est_lat, est_lon; - map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); - - global_pos.valid = local_pos.valid; - global_pos.timestamp = t; - global_pos.time_gps_usec = gps.time_gps_usec; - global_pos.lat = (int32_t)(est_lat * 1e7); - global_pos.lon = (int32_t)(est_lon * 1e7); - global_pos.alt = local_pos.home_alt - local_pos.z; - global_pos.relative_alt = -local_pos.z; - global_pos.vx = local_pos.vx; - global_pos.vy = local_pos.vy; - global_pos.vz = local_pos.vz; - global_pos.hdg = local_pos.hdg; - - orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); - } + /* publish global position */ + global_pos.valid = local_pos.global_xy; + if (local_pos.global_xy) { + double est_lat, est_lon; + map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon); + global_pos.lat = (int32_t)(est_lat * 1e7); + global_pos.lon = (int32_t)(est_lon * 1e7); + global_pos.time_gps_usec = gps.time_gps_usec; } + /* set valid values even if position is not valid */ + if (local_pos.v_xy_valid) { + global_pos.vx = local_pos.vx; + global_pos.vy = local_pos.vy; + global_pos.hdg = atan2f(local_pos.vy, local_pos.vx); + } + if (local_pos.z_valid) { + global_pos.relative_alt = -local_pos.z; + } + if (local_pos.global_z) { + global_pos.alt = local_pos.ref_alt - local_pos.z; + } + if (local_pos.v_z_valid) { + global_pos.vz = local_pos.vz; + } + global_pos.timestamp = t; + + orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } } 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 70248b3b72..801e207816 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -40,7 +40,6 @@ #include "position_estimator_inav_params.h" -PARAM_DEFINE_INT32(INAV_USE_GPS, 1); PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 1.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 50.0f); PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f); @@ -55,7 +54,6 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); int parameters_init(struct position_estimator_inav_param_handles *h) { - h->use_gps = param_find("INAV_USE_GPS"); h->w_alt_baro = param_find("INAV_W_ALT_BARO"); h->w_alt_acc = param_find("INAV_W_ALT_ACC"); h->w_alt_sonar = param_find("INAV_W_ALT_SONAR"); @@ -73,7 +71,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h) int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p) { - param_get(h->use_gps, &(p->use_gps)); param_get(h->w_alt_baro, &(p->w_alt_baro)); param_get(h->w_alt_acc, &(p->w_alt_acc)); param_get(h->w_alt_sonar, &(p->w_alt_sonar)); 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 1e70a3c48a..ed6f61e04c 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -41,7 +41,6 @@ #include struct position_estimator_inav_params { - int use_gps; float w_alt_baro; float w_alt_acc; float w_alt_sonar; @@ -56,7 +55,6 @@ struct position_estimator_inav_params { }; struct position_estimator_inav_param_handles { - param_t use_gps; param_t w_alt_baro; param_t w_alt_acc; param_t w_alt_sonar; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 30dc7df9e1..7f8648d95d 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1042,10 +1042,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_LPOS.vx = buf.local_pos.vx; log_msg.body.log_LPOS.vy = buf.local_pos.vy; log_msg.body.log_LPOS.vz = buf.local_pos.vz; - log_msg.body.log_LPOS.hdg = buf.local_pos.hdg; - log_msg.body.log_LPOS.home_lat = buf.local_pos.home_lat; - log_msg.body.log_LPOS.home_lon = buf.local_pos.home_lon; - log_msg.body.log_LPOS.home_alt = buf.local_pos.home_alt; + log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat; + log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon; + log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt; LOGBUFFER_WRITE_AND_COUNT(LPOS); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 38e2596b2a..dd98f65a9c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -106,10 +106,9 @@ struct log_LPOS_s { float vx; float vy; float vz; - float hdg; - int32_t home_lat; - int32_t home_lon; - float home_alt; + int32_t ref_lat; + int32_t ref_lon; + float ref_alt; }; /* --- LPSP - LOCAL POSITION SETPOINT --- */ @@ -260,7 +259,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), - LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,Heading,HomeLat,HomeLon,HomeAlt"), + LOG_FORMAT(LPOS, "fffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h index 9d3b4468c6..26e8f335b5 100644 --- a/src/modules/uORB/topics/vehicle_local_position.h +++ b/src/modules/uORB/topics/vehicle_local_position.h @@ -54,27 +54,26 @@ */ struct vehicle_local_position_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ - bool valid; /**< true if position satisfies validity criteria of estimator */ - - float x; /**< X positin in meters in NED earth-fixed frame */ - float y; /**< X positin in meters in NED earth-fixed frame */ - float z; /**< Z positin in meters in NED earth-fixed frame (negative altitude) */ - float absolute_alt; /**< Altitude as defined by pressure / GPS, LOGME */ - float vx; /**< Ground X Speed (Latitude), m/s in NED LOGME */ - float vy; /**< Ground Y Speed (Longitude), m/s in NED LOGME */ - float vz; /**< Ground Z Speed (Altitude), m/s in NED LOGME */ - float hdg; /**< Compass heading in radians -PI..+PI. */ - - // TODO Add covariances here - + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + bool xy_valid; /**< true if x and y are valid */ + bool z_valid; /**< true if z is valid */ + bool v_xy_valid; /**< true if vy and vy are valid */ + bool v_z_valid; /**< true if vz is valid */ + /* Position in local NED frame */ + float x; /**< X position in meters in NED earth-fixed frame */ + float y; /**< X position in meters in NED earth-fixed frame */ + float z; /**< Z position in meters in NED earth-fixed frame (negative altitude) */ + /* Velocity in NED frame */ + float vx; /**< Ground X Speed (Latitude), m/s in NED */ + float vy; /**< Ground Y Speed (Longitude), m/s in NED */ + float vz; /**< Ground Z Speed (Altitude), m/s in NED */ /* Reference position in GPS / WGS84 frame */ - uint64_t home_timestamp;/**< Time when home position was set */ - int32_t home_lat; /**< Latitude in 1E7 degrees LOGME */ - int32_t home_lon; /**< Longitude in 1E7 degrees LOGME */ - float home_alt; /**< Altitude in meters LOGME */ - float home_hdg; /**< Compass heading in radians -PI..+PI. */ - + bool global_xy; /**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */ + bool global_z; /**< true if z is valid and has valid global reference (ref_alt) */ + uint64_t ref_timestamp; /**< Time when reference position was set */ + int32_t ref_lat; /**< Reference point latitude in 1E7 degrees */ + int32_t ref_lon; /**< Reference point longitude in 1E7 degrees */ + float ref_alt; /**< Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level */ bool landed; /**< true if vehicle is landed */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6690fb2be2..4317e07f25 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -68,8 +68,7 @@ typedef enum { /* navigation state machine */ typedef enum { - NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed - NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization + NAVIGATION_STATE_DIRECT = 0, // true manual control, no any stabilization NAVIGATION_STATE_STABILIZE, // attitude stabilization NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization @@ -203,6 +202,7 @@ struct vehicle_status_s bool condition_launch_position_valid; /**< indicates a valid launch position */ bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; + bool condition_local_altitude_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ From 3370ceceaf706dda0856888b09c1086e8bf79c8d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 08:43:16 +0200 Subject: [PATCH 59/70] vehicle_control_mode.flag_armed added, reset integrals in multirotor_att_control when disarmed --- src/modules/commander/commander.cpp | 3 +- .../multirotor_att_control_main.c | 33 ++++++++----------- .../multirotor_attitude_control.c | 9 ++--- .../multirotor_attitude_control.h | 2 +- .../multirotor_rate_control.c | 7 ++-- .../multirotor_rate_control.h | 2 +- .../uORB/topics/vehicle_control_mode.h | 2 ++ 7 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ab7d2e6cf6..7d74e0cfe2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1235,8 +1235,9 @@ int commander_thread_main(int argc, char *argv[]) } /* publish control mode */ - if (navigation_state_changed) { + if (navigation_state_changed || arming_state_changed) { /* publish new navigation state */ + control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic control_mode.counter++; control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 5cad667f61..c057ef3647 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -142,16 +142,13 @@ mc_thread_main(int argc, char *argv[]) perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "multirotor_att_control_err"); /* welcome user */ - printf("[multirotor_att_control] starting\n"); + warnx("starting"); /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; - - /* store if we stopped a yaw movement */ bool reset_yaw_sp = true; /* prepare the handle for the failsafe throttle */ @@ -211,8 +208,7 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - /** STEP 1: Define which input is the dominating control input */ + /* define which input is the dominating control input */ if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { @@ -220,7 +216,6 @@ mc_thread_main(int argc, char *argv[]) rates_sp.pitch = offboard_sp.p2; rates_sp.yaw = offboard_sp.p3; rates_sp.thrust = offboard_sp.p4; -// printf("thrust_rate=%8.4f\n",offboard_sp.p4); rates_sp.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); @@ -229,13 +224,11 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = offboard_sp.p2; att_sp.yaw_body = offboard_sp.p3; att_sp.thrust = offboard_sp.p4; -// printf("thrust_att=%8.4f\n",offboard_sp.p4); att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the result to the vehicle actuators */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); } - } else if (control_mode.flag_control_manual_enabled) { /* direct manual input */ if (control_mode.flag_control_attitude_enabled) { @@ -265,7 +258,7 @@ mc_thread_main(int argc, char *argv[]) * multicopter (throttle = 0) does not make it jump up in the air * if shutting down his remote. */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { + if (isfinite(manual.throttle) && manual.throttle > 0.2f) { // TODO use landed status instead of throttle /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); att_sp.thrust = failsafe_throttle; @@ -305,7 +298,6 @@ mc_thread_main(int argc, char *argv[]) att_sp.yaw_body = att.yaw; reset_yaw_sp = false; } - control_yaw_position = true; } @@ -347,22 +339,25 @@ mc_thread_main(int argc, char *argv[]) } } - /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (control_mode.flag_control_attitude_enabled) { - multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); + /* check if we should we reset integrals */ + bool reset_integral = !control_mode.flag_armed || att_sp.thrust < 0.1f; // TODO use landed status instead of throttle + /* run attitude controller if needed */ + if (control_mode.flag_control_attitude_enabled) { + multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position, reset_integral); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); } /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); + /* run rates controller if needed */ if (control_mode.flag_control_rates_enabled) { /* get current rate setpoint */ - bool rates_sp_valid = false; - orb_check(rates_sp_sub, &rates_sp_valid); + bool rates_sp_updated = false; + orb_check(rates_sp_sub, &rates_sp_updated); - if (rates_sp_valid) { + if (rates_sp_updated) { orb_copy(ORB_ID(vehicle_rates_setpoint), rates_sp_sub, &rates_sp); } @@ -371,7 +366,7 @@ mc_thread_main(int argc, char *argv[]) rates[0] = att.rollspeed; rates[1] = att.pitchspeed; rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, rates, &actuators); + multirotor_control_rates(&rates_sp, rates, &actuators, reset_integral); } else { /* rates controller disabled, set actuators to zero for safety */ actuators.control[0] = 0.0f; @@ -391,7 +386,7 @@ mc_thread_main(int argc, char *argv[]) } /* end of poll return value check */ } - printf("[multirotor att control] stopping, disarming motors.\n"); + warnx("stopping, disarming motors"); /* kill all outputs */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 8f19c6a4bc..12d16f7c73 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -166,7 +166,7 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc } void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position) + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral) { static uint64_t last_run = 0; static uint64_t last_input = 0; @@ -210,13 +210,13 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } - /* reset integral if on ground */ - if (att_sp->thrust < 0.1f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_controller); pid_reset_integral(&roll_controller); + //TODO pid_reset_integral(&yaw_controller); } - /* calculate current control outputs */ /* control pitch (forward) output */ @@ -229,6 +229,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s if (control_yaw_position) { /* control yaw rate */ + // TODO use pid lib /* positive error: rotate to right, negative error, rotate to left (NED frame) */ // yaw_error = _wrap_pi(att_sp->yaw_body - att->yaw); diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h index e78f45c474..431a435f7d 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.h +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h @@ -60,6 +60,6 @@ #include void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_sp, - const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position); + const struct vehicle_attitude_s *att, struct vehicle_rates_setpoint_s *rates_sp, bool control_yaw_position, bool reset_integral); #endif /* MULTIROTOR_ATTITUDE_CONTROL_H_ */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e58d357d58..0a336be47d 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -152,7 +152,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], struct actuator_controls_s *actuators, bool reset_integral) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -193,10 +193,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f); } - /* reset integral if on ground */ - if (rate_sp->thrust < 0.01f) { + /* reset integrals if needed */ + if (reset_integral) { pid_reset_integral(&pitch_rate_controller); pid_reset_integral(&roll_rate_controller); + // TODO pid_reset_integral(&yaw_rate_controller); } /* control pitch (forward) output */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 362b5ed86d..ca7794c59b 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -59,6 +59,6 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], struct actuator_controls_s *actuators, bool reset_integral); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index fe169c6e6b..67aeac3728 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -64,6 +64,8 @@ struct vehicle_control_mode_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + bool flag_armed; + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ // XXX needs yet to be set by state machine helper From f96bb824d4fc6f6d36ddf24e8879d3025af39d70 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 19 Aug 2013 09:31:33 +0200 Subject: [PATCH 60/70] multirotor_pos_control: reset integrals when disarmed --- .../multirotor_pos_control.c | 16 ++++++++++++---- src/modules/multirotor_pos_control/thrust_pid.c | 5 +++++ src/modules/multirotor_pos_control/thrust_pid.h | 1 + 3 files changed, 18 insertions(+), 4 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index 80ce33cda5..b2f6b33e3c 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -301,7 +301,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) if (reset_sp_alt) { reset_sp_alt = false; local_pos_sp.z = local_pos.z; - z_vel_pid.integral = -manual.throttle; // thrust PID uses Z downside + thrust_pid_set_integral(&z_vel_pid, -manual.throttle); // thrust PID uses Z downside mavlink_log_info(mavlink_fd, "reset alt setpoint: z = %.2f, throttle = %.2f", local_pos_sp.z, manual.throttle); } @@ -309,8 +309,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_sp_pos = false; local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; - xy_vel_pids[0].integral = 0.0f; - xy_vel_pids[1].integral = 0.0f; + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); mavlink_log_info(mavlink_fd, "reset pos setpoint: x = %.2f, y = %.2f", local_pos_sp.x, local_pos_sp.y); } } else { @@ -439,17 +439,25 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) /* publish new velocity setpoint */ orb_publish(ORB_ID(vehicle_global_velocity_setpoint), global_vel_sp_pub, &global_vel_sp); - // TODO subcrive to velocity setpoint if altitude/position control disabled + // TODO subscribe to velocity setpoint if altitude/position control disabled if (control_mode.flag_control_climb_rate_enabled || control_mode.flag_control_velocity_enabled) { /* run velocity controllers, calculate thrust vector with attitude-thrust compensation */ float thrust_sp[3] = { 0.0f, 0.0f, 0.0f }; + bool reset_integral = !control_mode.flag_armed; if (control_mode.flag_control_climb_rate_enabled) { + if (reset_integral) { + thrust_pid_set_integral(&z_vel_pid, params.thr_min); + } thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]); att_sp.thrust = -thrust_sp[2]; } if (control_mode.flag_control_velocity_enabled) { /* calculate thrust set point in NED frame */ + if (reset_integral) { + pid_reset_integral(&xy_vel_pids[0]); + pid_reset_integral(&xy_vel_pids[1]); + } thrust_sp[0] = pid_calculate(&xy_vel_pids[0], global_vel_sp.vx, local_pos.vx, 0.0f, dt); thrust_sp[1] = pid_calculate(&xy_vel_pids[1], global_vel_sp.vy, local_pos.vy, 0.0f, dt); diff --git a/src/modules/multirotor_pos_control/thrust_pid.c b/src/modules/multirotor_pos_control/thrust_pid.c index 51dcd7df24..b985630aec 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.c +++ b/src/modules/multirotor_pos_control/thrust_pid.c @@ -182,3 +182,8 @@ __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, floa return pid->last_output; } + +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i) +{ + pid->integral = i; +} diff --git a/src/modules/multirotor_pos_control/thrust_pid.h b/src/modules/multirotor_pos_control/thrust_pid.h index 551c032fe2..5e169c1ba5 100644 --- a/src/modules/multirotor_pos_control/thrust_pid.h +++ b/src/modules/multirotor_pos_control/thrust_pid.h @@ -69,6 +69,7 @@ typedef struct { __EXPORT void thrust_pid_init(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max, uint8_t mode, float dt_min); __EXPORT int thrust_pid_set_parameters(thrust_pid_t *pid, float kp, float ki, float kd, float limit_min, float limit_max); __EXPORT float thrust_pid_calculate(thrust_pid_t *pid, float sp, float val, float dt, float r22); +__EXPORT void thrust_pid_set_integral(thrust_pid_t *pid, float i); __END_DECLS From 9610f7a0d7ba7abf7d88c4b3096285e3da68e04d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 09:53:00 +0200 Subject: [PATCH 61/70] Fixed merge compile errors --- src/modules/commander/commander.cpp | 103 ++++++++++++---------------- 1 file changed, 42 insertions(+), 61 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0f145e1eb1..daab7e4360 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -418,23 +418,14 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: if (is_safe(status, safety, armed)) { -<<<<<<< HEAD - if (((int)(cmd->param1)) == 1) { - /* reboot */ - systemreset(false); - } else if (((int)(cmd->param1)) == 3) { - /* reboot to bootloader */ -======= if (((int)(cmd->param1)) == 1) { /* reboot */ - up_systemreset(); + systemreset(false); } else if (((int)(cmd->param1)) == 3) { /* reboot to bootloader */ - // XXX implement - result = VEHICLE_CMD_RESULT_UNSUPPORTED; ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 + systemreset(true); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -768,13 +759,7 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { -<<<<<<< HEAD - hrt_abstime t = hrt_absolute_time(); - status_changed = false; - -======= ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 /* update parameters */ orb_check(param_changed_sub, &updated); @@ -882,17 +867,11 @@ int commander_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); -<<<<<<< HEAD - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if ((t - start_time) > 2000000 && battery.voltage_v > 0.001f) { -======= - - warnx("bat v: %2.2f", battery.voltage_v); + // warnx("bat v: %2.2f", battery.voltage_v); /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -1010,39 +989,39 @@ int commander_thread_main(int argc, char *argv[]) * set of position measurements is available. */ -<<<<<<< HEAD - /* store current state to reason later about a state change */ - // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - // bool global_pos_valid = status.condition_global_position_valid; - // bool local_pos_valid = status.condition_local_position_valid; - // bool airspeed_valid = status.condition_airspeed_valid; +// <<<<<<< HEAD +// /* store current state to reason later about a state change */ +// // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; +// // bool global_pos_valid = status.condition_global_position_valid; +// // bool local_pos_valid = status.condition_local_position_valid; +// // bool airspeed_valid = status.condition_airspeed_valid; - /* check for global or local position updates, set a timeout of 2s */ - if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { - status.condition_global_position_valid = true; +// /* check for global or local position updates, set a timeout of 2s */ +// if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { +// status.condition_global_position_valid = true; - } else { - status.condition_global_position_valid = false; - } +// } else { +// status.condition_global_position_valid = false; +// } - if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { - status.condition_local_position_valid = true; +// if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { +// status.condition_local_position_valid = true; - } else { - status.condition_local_position_valid = false; - } +// } else { +// status.condition_local_position_valid = false; +// } - /* Check for valid airspeed/differential pressure measurements */ - if (t - last_diff_pres_time < 2000000 && t > 2000000) { - status.condition_airspeed_valid = true; +// /* Check for valid airspeed/differential pressure measurements */ +// if (t - last_diff_pres_time < 2000000 && t > 2000000) { +// status.condition_airspeed_valid = true; - } else { - status.condition_airspeed_valid = false; - } +// } else { +// status.condition_airspeed_valid = false; +// } -======= ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 +// ======= +// >>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 orb_check(gps_sub, &updated); if (updated) { @@ -1362,9 +1341,9 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); - control_mode.timestamp = t; + control_mode.timestamp = t1; orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - armed.timestamp = t; + armed.timestamp = t1; orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } @@ -1433,6 +1412,18 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed) +{ + hrt_abstime t = hrt_absolute_time(); + bool valid_new = (t < timestamp + timeout && t > timeout && valid_in); + + if (*valid_out != valid_new) { + *valid_out = valid_new; + *changed = true; + } +} + void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed) { @@ -1512,18 +1503,8 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang /* not ready to arm, blink at 10Hz */ } -<<<<<<< HEAD rgbled_set_pattern(&pattern); } -======= - if (status->condition_global_position_valid) { - /* position estimated, solid */ - led_on(LED_BLUE); - - } else if (leds_counter == 6) { - /* waiting for position estimate, short blink at 1.25Hz */ - led_on(LED_BLUE); ->>>>>>> f96bb824d4fc6f6d36ddf24e8879d3025af39d70 /* give system warnings on error LED, XXX maybe add memory usage warning too */ if (status->load > 0.95f) { From 1ae5a6ac1d3da98e5612b77ff28afa86669c287f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:02:15 +0200 Subject: [PATCH 62/70] Minimized nshterm flags, fixed some pretty stupid non-standard coding in top, now behaving with two shell instances as expected --- src/systemcmds/nshterm/nshterm.c | 2 +- src/systemcmds/top/top.c | 18 ++++++++++-------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 2341068a22..41d108ffc5 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -91,7 +91,7 @@ nshterm_main(int argc, char *argv[]) } /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST | OCRNL); + uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 0d064a05eb..1ca3fc9281 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -107,9 +107,6 @@ top_main(void) float interval_time_ms_inv = 0.f; - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - /* clear screen */ printf("\033[2J"); @@ -256,17 +253,24 @@ top_main(void) interval_start_time = new_time; /* Sleep 200 ms waiting for user input five times ~ 1s */ - /* XXX use poll ... */ for (int k = 0; k < 5; k++) { char c; - if (read(console, &c, 1) == 1) { + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + switch (c) { case 0x03: // ctrl-c case 0x1b: // esc case 'c': case 'q': - close(console); return OK; /* not reached */ } @@ -278,7 +282,5 @@ top_main(void) new_time = hrt_absolute_time(); } - close(console); - return OK; } From a9743f04be59aee7bb96a5bb99ae8d8155eb19de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:09:06 +0200 Subject: [PATCH 63/70] Fixed status changed flag --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index daab7e4360..98aab8788a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1317,6 +1317,8 @@ int commander_thread_main(int argc, char *argv[]) if (arming_state_changed || main_state_changed || navigation_state_changed) { mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); status_changed = true; + } else { + status_changed = false; } hrt_abstime t1 = hrt_absolute_time(); @@ -1446,8 +1448,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang if (changed) { - warnx("changed"); - int i; rgbled_pattern_t pattern; memset(&pattern, 0, sizeof(pattern)); From cdd09333f946e746527c8e9af36e08dc3a29a975 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:02:15 +0200 Subject: [PATCH 64/70] Minimized nshterm flags, fixed some pretty stupid non-standard coding in top, now behaving with two shell instances as expected --- src/systemcmds/nshterm/nshterm.c | 2 +- src/systemcmds/top/top.c | 18 ++++++++++-------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c index 2341068a22..41d108ffc5 100644 --- a/src/systemcmds/nshterm/nshterm.c +++ b/src/systemcmds/nshterm/nshterm.c @@ -91,7 +91,7 @@ nshterm_main(int argc, char *argv[]) } /* Set ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag |= (ONLCR | OPOST | OCRNL); + uart_config.c_oflag |= (ONLCR | OPOST/* | OCRNL*/); if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", argv[1]); diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c index 0d064a05eb..1ca3fc9281 100644 --- a/src/systemcmds/top/top.c +++ b/src/systemcmds/top/top.c @@ -107,9 +107,6 @@ top_main(void) float interval_time_ms_inv = 0.f; - /* Open console directly to grab CTRL-C signal */ - int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); - /* clear screen */ printf("\033[2J"); @@ -256,17 +253,24 @@ top_main(void) interval_start_time = new_time; /* Sleep 200 ms waiting for user input five times ~ 1s */ - /* XXX use poll ... */ for (int k = 0; k < 5; k++) { char c; - if (read(console, &c, 1) == 1) { + struct pollfd fds; + int ret; + fds.fd = 0; /* stdin */ + fds.events = POLLIN; + ret = poll(&fds, 1, 0); + + if (ret > 0) { + + read(0, &c, 1); + switch (c) { case 0x03: // ctrl-c case 0x1b: // esc case 'c': case 'q': - close(console); return OK; /* not reached */ } @@ -278,7 +282,5 @@ top_main(void) new_time = hrt_absolute_time(); } - close(console); - return OK; } From 871b4c19bc65bf923887e0bd32e1889db1c71aca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 15:19:51 +0200 Subject: [PATCH 65/70] Added stop command to RGB led --- src/drivers/rgbled/rgbled.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp index 467379a776..05f079ede2 100644 --- a/src/drivers/rgbled/rgbled.cpp +++ b/src/drivers/rgbled/rgbled.cpp @@ -443,7 +443,7 @@ void rgbled_usage(); void rgbled_usage() { - warnx("missing command: try 'start', 'test', 'info', 'off', 'rgb'"); + warnx("missing command: try 'start', 'test', 'info', 'stop'/'off', 'rgb 30 40 50'"); warnx("options:"); warnx(" -b i2cbus (%d)", PX4_I2C_BUS_LED); errx(0, " -a addr (0x%x)", ADDR); @@ -534,7 +534,8 @@ rgbled_main(int argc, char *argv[]) exit(0); } - if (!strcmp(verb, "off")) { + if (!strcmp(verb, "stop") || !strcmp(verb, "off")) { + /* although technically it doesn't stop, this is the excepted syntax */ fd = open(RGBLED_DEVICE_PATH, 0); if (fd == -1) { errx(1, "Unable to open " RGBLED_DEVICE_PATH); From 12df5dd2699f163bc5551b2be611665fc58fb001 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Aug 2013 16:32:56 +0200 Subject: [PATCH 66/70] Corrected orientation of external mag --- src/drivers/hmc5883/hmc5883.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 9e9c067d53..692f890bdd 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -860,12 +860,13 @@ HMC5883::collect() _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; } else { #endif - /* the standard external mag seems to be rolled 180deg, therefore y and z inverted */ - _reports[_next_report].x = ((report.x * _range_scale) - _scale.x_offset) * _scale.x_scale; + /* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down, + * therefore switch and invert x and y */ + _reports[_next_report].x = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.x_offset) * _scale.x_scale; /* flip axes and negate value for y */ - _reports[_next_report].y = ((((report.y == -32768) ? 32767 : -report.y) * _range_scale) - _scale.y_offset) * _scale.y_scale; + _reports[_next_report].y = ((((report.x == -32768) ? 32767 : -report.x) * _range_scale) - _scale.y_offset) * _scale.y_scale; /* z remains z */ - _reports[_next_report].z = ((((report.z == -32768) ? 32767 : -report.z) * _range_scale) - _scale.z_offset) * _scale.z_scale; + _reports[_next_report].z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale; #ifdef PX4_I2C_BUS_ONBOARD } #endif From 00c9b8f87f40ffca47ade881087a50b6162fd185 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Aug 2013 16:34:12 +0200 Subject: [PATCH 67/70] Start the hmc5883 before the lsm303d so that an external mag is used --- ROMFS/px4fmu_common/init.d/rc.sensors | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 17591be5b3..762e80e1f0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -18,10 +18,12 @@ fi ms5611 start adc start +# mag might be external +hmc5883 start + if mpu6000 start then - echo "using MPU6000 and HMC5883L" - hmc5883 start + echo "using MPU6000" set BOARD fmuv1 else echo "using L3GD20 and LSM303D" From a95e48c0b25c75548e923657247cdf268e654097 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 19 Aug 2013 16:51:22 +0200 Subject: [PATCH 68/70] Don't stop the startup script if no external HMC5883 is connected --- ROMFS/px4fmu_common/init.d/rc.sensors | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 762e80e1f0..61bb097281 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -19,7 +19,10 @@ ms5611 start adc start # mag might be external -hmc5883 start +if hmc5883 start +then + echo "using HMC5883" +fi if mpu6000 start then From f4b5a17a7b6385869933cd195afd674fa532e735 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 17:35:07 +0200 Subject: [PATCH 69/70] Improved sensor startup and error checking --- ROMFS/px4fmu_common/init.d/rc.sensors | 4 ++++ src/modules/sensors/sensors.cpp | 2 +- src/systemcmds/preflight_check/preflight_check.c | 2 +- 3 files changed, 6 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 17591be5b3..4cfd59d543 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -24,6 +24,10 @@ then hmc5883 start set BOARD fmuv1 else + if hmc5883 start + then + echo "Using external mag" + fi echo "using L3GD20 and LSM303D" l3gd20 start lsm303d start diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index a6204c9fab..6e57a79a83 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -953,7 +953,7 @@ Sensors::baro_init() if (fd < 0) { warn("%s", BARO_DEVICE_PATH); - warnx("No barometer found, ignoring"); + errx(1, "FATAL: No barometer found"); } /* set the driver to poll at 150Hz */ diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index d1dd85d479..e7d9ce85fb 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -263,7 +263,7 @@ system_eval: led_toggle(leds, LED_BLUE); /* display and sound error */ - for (int i = 0; i < 150; i++) + for (int i = 0; i < 50; i++) { led_toggle(leds, LED_BLUE); led_toggle(leds, LED_AMBER); From 7bd952c7acf7bc1c588a123d395930525be1349c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Aug 2013 17:38:03 +0200 Subject: [PATCH 70/70] Prevented double HMC start after merge --- ROMFS/px4fmu_common/init.d/rc.sensors | 4 ---- 1 file changed, 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 02b70e7da4..61bb097281 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -29,10 +29,6 @@ then echo "using MPU6000" set BOARD fmuv1 else - if hmc5883 start - then - echo "Using external mag" - fi echo "using L3GD20 and LSM303D" l3gd20 start lsm303d start