diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index b3c9205586..5aef3ffc7f 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -32,23 +32,15 @@ set(config_module_list modules/sensors modules/simulator modules/mavlink + modules/dataman modules/attitude_estimator_ekf - modules/attitude_estimator_q + #modules/attitude_estimator_q modules/ekf_att_pos_estimator modules/position_estimator_inav modules/navigator modules/vtol_att_control - modules/mc_pos_control - modules/mc_att_control - modules/mc_pos_control_multiplatform - modules/mc_att_control_multiplatform - modules/land_detector - modules/fw_att_control - modules/fw_pos_control_l1 - modules/dataman - modules/sdlog2 - modules/commander modules/controllib + modules/ekf2 lib/mathlib lib/mathlib/math/filter lib/conversion @@ -59,6 +51,7 @@ set(config_module_list lib/launchdetection lib/terrain_estimation lib/runway_takeoff + lib/ecl/EKF/tests/base ) set(config_extra_builtin_cmds diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 88a3f3978e..71d53bec34 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -61,3 +61,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 mavlink boot_complete sdlog2 start -r 100 -e -t -a +ekf2 start diff --git a/src/lib/ecl b/src/lib/ecl index e5743d503c..8d6864304f 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit e5743d503c416693f11be3af13fdcd442e49cbce +Subproject commit 8d6864304fa3a18f728730f9e49fa07640a87e72 diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt new file mode 100644 index 0000000000..75db628be7 --- /dev/null +++ b/src/modules/ekf2/CMakeLists.txt @@ -0,0 +1,47 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# 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. +# +############################################################################# +set(MODULE_CFLAGS) +if (${OS} STREQUAL "nuttx") + list(APPEND MODULE_CFLAGS -Wframe-larger-than=30000) +endif() +px4_add_module( + MODULE modules__ekf2 + MAIN ekf2 + COMPILE_FLAGS ${MODULE_CFLAGS} + STACK 30000 + SRCS + ekf2_main.cpp + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp new file mode 100644 index 0000000000..d89aebcb0e --- /dev/null +++ b/src/modules/ekf2/ekf2_main.cpp @@ -0,0 +1,320 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * 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 ekf2_main.cpp + * Implementation of the attitude and position estimator. + * + * @author Roman Bapst + */ + +#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 + +#include + + +extern "C" __EXPORT int ekf2_main(int argc, char *argv[]); + + +class Ekf2; + +namespace ekf2 +{ +Ekf2 *instance; +} + + +class Ekf2 +{ +public: + /** + * Constructor + */ + Ekf2(); + + /** + * Destructor, also kills task. + */ + ~Ekf2(); + + /** + * Start task. + * + * @return OK on success. + */ + int start(); + + static void task_main_trampoline(int argc, char *argv[]); + + void task_main(); + + void print(); + +private: + static constexpr float _dt_max = 0.02; + bool _task_should_exit = false; /**< if true, task should exit */ + int _control_task = -1; /**< task handle for task */ + + int _sensors_sub = -1; + int _gps_sub = -1; + int _airspeed_sub = -1; + + EstimatorBase *_ekf; + + + void update_parameters(bool force); + + int update_subscriptions(); + +}; + +Ekf2::Ekf2() +{ + _ekf = new EstimatorBase(); +} + +Ekf2::~Ekf2() +{ + +} + +void Ekf2::print() +{ + _ekf->printStoredGps(); + _ekf->printStoredBaro(); + _ekf->printStoredMag(); + _ekf->printStoredIMU(); +} + +void Ekf2::task_main() +{ + // subscribe to relevant topics + _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + + px4_pollfd_struct_t fds[1]; + fds[0].fd = _sensors_sub; + fds[0].events = POLLIN; + + while (!_task_should_exit) { + int ret = px4_poll(fds, 1, 1000); + + if (ret < 0) { + // Poll error, sleep and try again + usleep(10000); + continue; + + } else if (ret == 0 || fds[0].revents != POLLIN) { + // Poll timeout or no new data, do nothing + continue; + } + + bool gps_updated = false; + bool airspeed_updated = false; + + sensor_combined_s sensors = {}; + vehicle_gps_position_s gps = {}; + airspeed_s airspeed = {}; + + orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); + + // update all other topics if they have new data + orb_check(_gps_sub, &gps_updated); + + if (gps_updated) { + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); + } + + orb_check(_airspeed_sub, &airspeed_updated); + + if (airspeed_updated) { + orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); + } + + hrt_abstime now = hrt_absolute_time(); + // push imu data into estimator + _ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0], + &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]); + + // read mag data + _ekf->setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]); + + // read baro data + _ekf->setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]); + + // read gps data if available + if (gps_updated) { + struct gps_message gps_msg = {}; + gps_msg.time_usec = gps.timestamp_position; + gps_msg.lat = gps.lat; + gps_msg.lon = gps.lon; + gps_msg.alt = gps.alt; + gps_msg.fix_type = gps.fix_type; + gps_msg.eph = gps.eph; + gps_msg.epv = gps.epv; + gps_msg.time_usec_vel = gps.timestamp_velocity; + gps_msg.vel_m_s = gps.vel_m_s; + gps_msg.vel_ned[0] = gps.vel_n_m_s; + gps_msg.vel_ned[1] = gps.vel_e_m_s; + gps_msg.vel_ned[2] = gps.vel_d_m_s; + gps_msg.vel_ned_valid = gps.vel_ned_valid; + + _ekf->setGpsData(gps.timestamp_position, &gps_msg); + } + + // read airspeed data if available + if (airspeed_updated) { + _ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s); + } + } +} + +void Ekf2::task_main_trampoline(int argc, char *argv[]) +{ + ekf2::instance->task_main(); +} + +int Ekf2::start() +{ + ASSERT(_control_task == -1); + + /* start the task */ + _control_task = px4_task_spawn_cmd("ekf2", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 5, + 30000, + (px4_main_t)&Ekf2::task_main_trampoline, + nullptr); + + if (_control_task < 0) { + PX4_WARN("task start failed"); + return -errno; + } + + return OK; +} + +int ekf2_main(int argc, char *argv[]) +{ + if (argc < 1) { + PX4_WARN("usage: ekf2 {start|stop|status}"); + return 1; + } + + if (!strcmp(argv[1], "start")) { + + if (ekf2::instance != nullptr) { + PX4_WARN("already running"); + return 1; + } + + ekf2::instance = new Ekf2(); + + if (ekf2::instance == nullptr) { + PX4_WARN("alloc failed"); + return 1; + } + + if (OK != ekf2::instance->start()) { + delete ekf2::instance; + ekf2::instance = nullptr; + PX4_WARN("start failed"); + return 1; + } + + return 0; + } + + if (!strcmp(argv[1], "stop")) { + if (ekf2::instance == nullptr) { + PX4_WARN("not running"); + return 1; + } + + delete ekf2::instance; + ekf2::instance = nullptr; + return 0; + } + + if (!strcmp(argv[1], "print")) { + if (ekf2::instance != nullptr) { + ekf2::instance->print(); + return 0; + } + + return 1; + } + + if (!strcmp(argv[1], "status")) { + if (ekf2::instance) { + PX4_WARN("running"); + return 0; + + } else { + PX4_WARN("not running"); + return 1; + } + } + + PX4_WARN("unrecognized command"); + return 1; +} \ No newline at end of file