diff options
Diffstat (limited to 'src')
92 files changed, 9324 insertions, 3570 deletions
diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 5c502f682..98c491ce6 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -655,13 +655,14 @@ BlinkM::led() /* indicate main control state */ if (vehicle_status_raw.main_state == MAIN_STATE_POSCTL) led_color_4 = LED_GREEN; - else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) + /* TODO: add other Auto modes */ + else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO_MISSION) led_color_4 = LED_BLUE; else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; - else + else led_color_4 = LED_OFF; led_color_5 = led_color_4; } diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 5342ccf78..8560716e9 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -280,8 +280,8 @@ GPS::task_main() _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 0.9f; - _report.epv_m = 1.8f; + _report.eph = 0.9f; + _report.epv = 1.8f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; @@ -451,7 +451,7 @@ GPS::print_info() warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report.fix_type, _report.satellites_visible, (double)(hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); - warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph_m, (double)_report.epv_m); + warnx("eph: %.2fm, epv: %.2fm", (double)_report.eph, (double)_report.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); warnx("rate publication:\t%6.2f Hz", (double)_rate); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 680f00d97..41716cd97 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -251,16 +251,16 @@ MTK::handle_message(gps_mtk_packet_t &packet) _gps_position->lon = 0; // Indicate this data is not usable and bail out - _gps_position->eph_m = 1000.0f; - _gps_position->epv_m = 1000.0f; + _gps_position->eph = 1000.0f; + _gps_position->epv = 1000.0f; _gps_position->fix_type = 0; return; } _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm _gps_position->fix_type = packet.fix_type; - _gps_position->eph_m = packet.hdop / 100.0f; // from cm to m - _gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph + _gps_position->eph = packet.hdop / 100.0f; // from cm to m + _gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph _gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad _gps_position->satellites_visible = packet.satellites; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index c143eeb0c..404607571 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -439,8 +439,8 @@ UBX::handle_message() _gps_position->lat = packet->lat; _gps_position->lon = packet->lon; _gps_position->alt = packet->height_msl; - _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _gps_position->eph = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv = (float)packet->vAcc * 1e-3f; // from mm to m _gps_position->timestamp_position = hrt_absolute_time(); _rate_count_lat_lon++; diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 5954c40da..1c81910f6 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -131,8 +131,8 @@ public: int set_motor_count(unsigned count); int set_motor_test(bool motortest); int set_overrideSecurityChecks(bool overrideSecurityChecks); - int set_px4mode(int px4mode); - int set_frametype(int frametype); + void set_px4mode(int px4mode); + void set_frametype(int frametype); unsigned int mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C); private: @@ -330,13 +330,13 @@ MK::set_update_rate(unsigned rate) return OK; } -int +void MK::set_px4mode(int px4mode) { _px4mode = px4mode; } -int +void MK::set_frametype(int frametype) { _frametype = frametype; diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 321fdd173..0edec3d0e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -544,7 +544,7 @@ void MPU6000::reset() write_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); irqrestore(state); - up_udelay(1000); + usleep(1000); // SAMPLE RATE _set_sample_rate(_sample_rate); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 8a4bfa18c..84ea9a3bc 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -741,7 +741,7 @@ PX4FMU::task_main() } for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { - if (_control_subs > 0) { + if (_control_subs[i] > 0) { ::close(_control_subs[i]); _control_subs[i] = -1; } diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index dd5e4d3e0..fdaa7f27b 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -182,7 +182,10 @@ float RoboClaw::getMotorPosition(e_motor motor) return _motor1Position; } else if (motor == MOTOR_2) { return _motor2Position; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } float RoboClaw::getMotorSpeed(e_motor motor) @@ -191,7 +194,10 @@ float RoboClaw::getMotorSpeed(e_motor motor) return _motor1Speed; } else if (motor == MOTOR_2) { return _motor2Speed; - } + } else { + warnx("Unknown motor value passed to RoboClaw::getMotorPosition"); + return NAN; + } } int RoboClaw::setMotorSpeed(e_motor motor, float value) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index a0a18bc2e..46db788a6 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -151,9 +151,6 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; - /* input conditioning */ if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index d2a231694..9894a34d7 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -114,9 +114,6 @@ float ECL_RollController::control_bodyrate(float pitch, if (dt_micros > 500000) lock_integrator = true; -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; //xxx: param - /* input conditioning */ // warnx("airspeed pre %.4f", (double)airspeed); if (!isfinite(airspeed)) { diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 79184e2cd..fe03b8065 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -84,7 +84,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, _rate_setpoint = 0.0f; if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) { float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch)); - if(denumerator != 0.0f) { //XXX: floating point comparison + if(fabsf(denumerator) > FLT_EPSILON) { _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator; // warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint); } @@ -132,11 +132,6 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (dt_micros > 500000) lock_integrator = true; - -// float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f); - float k_ff = 0; - - /* input conditioning */ if (!isfinite(airspeed)) { /* airspeed is NaN, +- INF or not available, pick center of band */ diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 87c1cf460..8b286af36 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -50,7 +50,7 @@ __BEGIN_DECLS -#include "geo_mag_declination.h" +#include "geo_lookup/geo_mag_declination.h" #include <stdbool.h> diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk index 9500a2bcc..d08ca4532 100644 --- a/src/lib/geo/module.mk +++ b/src/lib/geo/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2014 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 @@ -35,5 +35,4 @@ # Geo library # -SRCS = geo.c \ - geo_mag_declination.c +SRCS = geo.c diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo_lookup/geo_mag_declination.c index 09eac38f4..09eac38f4 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo_lookup/geo_mag_declination.c diff --git a/src/lib/geo/geo_mag_declination.h b/src/lib/geo_lookup/geo_mag_declination.h index 0ac062d6d..0ac062d6d 100644 --- a/src/lib/geo/geo_mag_declination.h +++ b/src/lib/geo_lookup/geo_mag_declination.h diff --git a/src/lib/geo_lookup/module.mk b/src/lib/geo_lookup/module.mk new file mode 100644 index 000000000..d7a10df2d --- /dev/null +++ b/src/lib/geo_lookup/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# Geo lookup table / data library +# + +SRCS = geo_mag_declination.c + +MAXOPTIMIZATION = -Os diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk index de0174e37..d92f0bb45 100644 --- a/src/lib/launchdetection/module.mk +++ b/src/lib/launchdetection/module.mk @@ -38,3 +38,5 @@ SRCS = LaunchDetector.cpp \ CatapultLaunchMethod.cpp \ launchdetection_params.c + +MAXOPTIMIZATION = -Os diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 5bcce1b4d..35dc39ec6 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -342,7 +342,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds if (gps_updated) { orb_copy(ORB_ID(vehicle_gps_position), sub_gps, &gps); - if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); /* update mag declination rotation matrix */ @@ -401,7 +401,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds hrt_abstime vel_t = 0; bool vel_valid = false; - if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph_m < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { + if (ekf_params.acc_comp == 1 && gps.fix_type >= 3 && gps.eph < 10.0f && gps.vel_ned_valid && hrt_absolute_time() < gps.timestamp_velocity + 500000) { vel_valid = true; if (gps_updated) { vel_t = gps.timestamp_velocity; @@ -410,7 +410,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds vel(2) = gps.vel_d_m_s; } - } else if (ekf_params.acc_comp == 2 && gps.eph_m < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { + } else if (ekf_params.acc_comp == 2 && gps.eph < 5.0f && global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { vel_valid = true; if (global_pos_updated) { vel_t = global_pos.timestamp; @@ -477,7 +477,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds parameters_update(&ekf_param_handles, &ekf_params); /* update mag declination rotation matrix */ - if (gps.eph_m < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { + if (gps.eph < 20.0f && hrt_elapsed_time(&gps.timestamp_position) < 1000000) { mag_decl = math::radians(get_mag_declination(gps.lat / 1e7f, gps.lon / 1e7f)); } else { diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 3ff3d9922..49a892609 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -61,11 +61,6 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f); /* offset estimation - UNUSED */ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R3, 0.0f); -/* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); - /* magnetic declination, in degrees */ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); @@ -85,10 +80,6 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h) h->r2 = param_find("EKF_ATT_V4_R2"); h->r3 = param_find("EKF_ATT_V4_R3"); - h->roll_off = param_find("ATT_ROLL_OFF3"); - h->pitch_off = param_find("ATT_PITCH_OFF3"); - h->yaw_off = param_find("ATT_YAW_OFF3"); - h->mag_decl = param_find("ATT_MAG_DECL"); h->acc_comp = param_find("ATT_ACC_COMP"); @@ -109,10 +100,6 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru param_get(h->r2, &(p->r[2])); param_get(h->r3, &(p->r[3])); - param_get(h->roll_off, &(p->roll_off)); - param_get(h->pitch_off, &(p->pitch_off)); - param_get(h->yaw_off, &(p->yaw_off)); - param_get(h->mag_decl, &(p->mag_decl)); p->mag_decl *= M_PI / 180.0f; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h index 74a141609..5985541ca 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h @@ -54,7 +54,6 @@ struct attitude_estimator_ekf_params { struct attitude_estimator_ekf_param_handles { param_t r0, r1, r2, r3; param_t q0, q1, q2, q3, q4; - param_t roll_off, pitch_off, yaw_off; param_t mag_decl; param_t acc_comp; }; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3c04b29e1..507f18e24 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1,11 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * Lorenz Meier <lm@inf.ethz.ch> - * Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> - * Anton Babushkin <anton.babushkin@me.com> + * Copyright (C) 2013-2014 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 @@ -38,8 +33,13 @@ /** * @file commander.cpp - * Main system state machine implementation. + * Main fail-safe handling. * + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> @@ -78,6 +78,8 @@ #include <uORB/topics/differential_pressure.h> #include <uORB/topics/safety.h> #include <uORB/topics/system_power.h> +#include <uORB/topics/mission_result.h> +#include <uORB/topics/telemetry_status.h> #include <drivers/drv_led.h> #include <drivers/drv_hrt.h> @@ -89,6 +91,7 @@ #include <systemlib/err.h> #include <systemlib/cpuload.h> #include <systemlib/rc_check.h> +#include <systemlib/state_table.h> #include "px4_custom_mode.h" #include "commander_helper.h" @@ -122,6 +125,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 +#define DL_TIMEOUT 5 * 1000* 1000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -389,109 +393,80 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub) { - /* result of the command */ - enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED; - bool ret = false; - /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status->system_id || ((cmd->target_component != status->component_id) && (cmd->target_component != 0))) { // component_id 0: valid for all components return false; } - /* only handle high-priority commands here */ + /* result of the command */ + enum VEHICLE_CMD_RESULT cmd_result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (uint8_t) cmd->param1; - uint8_t custom_main_mode = (uint8_t) cmd->param2; - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + uint8_t base_mode = (uint8_t)cmd->param1; + uint8_t custom_main_mode = (uint8_t)cmd->param2; - /* set HIL state */ - hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - int hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); - - /* if HIL got enabled, reset battery status state */ - if (hil_ret == OK && status->hil_state == HIL_STATE_ON) { - /* reset the arming mode to disarmed */ - arming_res = arming_state_transition(status, safety, ARMING_STATE_STANDBY, armed, mavlink_fd); + transition_result_t arming_ret = TRANSITION_NOT_CHANGED; - if (arming_res != TRANSITION_DENIED) { - mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + transition_result_t main_ret = TRANSITION_NOT_CHANGED; - } else { - mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); - } - } - - if (hil_ret == OK) { - ret = true; - } + /* set HIL state */ + hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; + transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status, mavlink_fd); // Transition the arming state - arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); - - if (arming_res == TRANSITION_CHANGED) { - ret = true; - } - - /* set main state */ - transition_result_t main_res = TRANSITION_DENIED; + arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command"); if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { /* use autopilot-specific mode */ if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) { /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status, MAIN_STATE_MANUAL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) { /* ALTCTL */ - main_res = main_state_transition(status, MAIN_STATE_ALTCTL); + main_ret = main_state_transition(status, MAIN_STATE_ALTCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) { /* POSCTL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status, MAIN_STATE_POSCTL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ - main_res = main_state_transition(status, MAIN_STATE_ACRO); + main_ret = main_state_transition(status, MAIN_STATE_ACRO); } } else { /* use base mode */ if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + main_ret = main_state_transition(status, MAIN_STATE_AUTO_MISSION); } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { /* POSCTL */ - main_res = main_state_transition(status, MAIN_STATE_POSCTL); + main_ret = main_state_transition(status, MAIN_STATE_POSCTL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ - main_res = main_state_transition(status, MAIN_STATE_MANUAL); + main_ret = main_state_transition(status, MAIN_STATE_MANUAL); } } } - if (main_res == TRANSITION_CHANGED) { - ret = true; - } - - if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + if (hil_ret != TRANSITION_DENIED && arming_ret != TRANSITION_DENIED && main_ret != TRANSITION_DENIED) { + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - break; } + break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm. @@ -510,10 +485,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe if (arming_res == TRANSITION_DENIED) { mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd"); - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } else { - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } } } @@ -524,18 +499,14 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe unsigned int mav_goto = cmd->param1; if (mav_goto == 0) { // MAV_GOTO_DO_HOLD - status->set_nav_state = NAV_STATE_LOITER; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE - status->set_nav_state = NAV_STATE_MISSION; - status->set_nav_state_timestamp = hrt_absolute_time(); + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd"); - result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7); @@ -543,6 +514,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } break; +#if 0 /* Flight termination */ case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command @@ -550,16 +522,16 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO if (armed->armed && cmd->param3 > 0.5 && parachute_enabled) { transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION); - result = VEHICLE_CMD_RESULT_ACCEPTED; - ret = true; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { /* reject parachute depoyment not armed */ - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } break; +#endif case VEHICLE_CMD_DO_SET_HOME: { bool use_current = cmd->param1 > 0.5f; @@ -573,10 +545,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->timestamp = hrt_absolute_time(); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } else { @@ -587,10 +559,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe home->timestamp = hrt_absolute_time(); - result = VEHICLE_CMD_RESULT_ACCEPTED; + cmd_result = VEHICLE_CMD_RESULT_ACCEPTED; } - if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + if (cmd_result == VEHICLE_CMD_RESULT_ACCEPTED) { warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home->lat, home->lon, (double)home->alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home->lat, home->lon, (double)home->alt); @@ -622,17 +594,18 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe break; } - if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* already warned about unsupported commands in "default" case */ - answer_command(*cmd, result); + answer_command(*cmd, cmd_result); } /* send any requested ACKs */ - if (cmd->confirmation > 0 && result != VEHICLE_CMD_RESULT_UNSUPPORTED) { + if (cmd->confirmation > 0 && cmd_result != VEHICLE_CMD_RESULT_UNSUPPORTED) { /* send acknowledge command */ // XXX TODO } + return true; } int commander_thread_main(int argc, char *argv[]) @@ -649,31 +622,41 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_component_id = param_find("MAV_COMP_ID"); param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT"); param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); + param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); /* welcome user */ warnx("starting"); char *main_states_str[MAIN_STATE_MAX]; - main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTCTL"; - main_states_str[2] = "POSCTL"; - main_states_str[3] = "AUTO"; - main_states_str[4] = "ACRO"; + main_states_str[MAIN_STATE_MANUAL] = "MANUAL"; + main_states_str[MAIN_STATE_ALTCTL] = "ALTCTL"; + main_states_str[MAIN_STATE_POSCTL] = "POSCTL"; + main_states_str[MAIN_STATE_AUTO_MISSION] = "AUTO_MISSION"; + main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; + main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; + main_states_str[MAIN_STATE_ACRO] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; - arming_states_str[0] = "INIT"; - arming_states_str[1] = "STANDBY"; - arming_states_str[2] = "ARMED"; - arming_states_str[3] = "ARMED_ERROR"; - arming_states_str[4] = "STANDBY_ERROR"; - arming_states_str[5] = "REBOOT"; - arming_states_str[6] = "IN_AIR_RESTORE"; - - char *failsafe_states_str[FAILSAFE_STATE_MAX]; - failsafe_states_str[0] = "NORMAL"; - failsafe_states_str[1] = "RTL"; - failsafe_states_str[2] = "LAND"; - failsafe_states_str[3] = "TERMINATION"; + arming_states_str[ARMING_STATE_INIT] = "INIT"; + arming_states_str[ARMING_STATE_STANDBY] = "STANDBY"; + arming_states_str[ARMING_STATE_ARMED] = "ARMED"; + arming_states_str[ARMING_STATE_ARMED_ERROR] = "ARMED_ERROR"; + arming_states_str[ARMING_STATE_STANDBY_ERROR] = "STANDBY_ERROR"; + arming_states_str[ARMING_STATE_REBOOT] = "REBOOT"; + arming_states_str[ARMING_STATE_IN_AIR_RESTORE] = "IN_AIR_RESTORE"; + + char *nav_states_str[NAVIGATION_STATE_MAX]; + nav_states_str[NAVIGATION_STATE_MANUAL] = "MANUAL"; + nav_states_str[NAVIGATION_STATE_ALTCTL] = "ALTCTL"; + nav_states_str[NAVIGATION_STATE_POSCTL] = "POSCTL"; + nav_states_str[NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; + nav_states_str[NAVIGATION_STATE_AUTO_LOITER] = "AUTO_LOITER"; + nav_states_str[NAVIGATION_STATE_AUTO_RTL] = "AUTO_RTL"; + nav_states_str[NAVIGATION_STATE_AUTO_RTGS] = "AUTO_RTGS"; + nav_states_str[NAVIGATION_STATE_ACRO] = "ACRO"; + nav_states_str[NAVIGATION_STATE_LAND] = "LAND"; + nav_states_str[NAVIGATION_STATE_DESCEND] = "DESCEND"; + nav_states_str[NAVIGATION_STATE_TERMINATION] = "TERMINATION"; /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; @@ -695,11 +678,10 @@ int commander_thread_main(int argc, char *argv[]) // We want to accept RC inputs as default status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; - status.set_nav_state = NAV_STATE_NONE; - status.set_nav_state_timestamp = 0; + status.nav_state = NAVIGATION_STATE_MANUAL; status.arming_state = ARMING_STATE_INIT; status.hil_state = HIL_STATE_OFF; - status.failsafe_state = FAILSAFE_STATE_NORMAL; + status.failsafe = false; /* neither manual nor offboard control commands have been received */ status.offboard_control_signal_found_once = false; @@ -708,6 +690,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark all signals lost as long as they haven't been found */ status.rc_signal_lost = true; status.offboard_control_signal_lost = true; + status.data_link_lost = true; /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -792,6 +775,11 @@ int commander_thread_main(int argc, char *argv[]) safety.safety_switch_available = false; safety.safety_off = false; + /* Subscribe to mission result topic */ + int mission_result_sub = orb_subscribe(ORB_ID(mission_result)); + struct mission_result_s mission_result; + memset(&mission_result, 0, sizeof(mission_result)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -802,6 +790,11 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to telemetry status */ + int telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); + struct telemetry_status_s telemetry; + memset(&telemetry, 0, sizeof(telemetry)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; @@ -874,6 +867,15 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); + transition_result_t arming_ret; + + int32_t datalink_loss_enabled = false; + + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = false; + bool main_state_changed = false; + bool failsafe_old = false; + while (!thread_should_exit) { if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) { @@ -881,6 +883,9 @@ int commander_thread_main(int argc, char *argv[]) mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + arming_ret = TRANSITION_NOT_CHANGED; + + /* update parameters */ orb_check(param_changed_sub, &updated); @@ -923,6 +928,7 @@ int commander_thread_main(int argc, char *argv[]) /* navigation parameters */ param_get(_param_takeoff_alt, &takeoff_alt); param_get(_param_enable_parachute, ¶chute_enabled); + param_get(_param_enable_datalink_loss, &datalink_loss_enabled); } orb_check(sp_man_sub, &updated); @@ -937,6 +943,12 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } + orb_check(telemetry_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(telemetry_status), telemetry_sub, &telemetry); + } + orb_check(sensor_sub, &updated); if (updated) { @@ -980,7 +992,8 @@ int commander_thread_main(int argc, char *argv[]) arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd)) { - mavlink_log_info(mavlink_fd, "#audio DISARMED by safety switch"); + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch"); + arming_state_changed = true; } } } @@ -993,6 +1006,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); } + /* update local position estimate */ + orb_check(local_position_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); + } + /* update condition_global_position_valid */ /* hysteresis for EPH/EPV */ bool eph_epv_good; @@ -1026,6 +1047,10 @@ int commander_thread_main(int argc, char *argv[]) home.lon = global_position.lon; home.alt = global_position.alt; + home.x = local_position.x; + home.y = local_position.y; + home.z = local_position.z; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -1042,14 +1067,6 @@ int commander_thread_main(int argc, char *argv[]) tune_positive(true); } - /* update local position estimate */ - orb_check(local_position_sub, &updated); - - if (updated) { - /* position changed */ - orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - } - /* update condition_local_position_valid and condition_local_altitude_valid */ /* hysteresis for EPH */ bool local_eph_good; @@ -1073,13 +1090,10 @@ int commander_thread_main(int argc, char *argv[]) check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(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); - static bool published_condition_landed_fw = false; - - if (status.is_rotary_wing && status.condition_local_altitude_valid) { + if (status.condition_local_altitude_valid) { if (status.condition_landed != local_position.landed) { status.condition_landed = local_position.landed; status_changed = true; - published_condition_landed_fw = false; //make sure condition_landed is published again if the system type changes if (status.condition_landed) { mavlink_log_critical(mavlink_fd, "#audio: LANDED"); @@ -1088,13 +1102,6 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "#audio: IN AIR"); } } - - } else { - if (!published_condition_landed_fw) { - status.condition_landed = false; // Fixedwing does not have a landing detector currently - published_condition_landed_fw = true; - status_changed = true; - } } /* update battery status */ @@ -1183,12 +1190,19 @@ int commander_thread_main(int argc, char *argv[]) status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd); + + if (arming_ret == TRANSITION_CHANGED) { + arming_state_changed = true; + } } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); - } + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { + arming_state_changed = true; + } + } status_changed = true; } @@ -1196,11 +1210,15 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { - // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + /* TODO: check for sensors */ + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd); + + if (arming_ret == TRANSITION_CHANGED) { + arming_state_changed = true; + } } else { - // XXX: Add emergency stuff if sensors are lost + /* TODO: Add emergency stuff if sensors are lost */ } @@ -1219,7 +1237,13 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); } - /* start RC input check */ + orb_check(mission_result_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(mission_result), mission_result_sub, &mission_result); + } + + /* RC input check */ if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { @@ -1236,11 +1260,6 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - transition_result_t arming_res; // store all transitions results here - - /* arm/disarm by RC */ - arming_res = TRANSITION_NOT_CHANGED; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && @@ -1251,7 +1270,10 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { + arming_state_changed = true; + } stick_off_counter = 0; } else { @@ -1273,7 +1295,10 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first."); } else { - arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); + arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd); + if (arming_ret == TRANSITION_CHANGED) { + arming_state_changed = true; + } } stick_on_counter = 0; @@ -1286,124 +1311,55 @@ int commander_thread_main(int argc, char *argv[]) stick_on_counter = 0; } - if (arming_res == TRANSITION_CHANGED) { + if (arming_ret == TRANSITION_CHANGED) { if (status.arming_state == ARMING_STATE_ARMED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } + arming_state_changed = true; - } else if (arming_res == TRANSITION_DENIED) { + } else if (arming_ret == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied"); } - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* recover from failsafe */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - } - /* evaluate the main state machine according to mode switches */ transition_result_t main_res = set_main_state_rc(&status, &sp_man); /* play tune on mode change only if armed, blink LED always */ if (main_res == TRANSITION_CHANGED) { tune_positive(armed.armed); + main_state_changed = true; } else if (main_res == TRANSITION_DENIED) { /* DENIED here indicates bug in the commander */ mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied"); } - /* set navigation state */ - /* RETURN switch, overrides MISSION switch */ - if (sp_man.return_switch == SWITCH_POS_ON) { - /* switch to RTL if not already landed after RTL and home position set */ - status.set_nav_state = NAV_STATE_RTL; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else { - - /* LOITER switch */ - if (sp_man.loiter_switch == SWITCH_POS_ON) { - /* stick is in LOITER position */ - status.set_nav_state = NAV_STATE_LOITER; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { - /* stick is in MISSION position */ - status.set_nav_state = NAV_STATE_MISSION; - status.set_nav_state_timestamp = hrt_absolute_time(); - - } else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) && - pos_sp_triplet.nav_state == NAV_STATE_RTL) { - /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - status.set_nav_state = NAV_STATE_MISSION; - status.set_nav_state_timestamp = hrt_absolute_time(); - } - } - } else { if (!status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST"); status.rc_signal_lost = true; status_changed = true; } + } - if (armed.armed) { - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO); - - if (auto_res == TRANSITION_NOT_CHANGED) { - last_auto_state_valid = hrt_absolute_time(); - } - - /* still invalid state after the timeout interval, execute failsafe */ - if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (auto_res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } - } - - } else { - /* failsafe for manual modes */ - transition_result_t manual_res = TRANSITION_DENIED; - - if (!status.condition_landed) { - /* vehicle is not landed, try to perform RTL */ - manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); - } - - if (manual_res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate) or not wanted, try LAND */ - manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); - - if (manual_res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); - } - } - } - - } else { - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* reset failsafe when disarmed */ - (void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); - } + /* data link check */ + if (hrt_absolute_time() < telemetry.heartbeat_time + DL_TIMEOUT) { + /* handle the case where data link was regained */ + if (status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: data link regained"); + status.data_link_lost = false; + status_changed = true; } - } - // TODO remove this hack - /* flight termination in manual mode if assist switch is on POSCTL position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) { - if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { - tune_positive(armed.armed); + } else { + if (!status.data_link_lost) { + mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: DATA LINK LOST"); + status.data_link_lost = true; + status_changed = true; } } @@ -1420,11 +1376,6 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check which state machines for changes, clear "changed" flag */ - bool arming_state_changed = check_arming_state_changed(); - bool main_state_changed = check_main_state_changed(); - bool failsafe_state_changed = check_failsafe_state_changed(); - hrt_abstime t1 = hrt_absolute_time(); /* print new state */ @@ -1441,6 +1392,10 @@ int commander_thread_main(int argc, char *argv[]) home.lon = global_position.lon; home.alt = global_position.alt; + home.x = local_position.x; + home.y = local_position.y; + home.z = local_position.z; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -1455,18 +1410,33 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ status.condition_home_position_valid = true; } + arming_state_changed = false; } was_armed = armed.armed; + /* now set navigation state according to failsafe and main state */ + bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, + mission_result.mission_finished); + + // TODO handle mode changes by commands if (main_state_changed) { status_changed = true; + warnx("main state: %s", main_states_str[status.main_state]); mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]); + main_state_changed = false; } - if (failsafe_state_changed) { + if (status.failsafe != failsafe_old) { status_changed = true; - mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %s", failsafe_states_str[status.failsafe_state]); + mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe); + failsafe_old = status.failsafe; + } + + if (nav_state_changed) { + status_changed = true; + warnx("nav state: %s", nav_states_str[status.nav_state]); + mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]); } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ @@ -1492,7 +1462,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tune on battery critical */ set_tune(TONE_BATTERY_WARNING_FAST_TUNE); - } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) { + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe) { /* play tune on battery warning or failsafe */ set_tune(TONE_BATTERY_WARNING_SLOW_TUNE); @@ -1596,7 +1566,7 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a if (set_normal_color) { /* set color */ - if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) { + if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe) { rgbled_set_color(RGBLED_COLOR_AMBER); /* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */ @@ -1675,10 +1645,10 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTCTL print_reject_mode(status, "POSCTL"); } + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { @@ -1689,27 +1659,62 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin print_reject_mode(status, "ALTCTL"); } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case SWITCH_POS_ON: // AUTO - res = main_state_transition(status, MAIN_STATE_AUTO); + if (sp_man->return_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_AUTO_RTL); - if (res != TRANSITION_DENIED) { - break; // changed successfully or already in this state + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + + print_reject_mode(status, "AUTO_RTL"); + + // fallback to LOITER if home position not set + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + + } else if (sp_man->loiter_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_AUTO_LOITER); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + + print_reject_mode(status, "AUTO_LOITER"); + + } else { + res = main_state_transition(status, MAIN_STATE_AUTO_MISSION); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + + print_reject_mode(status, "AUTO_MISSION"); } - // else fallback to ALTCTL (POSCTL likely will not work too) - print_reject_mode(status, "AUTO"); + // fallback to POSCTL + res = main_state_transition(status, MAIN_STATE_POSCTL); + + if (res != TRANSITION_DENIED) { + break; // changed successfully or already in this state + } + + // fallback to ALTCTL res = main_state_transition(status, MAIN_STATE_ALTCTL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to MANUAL + // fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1722,85 +1727,92 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin } void - set_control_mode() { - /* set vehicle_control_mode according to main state and failsafe state */ + /* set vehicle_control_mode according to set_navigation_state */ control_mode.flag_armed = armed.armed; + /* TODO: check this */ control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; - control_mode.flag_control_termination_enabled = false; - - /* set this flag when navigator should act */ - bool navigator_enabled = false; - - switch (status.failsafe_state) { - case FAILSAFE_STATE_NORMAL: - switch (status.main_state) { - case MAIN_STATE_MANUAL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = status.is_rotary_wing; - control_mode.flag_control_attitude_enabled = status.is_rotary_wing; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_ALTCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; - - case MAIN_STATE_POSCTL: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; - control_mode.flag_control_velocity_enabled = true; - break; - - case MAIN_STATE_AUTO: - navigator_enabled = true; - break; + switch (status.nav_state) { + case NAVIGATION_STATE_MANUAL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = status.is_rotary_wing; + control_mode.flag_control_attitude_enabled = status.is_rotary_wing; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; - case MAIN_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_altitude_enabled = false; - control_mode.flag_control_climb_rate_enabled = false; - control_mode.flag_control_position_enabled = false; - control_mode.flag_control_velocity_enabled = false; - break; + case NAVIGATION_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; - default: - break; - } + case NAVIGATION_STATE_ALTCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_POSCTL: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_RTL: - navigator_enabled = true; + case NAVIGATION_STATE_AUTO_MISSION: + case NAVIGATION_STATE_AUTO_LOITER: + case NAVIGATION_STATE_AUTO_RTL: + case NAVIGATION_STATE_AUTO_RTGS: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_LAND: - navigator_enabled = true; + case NAVIGATION_STATE_LAND: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + /* in failsafe LAND mode position may be not available */ + control_mode.flag_control_position_enabled = status.condition_local_position_valid; + control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; break; - case FAILSAFE_STATE_TERMINATION: + case NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = false; @@ -1816,21 +1828,6 @@ set_control_mode() default: break; } - - /* navigator has control, set control mode flags according to nav state*/ - if (navigator_enabled) { - control_mode.flag_control_manual_enabled = false; - control_mode.flag_control_auto_enabled = true; - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - - /* in failsafe LAND mode position may be not available */ - control_mode.flag_control_position_enabled = status.condition_local_position_valid; - control_mode.flag_control_velocity_enabled = status.condition_local_position_valid; - - control_mode.flag_control_altitude_enabled = true; - control_mode.flag_control_climb_rate_enabled = true; - } } void @@ -1973,8 +1970,6 @@ void *commander_low_prio_loop(void *arg) int calib_ret = ERROR; /* try to go to INIT/PREFLIGHT arming state */ - - // XXX disable interrupts in arming_state_transition if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 80ca68f21..4750f9d5c 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -39,7 +39,7 @@ * * @author Lorenz Meier <lm@inf.ethz.ch> * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> */ #include <nuttx/config.h> @@ -84,3 +84,14 @@ PARAM_DEFINE_INT32(BAT_N_CELLS, 3); * @group Battery Calibration */ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f); + +/** + * Datalink loss mode enabled. + * + * Set to 1 to enable actions triggered when the datalink is lost. + * + * @group commander + * @min 0 + * @max 1 + */ +PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index e0f8dc95d..7f5f93801 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -25,6 +25,7 @@ enum PX4_CUSTOM_SUB_MODE_AUTO { PX4_CUSTOM_SUB_MODE_AUTO_MISSION, PX4_CUSTOM_SUB_MODE_AUTO_RTL, PX4_CUSTOM_SUB_MODE_AUTO_LAND, + PX4_CUSTOM_SUB_MODE_AUTO_RTGS }; union px4_custom_mode { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 84f4d03af..1997729d4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler <thomasgubler@student.ethz.ch> - * Julian Oes <joes@student.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +34,9 @@ /** * @file state_machine_helper.cpp * State machine helper functions implementations + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> */ #include <stdio.h> @@ -59,30 +60,20 @@ #include "state_machine_helper.h" #include "commander_helper.h" -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - -static bool arming_state_changed = true; -static bool main_state_changed = true; -static bool failsafe_state_changed = true; - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even // though the transition is marked as true additional checks must be made. See arming_state_transition // code for those checks. static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = { - // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE - { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, - { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, - { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, - { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, - { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, - { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, - { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI + // INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE + { /* ARMING_STATE_INIT */ true, true, false, false, false, false, false }, + { /* ARMING_STATE_STANDBY */ true, true, true, true, false, false, false }, + { /* ARMING_STATE_ARMED */ false, true, true, false, false, false, true }, + { /* ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false }, + { /* ARMING_STATE_STANDBY_ERROR */ true, true, false, true, true, false, false }, + { /* ARMING_STATE_REBOOT */ true, true, false, false, true, true, true }, + { /* ARMING_STATE_IN_AIR_RESTORE */ false, false, false, false, false, false, false }, // NYI }; // You can index into the array with an arming_state_t in order to get it's textual representation @@ -191,7 +182,6 @@ arming_state_transition(struct vehicle_status_s *status, /// current armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY; ret = TRANSITION_CHANGED; status->arming_state = new_arming_state; - arming_state_changed = true; } } @@ -225,69 +215,58 @@ bool is_safe(const struct vehicle_status_s *status, const struct safety_s *safet } } -bool -check_arming_state_changed() -{ - if (arming_state_changed) { - arming_state_changed = false; - return true; - - } else { - return false; - } -} - transition_result_t main_state_transition(struct vehicle_status_s *status, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; - /* transition may be denied even if requested the same state because conditions may be changed */ + /* transition may be denied even if the same state is requested because conditions may have changed */ switch (new_main_state) { case MAIN_STATE_MANUAL: - ret = TRANSITION_CHANGED; - break; - case MAIN_STATE_ACRO: ret = TRANSITION_CHANGED; break; case MAIN_STATE_ALTCTL: - /* need at minimum altitude estimate */ + /* TODO: add this for fixedwing as well */ if (!status->is_rotary_wing || (status->condition_local_altitude_valid || status->condition_global_position_valid)) { ret = TRANSITION_CHANGED; } - break; case MAIN_STATE_POSCTL: - /* need at minimum local position estimate */ if (status->condition_local_position_valid || status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } - break; - case MAIN_STATE_AUTO: - + case MAIN_STATE_AUTO_MISSION: + case MAIN_STATE_AUTO_LOITER: /* need global position estimate */ if (status->condition_global_position_valid) { ret = TRANSITION_CHANGED; } + break; + case MAIN_STATE_AUTO_RTL: + /* need global position and home position */ + if (status->condition_global_position_valid && status->condition_home_position_valid) { + ret = TRANSITION_CHANGED; + } break; - } + case MAIN_STATE_MAX: + default: + break; + } if (ret == TRANSITION_CHANGED) { if (status->main_state != new_main_state) { status->main_state = new_main_state; - main_state_changed = true; - } else { ret = TRANSITION_NOT_CHANGED; } @@ -296,70 +275,35 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta return ret; } -bool -check_main_state_changed() -{ - if (main_state_changed) { - main_state_changed = false; - return true; - - } else { - return false; - } -} - -bool -check_failsafe_state_changed() -{ - if (failsafe_state_changed) { - failsafe_state_changed = false; - return true; - - } else { - return false; - } -} - /** -* Transition from one hil state to another -*/ -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) + * Transition from one hil state to another + */ +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - bool valid_transition = false; - int ret = ERROR; + transition_result_t ret = TRANSITION_DENIED; if (current_status->hil_state == new_state) { - valid_transition = true; + ret = TRANSITION_NOT_CHANGED; } else { - switch (new_state) { - case HIL_STATE_OFF: - /* we're in HIL and unexpected things can happen if we disable HIL now */ mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)"); - valid_transition = false; - + ret = TRANSITION_DENIED; break; case HIL_STATE_ON: - if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - - // Disable publication of all attached sensors - + /* Disable publication of all attached sensors */ /* list directory */ DIR *d; d = opendir("/dev"); if (d) { - struct dirent *direntry; char devname[24]; @@ -414,290 +358,210 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } - closedir(d); + ret = TRANSITION_CHANGED; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + } else { /* failed opening dir */ - warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); - return 1; + mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY"); + ret = TRANSITION_DENIED; } + } else { + mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed"); + ret = TRANSITION_DENIED; } - break; default: - warnx("Unknown hil state"); + warnx("Unknown HIL state"); break; } } - if (valid_transition) { + if (ret == TRANSITION_CHANGED) { current_status->hil_state = new_state; - current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - // XXX also set lockdown here - - ret = OK; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); } - return ret; } - /** -* Transition from one failsafe state to another -*/ -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state) + * Check failsafe and main status and set navigation status for navigator accordingly + */ +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished) { - transition_result_t ret = TRANSITION_DENIED; + navigation_state_t nav_state_old = status->nav_state; - /* transition may be denied even if requested the same state because conditions may be changed */ - if (status->failsafe_state == FAILSAFE_STATE_TERMINATION) { - /* transitions from TERMINATION to other states not allowed */ - if (new_failsafe_state == FAILSAFE_STATE_TERMINATION) { - ret = TRANSITION_NOT_CHANGED; + bool armed = (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR); + status->failsafe = false; + + /* evaluate main state to decide in normal (non-failsafe) mode */ + switch (status->main_state) { + case MAIN_STATE_ACRO: + case MAIN_STATE_MANUAL: + case MAIN_STATE_ALTCTL: + case MAIN_STATE_POSCTL: + /* require RC for all manual modes */ + if (status->rc_signal_lost && armed) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + } else { + switch (status->main_state) { + case MAIN_STATE_ACRO: + status->nav_state = NAVIGATION_STATE_ACRO; + break; + + case MAIN_STATE_MANUAL: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + + case MAIN_STATE_ALTCTL: + status->nav_state = NAVIGATION_STATE_ALTCTL; + break; + + case MAIN_STATE_POSCTL: + status->nav_state = NAVIGATION_STATE_POSCTL; + break; + + default: + status->nav_state = NAVIGATION_STATE_MANUAL; + break; + } } + break; - } else { - switch (new_failsafe_state) { - case FAILSAFE_STATE_NORMAL: - /* always allowed (except from TERMINATION state) */ - ret = TRANSITION_CHANGED; - break; + case MAIN_STATE_AUTO_MISSION: + /* go into failsafe + * - if either the datalink is enabled and lost as well as RC is lost + * - if there is no datalink and the mission is finished */ + if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || + (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { + status->failsafe = true; - case FAILSAFE_STATE_RTL: + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { + status->failsafe = true; - /* global position and home position required for RTL */ if (status->condition_global_position_valid && status->condition_home_position_valid) { - status->set_nav_state = NAV_STATE_RTL; - status->set_nav_state_timestamp = hrt_absolute_time(); - ret = TRANSITION_CHANGED; + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; } - break; + /* don't bother if RC is lost and mission is not yet finished */ + } else if (status->rc_signal_lost) { - case FAILSAFE_STATE_LAND: + /* this mode is ok, we don't need RC for missions */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + /* everything is perfect */ + status->nav_state = NAVIGATION_STATE_AUTO_MISSION; + } + break; + + case MAIN_STATE_AUTO_LOITER: + /* go into failsafe if datalink and RC is lost */ + if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) { + status->failsafe = true; - /* at least relative altitude estimate required for landing */ - if (status->condition_local_altitude_valid || status->condition_global_position_valid) { - status->set_nav_state = NAV_STATE_LAND; - status->set_nav_state_timestamp = hrt_absolute_time(); - ret = TRANSITION_CHANGED; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; } - break; + /* also go into failsafe if just datalink is lost */ + } else if (status->data_link_lost && data_link_loss_enabled) { + status->failsafe = true; - case FAILSAFE_STATE_TERMINATION: - /* always allowed */ - ret = TRANSITION_CHANGED; - break; + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } - default: - break; + /* go into failsafe if RC is lost and datalink loss is not set up */ + } else if (status->rc_signal_lost && !data_link_loss_enabled) { + status->failsafe = true; + + if (status->condition_global_position_valid && status->condition_home_position_valid) { + status->nav_state = NAVIGATION_STATE_AUTO_RTGS; + } else if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; + } else { + status->nav_state = NAVIGATION_STATE_TERMINATION; + } + + /* don't bother if RC is lost if datalink is connected */ + } else if (status->rc_signal_lost) { + + /* this mode is ok, we don't need RC for loitering */ + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; + } else { + /* everything is perfect */ + status->nav_state = NAVIGATION_STATE_AUTO_LOITER; } + break; - if (ret == TRANSITION_CHANGED) { - if (status->failsafe_state != new_failsafe_state) { - status->failsafe_state = new_failsafe_state; - failsafe_state_changed = true; + case MAIN_STATE_AUTO_RTL: + /* require global position and home */ + if ((!status->condition_global_position_valid || !status->condition_home_position_valid)) { + status->failsafe = true; + if (status->condition_local_position_valid) { + status->nav_state = NAVIGATION_STATE_LAND; + } else if (status->condition_local_altitude_valid) { + status->nav_state = NAVIGATION_STATE_DESCEND; } else { - ret = TRANSITION_NOT_CHANGED; + status->nav_state = NAVIGATION_STATE_TERMINATION; } + } else { + status->nav_state = NAVIGATION_STATE_AUTO_RTL; } + break; + + default: + break; } - return ret; + return status->nav_state != nav_state_old; } - - -// /* -// * Wrapper functions (to be used in the commander), all functions assume lock on current_status -// */ - -// /* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR -// * -// * START SUBSYSTEM/EMERGENCY FUNCTIONS -// * */ - -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_present &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was removed something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]; - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: GPS not present", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -// } - -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_enabled &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if a subsystem was disabled something went completely wrong */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - gyro disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - accelerometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - magnetometer disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency_always_critical(status_pub, current_status); -// break; - -// case SUBSYSTEM_TYPE_GPS: -// { -// uint8_t flight_env = (uint8_t)(global_data_parameter_storage->pm.param_values[PARAM_FLIGHT_ENV]); - -// if (flight_env == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { -// //global_data_send_mavlink_statustext_message_out("Commander: EMERGENCY - GPS disabled", MAV_SEVERITY_EMERGENCY); -// state_machine_emergency(status_pub, current_status); -// } -// } -// break; - -// default: -// break; -// } - -// } - - -///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ -// -//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -//{ -// int ret = 1; -// -//// /* Switch on HIL if in standby and not already in HIL mode */ -//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) -//// && !current_status->flag_hil_enabled) { -//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { -//// /* Enable HIL on request */ -//// current_status->flag_hil_enabled = true; -//// ret = OK; -//// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); -//// -//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_fmu_armed) { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") -//// -//// } else { -//// -//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") -//// } -//// } -// -// /* switch manual / auto */ -// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { -// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { -// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { -// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); -// -// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { -// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); -// } -// -// /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only arm in standby state */ -// // XXX REMOVE -// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); -// ret = OK; -// printf("[cmd] arming due to command request\n"); -// } -// } -// -// /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { -// /* only disarm in ground ready */ -// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); -// ret = OK; -// printf("[cmd] disarming due to command request\n"); -// } -// } -// -// /* NEVER actually switch off HIL without reboot */ -// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { -// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); -// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); -// ret = ERROR; -// } -// -// return ret; -//} - diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index abb917873..11072403e 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -56,25 +56,15 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); - bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); -bool check_arming_state_changed(); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, + arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); -bool check_main_state_changed(); - -transition_result_t failsafe_state_transition(struct vehicle_status_s *status, failsafe_state_t new_failsafe_state); - -bool check_navigation_state_changed(); - -bool check_failsafe_state_changed(); - -void set_navigation_state_changed(); +transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); -int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, const int mavlink_fd); +bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 0a6d3fa8f..e4f805dc0 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -49,11 +49,11 @@ #include <math.h> #include <poll.h> #include <time.h> -#include <drivers/drv_hrt.h> +#include <float.h> #define SENSOR_COMBINED_SUB - +#include <drivers/drv_hrt.h> #include <drivers/drv_gyro.h> #include <drivers/drv_accel.h> #include <drivers/drv_mag.h> @@ -74,6 +74,7 @@ #include <uORB/topics/estimator_status.h> #include <uORB/topics/actuator_armed.h> #include <uORB/topics/home_position.h> +#include <uORB/topics/wind_estimate.h> #include <systemlib/param/param.h> #include <systemlib/err.h> #include <geo/geo.h> @@ -82,7 +83,7 @@ #include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> -#include "estimator.h" +#include "estimator_23states.h" @@ -95,7 +96,6 @@ extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); -static uint64_t last_run = 0; static uint64_t IMUmsec = 0; static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; @@ -120,7 +120,7 @@ public: /** * Start the sensors task. * - * @return OK on success. + * @return OK on success. */ int start(); @@ -134,6 +134,20 @@ public: */ int trip_nan(); + /** + * Enable logging. + * + * @param enable Set to true to enable logging, false to disable + */ + int enable_logging(bool enable); + + /** + * Set debug level. + * + * @param debug Desired debug level - 0 to disable. + */ + int set_debuglevel(unsigned debug) { _debug = debug; return 0; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -158,6 +172,7 @@ private: orb_advert_t _global_pos_pub; /**< global position */ orb_advert_t _local_pos_pub; /**< position in local frame */ orb_advert_t _estimator_status_pub; /**< status of the estimator */ + orb_advert_t _wind_pub; /**< wind estimate */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct gyro_report _gyro; @@ -169,6 +184,7 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct vehicle_local_position_s _local_pos; /**< local vehicle position */ struct vehicle_gps_position_s _gps; /**< GPS position */ + struct wind_estimate_s _wind; /**< Wind estimate */ struct gyro_scale _gyro_offsets; struct accel_scale _accel_offsets; @@ -181,24 +197,28 @@ private: struct map_projection_reference_s _pos_ref; float _baro_ref; /**< barometer reference altitude */ - float _baro_gps_offset; /**< offset between GPS and baro */ + float _baro_ref_offset; /**< offset between initial baro reference and GPS init baro altitude */ + float _baro_gps_offset; /**< offset between baro altitude (at GPS init time) and GPS altitude */ - perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _loop_perf; ///< loop performance counter perf_counter_t _perf_gyro; ///<local performance counter for gyro updates - perf_counter_t _perf_accel; ///<local performance counter for accel updates perf_counter_t _perf_mag; ///<local performance counter for mag updates perf_counter_t _perf_gps; ///<local performance counter for gps updates perf_counter_t _perf_baro; ///<local performance counter for baro updates perf_counter_t _perf_airspeed; ///<local performance counter for airspeed updates + perf_counter_t _perf_reset; ///<local performance counter for filter resets - bool _initialized; bool _baro_init; bool _gps_initialized; - uint64_t _gps_start_time; - uint64_t _filter_start_time; + hrt_abstime _gps_start_time; + hrt_abstime _filter_start_time; + hrt_abstime _last_sensor_timestamp; + hrt_abstime _last_run; bool _gyro_valid; bool _accel_valid; bool _mag_valid; + bool _ekf_logging; ///< log EKF state + unsigned _debug; ///< debug level - default 0 int _mavlink_fd; @@ -246,6 +266,10 @@ private: AttPosEKF *_ekf; + float _velocity_xy_filtered; + float _velocity_z_filtered; + float _airspeed_filtered; + /** * Update our local parameter cache. */ @@ -268,9 +292,16 @@ private: static void task_main_trampoline(int argc, char *argv[]); /** - * Main sensor collection task. + * Main filter task. */ void task_main(); + + /** + * Check filter sanity state + * + * @return zero if ok, non-zero for a filter error condition. + */ + int check_filter_state(); }; namespace estimator @@ -312,6 +343,7 @@ FixedwingEstimator::FixedwingEstimator() : _global_pos_pub(-1), _local_pos_pub(-1), _estimator_status_pub(-1), + _wind_pub(-1), _att({}), _gyro({}), @@ -323,6 +355,7 @@ FixedwingEstimator::FixedwingEstimator() : _global_pos({}), _local_pos({}), _gps({}), + _wind({}), _gyro_offsets({}), _accel_offsets({}), @@ -333,29 +366,34 @@ FixedwingEstimator::FixedwingEstimator() : #endif _baro_ref(0.0f), + _baro_ref_offset(0.0f), _baro_gps_offset(0.0f), /* performance counters */ - _loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")), - _perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")), - _perf_accel(perf_alloc(PC_COUNT, "ekf_att_pos_accel_upd")), - _perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")), - _perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")), - _perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")), - _perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")), + _loop_perf(perf_alloc(PC_ELAPSED, "ekf_att_pos_estimator")), + _perf_gyro(perf_alloc(PC_INTERVAL, "ekf_att_pos_gyro_upd")), + _perf_mag(perf_alloc(PC_INTERVAL, "ekf_att_pos_mag_upd")), + _perf_gps(perf_alloc(PC_INTERVAL, "ekf_att_pos_gps_upd")), + _perf_baro(perf_alloc(PC_INTERVAL, "ekf_att_pos_baro_upd")), + _perf_airspeed(perf_alloc(PC_INTERVAL, "ekf_att_pos_aspd_upd")), + _perf_reset(perf_alloc(PC_COUNT, "ekf_att_pos_reset")), /* states */ - _initialized(false), _baro_init(false), _gps_initialized(false), _gyro_valid(false), _accel_valid(false), _mag_valid(false), + _ekf_logging(true), + _debug(0), _mavlink_fd(-1), - _ekf(nullptr) + _ekf(nullptr), + _velocity_xy_filtered(0.0f), + _velocity_z_filtered(0.0f), + _airspeed_filtered(0.0f) { - last_run = hrt_absolute_time(); + _last_run = hrt_absolute_time(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); @@ -439,10 +477,20 @@ FixedwingEstimator::~FixedwingEstimator() } while (_estimator_task != -1); } + delete _ekf; + estimator::g_estimator = nullptr; } int +FixedwingEstimator::enable_logging(bool logging) +{ + _ekf_logging = logging; + + return 0; +} + +int FixedwingEstimator::parameters_update() { @@ -500,6 +548,151 @@ FixedwingEstimator::vehicle_status_poll() } } +int +FixedwingEstimator::check_filter_state() +{ + /* + * CHECK IF THE INPUT DATA IS SANE + */ + + struct ekf_status_report ekf_report; + + int check = _ekf->CheckAndBound(&ekf_report); + + const char* ekfname = "att pos estimator: "; + + switch (check) { + case 0: + /* all ok */ + break; + case 1: + { + const char* str = "NaN in states, resetting"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 2: + { + const char* str = "stale IMU data, resetting"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 3: + { + const char* str = "switching to dynamic state"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 4: + { + const char* str = "excessive gyro offsets"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 5: + { + const char* str = "GPS velocity divergence"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + case 6: + { + const char* str = "excessive covariances"; + warnx("%s", str); + mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); + break; + } + + default: + { + const char* str = "unknown reset condition"; + warnx("%s", str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + } + } + + struct estimator_status_report rep; + memset(&rep, 0, sizeof(rep)); + + // If error flag is set, we got a filter reset + if (check && ekf_report.error) { + + // Count the reset condition + perf_count(_perf_reset); + + } else if (_ekf_logging) { + _ekf->GetFilterState(&ekf_report); + } + + if (_ekf_logging || check) { + rep.timestamp = hrt_absolute_time(); + + rep.nan_flags |= (((uint8_t)ekf_report.angNaN) << 0); + rep.nan_flags |= (((uint8_t)ekf_report.summedDelVelNaN) << 1); + rep.nan_flags |= (((uint8_t)ekf_report.KHNaN) << 2); + rep.nan_flags |= (((uint8_t)ekf_report.KHPNaN) << 3); + rep.nan_flags |= (((uint8_t)ekf_report.PNaN) << 4); + rep.nan_flags |= (((uint8_t)ekf_report.covarianceNaN) << 5); + rep.nan_flags |= (((uint8_t)ekf_report.kalmanGainsNaN) << 6); + rep.nan_flags |= (((uint8_t)ekf_report.statesNaN) << 7); + + rep.health_flags |= (((uint8_t)ekf_report.velHealth) << 0); + rep.health_flags |= (((uint8_t)ekf_report.posHealth) << 1); + rep.health_flags |= (((uint8_t)ekf_report.hgtHealth) << 2); + rep.health_flags |= (((uint8_t)!ekf_report.gyroOffsetsExcessive) << 3); + + rep.timeout_flags |= (((uint8_t)ekf_report.velTimeout) << 0); + rep.timeout_flags |= (((uint8_t)ekf_report.posTimeout) << 1); + rep.timeout_flags |= (((uint8_t)ekf_report.hgtTimeout) << 2); + rep.timeout_flags |= (((uint8_t)ekf_report.imuTimeout) << 3); + + if (_debug > 10) { + + if (rep.health_flags < ((1 << 0) | (1 << 1) | (1 << 2) | (1 << 3))) { + warnx("health: VEL:%s POS:%s HGT:%s OFFS:%s", + ((rep.health_flags & (1 << 0)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 1)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 2)) ? "OK" : "ERR"), + ((rep.health_flags & (1 << 3)) ? "OK" : "ERR")); + } + + if (rep.timeout_flags) { + warnx("timeout: %s%s%s%s", + ((rep.timeout_flags & (1 << 0)) ? "VEL " : ""), + ((rep.timeout_flags & (1 << 1)) ? "POS " : ""), + ((rep.timeout_flags & (1 << 2)) ? "HGT " : ""), + ((rep.timeout_flags & (1 << 3)) ? "IMU " : "")); + } + } + + // Copy all states or at least all that we can fit + unsigned ekf_n_states = ekf_report.n_states; + unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); + rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; + + for (unsigned i = 0; i < rep.n_states; i++) { + rep.states[i] = ekf_report.states[i]; + } + + for (unsigned i = 0; i < rep.n_states; i++) { + rep.states[i] = ekf_report.states[i]; + } + + if (_estimator_status_pub > 0) { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); + } else { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); + } + } + + return check; +} + void FixedwingEstimator::task_main_trampoline(int argc, char *argv[]) { @@ -516,7 +709,7 @@ FixedwingEstimator::task_main() _filter_start_time = hrt_absolute_time(); if (!_ekf) { - errx(1, "failed allocating EKF filter - out of RAM!"); + errx(1, "OUT OF MEM!"); } /* @@ -544,7 +737,7 @@ FixedwingEstimator::task_main() #else _sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); /* XXX remove this!, BUT increase the data buffer size! */ - orb_set_interval(_sensor_combined_sub, 4); + orb_set_interval(_sensor_combined_sub, 9); #endif /* sets also parameters in the EKF object */ @@ -572,6 +765,13 @@ FixedwingEstimator::task_main() bool newAdsData = false; bool newDataMag = false; + float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m) + + uint64_t last_gps = 0; + _gps.vel_n_m_s = 0.0f; + _gps.vel_e_m_s = 0.0f; + _gps.vel_d_m_s = 0.0f; + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -583,7 +783,7 @@ FixedwingEstimator::task_main() /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { - warn("poll error %d, %d", pret, errno); + warn("POLL ERR %d, %d", pret, errno); continue; } @@ -609,8 +809,6 @@ FixedwingEstimator::task_main() bool accel_updated; bool mag_updated; - hrt_abstime last_sensor_timestamp; - perf_count(_perf_gyro); /* Reset baro reference if switching to HIL, reset sensor states */ @@ -634,12 +832,12 @@ FixedwingEstimator::task_main() _baro_init = false; _gps_initialized = false; - last_sensor_timestamp = hrt_absolute_time(); - last_run = last_sensor_timestamp; + _last_sensor_timestamp = hrt_absolute_time(); + _last_run = _last_sensor_timestamp; _ekf->ZeroVariables(); _ekf->dtIMU = 0.01f; - _filter_start_time = last_sensor_timestamp; + _filter_start_time = _last_sensor_timestamp; /* now skip this loop and get data on the next one, which will also re-init the filter */ continue; @@ -657,15 +855,14 @@ FixedwingEstimator::task_main() orb_check(_accel_sub, &accel_updated); if (accel_updated) { - perf_count(_perf_accel); orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } - last_sensor_timestamp = _gyro.timestamp; + _last_sensor_timestamp = _gyro.timestamp; IMUmsec = _gyro.timestamp / 1e3f; - float deltaT = (_gyro.timestamp - last_run) / 1e6f; - last_run = _gyro.timestamp; + float deltaT = (_gyro.timestamp - _last_run) / 1e6f; + _last_run = _gyro.timestamp; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { @@ -725,17 +922,17 @@ FixedwingEstimator::task_main() // Copy gyro and accel - last_sensor_timestamp = _sensor_combined.timestamp; + _last_sensor_timestamp = _sensor_combined.timestamp; IMUmsec = _sensor_combined.timestamp / 1e3f; - float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; + float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; } - last_run = _sensor_combined.timestamp; + _last_run = _sensor_combined.timestamp; // Always store data, independent of init status /* fill in last data set */ @@ -753,6 +950,7 @@ FixedwingEstimator::task_main() } _gyro_valid = true; + perf_count(_perf_gyro); } if (accel_updated) { @@ -784,6 +982,8 @@ FixedwingEstimator::task_main() #endif + //warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z); + bool airspeed_updated; orb_check(_airspeed_sub, &airspeed_updated); @@ -803,7 +1003,7 @@ FixedwingEstimator::task_main() if (gps_updated) { - uint64_t last_gps = _gps.timestamp_position; + last_gps = _gps.timestamp_position; orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps); perf_count(_perf_gps); @@ -871,6 +1071,8 @@ FixedwingEstimator::task_main() warnx("ALT REF INIT"); } + perf_count(_perf_baro); + newHgtData = true; } else { newHgtData = false; @@ -921,127 +1123,39 @@ FixedwingEstimator::task_main() newDataMag = false; } - - /** - * CHECK IF THE INPUT DATA IS SANE + /* + * CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY */ - int check = _ekf->CheckAndBound(); - - const char* ekfname = "[ekf] "; - - switch (check) { - case 0: - /* all ok */ - break; - case 1: - { - const char* str = "NaN in states, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 2: - { - const char* str = "stale IMU data, resetting"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - break; - } - case 3: - { - const char* str = "switching to dynamic state"; - warnx("%s", str); - mavlink_log_info(_mavlink_fd, "%s%s", ekfname, str); - break; - } - - default: - { - const char* str = "unknown reset condition"; - warnx("%s", str); - mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); - } - } - - // warn on fatal resets - if (check == 1) { - warnx("NUMERIC ERROR IN FILTER"); - } - - // If non-zero, we got a filter reset - if (check) { - - struct ekf_status_report ekf_report; - - _ekf->GetLastErrorState(&ekf_report); - - struct estimator_status_report rep; - memset(&rep, 0, sizeof(rep)); - rep.timestamp = hrt_absolute_time(); - - rep.states_nan = ekf_report.statesNaN; - rep.covariance_nan = ekf_report.covarianceNaN; - rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; - - // Copy all states or at least all that we can fit - unsigned i = 0; - unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); - unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); - rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; - - while ((i < ekf_n_states) && (i < max_states)) { - - rep.states[i] = ekf_report.states[i]; - i++; - } - - if (_estimator_status_pub > 0) { - orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &rep); - } else { - _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); - } - - /* set sensors to de-initialized state */ - _gyro_valid = false; - _accel_valid = false; - _mag_valid = false; - - _baro_init = false; - _gps_initialized = false; - last_sensor_timestamp = hrt_absolute_time(); - last_run = last_sensor_timestamp; - - _ekf->ZeroVariables(); - _ekf->dtIMU = 0.01f; - - // Let the system re-initialize itself + if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) { continue; - } - /** * PART TWO: EXECUTE THE FILTER + * + * We run the filter only once all data has been fetched **/ - if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) { + if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) { float initVelNED[3]; - if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { - - initVelNED[0] = _gps.vel_n_m_s; - initVelNED[1] = _gps.vel_e_m_s; - initVelNED[2] = _gps.vel_d_m_s; + /* Initialize the filter first */ + if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) { // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; float gps_alt = _gps.alt / 1e3f; + initVelNED[0] = _gps.vel_n_m_s; + initVelNED[1] = _gps.vel_e_m_s; + initVelNED[2] = _gps.vel_d_m_s; + // Set up height correctly orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_gps_offset = _baro_ref - _baro.altitude; + _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame + _baro_gps_offset = _baro.altitude - gps_alt; _ekf->baroHgt = _baro.altitude; _ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref)); @@ -1065,10 +1179,13 @@ FixedwingEstimator::task_main() map_projection_init(&_pos_ref, lat, lon); mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + + #if 0 warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); - warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset); - warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination)); + warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset); + warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination)); + #endif _gps_initialized = true; @@ -1077,263 +1194,322 @@ FixedwingEstimator::task_main() initVelNED[0] = 0.0f; initVelNED[1] = 0.0f; initVelNED[2] = 0.0f; - _ekf->posNED[0] = 0.0f; - _ekf->posNED[1] = 0.0f; - _ekf->posNED[2] = 0.0f; - - _ekf->posNE[0] = _ekf->posNED[0]; - _ekf->posNE[1] = _ekf->posNED[1]; + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; _local_pos.ref_alt = _baro_ref; + _baro_ref_offset = 0.0f; _baro_gps_offset = 0.0f; _ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f); - } - } - - // If valid IMU data and states initialised, predict states and covariances - if (_ekf->statesInitialised) { - // Run the strapdown INS equations every IMU update - _ekf->UpdateStrapdownEquationsNED(); -#if 0 - // debug code - could be tunred into a filter mnitoring/watchdog function - float tempQuat[4]; - - for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; - - quat2eul(eulerEst, tempQuat); - - for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; - - if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; - - if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; - -#endif - // store the predicted states for subsequent use by measurement fusion - _ekf->StoreStates(IMUmsec); - // Check if on ground - status is used by covariance prediction - _ekf->OnGroundCheck(); - // sum delta angles and time used by covariance prediction - _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; - _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; - dt += _ekf->dtIMU; - - // perform a covariance prediction if the total delta angle has exceeded the limit - // or the time limit will be exceeded at the next IMU update - if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { - _ekf->CovariancePrediction(dt); - _ekf->summedDelAng.zero(); - _ekf->summedDelVel.zero(); - dt = 0.0f; - } + } else if (_ekf->statesInitialised) { - _initialized = true; - } + // We're apparently initialized in this case now - // Fuse GPS Measurements - if (newDataGps && _gps_initialized) { - // Convert GPS measurements to Pos NE, hgt and Vel NED - _ekf->velNED[0] = _gps.vel_n_m_s; - _ekf->velNED[1] = _gps.vel_e_m_s; - _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->calcposNED(_ekf->posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); - - _ekf->posNE[0] = _ekf->posNED[0]; - _ekf->posNE[1] = _ekf->posNED[1]; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - - } else if (_ekf->statesInitialised) { - // Convert GPS measurements to Pos NE, hgt and Vel NED - _ekf->velNED[0] = 0.0f; - _ekf->velNED[1] = 0.0f; - _ekf->velNED[2] = 0.0f; - _ekf->posNED[0] = 0.0f; - _ekf->posNED[1] = 0.0f; - _ekf->posNED[2] = 0.0f; - - _ekf->posNE[0] = _ekf->posNED[0]; - _ekf->posNE[1] = _ekf->posNED[1]; - // set fusion flags - _ekf->fuseVelData = true; - _ekf->fusePosData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); - _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); + int check = check_filter_state(); - } else { - _ekf->fuseVelData = false; - _ekf->fusePosData = false; - } + if (check) { + // Let the system re-initialize itself + continue; + } - if (newHgtData && _ekf->statesInitialised) { - // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); - _ekf->fuseHgtData = true; - // recall states stored at time of measurement after adjusting for delays - _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); - // run the fusion step - _ekf->FuseVelposNED(); - } else { - _ekf->fuseHgtData = false; - } + // Run the strapdown INS equations every IMU update + _ekf->UpdateStrapdownEquationsNED(); + #if 0 + // debug code - could be tunred into a filter mnitoring/watchdog function + float tempQuat[4]; - // Fuse Magnetometer Measurements - if (newDataMag && _ekf->statesInitialised) { - _ekf->fuseMagData = true; - _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data + for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j]; - } else { - _ekf->fuseMagData = false; - } + quat2eul(eulerEst, tempQuat); - if (_ekf->statesInitialised) _ekf->FuseMagnetometer(); + for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j]; - // Fuse Airspeed Measurements - if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { - _ekf->fuseVtasData = true; - _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data - _ekf->FuseAirspeed(); + if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi; - } else { - _ekf->fuseVtasData = false; - } + if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi; - // Publish results - if (_initialized && (check == OK)) { - - - - // State vector: - // 0-3: quaternions (q0, q1, q2, q3) - // 4-6: Velocity - m/sec (North, East, Down) - // 7-9: Position - m (North, East, Down) - // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - milligauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - milligauss (X,Y,Z) - - math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); - math::Matrix<3, 3> R = q.to_dcm(); - math::Vector<3> euler = R.to_euler(); - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = R(i, j); - - _att.timestamp = last_sensor_timestamp; - _att.q[0] = _ekf->states[0]; - _att.q[1] = _ekf->states[1]; - _att.q[2] = _ekf->states[2]; - _att.q[3] = _ekf->states[3]; - _att.q_valid = true; - _att.R_valid = true; - - _att.timestamp = last_sensor_timestamp; - _att.roll = euler(0); - _att.pitch = euler(1); - _att.yaw = euler(2); - - _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; - _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; - _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; - // gyro offsets - _att.rate_offsets[0] = _ekf->states[10]; - _att.rate_offsets[1] = _ekf->states[11]; - _att.rate_offsets[2] = _ekf->states[12]; - - /* lazily publish the attitude only once available */ - if (_att_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + #endif + // store the predicted states for subsequent use by measurement fusion + _ekf->StoreStates(IMUmsec); + // Check if on ground - status is used by covariance prediction + _ekf->OnGroundCheck(); + // sum delta angles and time used by covariance prediction + _ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng; + _ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU; + dt += _ekf->dtIMU; + + // perform a covariance prediction if the total delta angle has exceeded the limit + // or the time limit will be exceeded at the next IMU update + if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { + _ekf->CovariancePrediction(dt); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); + dt = 0.0f; + } - } else { - /* advertise and publish */ - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); - } - } + // Fuse GPS Measurements + if (newDataGps && _gps_initialized) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + + float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f; + + // Calculate acceleration predicted by GPS velocity change + if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) || + (fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) || + (fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) { + + _ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt; + _ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt; + _ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt; + } + + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + _ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef); + + _ekf->posNE[0] = posNED[0]; + _ekf->posNE[1] = posNED[1]; + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); + + } else if (_ekf->statesInitialised) { + // Convert GPS measurements to Pos NE, hgt and Vel NED + _ekf->velNED[0] = 0.0f; + _ekf->velNED[1] = 0.0f; + _ekf->velNED[2] = 0.0f; + + _ekf->posNE[0] = 0.0f; + _ekf->posNE[1] = 0.0f; + // set fusion flags + _ekf->fuseVelData = true; + _ekf->fusePosData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms)); + _ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); + + } else { + _ekf->fuseVelData = false; + _ekf->fusePosData = false; + } - if (_gps_initialized) { - _local_pos.timestamp = last_sensor_timestamp; - _local_pos.x = _ekf->states[7]; - _local_pos.y = _ekf->states[8]; - // XXX need to announce change of Z reference somehow elegantly - _local_pos.z = _ekf->states[9] - _baro_gps_offset; + if (newHgtData && _ekf->statesInitialised) { + // Could use a blend of GPS and baro alt data if desired + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + _ekf->fuseHgtData = true; + // recall states stored at time of measurement after adjusting for delays + _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); + // run the fusion step + _ekf->FuseVelposNED(); + + } else { + _ekf->fuseHgtData = false; + } - _local_pos.vx = _ekf->states[4]; - _local_pos.vy = _ekf->states[5]; - _local_pos.vz = _ekf->states[6]; + // Fuse Magnetometer Measurements + if (newDataMag && _ekf->statesInitialised) { + _ekf->fuseMagData = true; + _ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data - _local_pos.xy_valid = _gps_initialized; - _local_pos.z_valid = true; - _local_pos.v_xy_valid = _gps_initialized; - _local_pos.v_z_valid = true; - _local_pos.xy_global = true; + _ekf->magstate.obsIndex = 0; + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); + _ekf->FuseMagnetometer(); - _local_pos.z_global = false; - _local_pos.yaw = _att.yaw; + } else { + _ekf->fuseMagData = false; + } - /* lazily publish the local position only once available */ - if (_local_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + // Fuse Airspeed Measurements + if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) { + _ekf->fuseVtasData = true; + _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data + _ekf->FuseAirspeed(); - } else { - /* advertise and publish */ - _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); - } + } else { + _ekf->fuseVtasData = false; + } - _global_pos.timestamp = _local_pos.timestamp; - if (_local_pos.xy_global) { - double est_lat, est_lon; - map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); - _global_pos.lat = est_lat; - _global_pos.lon = est_lon; - _global_pos.time_gps_usec = _gps.time_gps_usec; - _global_pos.eph = _gps.eph_m; - _global_pos.epv = _gps.epv_m; - } + // Output results + math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + math::Matrix<3, 3> R = q.to_dcm(); + math::Vector<3> euler = R.to_euler(); + + for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) + _att.R[i][j] = R(i, j); + + _att.timestamp = _last_sensor_timestamp; + _att.q[0] = _ekf->states[0]; + _att.q[1] = _ekf->states[1]; + _att.q[2] = _ekf->states[2]; + _att.q[3] = _ekf->states[3]; + _att.q_valid = true; + _att.R_valid = true; + + _att.timestamp = _last_sensor_timestamp; + _att.roll = euler(0); + _att.pitch = euler(1); + _att.yaw = euler(2); + + _att.rollspeed = _ekf->angRate.x - _ekf->states[10]; + _att.pitchspeed = _ekf->angRate.y - _ekf->states[11]; + _att.yawspeed = _ekf->angRate.z - _ekf->states[12]; + // gyro offsets + _att.rate_offsets[0] = _ekf->states[10]; + _att.rate_offsets[1] = _ekf->states[11]; + _att.rate_offsets[2] = _ekf->states[12]; + + /* lazily publish the attitude only once available */ + if (_att_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att); + + } else { + /* advertise and publish */ + _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att); + } - if (_local_pos.v_xy_valid) { - _global_pos.vel_n = _local_pos.vx; - _global_pos.vel_e = _local_pos.vy; - } else { - _global_pos.vel_n = 0.0f; - _global_pos.vel_e = 0.0f; - } + if (_gps_initialized) { + _local_pos.timestamp = _last_sensor_timestamp; + _local_pos.x = _ekf->states[7]; + _local_pos.y = _ekf->states[8]; + // XXX need to announce change of Z reference somehow elegantly + _local_pos.z = _ekf->states[9] - _baro_ref_offset; + + _local_pos.vx = _ekf->states[4]; + _local_pos.vy = _ekf->states[5]; + _local_pos.vz = _ekf->states[6]; + + _local_pos.xy_valid = _gps_initialized; + _local_pos.z_valid = true; + _local_pos.v_xy_valid = _gps_initialized; + _local_pos.v_z_valid = true; + _local_pos.xy_global = true; + + _velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy); + _velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz); + _airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s; + + + /* crude land detector for fixedwing only, + * TODO: adapt so that it works for both, maybe move to another location + */ + if (_velocity_xy_filtered < 5 + && _velocity_z_filtered < 10 + && _airspeed_filtered < 10) { + _local_pos.landed = true; + } else { + _local_pos.landed = false; + } + + _local_pos.z_global = false; + _local_pos.yaw = _att.yaw; + + /* lazily publish the local position only once available */ + if (_local_pos_pub > 0) { + /* publish the attitude setpoint */ + orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos); + + } else { + /* advertise and publish */ + _local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos); + } + + _global_pos.timestamp = _local_pos.timestamp; + + if (_local_pos.xy_global) { + double est_lat, est_lon; + map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon); + _global_pos.lat = est_lat; + _global_pos.lon = est_lon; + _global_pos.time_gps_usec = _gps.time_gps_usec; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + } + + if (_local_pos.v_xy_valid) { + _global_pos.vel_n = _local_pos.vx; + _global_pos.vel_e = _local_pos.vy; + } else { + _global_pos.vel_n = 0.0f; + _global_pos.vel_e = 0.0f; + } + + /* local pos alt is negative, change sign and add alt offsets */ + _global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset; + + if (_local_pos.v_z_valid) { + _global_pos.vel_d = _local_pos.vz; + } + + + _global_pos.yaw = _local_pos.yaw; + + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; + + _global_pos.timestamp = _local_pos.timestamp; + + /* lazily publish the global position only once available */ + if (_global_pos_pub > 0) { + /* publish the global position */ + orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); + + } else { + /* advertise and publish */ + _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + } + + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = 0.0f; // XXX get form filter + _wind.covariance_east = 0.0f; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } + + } - /* local pos alt is negative, change sign and add alt offsets */ - _global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z); + } - if (_local_pos.v_z_valid) { - _global_pos.vel_d = _local_pos.vz; } - _global_pos.yaw = _local_pos.yaw; - - _global_pos.eph = _gps.eph_m; - _global_pos.epv = _gps.epv_m; - - _global_pos.timestamp = _local_pos.timestamp; - - /* lazily publish the global position only once available */ - if (_global_pos_pub > 0) { - /* publish the attitude setpoint */ - orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos); - - } else { - /* advertise and publish */ - _global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos); + if (hrt_elapsed_time(&_wind.timestamp) > 99000) { + _wind.timestamp = _global_pos.timestamp; + _wind.windspeed_north = _ekf->states[14]; + _wind.windspeed_east = _ekf->states[15]; + _wind.covariance_north = _ekf->P[14][14]; + _wind.covariance_east = _ekf->P[15][15]; + + /* lazily publish the wind estimate only once available */ + if (_wind_pub > 0) { + /* publish the wind estimate */ + orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind); + + } else { + /* advertise and publish */ + _wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind); + } } } @@ -1384,32 +1560,44 @@ FixedwingEstimator::print_status() // 4-6: Velocity - m/sec (North, East, Down) // 7-9: Position - m (North, East, Down) // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + // 13: Accelerometer offset + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec); - printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt); + printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f)); + printf("ref alt: %8.4f baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref, (double)_baro_ref_offset, (double)_baro_gps_offset); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); - printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); - printf("states (vel m/s) [5-7]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); - printf("states (pos m) [8-10]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); - printf("states (delta ang) [11-13]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); - printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); - printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); - printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + printf("states (quat) [0-3]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); + printf("states (vel m/s) [4-6]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[4], (double)_ekf->states[5], (double)_ekf->states[6]); + printf("states (pos m) [7-9]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[7], (double)_ekf->states[8], (double)_ekf->states[9]); + printf("states (delta ang) [10-12]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[10], (double)_ekf->states[11], (double)_ekf->states[12]); + + if (n_states == 23) { + printf("states (accel offs) [13]: %8.4f\n", (double)_ekf->states[13]); + printf("states (wind) [14-15]: %8.4f, %8.4f\n", (double)_ekf->states[14], (double)_ekf->states[15]); + printf("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[16], (double)_ekf->states[17], (double)_ekf->states[18]); + printf("states (body mag) [19-21]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[19], (double)_ekf->states[20], (double)_ekf->states[21]); + printf("states (terrain) [22]: %8.4f\n", (double)_ekf->states[22]); + + } else { + printf("states (wind) [13-14]: %8.4f, %8.4f\n", (double)_ekf->states[13], (double)_ekf->states[14]); + printf("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[15], (double)_ekf->states[16], (double)_ekf->states[17]); + printf("states (body mag) [18-20]: %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[18], (double)_ekf->states[19], (double)_ekf->states[20]); + } printf("states: %s %s %s %s %s %s %s %s %s %s\n", - (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", - (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", - (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", - (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", - (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", - (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", - (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", - (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", - (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", - (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); + (_ekf->statesInitialised) ? "INITIALIZED" : "NON_INIT", + (_ekf->onGround) ? "ON_GROUND" : "AIRBORNE", + (_ekf->fuseVelData) ? "FUSE_VEL" : "INH_VEL", + (_ekf->fusePosData) ? "FUSE_POS" : "INH_POS", + (_ekf->fuseHgtData) ? "FUSE_HGT" : "INH_HGT", + (_ekf->fuseMagData) ? "FUSE_MAG" : "INH_MAG", + (_ekf->fuseVtasData) ? "FUSE_VTAS" : "INH_VTAS", + (_ekf->useAirspeed) ? "USE_AIRSPD" : "IGN_AIRSPD", + (_ekf->useCompass) ? "USE_COMPASS" : "IGN_COMPASS", + (_ekf->staticMode) ? "STATIC_MODE" : "DYNAMIC_MODE"); } int FixedwingEstimator::trip_nan() { @@ -1464,7 +1652,7 @@ int FixedwingEstimator::trip_nan() { int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) - errx(1, "usage: ekf_att_pos_estimator {start|stop|status}"); + errx(1, "usage: ekf_att_pos_estimator {start|stop|status|logging}"); if (!strcmp(argv[1], "start")) { @@ -1518,6 +1706,29 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) } } + if (!strcmp(argv[1], "logging")) { + if (estimator::g_estimator) { + int ret = estimator::g_estimator->enable_logging(true); + + exit(ret); + + } else { + errx(1, "not running"); + } + } + + if (!strcmp(argv[1], "debug")) { + if (estimator::g_estimator) { + int debug = strtoul(argv[2], NULL, 10); + int ret = estimator::g_estimator->set_debuglevel(debug); + + exit(ret); + + } else { + errx(1, "not running"); + } + } + warnx("unrecognized command"); return 1; } diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.cpp b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp new file mode 100644 index 000000000..67bfec4ea --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.cpp @@ -0,0 +1,2142 @@ +#include "estimator_21states.h" + +#include <string.h> + +AttPosEKF::AttPosEKF() : + fusionModeGPS(0), + covSkipCount(0), + EAS2TAS(1.0f), + statesInitialised(false), + fuseVelData(false), + fusePosData(false), + fuseHgtData(false), + fuseMagData(false), + fuseVtasData(false), + onGround(true), + staticMode(true), + useAirspeed(true), + useCompass(true), + numericalProtection(true), + storeIndex(0), + magDeclination(0.0f) +{ + InitialiseParameters(); +} + +AttPosEKF::~AttPosEKF() +{ +} + +void AttPosEKF::UpdateStrapdownEquationsNED() +{ + Vector3f delVelNav; + float q00; + float q11; + float q22; + float q33; + float q01; + float q02; + float q03; + float q12; + float q13; + float q23; + Mat3f Tbn; + Mat3f Tnb; + float rotationMag; + float qUpdated[4]; + float quatMag; + double deltaQuat[4]; + const Vector3f gravityNED = {0.0,0.0,GRAVITY_MSS}; + +// Remove sensor bias errors + correctedDelAng.x = dAngIMU.x - states[10]; + correctedDelAng.y = dAngIMU.y - states[11]; + correctedDelAng.z = dAngIMU.z - states[12]; + dVelIMU.x = dVelIMU.x; + dVelIMU.y = dVelIMU.y; + dVelIMU.z = dVelIMU.z; + +// Save current measurements + Vector3f prevDelAng = correctedDelAng; + +// Apply corrections for earths rotation rate and coning errors +// * and + operators have been overloaded + correctedDelAng = correctedDelAng - Tnb*earthRateNED*dtIMU + 8.333333333333333e-2f*(prevDelAng % correctedDelAng); +// Convert the rotation vector to its equivalent quaternion + rotationMag = correctedDelAng.length(); + if (rotationMag < 1e-12f) + { + deltaQuat[0] = 1.0; + deltaQuat[1] = 0.0; + deltaQuat[2] = 0.0; + deltaQuat[3] = 0.0; + } + else + { + deltaQuat[0] = cos(0.5f*rotationMag); + double rotScaler = (sin(0.5f*rotationMag))/rotationMag; + deltaQuat[1] = correctedDelAng.x*rotScaler; + deltaQuat[2] = correctedDelAng.y*rotScaler; + deltaQuat[3] = correctedDelAng.z*rotScaler; + } + +// Update the quaternions by rotating from the previous attitude through +// the delta angle rotation quaternion + qUpdated[0] = states[0]*deltaQuat[0] - states[1]*deltaQuat[1] - states[2]*deltaQuat[2] - states[3]*deltaQuat[3]; + qUpdated[1] = states[0]*deltaQuat[1] + states[1]*deltaQuat[0] + states[2]*deltaQuat[3] - states[3]*deltaQuat[2]; + qUpdated[2] = states[0]*deltaQuat[2] + states[2]*deltaQuat[0] + states[3]*deltaQuat[1] - states[1]*deltaQuat[3]; + qUpdated[3] = states[0]*deltaQuat[3] + states[3]*deltaQuat[0] + states[1]*deltaQuat[2] - states[2]*deltaQuat[1]; + +// Normalise the quaternions and update the quaternion states + quatMag = sqrtf(sq(qUpdated[0]) + sq(qUpdated[1]) + sq(qUpdated[2]) + sq(qUpdated[3])); + if (quatMag > 1e-16f) + { + float quatMagInv = 1.0f/quatMag; + states[0] = quatMagInv*qUpdated[0]; + states[1] = quatMagInv*qUpdated[1]; + states[2] = quatMagInv*qUpdated[2]; + states[3] = quatMagInv*qUpdated[3]; + } + +// Calculate the body to nav cosine matrix + q00 = sq(states[0]); + q11 = sq(states[1]); + q22 = sq(states[2]); + q33 = sq(states[3]); + q01 = states[0]*states[1]; + q02 = states[0]*states[2]; + q03 = states[0]*states[3]; + q12 = states[1]*states[2]; + q13 = states[1]*states[3]; + q23 = states[2]*states[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); + + Tnb = Tbn.transpose(); + +// transform body delta velocities to delta velocities in the nav frame +// * and + operators have been overloaded + //delVelNav = Tbn*dVelIMU + gravityNED*dtIMU; + delVelNav.x = Tbn.x.x*dVelIMU.x + Tbn.x.y*dVelIMU.y + Tbn.x.z*dVelIMU.z + gravityNED.x*dtIMU; + delVelNav.y = Tbn.y.x*dVelIMU.x + Tbn.y.y*dVelIMU.y + Tbn.y.z*dVelIMU.z + gravityNED.y*dtIMU; + delVelNav.z = Tbn.z.x*dVelIMU.x + Tbn.z.y*dVelIMU.y + Tbn.z.z*dVelIMU.z + gravityNED.z*dtIMU; + +// calculate the magnitude of the nav acceleration (required for GPS +// variance estimation) + accNavMag = delVelNav.length()/dtIMU; + +// If calculating position save previous velocity + float lastVelocity[3]; + lastVelocity[0] = states[4]; + lastVelocity[1] = states[5]; + lastVelocity[2] = states[6]; + +// Sum delta velocities to get velocity + states[4] = states[4] + delVelNav.x; + states[5] = states[5] + delVelNav.y; + states[6] = states[6] + delVelNav.z; + +// If calculating postions, do a trapezoidal integration for position + states[7] = states[7] + 0.5f*(states[4] + lastVelocity[0])*dtIMU; + states[8] = states[8] + 0.5f*(states[5] + lastVelocity[1])*dtIMU; + states[9] = states[9] + 0.5f*(states[6] + lastVelocity[2])*dtIMU; + + // Constrain states (to protect against filter divergence) + //ConstrainStates(); +} + +void AttPosEKF::CovariancePrediction(float dt) +{ + // scalars + float daxCov; + float dayCov; + float dazCov; + float dvxCov; + float dvyCov; + float dvzCov; + float dvx; + float dvy; + float dvz; + float dax; + float day; + float daz; + float q0; + float q1; + float q2; + float q3; + float dax_b; + float day_b; + float daz_b; + + // arrays + float processNoise[21]; + float SF[14]; + float SG[8]; + float SQ[11]; + float SPP[13] = {0}; + float nextP[21][21]; + + // calculate covariance prediction process noise + windVelSigma = dt*0.1f; + dAngBiasSigma = dt*5.0e-7f; + magEarthSigma = dt*3.0e-4f; + magBodySigma = dt*3.0e-4f; + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; + if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale; + for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma; + for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; + for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; + for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); + + // set variables used to calculate covariance growth + dvx = summedDelVel.x; + dvy = summedDelVel.y; + dvz = summedDelVel.z; + dax = summedDelAng.x; + day = summedDelAng.y; + daz = summedDelAng.z; + q0 = states[0]; + q1 = states[1]; + q2 = states[2]; + q3 = states[3]; + dax_b = states[10]; + day_b = states[11]; + daz_b = states[12]; + daxCov = sq(dt*1.4544411e-2f); + dayCov = sq(dt*1.4544411e-2f); + dazCov = sq(dt*1.4544411e-2f); + if (onGround) dazCov = dazCov * sq(yawVarScale); + dvxCov = sq(dt*0.5f); + dvyCov = sq(dt*0.5f); + dvzCov = sq(dt*0.5f); + + // Predicted covariance calculation + SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; + SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; + SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; + SF[3] = day/2 - day_b/2; + SF[4] = daz/2 - daz_b/2; + SF[5] = dax/2 - dax_b/2; + SF[6] = dax_b/2 - dax/2; + SF[7] = daz_b/2 - daz/2; + SF[8] = day_b/2 - day/2; + SF[9] = q1/2; + SF[10] = q2/2; + SF[11] = q3/2; + SF[12] = 2*dvz*q0; + SF[13] = 2*dvy*q1; + + SG[0] = q0/2; + SG[1] = sq(q3); + SG[2] = sq(q2); + SG[3] = sq(q1); + SG[4] = sq(q0); + SG[5] = 2*q2*q3; + SG[6] = 2*q1*q3; + SG[7] = 2*q1*q2; + + SQ[0] = dvzCov*(SG[5] - 2*q0*q1)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvyCov*(SG[5] + 2*q0*q1)*(SG[1] - SG[2] + SG[3] - SG[4]) + dvxCov*(SG[6] - 2*q0*q2)*(SG[7] + 2*q0*q3); + SQ[1] = dvzCov*(SG[6] + 2*q0*q2)*(SG[1] - SG[2] - SG[3] + SG[4]) - dvxCov*(SG[6] - 2*q0*q2)*(SG[1] + SG[2] - SG[3] - SG[4]) + dvyCov*(SG[5] + 2*q0*q1)*(SG[7] - 2*q0*q3); + SQ[2] = dvzCov*(SG[5] - 2*q0*q1)*(SG[6] + 2*q0*q2) - dvyCov*(SG[7] - 2*q0*q3)*(SG[1] - SG[2] + SG[3] - SG[4]) - dvxCov*(SG[7] + 2*q0*q3)*(SG[1] + SG[2] - SG[3] - SG[4]); + SQ[3] = (dayCov*q1*SG[0])/2 - (dazCov*q1*SG[0])/2 - (daxCov*q2*q3)/4; + SQ[4] = (dazCov*q2*SG[0])/2 - (daxCov*q2*SG[0])/2 - (dayCov*q1*q3)/4; + SQ[5] = (daxCov*q3*SG[0])/2 - (dayCov*q3*SG[0])/2 - (dazCov*q1*q2)/4; + SQ[6] = (daxCov*q1*q2)/4 - (dazCov*q3*SG[0])/2 - (dayCov*q1*q2)/4; + SQ[7] = (dazCov*q1*q3)/4 - (daxCov*q1*q3)/4 - (dayCov*q2*SG[0])/2; + SQ[8] = (dayCov*q2*q3)/4 - (daxCov*q1*SG[0])/2 - (dazCov*q2*q3)/4; + SQ[9] = sq(SG[0]); + SQ[10] = sq(q1); + + SPP[0] = SF[12] + SF[13] - 2*dvx*q2; + SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; + SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; + SPP[3] = SF[11]; + SPP[4] = SF[10]; + SPP[5] = SF[9]; + SPP[6] = SF[7]; + SPP[7] = SF[8]; + + nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]); + nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]); + nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]); + nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]); + nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]); + nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]); + nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]; + nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]; + nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]; + nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3]; + nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3]; + nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3]; + nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3]; + nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3]; + nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3]; + nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3]; + nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3]; + nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2); + nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2); + nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2; + nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2; + nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2; + nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2; + nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2; + nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2; + nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2); + nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2); + nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2; + nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2); + nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2; + nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2); + nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2; + nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]); + nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2; + nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2; + nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]); + nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]); + nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]); + nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]); + nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]; + nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]; + nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]; + nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2]; + nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2]; + nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2]; + nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2]; + nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2]; + nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2]; + nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2]; + nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2]; + nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]); + nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2; + nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2; + nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]); + nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]); + nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]); + nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]); + nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]); + nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]; + nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]; + nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]; + nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0]; + nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0]; + nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0]; + nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0]; + nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0]; + nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0]; + nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0]; + nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0]; + nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]); + nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2; + nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2; + nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]); + nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]); + nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]); + nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]); + nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]; + nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]; + nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]; + nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1]; + nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1]; + nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1]; + nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1]; + nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1]; + nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1]; + nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1]; + nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1]; + nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt); + nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; + nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt); + nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); + nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); + nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); + nextP[7][10] = P[7][10] + P[4][10]*dt; + nextP[7][11] = P[7][11] + P[4][11]*dt; + nextP[7][12] = P[7][12] + P[4][12]*dt; + nextP[7][13] = P[7][13] + P[4][13]*dt; + nextP[7][14] = P[7][14] + P[4][14]*dt; + nextP[7][15] = P[7][15] + P[4][15]*dt; + nextP[7][16] = P[7][16] + P[4][16]*dt; + nextP[7][17] = P[7][17] + P[4][17]*dt; + nextP[7][18] = P[7][18] + P[4][18]*dt; + nextP[7][19] = P[7][19] + P[4][19]*dt; + nextP[7][20] = P[7][20] + P[4][20]*dt; + nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt); + nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; + nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt); + nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); + nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); + nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); + nextP[8][10] = P[8][10] + P[5][10]*dt; + nextP[8][11] = P[8][11] + P[5][11]*dt; + nextP[8][12] = P[8][12] + P[5][12]*dt; + nextP[8][13] = P[8][13] + P[5][13]*dt; + nextP[8][14] = P[8][14] + P[5][14]*dt; + nextP[8][15] = P[8][15] + P[5][15]*dt; + nextP[8][16] = P[8][16] + P[5][16]*dt; + nextP[8][17] = P[8][17] + P[5][17]*dt; + nextP[8][18] = P[8][18] + P[5][18]*dt; + nextP[8][19] = P[8][19] + P[5][19]*dt; + nextP[8][20] = P[8][20] + P[5][20]*dt; + nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt); + nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; + nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt); + nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); + nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); + nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); + nextP[9][10] = P[9][10] + P[6][10]*dt; + nextP[9][11] = P[9][11] + P[6][11]*dt; + nextP[9][12] = P[9][12] + P[6][12]*dt; + nextP[9][13] = P[9][13] + P[6][13]*dt; + nextP[9][14] = P[9][14] + P[6][14]*dt; + nextP[9][15] = P[9][15] + P[6][15]*dt; + nextP[9][16] = P[9][16] + P[6][16]*dt; + nextP[9][17] = P[9][17] + P[6][17]*dt; + nextP[9][18] = P[9][18] + P[6][18]*dt; + nextP[9][19] = P[9][19] + P[6][19]*dt; + nextP[9][20] = P[9][20] + P[6][20]*dt; + nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3]; + nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2; + nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2; + nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2]; + nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0]; + nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1]; + nextP[10][7] = P[10][7] + P[10][4]*dt; + nextP[10][8] = P[10][8] + P[10][5]*dt; + nextP[10][9] = P[10][9] + P[10][6]*dt; + nextP[10][10] = P[10][10]; + nextP[10][11] = P[10][11]; + nextP[10][12] = P[10][12]; + nextP[10][13] = P[10][13]; + nextP[10][14] = P[10][14]; + nextP[10][15] = P[10][15]; + nextP[10][16] = P[10][16]; + nextP[10][17] = P[10][17]; + nextP[10][18] = P[10][18]; + nextP[10][19] = P[10][19]; + nextP[10][20] = P[10][20]; + nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3]; + nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2; + nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2; + nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2]; + nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0]; + nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1]; + nextP[11][7] = P[11][7] + P[11][4]*dt; + nextP[11][8] = P[11][8] + P[11][5]*dt; + nextP[11][9] = P[11][9] + P[11][6]*dt; + nextP[11][10] = P[11][10]; + nextP[11][11] = P[11][11]; + nextP[11][12] = P[11][12]; + nextP[11][13] = P[11][13]; + nextP[11][14] = P[11][14]; + nextP[11][15] = P[11][15]; + nextP[11][16] = P[11][16]; + nextP[11][17] = P[11][17]; + nextP[11][18] = P[11][18]; + nextP[11][19] = P[11][19]; + nextP[11][20] = P[11][20]; + nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3]; + nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2; + nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2; + nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2]; + nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0]; + nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1]; + nextP[12][7] = P[12][7] + P[12][4]*dt; + nextP[12][8] = P[12][8] + P[12][5]*dt; + nextP[12][9] = P[12][9] + P[12][6]*dt; + nextP[12][10] = P[12][10]; + nextP[12][11] = P[12][11]; + nextP[12][12] = P[12][12]; + nextP[12][13] = P[12][13]; + nextP[12][14] = P[12][14]; + nextP[12][15] = P[12][15]; + nextP[12][16] = P[12][16]; + nextP[12][17] = P[12][17]; + nextP[12][18] = P[12][18]; + nextP[12][19] = P[12][19]; + nextP[12][20] = P[12][20]; + nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3]; + nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2; + nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2; + nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2]; + nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0]; + nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1]; + nextP[13][7] = P[13][7] + P[13][4]*dt; + nextP[13][8] = P[13][8] + P[13][5]*dt; + nextP[13][9] = P[13][9] + P[13][6]*dt; + nextP[13][10] = P[13][10]; + nextP[13][11] = P[13][11]; + nextP[13][12] = P[13][12]; + nextP[13][13] = P[13][13]; + nextP[13][14] = P[13][14]; + nextP[13][15] = P[13][15]; + nextP[13][16] = P[13][16]; + nextP[13][17] = P[13][17]; + nextP[13][18] = P[13][18]; + nextP[13][19] = P[13][19]; + nextP[13][20] = P[13][20]; + nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3]; + nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2; + nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2; + nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2]; + nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0]; + nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1]; + nextP[14][7] = P[14][7] + P[14][4]*dt; + nextP[14][8] = P[14][8] + P[14][5]*dt; + nextP[14][9] = P[14][9] + P[14][6]*dt; + nextP[14][10] = P[14][10]; + nextP[14][11] = P[14][11]; + nextP[14][12] = P[14][12]; + nextP[14][13] = P[14][13]; + nextP[14][14] = P[14][14]; + nextP[14][15] = P[14][15]; + nextP[14][16] = P[14][16]; + nextP[14][17] = P[14][17]; + nextP[14][18] = P[14][18]; + nextP[14][19] = P[14][19]; + nextP[14][20] = P[14][20]; + nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3]; + nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2; + nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2; + nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2]; + nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0]; + nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1]; + nextP[15][7] = P[15][7] + P[15][4]*dt; + nextP[15][8] = P[15][8] + P[15][5]*dt; + nextP[15][9] = P[15][9] + P[15][6]*dt; + nextP[15][10] = P[15][10]; + nextP[15][11] = P[15][11]; + nextP[15][12] = P[15][12]; + nextP[15][13] = P[15][13]; + nextP[15][14] = P[15][14]; + nextP[15][15] = P[15][15]; + nextP[15][16] = P[15][16]; + nextP[15][17] = P[15][17]; + nextP[15][18] = P[15][18]; + nextP[15][19] = P[15][19]; + nextP[15][20] = P[15][20]; + nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3]; + nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2; + nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2; + nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2]; + nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0]; + nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1]; + nextP[16][7] = P[16][7] + P[16][4]*dt; + nextP[16][8] = P[16][8] + P[16][5]*dt; + nextP[16][9] = P[16][9] + P[16][6]*dt; + nextP[16][10] = P[16][10]; + nextP[16][11] = P[16][11]; + nextP[16][12] = P[16][12]; + nextP[16][13] = P[16][13]; + nextP[16][14] = P[16][14]; + nextP[16][15] = P[16][15]; + nextP[16][16] = P[16][16]; + nextP[16][17] = P[16][17]; + nextP[16][18] = P[16][18]; + nextP[16][19] = P[16][19]; + nextP[16][20] = P[16][20]; + nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3]; + nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2; + nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2; + nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2]; + nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0]; + nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1]; + nextP[17][7] = P[17][7] + P[17][4]*dt; + nextP[17][8] = P[17][8] + P[17][5]*dt; + nextP[17][9] = P[17][9] + P[17][6]*dt; + nextP[17][10] = P[17][10]; + nextP[17][11] = P[17][11]; + nextP[17][12] = P[17][12]; + nextP[17][13] = P[17][13]; + nextP[17][14] = P[17][14]; + nextP[17][15] = P[17][15]; + nextP[17][16] = P[17][16]; + nextP[17][17] = P[17][17]; + nextP[17][18] = P[17][18]; + nextP[17][19] = P[17][19]; + nextP[17][20] = P[17][20]; + nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3]; + nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2; + nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2; + nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2]; + nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0]; + nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1]; + nextP[18][7] = P[18][7] + P[18][4]*dt; + nextP[18][8] = P[18][8] + P[18][5]*dt; + nextP[18][9] = P[18][9] + P[18][6]*dt; + nextP[18][10] = P[18][10]; + nextP[18][11] = P[18][11]; + nextP[18][12] = P[18][12]; + nextP[18][13] = P[18][13]; + nextP[18][14] = P[18][14]; + nextP[18][15] = P[18][15]; + nextP[18][16] = P[18][16]; + nextP[18][17] = P[18][17]; + nextP[18][18] = P[18][18]; + nextP[18][19] = P[18][19]; + nextP[18][20] = P[18][20]; + nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3]; + nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2; + nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2; + nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2]; + nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0]; + nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1]; + nextP[19][7] = P[19][7] + P[19][4]*dt; + nextP[19][8] = P[19][8] + P[19][5]*dt; + nextP[19][9] = P[19][9] + P[19][6]*dt; + nextP[19][10] = P[19][10]; + nextP[19][11] = P[19][11]; + nextP[19][12] = P[19][12]; + nextP[19][13] = P[19][13]; + nextP[19][14] = P[19][14]; + nextP[19][15] = P[19][15]; + nextP[19][16] = P[19][16]; + nextP[19][17] = P[19][17]; + nextP[19][18] = P[19][18]; + nextP[19][19] = P[19][19]; + nextP[19][20] = P[19][20]; + nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3]; + nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2; + nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2; + nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2]; + nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0]; + nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1]; + nextP[20][7] = P[20][7] + P[20][4]*dt; + nextP[20][8] = P[20][8] + P[20][5]*dt; + nextP[20][9] = P[20][9] + P[20][6]*dt; + nextP[20][10] = P[20][10]; + nextP[20][11] = P[20][11]; + nextP[20][12] = P[20][12]; + nextP[20][13] = P[20][13]; + nextP[20][14] = P[20][14]; + nextP[20][15] = P[20][15]; + nextP[20][16] = P[20][16]; + nextP[20][17] = P[20][17]; + nextP[20][18] = P[20][18]; + nextP[20][19] = P[20][19]; + nextP[20][20] = P[20][20]; + + for (unsigned i = 0; i < n_states; i++) + { + nextP[i][i] = nextP[i][i] + processNoise[i]; + } + + // If on ground or no magnetometer fitted, inhibit magnetometer bias updates by + // setting the coresponding covariance terms to zero. + if (onGround || !useCompass) + { + zeroRows(nextP,15,20); + zeroCols(nextP,15,20); + } + + // If on ground or not using airspeed sensing, inhibit wind velocity + // covariance growth. + if (onGround || !useAirspeed) + { + zeroRows(nextP,13,14); + zeroCols(nextP,13,14); + } + + // If the total position variance exceds 1E6 (1000m), then stop covariance + // growth by setting the predicted to the previous values + // This prevent an ill conditioned matrix from occurring for long periods + // without GPS + if ((P[7][7] + P[8][8]) > 1E6f) + { + for (uint8_t i=7; i<=8; i++) + { + for (unsigned j = 0; j < n_states; j++) + { + nextP[i][j] = P[i][j]; + nextP[j][i] = P[j][i]; + } + } + } + + if (onGround || staticMode) { + // copy the portion of the variances we want to + // propagate + for (unsigned i = 0; i < 14; i++) { + P[i][i] = nextP[i][i]; + + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + if ((i > 12) || (j > 12)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + } + P[j][i] = P[i][j]; + } + } + } + + } else { + + // Copy covariance + for (unsigned i = 0; i < n_states; i++) { + P[i][i] = nextP[i][i]; + } + + // force symmetry for observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); + P[j][i] = P[i][j]; + } + } + + } + + ConstrainVariances(); +} + +void AttPosEKF::FuseVelposNED() +{ + +// declare variables used by fault isolation logic + uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure + uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available + uint32_t hgtRetryTime = 5000; // height measurement retry time + uint32_t horizRetryTime; + +// declare variables used to check measurement errors + float velInnov[3] = {0.0f,0.0f,0.0f}; + float posInnov[2] = {0.0f,0.0f}; + float hgtInnov = 0.0f; + +// declare variables used to control access to arrays + bool fuseData[6] = {false,false,false,false,false,false}; + uint8_t stateIndex; + uint8_t obsIndex; + uint8_t indexLimit; + +// declare variables used by state and covariance update calculations + float velErr; + float posErr; + float R_OBS[6]; + float observation[6]; + float SK; + float quatMag; + +// Perform sequential fusion of GPS measurements. This assumes that the +// errors in the different velocity and position components are +// uncorrelated which is not true, however in the absence of covariance +// data from the GPS receiver it is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion + if (fuseVelData || fusePosData || fuseHgtData) + { + // set the GPS data timeout depending on whether airspeed data is present + if (useAirspeed) horizRetryTime = gpsRetryTime; + else horizRetryTime = gpsRetryTimeNoTAS; + + // Form the observation vector + for (uint8_t i=0; i<=2; i++) observation[i] = velNED[i]; + for (uint8_t i=3; i<=4; i++) observation[i] = posNE[i-3]; + observation[5] = -(hgtMea); + + // Estimate the GPS Velocity, GPS horiz position and height measurement variances. + velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring + posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring + R_OBS[0] = 0.04f + sq(velErr); + R_OBS[1] = R_OBS[0]; + R_OBS[2] = 0.08f + sq(velErr); + R_OBS[3] = R_OBS[2]; + R_OBS[4] = 4.0f + sq(posErr); + R_OBS[5] = 4.0f; + + // Set innovation variances to zero default + for (uint8_t i = 0; i<=5; i++) + { + varInnovVelPos[i] = 0.0f; + } + // calculate innovations and check GPS data validity using an innovation consistency check + if (fuseVelData) + { + // test velocity measurements + uint8_t imax = 2; + if (fusionModeGPS == 1) imax = 1; + for (uint8_t i = 0; i<=imax; i++) + { + velInnov[i] = statesAtVelTime[i+4] - velNED[i]; + stateIndex = 4 + i; + varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS[i]; + } + // apply a 5-sigma threshold + current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]); + current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime; + if (current_ekf_state.velHealth || current_ekf_state.velTimeout) + { + current_ekf_state.velHealth = true; + current_ekf_state.velFailTime = millis(); + } + else + { + current_ekf_state.velHealth = false; + } + } + if (fusePosData) + { + // test horizontal position measurements + posInnov[0] = statesAtPosTime[7] - posNE[0]; + posInnov[1] = statesAtPosTime[8] - posNE[1]; + varInnovVelPos[3] = P[7][7] + R_OBS[3]; + varInnovVelPos[4] = P[8][8] + R_OBS[4]; + // apply a 10-sigma threshold + current_ekf_state.posHealth = (sq(posInnov[0]) + sq(posInnov[1])) < 100.0f*(varInnovVelPos[3] + varInnovVelPos[4]); + current_ekf_state.posTimeout = (millis() - current_ekf_state.posFailTime) > horizRetryTime; + if (current_ekf_state.posHealth || current_ekf_state.posTimeout) + { + current_ekf_state.posHealth = true; + current_ekf_state.posFailTime = millis(); + } + else + { + current_ekf_state.posHealth = false; + } + } + // test height measurements + if (fuseHgtData) + { + hgtInnov = statesAtHgtTime[9] + hgtMea; + varInnovVelPos[5] = P[9][9] + R_OBS[5]; + // apply a 10-sigma threshold + current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5]; + current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime; + if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout) + { + current_ekf_state.hgtHealth = true; + current_ekf_state.hgtFailTime = millis(); + } + else + { + current_ekf_state.hgtHealth = false; + } + } + // Set range for sequential fusion of velocity and position measurements depending + // on which data is available and its health + if (fuseVelData && fusionModeGPS == 0 && current_ekf_state.velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + fuseData[2] = true; + } + if (fuseVelData && fusionModeGPS == 1 && current_ekf_state.velHealth) + { + fuseData[0] = true; + fuseData[1] = true; + } + if (fusePosData && fusionModeGPS <= 2 && current_ekf_state.posHealth) + { + fuseData[3] = true; + fuseData[4] = true; + } + if (fuseHgtData && current_ekf_state.hgtHealth) + { + fuseData[5] = true; + } + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 20; + } + else + { + indexLimit = 12; + } + // Fuse measurements sequentially + for (obsIndex=0; obsIndex<=5; obsIndex++) + { + if (fuseData[obsIndex]) + { + stateIndex = 4 + obsIndex; + // Calculate the measurement innovation, using states from a + // different time coordinate if fusing height data + if (obsIndex >= 0 && obsIndex <= 2) + { + innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 3 || obsIndex == 4) + { + innovVelPos[obsIndex] = statesAtPosTime[stateIndex] - observation[obsIndex]; + } + else if (obsIndex == 5) + { + innovVelPos[obsIndex] = statesAtHgtTime[stateIndex] - observation[obsIndex]; + } + // Calculate the Kalman Gain + // Calculate innovation variances - also used for data logging + varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; + SK = 1.0/varInnovVelPos[obsIndex]; + for (uint8_t i= 0; i<=indexLimit; i++) + { + Kfusion[i] = P[i][stateIndex]*SK; + } + // Calculate state corrections and re-normalise the quaternions + for (uint8_t i = 0; i<=indexLimit; i++) + { + states[i] = states[i] - Kfusion[i] * innovVelPos[obsIndex]; + } + quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) // divide by 0 protection + { + for (uint8_t i = 0; i<=3; i++) + { + states[i] = states[i] / quatMag; + } + } + // Update the covariance - take advantage of direct observation of a + // single state at index = stateIndex to reduce computations + // Optimised implementation of standard equation P = (I - K*H)*P; + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + KHP[i][j] = Kfusion[i] * P[stateIndex][j]; + } + } + for (uint8_t i= 0; i<=indexLimit; i++) + { + for (uint8_t j= 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + } + + ForceSymmetry(); + ConstrainVariances(); + + //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); +} + +void AttPosEKF::FuseMagnetometer() +{ + uint8_t obsIndex; + uint8_t indexLimit; + float DCM[3][3] = + { + {1.0f,0.0f,0.0f} , + {0.0f,1.0f,0.0f} , + {0.0f,0.0f,1.0f} + }; + float MagPred[3] = {0.0f,0.0f,0.0f}; + float SK_MX[6]; + float SK_MY[5]; + float SK_MZ[6]; + float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; + +// Perform sequential fusion of Magnetometer measurements. +// This assumes that the errors in the different components are +// uncorrelated which is not true, however in the absence of covariance +// data fit is the only assumption we can make +// so we might as well take advantage of the computational efficiencies +// associated with sequential fusion + if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + { + // Limit range of states modified when on ground + if(!onGround) + { + indexLimit = 20; + } + else + { + indexLimit = 12; + } + + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float magN = 0.4f; + static float magE = 0.0f; + static float magD = 0.3f; + + static float R_MAG = 0.0025f; + + float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; + + // Sequential fusion of XYZ components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseMagData) + { + static float magXbias = 0.0f; + static float magYbias = 0.0f; + static float magZbias = 0.0f; + + // Copy required states to local variable names + q0 = statesAtMagMeasTime[0]; + q1 = statesAtMagMeasTime[1]; + q2 = statesAtMagMeasTime[2]; + q3 = statesAtMagMeasTime[3]; + magN = statesAtMagMeasTime[15]; + magE = statesAtMagMeasTime[16]; + magD = statesAtMagMeasTime[17]; + magXbias = statesAtMagMeasTime[18]; + magYbias = statesAtMagMeasTime[19]; + magZbias = statesAtMagMeasTime[20]; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM[0][1] = 2*(q1*q2 + q0*q3); + DCM[0][2] = 2*(q1*q3-q0*q2); + DCM[1][0] = 2*(q1*q2 - q0*q3); + DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM[1][2] = 2*(q2*q3 + q0*q1); + DCM[2][0] = 2*(q1*q3 + q0*q2); + DCM[2][1] = 2*(q2*q3 - q0*q1); + DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; + MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; + MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; + + // scale magnetometer observation error with total angular rate + R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; + SH_MAG[1] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SH_MAG[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SH_MAG[3] = sq(q3); + SH_MAG[4] = sq(q2); + SH_MAG[5] = sq(q1); + SH_MAG[6] = sq(q0); + SH_MAG[7] = 2*magN*q0; + SH_MAG[8] = 2*magE*q3; + + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[1] = SH_MAG[0]; + H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; + H_MAG[3] = SH_MAG[2]; + H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[16] = 2*q0*q3 + 2*q1*q2; + H_MAG[17] = 2*q1*q3 - 2*q0*q2; + H_MAG[18] = 1.0f; + + // Calculate Kalman gain + SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; + SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; + SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MX[4] = 2*q0*q2 - 2*q1*q3; + SK_MX[5] = 2*q0*q3 + 2*q1*q2; + Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]); + Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); + varInnovMag[0] = 1.0f/SK_MX[0]; + innovMag[0] = MagPred[0] - magData.x; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (unsigned int i=0; i<n_states; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[2]; + H_MAG[1] = SH_MAG[1]; + H_MAG[2] = SH_MAG[0]; + H_MAG[3] = 2*magD*q2 - SH_MAG[8] - SH_MAG[7]; + H_MAG[15] = 2*q1*q2 - 2*q0*q3; + H_MAG[16] = SH_MAG[4] - SH_MAG[3] - SH_MAG[5] + SH_MAG[6]; + H_MAG[17] = 2*q0*q1 + 2*q2*q3; + H_MAG[19] = 1; + + // Calculate Kalman gain + SK_MY[0] = 1/(P[19][19] + R_MAG + P[0][19]*SH_MAG[2] + P[1][19]*SH_MAG[1] + P[2][19]*SH_MAG[0] - P[16][19]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[19][15] + P[0][15]*SH_MAG[2] + P[1][15]*SH_MAG[1] + P[2][15]*SH_MAG[0] - P[16][15]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][15]*(2*q0*q3 - 2*q1*q2) + P[17][15]*(2*q0*q1 + 2*q2*q3) - P[3][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[19][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[16][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][17]*(2*q0*q3 - 2*q1*q2) + P[17][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[16][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][3]*(2*q0*q3 - 2*q1*q2) + P[17][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[15][19]*(2*q0*q3 - 2*q1*q2) + P[17][19]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[19][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[16][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][0]*(2*q0*q3 - 2*q1*q2) + P[17][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[19][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[16][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][1]*(2*q0*q3 - 2*q1*q2) + P[17][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[16][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][2]*(2*q0*q3 - 2*q1*q2) + P[17][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[16][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[15][16]*(2*q0*q3 - 2*q1*q2) + P[17][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]; + SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MY[3] = 2*q0*q3 - 2*q1*q2; + SK_MY[4] = 2*q0*q1 + 2*q2*q3; + Kfusion[0] = SK_MY[0]*(P[0][19] + P[0][0]*SH_MAG[2] + P[0][1]*SH_MAG[1] + P[0][2]*SH_MAG[0] - P[0][3]*SK_MY[2] - P[0][16]*SK_MY[1] - P[0][15]*SK_MY[3] + P[0][17]*SK_MY[4]); + Kfusion[1] = SK_MY[0]*(P[1][19] + P[1][0]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[1][2]*SH_MAG[0] - P[1][3]*SK_MY[2] - P[1][16]*SK_MY[1] - P[1][15]*SK_MY[3] + P[1][17]*SK_MY[4]); + Kfusion[2] = SK_MY[0]*(P[2][19] + P[2][0]*SH_MAG[2] + P[2][1]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[2][3]*SK_MY[2] - P[2][16]*SK_MY[1] - P[2][15]*SK_MY[3] + P[2][17]*SK_MY[4]); + Kfusion[3] = SK_MY[0]*(P[3][19] + P[3][0]*SH_MAG[2] + P[3][1]*SH_MAG[1] + P[3][2]*SH_MAG[0] - P[3][3]*SK_MY[2] - P[3][16]*SK_MY[1] - P[3][15]*SK_MY[3] + P[3][17]*SK_MY[4]); + Kfusion[4] = SK_MY[0]*(P[4][19] + P[4][0]*SH_MAG[2] + P[4][1]*SH_MAG[1] + P[4][2]*SH_MAG[0] - P[4][3]*SK_MY[2] - P[4][16]*SK_MY[1] - P[4][15]*SK_MY[3] + P[4][17]*SK_MY[4]); + Kfusion[5] = SK_MY[0]*(P[5][19] + P[5][0]*SH_MAG[2] + P[5][1]*SH_MAG[1] + P[5][2]*SH_MAG[0] - P[5][3]*SK_MY[2] - P[5][16]*SK_MY[1] - P[5][15]*SK_MY[3] + P[5][17]*SK_MY[4]); + Kfusion[6] = SK_MY[0]*(P[6][19] + P[6][0]*SH_MAG[2] + P[6][1]*SH_MAG[1] + P[6][2]*SH_MAG[0] - P[6][3]*SK_MY[2] - P[6][16]*SK_MY[1] - P[6][15]*SK_MY[3] + P[6][17]*SK_MY[4]); + Kfusion[7] = SK_MY[0]*(P[7][19] + P[7][0]*SH_MAG[2] + P[7][1]*SH_MAG[1] + P[7][2]*SH_MAG[0] - P[7][3]*SK_MY[2] - P[7][16]*SK_MY[1] - P[7][15]*SK_MY[3] + P[7][17]*SK_MY[4]); + Kfusion[8] = SK_MY[0]*(P[8][19] + P[8][0]*SH_MAG[2] + P[8][1]*SH_MAG[1] + P[8][2]*SH_MAG[0] - P[8][3]*SK_MY[2] - P[8][16]*SK_MY[1] - P[8][15]*SK_MY[3] + P[8][17]*SK_MY[4]); + Kfusion[9] = SK_MY[0]*(P[9][19] + P[9][0]*SH_MAG[2] + P[9][1]*SH_MAG[1] + P[9][2]*SH_MAG[0] - P[9][3]*SK_MY[2] - P[9][16]*SK_MY[1] - P[9][15]*SK_MY[3] + P[9][17]*SK_MY[4]); + Kfusion[10] = SK_MY[0]*(P[10][19] + P[10][0]*SH_MAG[2] + P[10][1]*SH_MAG[1] + P[10][2]*SH_MAG[0] - P[10][3]*SK_MY[2] - P[10][16]*SK_MY[1] - P[10][15]*SK_MY[3] + P[10][17]*SK_MY[4]); + Kfusion[11] = SK_MY[0]*(P[11][19] + P[11][0]*SH_MAG[2] + P[11][1]*SH_MAG[1] + P[11][2]*SH_MAG[0] - P[11][3]*SK_MY[2] - P[11][16]*SK_MY[1] - P[11][15]*SK_MY[3] + P[11][17]*SK_MY[4]); + Kfusion[12] = SK_MY[0]*(P[12][19] + P[12][0]*SH_MAG[2] + P[12][1]*SH_MAG[1] + P[12][2]*SH_MAG[0] - P[12][3]*SK_MY[2] - P[12][16]*SK_MY[1] - P[12][15]*SK_MY[3] + P[12][17]*SK_MY[4]); + Kfusion[13] = SK_MY[0]*(P[13][19] + P[13][0]*SH_MAG[2] + P[13][1]*SH_MAG[1] + P[13][2]*SH_MAG[0] - P[13][3]*SK_MY[2] - P[13][16]*SK_MY[1] - P[13][15]*SK_MY[3] + P[13][17]*SK_MY[4]); + Kfusion[14] = SK_MY[0]*(P[14][19] + P[14][0]*SH_MAG[2] + P[14][1]*SH_MAG[1] + P[14][2]*SH_MAG[0] - P[14][3]*SK_MY[2] - P[14][16]*SK_MY[1] - P[14][15]*SK_MY[3] + P[14][17]*SK_MY[4]); + Kfusion[15] = SK_MY[0]*(P[15][19] + P[15][0]*SH_MAG[2] + P[15][1]*SH_MAG[1] + P[15][2]*SH_MAG[0] - P[15][3]*SK_MY[2] - P[15][16]*SK_MY[1] - P[15][15]*SK_MY[3] + P[15][17]*SK_MY[4]); + Kfusion[16] = SK_MY[0]*(P[16][19] + P[16][0]*SH_MAG[2] + P[16][1]*SH_MAG[1] + P[16][2]*SH_MAG[0] - P[16][3]*SK_MY[2] - P[16][16]*SK_MY[1] - P[16][15]*SK_MY[3] + P[16][17]*SK_MY[4]); + Kfusion[17] = SK_MY[0]*(P[17][19] + P[17][0]*SH_MAG[2] + P[17][1]*SH_MAG[1] + P[17][2]*SH_MAG[0] - P[17][3]*SK_MY[2] - P[17][16]*SK_MY[1] - P[17][15]*SK_MY[3] + P[17][17]*SK_MY[4]); + Kfusion[18] = SK_MY[0]*(P[18][19] + P[18][0]*SH_MAG[2] + P[18][1]*SH_MAG[1] + P[18][2]*SH_MAG[0] - P[18][3]*SK_MY[2] - P[18][16]*SK_MY[1] - P[18][15]*SK_MY[3] + P[18][17]*SK_MY[4]); + Kfusion[19] = SK_MY[0]*(P[19][19] + P[19][0]*SH_MAG[2] + P[19][1]*SH_MAG[1] + P[19][2]*SH_MAG[0] - P[19][3]*SK_MY[2] - P[19][16]*SK_MY[1] - P[19][15]*SK_MY[3] + P[19][17]*SK_MY[4]); + Kfusion[20] = SK_MY[0]*(P[20][19] + P[20][0]*SH_MAG[2] + P[20][1]*SH_MAG[1] + P[20][2]*SH_MAG[0] - P[20][3]*SK_MY[2] - P[20][16]*SK_MY[1] - P[20][15]*SK_MY[3] + P[20][17]*SK_MY[4]); + varInnovMag[1] = 1.0f/SK_MY[0]; + innovMag[1] = MagPred[1] - magData.y; + } + else if (obsIndex == 2) // we are now fusing the Z measurement + { + // Calculate observation jacobians + for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + H_MAG[0] = SH_MAG[1]; + H_MAG[1] = 2*magN*q3 - 2*magE*q0 - 2*magD*q1; + H_MAG[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + H_MAG[3] = SH_MAG[0]; + H_MAG[15] = 2*q0*q2 + 2*q1*q3; + H_MAG[16] = 2*q2*q3 - 2*q0*q1; + H_MAG[17] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + H_MAG[20] = 1; + + // Calculate Kalman gain + SK_MZ[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[1] + P[3][20]*SH_MAG[0] + P[17][20]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[20][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[17][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][1]*(2*q0*q2 + 2*q1*q3) - P[16][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[17][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][2]*(2*q0*q2 + 2*q1*q3) - P[16][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[17][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][0]*(2*q0*q2 + 2*q1*q3) - P[16][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[17][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][3]*(2*q0*q2 + 2*q1*q3) - P[16][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[17][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][17]*(2*q0*q2 + 2*q1*q3) - P[16][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[15][20]*(2*q0*q2 + 2*q1*q3) - P[16][20]*(2*q0*q1 - 2*q2*q3) - P[1][20]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[20][15] + P[0][15]*SH_MAG[1] + P[3][15]*SH_MAG[0] + P[17][15]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][15]*(2*q0*q2 + 2*q1*q3) - P[16][15]*(2*q0*q1 - 2*q2*q3) - P[1][15]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[20][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[17][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[15][16]*(2*q0*q2 + 2*q1*q3) - P[16][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]; + SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3; + SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; + SK_MZ[4] = 2*q0*q1 - 2*q2*q3; + SK_MZ[5] = 2*q0*q2 + 2*q1*q3; + Kfusion[0] = SK_MZ[0]*(P[0][20] + P[0][0]*SH_MAG[1] + P[0][3]*SH_MAG[0] - P[0][1]*SK_MZ[2] + P[0][2]*SK_MZ[3] + P[0][17]*SK_MZ[1] + P[0][15]*SK_MZ[5] - P[0][16]*SK_MZ[4]); + Kfusion[1] = SK_MZ[0]*(P[1][20] + P[1][0]*SH_MAG[1] + P[1][3]*SH_MAG[0] - P[1][1]*SK_MZ[2] + P[1][2]*SK_MZ[3] + P[1][17]*SK_MZ[1] + P[1][15]*SK_MZ[5] - P[1][16]*SK_MZ[4]); + Kfusion[2] = SK_MZ[0]*(P[2][20] + P[2][0]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[2][1]*SK_MZ[2] + P[2][2]*SK_MZ[3] + P[2][17]*SK_MZ[1] + P[2][15]*SK_MZ[5] - P[2][16]*SK_MZ[4]); + Kfusion[3] = SK_MZ[0]*(P[3][20] + P[3][0]*SH_MAG[1] + P[3][3]*SH_MAG[0] - P[3][1]*SK_MZ[2] + P[3][2]*SK_MZ[3] + P[3][17]*SK_MZ[1] + P[3][15]*SK_MZ[5] - P[3][16]*SK_MZ[4]); + Kfusion[4] = SK_MZ[0]*(P[4][20] + P[4][0]*SH_MAG[1] + P[4][3]*SH_MAG[0] - P[4][1]*SK_MZ[2] + P[4][2]*SK_MZ[3] + P[4][17]*SK_MZ[1] + P[4][15]*SK_MZ[5] - P[4][16]*SK_MZ[4]); + Kfusion[5] = SK_MZ[0]*(P[5][20] + P[5][0]*SH_MAG[1] + P[5][3]*SH_MAG[0] - P[5][1]*SK_MZ[2] + P[5][2]*SK_MZ[3] + P[5][17]*SK_MZ[1] + P[5][15]*SK_MZ[5] - P[5][16]*SK_MZ[4]); + Kfusion[6] = SK_MZ[0]*(P[6][20] + P[6][0]*SH_MAG[1] + P[6][3]*SH_MAG[0] - P[6][1]*SK_MZ[2] + P[6][2]*SK_MZ[3] + P[6][17]*SK_MZ[1] + P[6][15]*SK_MZ[5] - P[6][16]*SK_MZ[4]); + Kfusion[7] = SK_MZ[0]*(P[7][20] + P[7][0]*SH_MAG[1] + P[7][3]*SH_MAG[0] - P[7][1]*SK_MZ[2] + P[7][2]*SK_MZ[3] + P[7][17]*SK_MZ[1] + P[7][15]*SK_MZ[5] - P[7][16]*SK_MZ[4]); + Kfusion[8] = SK_MZ[0]*(P[8][20] + P[8][0]*SH_MAG[1] + P[8][3]*SH_MAG[0] - P[8][1]*SK_MZ[2] + P[8][2]*SK_MZ[3] + P[8][17]*SK_MZ[1] + P[8][15]*SK_MZ[5] - P[8][16]*SK_MZ[4]); + Kfusion[9] = SK_MZ[0]*(P[9][20] + P[9][0]*SH_MAG[1] + P[9][3]*SH_MAG[0] - P[9][1]*SK_MZ[2] + P[9][2]*SK_MZ[3] + P[9][17]*SK_MZ[1] + P[9][15]*SK_MZ[5] - P[9][16]*SK_MZ[4]); + Kfusion[10] = SK_MZ[0]*(P[10][20] + P[10][0]*SH_MAG[1] + P[10][3]*SH_MAG[0] - P[10][1]*SK_MZ[2] + P[10][2]*SK_MZ[3] + P[10][17]*SK_MZ[1] + P[10][15]*SK_MZ[5] - P[10][16]*SK_MZ[4]); + Kfusion[11] = SK_MZ[0]*(P[11][20] + P[11][0]*SH_MAG[1] + P[11][3]*SH_MAG[0] - P[11][1]*SK_MZ[2] + P[11][2]*SK_MZ[3] + P[11][17]*SK_MZ[1] + P[11][15]*SK_MZ[5] - P[11][16]*SK_MZ[4]); + Kfusion[12] = SK_MZ[0]*(P[12][20] + P[12][0]*SH_MAG[1] + P[12][3]*SH_MAG[0] - P[12][1]*SK_MZ[2] + P[12][2]*SK_MZ[3] + P[12][17]*SK_MZ[1] + P[12][15]*SK_MZ[5] - P[12][16]*SK_MZ[4]); + Kfusion[13] = SK_MZ[0]*(P[13][20] + P[13][0]*SH_MAG[1] + P[13][3]*SH_MAG[0] - P[13][1]*SK_MZ[2] + P[13][2]*SK_MZ[3] + P[13][17]*SK_MZ[1] + P[13][15]*SK_MZ[5] - P[13][16]*SK_MZ[4]); + Kfusion[14] = SK_MZ[0]*(P[14][20] + P[14][0]*SH_MAG[1] + P[14][3]*SH_MAG[0] - P[14][1]*SK_MZ[2] + P[14][2]*SK_MZ[3] + P[14][17]*SK_MZ[1] + P[14][15]*SK_MZ[5] - P[14][16]*SK_MZ[4]); + Kfusion[15] = SK_MZ[0]*(P[15][20] + P[15][0]*SH_MAG[1] + P[15][3]*SH_MAG[0] - P[15][1]*SK_MZ[2] + P[15][2]*SK_MZ[3] + P[15][17]*SK_MZ[1] + P[15][15]*SK_MZ[5] - P[15][16]*SK_MZ[4]); + Kfusion[16] = SK_MZ[0]*(P[16][20] + P[16][0]*SH_MAG[1] + P[16][3]*SH_MAG[0] - P[16][1]*SK_MZ[2] + P[16][2]*SK_MZ[3] + P[16][17]*SK_MZ[1] + P[16][15]*SK_MZ[5] - P[16][16]*SK_MZ[4]); + Kfusion[17] = SK_MZ[0]*(P[17][20] + P[17][0]*SH_MAG[1] + P[17][3]*SH_MAG[0] - P[17][1]*SK_MZ[2] + P[17][2]*SK_MZ[3] + P[17][17]*SK_MZ[1] + P[17][15]*SK_MZ[5] - P[17][16]*SK_MZ[4]); + Kfusion[18] = SK_MZ[0]*(P[18][20] + P[18][0]*SH_MAG[1] + P[18][3]*SH_MAG[0] - P[18][1]*SK_MZ[2] + P[18][2]*SK_MZ[3] + P[18][17]*SK_MZ[1] + P[18][15]*SK_MZ[5] - P[18][16]*SK_MZ[4]); + Kfusion[19] = SK_MZ[0]*(P[19][20] + P[19][0]*SH_MAG[1] + P[19][3]*SH_MAG[0] - P[19][1]*SK_MZ[2] + P[19][2]*SK_MZ[3] + P[19][17]*SK_MZ[1] + P[19][15]*SK_MZ[5] - P[19][16]*SK_MZ[4]); + Kfusion[20] = SK_MZ[0]*(P[20][20] + P[20][0]*SH_MAG[1] + P[20][3]*SH_MAG[0] - P[20][1]*SK_MZ[2] + P[20][2]*SK_MZ[3] + P[20][17]*SK_MZ[1] + P[20][15]*SK_MZ[5] - P[20][16]*SK_MZ[4]); + varInnovMag[2] = 1.0f/SK_MZ[0]; + innovMag[2] = MagPred[2] - magData.z; + + } + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j= 0; j<=indexLimit; j++) + { + states[j] = states[j] - Kfusion[j] * innovMag[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=3; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + for (uint8_t j = 4; j<=17; j++) KH[i][j] = 0.0f; + if (!onGround) + { + for (uint8_t j = 15; j<=20; j++) + { + KH[i][j] = Kfusion[i] * H_MAG[j]; + } + } + else + { + for (uint8_t j = 15; j<=20; j++) + { + KH[i][j] = 0.0f; + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k<=3; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + if (!onGround) + { + for (uint8_t k = 15; k<=20; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + } + } + for (uint8_t i = 0; i<=indexLimit; i++) + { + for (uint8_t j = 0; j<=indexLimit; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); +} + +void AttPosEKF::FuseAirspeed() +{ + float vn; + float ve; + float vd; + float vwn; + float vwe; + const float R_TAS = 2.0f; + float SH_TAS[3]; + float Kfusion[21]; + float VtasPred; + + // Copy required states to local variable names + vn = statesAtVtasMeasTime[4]; + ve = statesAtVtasMeasTime[5]; + vd = statesAtVtasMeasTime[6]; + vwn = statesAtVtasMeasTime[13]; + vwe = statesAtVtasMeasTime[14]; + + // Need to check that it is flying before fusing airspeed data + // Calculate the predicted airspeed + VtasPred = sqrtf((ve - vwe)*(ve - vwe) + (vn - vwn)*(vn - vwn) + vd*vd); + // Perform fusion of True Airspeed measurement + if (useAirspeed && fuseVtasData && (VtasPred > 1.0f) && (VtasMeas > 8.0f)) + { + // Calculate observation jacobians + SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); + SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; + SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; + + float H_TAS[21]; + for (uint8_t i=0; i<=20; i++) H_TAS[i] = 0.0f; + H_TAS[4] = SH_TAS[2]; + H_TAS[5] = SH_TAS[1]; + H_TAS[6] = vd*SH_TAS[0]; + H_TAS[13] = -SH_TAS[2]; + H_TAS[14] = -SH_TAS[1]; + + // Calculate Kalman gains + float SK_TAS = 1.0f/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[13][4]*SH_TAS[2] - P[14][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[13][5]*SH_TAS[2] - P[14][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][13]*SH_TAS[2] + P[5][13]*SH_TAS[1] - P[13][13]*SH_TAS[2] - P[14][13]*SH_TAS[1] + P[6][13]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[13][14]*SH_TAS[2] - P[14][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[13][6]*SH_TAS[2] - P[14][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0])); + Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][13]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][14]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]); + Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][13]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][14]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]); + Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][13]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][14]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]); + Kfusion[3] = SK_TAS*(P[3][4]*SH_TAS[2] - P[3][13]*SH_TAS[2] + P[3][5]*SH_TAS[1] - P[3][14]*SH_TAS[1] + P[3][6]*vd*SH_TAS[0]); + Kfusion[4] = SK_TAS*(P[4][4]*SH_TAS[2] - P[4][13]*SH_TAS[2] + P[4][5]*SH_TAS[1] - P[4][14]*SH_TAS[1] + P[4][6]*vd*SH_TAS[0]); + Kfusion[5] = SK_TAS*(P[5][4]*SH_TAS[2] - P[5][13]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[5][14]*SH_TAS[1] + P[5][6]*vd*SH_TAS[0]); + Kfusion[6] = SK_TAS*(P[6][4]*SH_TAS[2] - P[6][13]*SH_TAS[2] + P[6][5]*SH_TAS[1] - P[6][14]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]); + Kfusion[7] = SK_TAS*(P[7][4]*SH_TAS[2] - P[7][13]*SH_TAS[2] + P[7][5]*SH_TAS[1] - P[7][14]*SH_TAS[1] + P[7][6]*vd*SH_TAS[0]); + Kfusion[8] = SK_TAS*(P[8][4]*SH_TAS[2] - P[8][13]*SH_TAS[2] + P[8][5]*SH_TAS[1] - P[8][14]*SH_TAS[1] + P[8][6]*vd*SH_TAS[0]); + Kfusion[9] = SK_TAS*(P[9][4]*SH_TAS[2] - P[9][13]*SH_TAS[2] + P[9][5]*SH_TAS[1] - P[9][14]*SH_TAS[1] + P[9][6]*vd*SH_TAS[0]); + Kfusion[10] = SK_TAS*(P[10][4]*SH_TAS[2] - P[10][13]*SH_TAS[2] + P[10][5]*SH_TAS[1] - P[10][14]*SH_TAS[1] + P[10][6]*vd*SH_TAS[0]); + Kfusion[11] = SK_TAS*(P[11][4]*SH_TAS[2] - P[11][13]*SH_TAS[2] + P[11][5]*SH_TAS[1] - P[11][14]*SH_TAS[1] + P[11][6]*vd*SH_TAS[0]); + Kfusion[12] = SK_TAS*(P[12][4]*SH_TAS[2] - P[12][13]*SH_TAS[2] + P[12][5]*SH_TAS[1] - P[12][14]*SH_TAS[1] + P[12][6]*vd*SH_TAS[0]); + Kfusion[13] = SK_TAS*(P[13][4]*SH_TAS[2] - P[13][13]*SH_TAS[2] + P[13][5]*SH_TAS[1] - P[13][14]*SH_TAS[1] + P[13][6]*vd*SH_TAS[0]); + Kfusion[14] = SK_TAS*(P[14][4]*SH_TAS[2] - P[14][13]*SH_TAS[2] + P[14][5]*SH_TAS[1] - P[14][14]*SH_TAS[1] + P[14][6]*vd*SH_TAS[0]); + Kfusion[15] = SK_TAS*(P[15][4]*SH_TAS[2] - P[15][13]*SH_TAS[2] + P[15][5]*SH_TAS[1] - P[15][14]*SH_TAS[1] + P[15][6]*vd*SH_TAS[0]); + Kfusion[16] = SK_TAS*(P[16][4]*SH_TAS[2] - P[16][13]*SH_TAS[2] + P[16][5]*SH_TAS[1] - P[16][14]*SH_TAS[1] + P[16][6]*vd*SH_TAS[0]); + Kfusion[17] = SK_TAS*(P[17][4]*SH_TAS[2] - P[17][13]*SH_TAS[2] + P[17][5]*SH_TAS[1] - P[17][14]*SH_TAS[1] + P[17][6]*vd*SH_TAS[0]); + Kfusion[18] = SK_TAS*(P[18][4]*SH_TAS[2] - P[18][13]*SH_TAS[2] + P[18][5]*SH_TAS[1] - P[18][14]*SH_TAS[1] + P[18][6]*vd*SH_TAS[0]); + Kfusion[19] = SK_TAS*(P[19][4]*SH_TAS[2] - P[19][13]*SH_TAS[2] + P[19][5]*SH_TAS[1] - P[19][14]*SH_TAS[1] + P[19][6]*vd*SH_TAS[0]); + Kfusion[20] = SK_TAS*(P[20][4]*SH_TAS[2] - P[20][13]*SH_TAS[2] + P[20][5]*SH_TAS[1] - P[20][14]*SH_TAS[1] + P[20][6]*vd*SH_TAS[0]); + varInnovVtas = 1.0f/SK_TAS; + + // Calculate the measurement innovation + innovVtas = VtasPred - VtasMeas; + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovVtas*innovVtas*SK_TAS) < 25.0) + { + // correct the state vector + for (uint8_t j=0; j<=20; j++) + { + states[j] = states[j] - Kfusion[j] * innovVtas; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12f) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in H to reduce the + // number of operations + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; + for (uint8_t j = 4; j<=6; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0; + for (uint8_t j = 13; j<=14; j++) + { + KH[i][j] = Kfusion[i] * H_TAS[j]; + } + for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; + } + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=20; j++) + { + KHP[i][j] = 0.0; + for (uint8_t k = 4; k<=6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + for (uint8_t k = 13; k<=14; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + } + } + for (uint8_t i = 0; i<=20; i++) + { + for (uint8_t j = 0; j<=20; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + } + + ForceSymmetry(); + ConstrainVariances(); +} + +void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (row=first; row<=last; row++) + { + for (col=0; col<n_states; col++) + { + covMat[row][col] = 0.0; + } + } +} + +void AttPosEKF::zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (col=first; col<=last; col++) + { + for (row=0; row < n_states; row++) + { + covMat[row][col] = 0.0; + } + } +} + +float AttPosEKF::sq(float valIn) +{ + return valIn*valIn; +} + +// Store states in a history array along with time stamp +void AttPosEKF::StoreStates(uint64_t timestamp_ms) +{ + for (unsigned i=0; i<n_states; i++) + storedStates[i][storeIndex] = states[i]; + statetimeStamp[storeIndex] = timestamp_ms; + storeIndex++; + if (storeIndex == data_buffer_size) + storeIndex = 0; +} + +void AttPosEKF::ResetStoredStates() +{ + // reset all stored states + memset(&storedStates[0][0], 0, sizeof(storedStates)); + memset(&statetimeStamp[0], 0, sizeof(statetimeStamp)); + + // reset store index to first + storeIndex = 0; + + // overwrite all existing states + for (unsigned i = 0; i < n_states; i++) { + storedStates[i][storeIndex] = states[i]; + } + + statetimeStamp[storeIndex] = millis(); + + // increment to next storage index + storeIndex++; +} + +// Output the state vector stored at the time that best matches that specified by msec +int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec) +{ + int ret = 0; + + // int64_t bestTimeDelta = 200; + // unsigned bestStoreIndex = 0; + // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + // { + // // The time delta can also end up as negative number, + // // since we might compare future to past or past to future + // // therefore cast to int64. + // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; + // if (timeDelta < 0) { + // timeDelta = -timeDelta; + // } + + // if (timeDelta < bestTimeDelta) + // { + // bestStoreIndex = storeIndex; + // bestTimeDelta = timeDelta; + // } + // } + // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + // { + // for (uint8_t i=0; i < n_states; i++) { + // if (isfinite(storedStates[i][bestStoreIndex])) { + // statesForFusion[i] = storedStates[i][bestStoreIndex]; + // } else if (isfinite(states[i])) { + // statesForFusion[i] = states[i]; + // } else { + // // There is not much we can do here, except reporting the error we just + // // found. + // ret++; + // } + // } + // else // otherwise output current state + // { + for (uint8_t i=0; i < n_states; i++) { + if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + ret++; + } + } + // } + + return ret; +} + +void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) +{ + // Calculate the nav to body cosine matrix + float q00 = sq(quat[0]); + float q11 = sq(quat[1]); + float q22 = sq(quat[2]); + float q33 = sq(quat[3]); + float q01 = quat[0]*quat[1]; + float q02 = quat[0]*quat[2]; + float q03 = quat[0]*quat[3]; + float q12 = quat[1]*quat[2]; + float q13 = quat[1]*quat[3]; + float q23 = quat[2]*quat[3]; + + Tnb.x.x = q00 + q11 - q22 - q33; + Tnb.y.y = q00 - q11 + q22 - q33; + Tnb.z.z = q00 - q11 - q22 + q33; + Tnb.y.x = 2*(q12 - q03); + Tnb.z.x = 2*(q13 + q02); + Tnb.x.y = 2*(q12 + q03); + Tnb.z.y = 2*(q23 - q01); + Tnb.x.z = 2*(q13 - q02); + Tnb.y.z = 2*(q23 + q01); +} + +void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) +{ + // Calculate the body to nav cosine matrix + float q00 = sq(quat[0]); + float q11 = sq(quat[1]); + float q22 = sq(quat[2]); + float q33 = sq(quat[3]); + float q01 = quat[0]*quat[1]; + float q02 = quat[0]*quat[2]; + float q03 = quat[0]*quat[3]; + float q12 = quat[1]*quat[2]; + float q13 = quat[1]*quat[3]; + float q23 = quat[2]*quat[3]; + + Tbn.x.x = q00 + q11 - q22 - q33; + Tbn.y.y = q00 - q11 + q22 - q33; + Tbn.z.z = q00 - q11 - q22 + q33; + Tbn.x.y = 2*(q12 - q03); + Tbn.x.z = 2*(q13 + q02); + Tbn.y.x = 2*(q12 + q03); + Tbn.y.z = 2*(q23 - q01); + Tbn.z.x = 2*(q13 - q02); + Tbn.z.y = 2*(q23 + q01); +} + +void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) +{ + float u1 = cos(0.5f*eul[0]); + float u2 = cos(0.5f*eul[1]); + float u3 = cos(0.5f*eul[2]); + float u4 = sin(0.5f*eul[0]); + float u5 = sin(0.5f*eul[1]); + float u6 = sin(0.5f*eul[2]); + quat[0] = u1*u2*u3+u4*u5*u6; + quat[1] = u4*u2*u3-u1*u5*u6; + quat[2] = u1*u5*u3+u4*u2*u6; + quat[3] = u1*u2*u6-u4*u5*u3; +} + +void AttPosEKF::quat2eul(float (&y)[3], const float (&u)[4]) +{ + y[0] = atan2f((2.0f*(u[2]*u[3]+u[0]*u[1])) , (u[0]*u[0]-u[1]*u[1]-u[2]*u[2]+u[3]*u[3])); + y[1] = -asinf(2.0f*(u[1]*u[3]-u[0]*u[2])); + y[2] = atan2f((2.0f*(u[1]*u[2]+u[0]*u[3])) , (u[0]*u[0]+u[1]*u[1]-u[2]*u[2]-u[3]*u[3])); +} + +void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD) +{ + velNED[0] = gpsGndSpd*cosf(gpsCourse); + velNED[1] = gpsGndSpd*sinf(gpsCourse); + velNED[2] = gpsVelD; +} + +void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +{ + posNED[0] = earthRadius * (lat - latRef); + posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); + posNED[2] = -(hgt - hgtRef); +} + +void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +{ + lat = latRef + posNED[0] * earthRadiusInv; + lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); + hgt = hgtRef - posNED[2]; +} + +void AttPosEKF::OnGroundCheck() +{ + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + if (staticMode) { + staticMode = !(GPSstatus > GPS_FIX_2D); + } +} + +void AttPosEKF::calcEarthRateNED(Vector3f &omega, float latitude) +{ + //Define Earth rotation vector in the NED navigation frame + omega.x = earthRate*cosf(latitude); + omega.y = 0.0f; + omega.z = -earthRate*sinf(latitude); +} + +void AttPosEKF::CovarianceInit() +{ + // Calculate the initial covariance matrix P + P[0][0] = 0.25f * sq(1.0f*deg2rad); + P[1][1] = 0.25f * sq(1.0f*deg2rad); + P[2][2] = 0.25f * sq(1.0f*deg2rad); + P[3][3] = 0.25f * sq(10.0f*deg2rad); + P[4][4] = sq(0.7); + P[5][5] = P[4][4]; + P[6][6] = sq(0.7); + P[7][7] = sq(15.0); + P[8][8] = P[7][7]; + P[9][9] = sq(5.0); + P[10][10] = sq(0.1*deg2rad*dtIMU); + P[11][11] = P[10][10]; + P[12][12] = P[10][10]; + P[13][13] = sq(8.0f); + P[14][4] = P[13][13]; + P[15][15] = sq(0.02f); + P[16][16] = P[15][15]; + P[17][17] = P[15][15]; + P[18][18] = sq(0.02f); + P[19][19] = P[18][18]; + P[20][20] = P[18][18]; +} + +float AttPosEKF::ConstrainFloat(float val, float min, float max) +{ + return (val > max) ? max : ((val < min) ? min : val); +} + +void AttPosEKF::ConstrainVariances() +{ + if (!numericalProtection) { + return; + } + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + + // Constrain quaternion variances + for (unsigned i = 0; i < 4; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Constrain velocitie variances + for (unsigned i = 4; i < 7; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Constrain position variances + for (unsigned i = 7; i < 10; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); + } + + // Angle bias variances + for (unsigned i = 10; i < 13; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU)); + } + + // Wind velocity variances + for (unsigned i = 13; i < 15; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); + } + + // Earth magnetic field variances + for (unsigned i = 15; i < 18; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + + // Body magnetic field variances + for (unsigned i = 18; i < 21; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); + } + +} + +void AttPosEKF::ConstrainStates() +{ + if (!numericalProtection) { + return; + } + + // State vector: + // 0-3: quaternions (q0, q1, q2, q3) + // 4-6: Velocity - m/sec (North, East, Down) + // 7-9: Position - m (North, East, Down) + // 10-12: Delta Angle bias - rad (X,Y,Z) + // 13-14: Wind Vector - m/sec (North,East) + // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) + // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + + + // Constrain quaternion + for (unsigned i = 0; i < 4; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Constrain velocities to what GPS can do for us + for (unsigned i = 4; i < 7; i++) { + states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); + } + + // Constrain position to a reasonable vehicle range (in meters) + for (unsigned i = 7; i < 9; i++) { + states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); + } + + // Constrain altitude + states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); + + // Angle bias limit - set to 8 degrees / sec + for (unsigned i = 10; i < 13; i++) { + states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); + } + + // Wind velocity limits - assume 120 m/s max velocity + for (unsigned i = 13; i < 15; i++) { + states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); + } + + // Earth magnetic field limits (in Gauss) + for (unsigned i = 15; i < 18; i++) { + states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); + } + + // Body magnetic field variances (in Gauss). + // the max offset should be in this range. + for (unsigned i = 18; i < 21; i++) { + states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); + } + +} + +void AttPosEKF::ForceSymmetry() +{ + if (!numericalProtection) { + return; + } + + // Force symmetry on the covariance matrix to prevent ill-conditioning + // of the matrix which would cause the filter to blow-up + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) + { + P[i][j] = 0.5f * (P[i][j] + P[j][i]); + P[j][i] = P[i][j]; + } + } +} + +bool AttPosEKF::FilterHealthy() +{ + if (!statesInitialised) { + return false; + } + + // XXX Check state vector for NaNs and ill-conditioning + + // Check if any of the major inputs timed out + if (current_ekf_state.posTimeout || current_ekf_state.velTimeout || current_ekf_state.hgtTimeout) { + return false; + } + + // Nothing fired, return ok. + return true; +} + +/** + * Reset the filter position states + * + * This resets the position to the last GPS measurement + * or to zero in case of static position. + */ +void AttPosEKF::ResetPosition(void) +{ + if (staticMode) { + states[7] = 0; + states[8] = 0; + } else if (GPSstatus >= GPS_FIX_3D) { + + // reset the states from the GPS measurements + states[7] = posNE[0]; + states[8] = posNE[1]; + } +} + +/** + * Reset the height state. + * + * This resets the height state with the last altitude measurements + */ +void AttPosEKF::ResetHeight(void) +{ + // write to the state vector + states[9] = -hgtMea; +} + +/** + * Reset the velocity state. + */ +void AttPosEKF::ResetVelocity(void) +{ + if (staticMode) { + states[4] = 0.0f; + states[5] = 0.0f; + states[6] = 0.0f; + } else if (GPSstatus >= GPS_FIX_3D) { + + states[4] = velNED[0]; // north velocity from last reading + states[5] = velNED[1]; // east velocity from last reading + states[6] = velNED[2]; // down velocity from last reading + } +} + + +void AttPosEKF::FillErrorReport(struct ekf_status_report *err) +{ + for (int i = 0; i < n_states; i++) + { + err->states[i] = states[i]; + } + + err->velHealth = current_ekf_state.velHealth; + err->posHealth = current_ekf_state.posHealth; + err->hgtHealth = current_ekf_state.hgtHealth; + err->velTimeout = current_ekf_state.velTimeout; + err->posTimeout = current_ekf_state.posTimeout; + err->hgtTimeout = current_ekf_state.hgtTimeout; +} + +bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { + bool err = false; + + // check all states and covariance matrices + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + if (!isfinite(KH[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(KHP[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // intermediate result used for covariance updates + if (!isfinite(P[i][j])) { + + err_report->covarianceNaN = true; + err = true; + } // covariance matrix + } + + if (!isfinite(Kfusion[i])) { + + err_report->kalmanGainsNaN = true; + err = true; + } // Kalman gains + + if (!isfinite(states[i])) { + + err_report->statesNaN = true; + err = true; + } // state matrix + } + + if (err) { + FillErrorReport(err_report); + } + + return err; + +} + +/** + * Check the filter inputs and bound its operational state + * + * This check will reset the filter states if required + * due to a failure of consistency or timeout checks. + * it should be run after the measurement data has been + * updated, but before any of the fusion steps are + * executed. + */ +int AttPosEKF::CheckAndBound() +{ + + // Store the old filter state + bool currStaticMode = staticMode; + + // Reset the filter if the states went NaN + if (StatesNaN(&last_ekf_error)) { + + InitializeDynamic(velNED, magDeclination); + + return 1; + } + + // Reset the filter if the IMU data is too old + if (dtIMU > 0.2f) { + + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + // that's all we can do here, return + return 2; + } + + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + + // Check if we switched between states + if (currStaticMode != staticMode) { + ResetVelocity(); + ResetPosition(); + ResetHeight(); + ResetStoredStates(); + + return 3; + } + + return 0; +} + +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) +{ + float initialRoll, initialPitch; + float cosRoll, sinRoll, cosPitch, sinPitch; + float magX, magY; + float initialHdg, cosHeading, sinHeading; + + initialRoll = atan2(-ay, -az); + initialPitch = atan2(ax, -az); + + cosRoll = cosf(initialRoll); + sinRoll = sinf(initialRoll); + cosPitch = cosf(initialPitch); + sinPitch = sinf(initialPitch); + + magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; + + magY = my * cosRoll - mz * sinRoll; + + initialHdg = atan2f(-magY, magX); + /* true heading is the mag heading minus declination */ + initialHdg += declination; + + cosRoll = cosf(initialRoll * 0.5f); + sinRoll = sinf(initialRoll * 0.5f); + + cosPitch = cosf(initialPitch * 0.5f); + sinPitch = sinf(initialPitch * 0.5f); + + cosHeading = cosf(initialHdg * 0.5f); + sinHeading = sinf(initialHdg * 0.5f); + + initQuat[0] = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; + initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; + initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; + initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; +} + +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) +{ + + // Clear the init flag + statesInitialised = false; + + magDeclination = declination; + + ZeroVariables(); + + // Calculate initial filter quaternion states from raw measurements + float initQuat[4]; + Vector3f initMagXYZ; + initMagXYZ = magData - magBias; + AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, magDeclination, initQuat); + + // Calculate initial Tbn matrix and rotate Mag measurements into NED + // to set initial NED magnetic field states + Mat3f DCM; + quat2Tbn(DCM, initQuat); + Vector3f initMagNED; + initMagXYZ = magData - magBias; + initMagNED.x = DCM.x.x*initMagXYZ.x + DCM.x.y*initMagXYZ.y + DCM.x.z*initMagXYZ.z; + initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; + initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; + + + + // write to state vector + for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions + for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities + for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel + states[15] = initMagNED.x; // Magnetic Field North + states[16] = initMagNED.y; // Magnetic Field East + states[17] = initMagNED.z; // Magnetic Field Down + states[18] = magBias.x; // Magnetic Field Bias X + states[19] = magBias.y; // Magnetic Field Bias Y + states[20] = magBias.z; // Magnetic Field Bias Z + + statesInitialised = true; + + // initialise the covariance matrix + CovarianceInit(); + + //Define Earth rotation vector in the NED navigation frame + calcEarthRateNED(earthRateNED, latRef); + + //Initialise summed variables used by covariance prediction + summedDelAng.x = 0.0f; + summedDelAng.y = 0.0f; + summedDelAng.z = 0.0f; + summedDelVel.x = 0.0f; + summedDelVel.y = 0.0f; + summedDelVel.z = 0.0f; +} + +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) +{ + //store initial lat,long and height + latRef = referenceLat; + lonRef = referenceLon; + hgtRef = referenceHgt; + + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + + InitializeDynamic(initvelNED, declination); +} + +void AttPosEKF::ZeroVariables() +{ + // Do the data structure init + for (unsigned i = 0; i < n_states; i++) { + for (unsigned j = 0; j < n_states; j++) { + KH[i][j] = 0.0f; // intermediate result used for covariance updates + KHP[i][j] = 0.0f; // intermediate result used for covariance updates + P[i][j] = 0.0f; // covariance matrix + } + + Kfusion[i] = 0.0f; // Kalman gains + states[i] = 0.0f; // state matrix + } + + for (unsigned i = 0; i < data_buffer_size; i++) { + + for (unsigned j = 0; j < n_states; j++) { + storedStates[j][i] = 0.0f; + } + + statetimeStamp[i] = 0; + } + + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); +} + +void AttPosEKF::GetFilterState(struct ekf_status_report *state) +{ + memcpy(state, ¤t_ekf_state, sizeof(state)); +} + +void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) +{ + memcpy(last_error, &last_ekf_error, sizeof(last_error)); +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_21states.h b/src/modules/ekf_att_pos_estimator/estimator_21states.h new file mode 100644 index 000000000..a19ff1995 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_21states.h @@ -0,0 +1,247 @@ +#pragma once + +#include "estimator_utilities.h" + +class AttPosEKF { + +public: + + AttPosEKF(); + ~AttPosEKF(); + + /* ############################################## + * + * M A I N F I L T E R P A R A M E T E R S + * + * ########################################### */ + + /* + * parameters are defined here and initialised in + * the InitialiseParameters() (which is just 20 lines down) + */ + + float covTimeStepMax; // maximum time allowed between covariance predictions + float covDelAngMax; // maximum delta angle between covariance predictions + float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + + float yawVarScale; + float windVelSigma; + float dAngBiasSigma; + float dVelBiasSigma; + float magEarthSigma; + float magBodySigma; + float gndHgtSigma; + + float vneSigma; + float vdSigma; + float posNeSigma; + float posDSigma; + float magMeasurementSigma; + float airspeedMeasurementSigma; + + float gyroProcessNoise; + float accelProcessNoise; + + float EAS2TAS; // ratio f true to equivalent airspeed + + void InitialiseParameters() + { + covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions + covDelAngMax = 0.02f; // maximum delta angle between covariance predictions + rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + EAS2TAS = 1.0f; + + yawVarScale = 1.0f; + windVelSigma = 0.1f; + dAngBiasSigma = 5.0e-7f; + dVelBiasSigma = 1e-4f; + magEarthSigma = 3.0e-4f; + magBodySigma = 3.0e-4f; + gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma + + vneSigma = 0.2f; + vdSigma = 0.3f; + posNeSigma = 2.0f; + posDSigma = 2.0f; + + magMeasurementSigma = 0.05; + airspeedMeasurementSigma = 1.4f; + gyroProcessNoise = 1.4544411e-2f; + accelProcessNoise = 0.5f; + } + + // Global variables + float KH[n_states][n_states]; // intermediate result used for covariance updates + float KHP[n_states][n_states]; // intermediate result used for covariance updates + float P[n_states][n_states]; // covariance matrix + float Kfusion[n_states]; // Kalman gains + float states[n_states]; // state matrix + float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps + uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored + + float statesAtVelTime[n_states]; // States at the effective measurement time for posNE and velNED measurements + float statesAtPosTime[n_states]; // States at the effective measurement time for posNE and velNED measurements + float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement + float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time + float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time + + Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) + Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) + Vector3f summedDelAng; // summed delta angles about the xyz body axes corrected for errors (rad) + Vector3f summedDelVel; // summed delta velocities along the XYZ body axes corrected for errors (m/s) + float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) + Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) + Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) + Vector3f dVelIMU; + Vector3f dAngIMU; + float dtIMU; // time lapsed since the last IMU measurement or covariance update (sec) + uint8_t fusionModeGPS; // 0 = GPS outputs 3D velocity, 1 = GPS outputs 2D velocity, 2 = GPS outputs no velocity + float innovVelPos[6]; // innovation output + float varInnovVelPos[6]; // innovation variance output + + float velNED[3]; // North, East, Down velocity obs (m/s) + float posNE[2]; // North, East position obs (m) + float hgtMea; // measured height (m) + float posNED[3]; // North, East Down position (m) + + float innovMag[3]; // innovation output + float varInnovMag[3]; // innovation variance output + Vector3f magData; // magnetometer flux radings in X,Y,Z body axes + float innovVtas; // innovation output + float varInnovVtas; // innovation variance output + float VtasMeas; // true airspeed measurement (m/s) + float magDeclination; + float latRef; // WGS-84 latitude of reference point (rad) + float lonRef; // WGS-84 longitude of reference point (rad) + float hgtRef; // WGS-84 height of reference point (m) + Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes + uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction + + // GPS input data variables + float gpsCourse; + float gpsVelD; + float gpsLat; + float gpsLon; + float gpsHgt; + uint8_t GPSstatus; + + // Baro input + float baroHgt; + + bool statesInitialised; + + bool fuseVelData; // this boolean causes the posNE and velNED obs to be fused + bool fusePosData; // this boolean causes the posNE and velNED obs to be fused + bool fuseHgtData; // this boolean causes the hgtMea obs to be fused + bool fuseMagData; // boolean true when magnetometer data is to be fused + bool fuseVtasData; // boolean true when airspeed data is to be fused + + bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) + bool staticMode; ///< boolean true if no position feedback is fused + bool useAirspeed; ///< boolean true if airspeed data is being used + bool useCompass; ///< boolean true if magnetometer data is being used + + struct ekf_status_report current_ekf_state; + struct ekf_status_report last_ekf_error; + + bool numericalProtection; + + unsigned storeIndex; + + +void UpdateStrapdownEquationsNED(); + +void CovariancePrediction(float dt); + +void FuseVelposNED(); + +void FuseMagnetometer(); + +void FuseAirspeed(); + +void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); + +void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); + +void quatNorm(float (&quatOut)[4], const float quatIn[4]); + +// store staes along with system time stamp in msces +void StoreStates(uint64_t timestamp_ms); + +/** + * Recall the state vector. + * + * Recalls the vector stored at closest time to the one specified by msec + * + * @return zero on success, integer indicating the number of invalid states on failure. + * Does only copy valid states, if the statesForFusion vector was initialized + * correctly by the caller, the result can be safely used, but is a mixture + * time-wise where valid states were updated and invalid remained at the old + * value. + */ +int RecallStates(float statesForFusion[n_states], uint64_t msec); + +void ResetStoredStates(); + +void quat2Tbn(Mat3f &Tbn, const float (&quat)[4]); + +void calcEarthRateNED(Vector3f &omega, float latitude); + +static void eul2quat(float (&quat)[4], const float (&eul)[3]); + +static void quat2eul(float (&eul)[3], const float (&quat)[4]); + +static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); + +static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); + +static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); + +static float sq(float valIn); + +void OnGroundCheck(); + +void CovarianceInit(); + +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); + +float ConstrainFloat(float val, float min, float max); + +void ConstrainVariances(); + +void ConstrainStates(); + +void ForceSymmetry(); + +int CheckAndBound(); + +void ResetPosition(); + +void ResetVelocity(); + +void ZeroVariables(); + +void GetFilterState(struct ekf_status_report *state); + +void GetLastErrorState(struct ekf_status_report *last_error); + +bool StatesNaN(struct ekf_status_report *err_report); +void FillErrorReport(struct ekf_status_report *err); + +void InitializeDynamic(float (&initvelNED)[3], float declination); + +protected: + +bool FilterHealthy(); + +void ResetHeight(void); + +void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); + +}; + +uint32_t millis(); + diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp index 5db1adbb3..9622f7e40 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp @@ -1,143 +1,9 @@ -#include "estimator.h" +#include "estimator_23states.h" #include <string.h> -#include <stdarg.h> - -// Define EKF_DEBUG here to enable the debug print calls -// if the macro is not set, these will be completely -// optimized out by the compiler. -//#define EKF_DEBUG - -#ifdef EKF_DEBUG #include <stdio.h> +#include <stdarg.h> -static void -ekf_debug_print(const char *fmt, va_list args) -{ - fprintf(stderr, "%s: ", "[ekf]"); - vfprintf(stderr, fmt, args); - - fprintf(stderr, "\n"); -} - -static void -ekf_debug(const char *fmt, ...) -{ - va_list args; - - va_start(args, fmt); - ekf_debug_print(fmt, args); -} - -#else - -static void ekf_debug(const char *fmt, ...) { while(0){} } -#endif - -float Vector3f::length(void) const -{ - return sqrt(x*x + y*y + z*z); -} - -void Vector3f::zero(void) -{ - x = 0.0f; - y = 0.0f; - z = 0.0f; -} - -Mat3f::Mat3f() { - identity(); -} - -void Mat3f::identity() { - x.x = 1.0f; - x.y = 0.0f; - x.z = 0.0f; - - y.x = 0.0f; - y.y = 1.0f; - y.z = 0.0f; - - z.x = 0.0f; - z.y = 0.0f; - z.z = 1.0f; -} - -Mat3f Mat3f::transpose(void) const -{ - Mat3f ret = *this; - swap_var(ret.x.y, ret.y.x); - swap_var(ret.x.z, ret.z.x); - swap_var(ret.y.z, ret.z.y); - return ret; -} - -// overload + operator to provide a vector addition -Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x + vecIn2.x; - vecOut.y = vecIn1.y + vecIn2.y; - vecOut.z = vecIn1.z + vecIn2.z; - return vecOut; -} - -// overload - operator to provide a vector subtraction -Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x - vecIn2.x; - vecOut.y = vecIn1.y - vecIn2.y; - vecOut.z = vecIn1.z - vecIn2.z; - return vecOut; -} - -// overload * operator to provide a matrix vector product -Vector3f operator*( Mat3f matIn, Vector3f vecIn) -{ - Vector3f vecOut; - vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; - vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; - vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; - return vecOut; -} - -// overload % operator to provide a vector cross product -Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) -{ - Vector3f vecOut; - vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; - vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; - vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; - return vecOut; -} - -// overload * operator to provide a vector scaler product -Vector3f operator*(Vector3f vecIn1, float sclIn1) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x * sclIn1; - vecOut.y = vecIn1.y * sclIn1; - vecOut.z = vecIn1.z * sclIn1; - return vecOut; -} - -// overload * operator to provide a vector scaler product -Vector3f operator*(float sclIn1, Vector3f vecIn1) -{ - Vector3f vecOut; - vecOut.x = vecIn1.x * sclIn1; - vecOut.y = vecIn1.y * sclIn1; - vecOut.z = vecIn1.z * sclIn1; - return vecOut; -} - -void swap_var(float &d1, float &d2) -{ - float tmp = d1; - d1 = d2; - d2 = tmp; -} +#define EKF_COVARIANCE_DIVERGED 1.0e8f AttPosEKF::AttPosEKF() @@ -145,7 +11,42 @@ AttPosEKF::AttPosEKF() * instead to allow clean in-air re-initialization. */ { + summedDelAng.zero(); + summedDelVel.zero(); + + fusionModeGPS = 0; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + onGround = true; + staticMode = true; + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + numericalProtection = true; + refSet = false; + storeIndex = 0; + gpsHgt = 0.0f; + baroHgt = 0.0f; + GPSstatus = 0; + VtasMeas = 0.0f; + magDeclination = 0.0f; + dAngIMU.zero(); + dVelIMU.zero(); + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + accelGPSNED[0] = 0.0f; + accelGPSNED[1] = 0.0f; + accelGPSNED[2] = 0.0f; + delAngTotal.zero(); + ekfDiverged = false; + lastReset = 0; + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); ZeroVariables(); InitialiseParameters(); } @@ -181,6 +82,10 @@ void AttPosEKF::UpdateStrapdownEquationsNED() dVelIMU.y = dVelIMU.y; dVelIMU.z = dVelIMU.z - states[13]; + delAngTotal.x += correctedDelAng.x; + delAngTotal.y += correctedDelAng.y; + delAngTotal.z += correctedDelAng.z; + // Save current measurements Vector3f prevDelAng = correctedDelAng; @@ -199,8 +104,12 @@ void AttPosEKF::UpdateStrapdownEquationsNED() } else { - deltaQuat[0] = cosf(0.5f*rotationMag); - float rotScaler = (sinf(0.5f*rotationMag))/rotationMag; + // We are using double here as we are unsure how small + // the angle differences are and if we get into numeric + // issues with float. The runtime impact is not measurable + // for these quantities. + deltaQuat[0] = cos(0.5*(double)rotationMag); + float rotScaler = (sin(0.5*(double)rotationMag))/(double)rotationMag; deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; @@ -312,7 +221,8 @@ void AttPosEKF::CovariancePrediction(float dt) float nextP[n_states][n_states]; // calculate covariance prediction process noise - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i= 0; i<4; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i= 4; i<10; i++) processNoise[i] = 1.0e-9f; for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; // scale gyro bias noise when on ground to allow for faster bias estimation for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; @@ -977,20 +887,20 @@ void AttPosEKF::CovariancePrediction(float dt) // propagate for (unsigned i = 0; i <= 13; i++) { P[i][i] = nextP[i][i]; + } - // force symmetry for observable states - // force zero for non-observable states - for (unsigned i = 1; i < n_states; i++) + // force symmetry for observable states + // force zero for non-observable states + for (unsigned i = 1; i < n_states; i++) + { + for (uint8_t j = 0; j < i; j++) { - for (uint8_t j = 0; j < i; j++) - { - if ((i > 13) || (j > 13)) { - P[i][j] = 0.0f; - } else { - P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); - } - P[j][i] = P[i][j]; + if ((i > 13) || (j > 13)) { + P[i][j] = 0.0f; + } else { + P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); } + P[j][i] = P[i][j]; } } @@ -1020,9 +930,9 @@ void AttPosEKF::FuseVelposNED() { // declare variables used by fault isolation logic - uint32_t gpsRetryTime = 30000; // time in msec before GPS fusion will be retried following innovation consistency failure - uint32_t gpsRetryTimeNoTAS = 5000; // retry time if no TAS measurement available - uint32_t hgtRetryTime = 5000; // height measurement retry time + uint32_t gpsRetryTime = 3000; // time in msec before GPS fusion will be retried following innovation consistency failure + uint32_t gpsRetryTimeNoTAS = 500; // retry time if no TAS measurement available + uint32_t hgtRetryTime = 500; // height measurement retry time uint32_t horizRetryTime; // declare variables used to check measurement errors @@ -1178,7 +1088,7 @@ void AttPosEKF::FuseVelposNED() stateIndex = 4 + obsIndex; // Calculate the measurement innovation, using states from a // different time coordinate if fusing height data - if (obsIndex >= 0 && obsIndex <= 2) + if (obsIndex <= 2) { innovVelPos[obsIndex] = statesAtVelTime[stateIndex] - observation[obsIndex]; } @@ -1193,7 +1103,7 @@ void AttPosEKF::FuseVelposNED() // Calculate the Kalman Gain // Calculate innovation variances - also used for data logging varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex]; - SK = 1.0/varInnovVelPos[obsIndex]; + SK = 1.0/(double)varInnovVelPos[obsIndex]; for (uint8_t i= 0; i<=indexLimit; i++) { Kfusion[i] = P[i][stateIndex]*SK; @@ -1277,7 +1187,7 @@ void AttPosEKF::FuseMagnetometer() // data fit is the only assumption we can make // so we might as well take advantage of the computational efficiencies // associated with sequential fusion - if (useCompass && (fuseMagData || obsIndex == 1 || obsIndex == 2)) + if (useCompass && fuseMagData && (obsIndex < 3)) { // Limit range of states modified when on ground if(!onGround) @@ -1293,7 +1203,7 @@ void AttPosEKF::FuseMagnetometer() // three prediction time steps. // Calculate observation jacobians and Kalman gains - if (fuseMagData) + if (obsIndex == 0) { // Copy required states to local variable names q0 = statesAtMagMeasTime[0]; @@ -1388,11 +1298,6 @@ void AttPosEKF::FuseMagnetometer() Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - fuseMagData = false; } else if (obsIndex == 1) // we are now fusing the Y measurement { @@ -1508,7 +1413,7 @@ void AttPosEKF::FuseMagnetometer() } // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0) + if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0f) { // correct the state vector for (uint8_t j= 0; j < indexLimit; j++) @@ -1517,7 +1422,7 @@ void AttPosEKF::FuseMagnetometer() } // normalise the quaternion states float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) + if (quatMag > 1e-12f) { for (uint8_t j= 0; j<=3; j++) { @@ -1612,7 +1517,7 @@ void AttPosEKF::FuseAirspeed() SH_TAS[0] = 1/(sqrt(sq(ve - vwe) + sq(vn - vwn) + sq(vd))); SH_TAS[1] = (SH_TAS[0]*(2.0f*ve - 2*vwe))/2.0f; SH_TAS[2] = (SH_TAS[0]*(2.0f*vn - 2*vwn))/2.0f; - + float H_TAS[n_states]; for (uint8_t i = 0; i < n_states; i++) H_TAS[i] = 0.0f; H_TAS[4] = SH_TAS[2]; @@ -1661,7 +1566,7 @@ void AttPosEKF::FuseAirspeed() // Calculate the measurement innovation innovVtas = VtasPred - VtasMeas; // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovVtas*innovVtas*SK_TAS) < 25.0) + if ((innovVtas*innovVtas*SK_TAS) < 25.0f) { // correct the state vector for (uint8_t j=0; j <= 22; j++) @@ -1758,7 +1663,7 @@ void AttPosEKF::FuseRangeFinder() // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data SH_RNG[4] = sin(rngFinderPitch); - cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch); + cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cosf(rngFinderPitch); if (useRangeFinder && cosRngTilt > 0.87f) { // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset @@ -1855,21 +1760,21 @@ int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) int64_t bestTimeDelta = 200; unsigned bestStoreIndex = 0; - for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + for (unsigned storeIndexLocal = 0; storeIndexLocal < data_buffer_size; storeIndexLocal++) { // Work around a GCC compiler bug - we know 64bit support on ARM is // sketchy in GCC. uint64_t timeDelta; - if (msec > statetimeStamp[storeIndex]) { - timeDelta = msec - statetimeStamp[storeIndex]; + if (msec > statetimeStamp[storeIndexLocal]) { + timeDelta = msec - statetimeStamp[storeIndexLocal]; } else { - timeDelta = statetimeStamp[storeIndex] - msec; + timeDelta = statetimeStamp[storeIndexLocal] - msec; } - if (timeDelta < bestTimeDelta) + if (timeDelta < (uint64_t)bestTimeDelta) { - bestStoreIndex = storeIndex; + bestStoreIndex = storeIndexLocal; bestTimeDelta = timeDelta; } } @@ -1926,7 +1831,7 @@ void AttPosEKF::quat2Tnb(Mat3f &Tnb, const float (&quat)[4]) Tnb.y.z = 2*(q23 + q01); } -void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) +void AttPosEKF::quat2Tbn(Mat3f &Tbn_ret, const float (&quat)[4]) { // Calculate the body to nav cosine matrix float q00 = sq(quat[0]); @@ -1940,15 +1845,15 @@ void AttPosEKF::quat2Tbn(Mat3f &Tbn, const float (&quat)[4]) float q13 = quat[1]*quat[3]; float q23 = quat[2]*quat[3]; - Tbn.x.x = q00 + q11 - q22 - q33; - Tbn.y.y = q00 - q11 + q22 - q33; - Tbn.z.z = q00 - q11 - q22 + q33; - Tbn.x.y = 2*(q12 - q03); - Tbn.x.z = 2*(q13 + q02); - Tbn.y.x = 2*(q12 + q03); - Tbn.y.z = 2*(q23 - q01); - Tbn.z.x = 2*(q13 - q02); - Tbn.z.y = 2*(q23 + q01); + Tbn_ret.x.x = q00 + q11 - q22 - q33; + Tbn_ret.y.y = q00 - q11 + q22 - q33; + Tbn_ret.z.z = q00 - q11 - q22 + q33; + Tbn_ret.x.y = 2*(q12 - q03); + Tbn_ret.x.z = 2*(q13 + q02); + Tbn_ret.y.x = 2*(q12 + q03); + Tbn_ret.y.z = 2*(q23 - q01); + Tbn_ret.z.x = 2*(q13 - q02); + Tbn_ret.z.y = 2*(q23 + q01); } void AttPosEKF::eul2quat(float (&quat)[4], const float (&eul)[3]) @@ -1979,17 +1884,17 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, velNED[2] = gpsVelD; } -void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef) +void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latReference, double lonReference, float hgtReference) { - posNED[0] = earthRadius * (lat - latRef); - posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); - posNED[2] = -(hgt - hgtRef); + posNED[0] = earthRadius * (lat - latReference); + posNED[1] = earthRadius * cos(latReference) * (lon - lonReference); + posNED[2] = -(hgt - hgtReference); } -void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef) { - lat = latRef + posNED[0] * earthRadiusInv; - lon = lonRef + posNED[1] * earthRadiusInv / cos(latRef); + lat = latRef + (double)posNED[0] * earthRadiusInv; + lon = lonRef + (double)posNED[1] * earthRadiusInv / cos(latRef); hgt = hgtRef - posNED[2]; } @@ -2042,10 +1947,10 @@ float AttPosEKF::ConstrainFloat(float val, float min, float max) float ret; if (val > max) { ret = max; - ekf_debug("> max: %8.4f, val: %8.4f", max, val); + ekf_debug("> max: %8.4f, val: %8.4f", (double)max, (double)val); } else if (val < min) { ret = min; - ekf_debug("< min: %8.4f, val: %8.4f", min, val); + ekf_debug("< min: %8.4f, val: %8.4f", (double)min, (double)val); } else { ret = val; } @@ -2194,10 +2099,71 @@ void AttPosEKF::ForceSymmetry() { P[i][j] = 0.5f * (P[i][j] + P[j][i]); P[j][i] = P[i][j]; + + if ((fabsf(P[i][j]) > EKF_COVARIANCE_DIVERGED) || + (fabsf(P[j][i]) > EKF_COVARIANCE_DIVERGED)) { + current_ekf_state.covariancesExcessive = true; + current_ekf_state.error |= true; + InitializeDynamic(velNED, magDeclination); + return; + } + + float symmetric = 0.5f * (P[i][j] + P[j][i]); + P[i][j] = symmetric; + P[j][i] = symmetric; } } } +bool AttPosEKF::GyroOffsetsDiverged() +{ + // Detect divergence by looking for rapid changes of the gyro offset + Vector3f current_bias; + current_bias.x = states[10]; + current_bias.y = states[11]; + current_bias.z = states[12]; + + Vector3f delta = current_bias - lastGyroOffset; + float delta_len = delta.length(); + float delta_len_scaled = 0.0f; + + // Protect against division by zero + if (delta_len > 0.0f) { + float cov_mag = ConstrainFloat((P[10][10] + P[11][11] + P[12][12]), 1e-12f, 1e-8f); + delta_len_scaled = (5e-7 / (double)cov_mag) * (double)delta_len / (double)dtIMU; + } + + bool diverged = (delta_len_scaled > 1.0f); + lastGyroOffset = current_bias; + current_ekf_state.error |= diverged; + current_ekf_state.gyroOffsetsExcessive = diverged; + + return diverged; +} + +bool AttPosEKF::VelNEDDiverged() +{ + Vector3f current_vel; + current_vel.x = states[4]; + current_vel.y = states[5]; + current_vel.z = states[6]; + + Vector3f gps_vel; + gps_vel.x = velNED[0]; + gps_vel.y = velNED[1]; + gps_vel.z = velNED[2]; + + Vector3f delta = current_vel - gps_vel; + float delta_len = delta.length(); + + bool excessive = (delta_len > 20.0f); + + current_ekf_state.error |= excessive; + current_ekf_state.velOffsetExcessive = excessive; + + return excessive; +} + bool AttPosEKF::FilterHealthy() { if (!statesInitialised) { @@ -2262,42 +2228,26 @@ void AttPosEKF::ResetVelocity(void) } } - -void AttPosEKF::FillErrorReport(struct ekf_status_report *err) -{ - for (unsigned i = 0; i < n_states; i++) - { - err->states[i] = states[i]; - } - - err->velHealth = current_ekf_state.velHealth; - err->posHealth = current_ekf_state.posHealth; - err->hgtHealth = current_ekf_state.hgtHealth; - err->velTimeout = current_ekf_state.velTimeout; - err->posTimeout = current_ekf_state.posTimeout; - err->hgtTimeout = current_ekf_state.hgtTimeout; -} - -bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { +bool AttPosEKF::StatesNaN() { bool err = false; // check all integrators if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { - err_report->statesNaN = true; + current_ekf_state.angNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { - err_report->statesNaN = true; + current_ekf_state.angNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { - err_report->statesNaN = true; + current_ekf_state.summedDelVelNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; goto out; @@ -2308,7 +2258,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { for (unsigned j = 0; j < n_states; j++) { if (!isfinite(KH[i][j])) { - err_report->covarianceNaN = true; + current_ekf_state.KHNaN = true; err = true; ekf_debug("KH NaN"); goto out; @@ -2316,7 +2266,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KHP[i][j])) { - err_report->covarianceNaN = true; + current_ekf_state.KHPNaN = true; err = true; ekf_debug("KHP NaN"); goto out; @@ -2324,7 +2274,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(P[i][j])) { - err_report->covarianceNaN = true; + current_ekf_state.covarianceNaN = true; err = true; ekf_debug("P NaN"); } // covariance matrix @@ -2332,7 +2282,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(Kfusion[i])) { - err_report->kalmanGainsNaN = true; + current_ekf_state.kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); err = true; goto out; @@ -2340,7 +2290,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(states[i])) { - err_report->statesNaN = true; + current_ekf_state.statesNaN = true; ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); err = true; goto out; @@ -2349,7 +2299,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { out: if (err) { - FillErrorReport(err_report); + current_ekf_state.error |= true; } return err; @@ -2365,47 +2315,105 @@ out: * updated, but before any of the fusion steps are * executed. */ -int AttPosEKF::CheckAndBound() +int AttPosEKF::CheckAndBound(struct ekf_status_report *last_error) { // Store the old filter state bool currStaticMode = staticMode; + // Limit reset rate to 5 Hz to allow the filter + // to settle + if (millis() - lastReset < 200) { + return 0; + } + + if (ekfDiverged) { + ekfDiverged = false; + } + + int ret = 0; + + // Check if we're on ground - this also sets static mode. + OnGroundCheck(); + // Reset the filter if the states went NaN - if (StatesNaN(&last_ekf_error)) { + if (StatesNaN()) { ekf_debug("re-initializing dynamic"); - InitializeDynamic(velNED, magDeclination); + // Reset and fill error report + InitializeDynamic(velNED, magDeclination); - return 1; + ret = 1; } // Reset the filter if the IMU data is too old if (dtIMU > 0.3f) { + current_ekf_state.imuTimeout = true; + + // Fill error report + GetFilterState(&last_ekf_error); + ResetVelocity(); ResetPosition(); ResetHeight(); ResetStoredStates(); + // Timeout cleared with this reset + current_ekf_state.imuTimeout = false; + // that's all we can do here, return - return 2; + ret = 2; } - // Check if we're on ground - this also sets static mode. - OnGroundCheck(); - // Check if we switched between states if (currStaticMode != staticMode) { + // Fill error report, but not setting error flag + GetFilterState(&last_ekf_error); + ResetVelocity(); ResetPosition(); ResetHeight(); ResetStoredStates(); - return 3; + ret = 3; + } + + // Reset the filter if gyro offsets are excessive + if (GyroOffsetsDiverged()) { + + // Reset and fill error report + InitializeDynamic(velNED, magDeclination); + + // that's all we can do here, return + ret = 4; + } + + // Reset the filter if it diverges too far from GPS + if (VelNEDDiverged()) { + + // Reset and fill error report + InitializeDynamic(velNED, magDeclination); + + // that's all we can do here, return + ret = 5; + } + + // The excessive covariance detection already + // reset the filter. Just need to report here. + if (last_ekf_error.covariancesExcessive) { + ret = 6; + } + + if (ret) { + ekfDiverged = true; + lastReset = millis(); + + // This reads the last error and clears it + GetLastErrorState(last_error); } - return 0; + return ret; } void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) @@ -2456,6 +2464,30 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { + if (current_ekf_state.error) { + GetFilterState(&last_ekf_error); + } + + ZeroVariables(); + + // Reset error states + current_ekf_state.error = false; + current_ekf_state.angNaN = false; + current_ekf_state.summedDelVelNaN = false; + current_ekf_state.KHNaN = false; + current_ekf_state.KHPNaN = false; + current_ekf_state.PNaN = false; + current_ekf_state.covarianceNaN = false; + current_ekf_state.kalmanGainsNaN = false; + current_ekf_state.statesNaN = false; + + current_ekf_state.velHealth = true; + //current_ekf_state.posHealth = ?; + //current_ekf_state.hgtHealth = ?; + + current_ekf_state.velTimeout = false; + //current_ekf_state.posTimeout = ?; + //current_ekf_state.hgtTimeout = ?; // Fill variables with valid data velNED[0] = initvelNED[0]; @@ -2494,7 +2526,11 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities - for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel + // positions: + states[7] = posNE[0]; + states[8] = posNE[1]; + states[9] = -hgtMea; + for (uint8_t j=10; j<=15; j++) states[j] = 0.0f; // dAngBias, dVelBias, windVel states[16] = initMagNED.x; // Magnetic Field North states[17] = initMagNED.y; // Magnetic Field East states[18] = initMagNED.z; // Magnetic Field Down @@ -2525,14 +2561,16 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do hgtRef = referenceHgt; refSet = true; - // we are at reference altitude, so measurement must be zero - hgtMea = 0.0f; + // we are at reference position, so measurement must be zero + posNE[0] = 0.0f; + posNE[1] = 0.0f; + + // we are at an unknown, possibly non-zero altitude - so altitude + // is not reset (hgtMea) // the baro offset must be this difference now baroHgtOffset = baroHgt - referenceHgt; - memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - InitializeDynamic(initvelNED, declination); } @@ -2540,27 +2578,8 @@ void AttPosEKF::ZeroVariables() { // Initialize on-init initialized variables - fusionModeGPS = 0; - covSkipCount = 0; - statesInitialised = false; - fuseVelData = false; - fusePosData = false; - fuseHgtData = false; - fuseMagData = false; - fuseVtasData = false; - onGround = true; - staticMode = true; - useAirspeed = true; - useCompass = true; - useRangeFinder = true; - numericalProtection = true; - refSet = false; + storeIndex = 0; - gpsHgt = 0.0f; - baroHgt = 0.0f; - GPSstatus = 0; - VtasMeas = 0.0f; - magDeclination = 0.0f; // Do the data structure init for (unsigned i = 0; i < n_states; i++) { @@ -2577,9 +2596,7 @@ void AttPosEKF::ZeroVariables() correctedDelAng.zero(); summedDelAng.zero(); summedDelVel.zero(); - - dAngIMU.zero(); - dVelIMU.zero(); + lastGyroOffset.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { @@ -2598,12 +2615,27 @@ void AttPosEKF::ZeroVariables() } -void AttPosEKF::GetFilterState(struct ekf_status_report *state) +void AttPosEKF::GetFilterState(struct ekf_status_report *err) { - memcpy(state, ¤t_ekf_state, sizeof(*state)); + + // Copy states + for (unsigned i = 0; i < n_states; i++) { + current_ekf_state.states[i] = states[i]; + } + current_ekf_state.n_states = n_states; + + memcpy(err, ¤t_ekf_state, sizeof(*err)); + + // err->velHealth = current_ekf_state.velHealth; + // err->posHealth = current_ekf_state.posHealth; + // err->hgtHealth = current_ekf_state.hgtHealth; + // err->velTimeout = current_ekf_state.velTimeout; + // err->posTimeout = current_ekf_state.posTimeout; + // err->hgtTimeout = current_ekf_state.hgtTimeout; } void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) { memcpy(last_error, &last_ekf_error, sizeof(*last_error)); + memset(&last_ekf_error, 0, sizeof(last_ekf_error)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator_23states.h index e821089f2..7aad849f9 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator_23states.h @@ -1,76 +1,10 @@ -#include <math.h> -#include <stdint.h> - #pragma once -#define GRAVITY_MSS 9.80665f -#define deg2rad 0.017453292f -#define rad2deg 57.295780f -#define pi 3.141592657f -#define earthRate 0.000072921f -#define earthRadius 6378145.0f -#define earthRadiusInv 1.5678540e-7f - -class Vector3f -{ -private: -public: - float x; - float y; - float z; - - float length(void) const; - void zero(void); -}; - -class Mat3f -{ -private: -public: - Vector3f x; - Vector3f y; - Vector3f z; - - Mat3f(); - - void identity(); - Mat3f transpose(void) const; -}; - -Vector3f operator*(float sclIn1, Vector3f vecIn1); -Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator*( Mat3f matIn, Vector3f vecIn); -Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); -Vector3f operator*(Vector3f vecIn1, float sclIn1); - -void swap_var(float &d1, float &d2); +#include "estimator_utilities.h" const unsigned int n_states = 23; const unsigned int data_buffer_size = 50; -enum GPS_FIX { - GPS_FIX_NOFIX = 0, - GPS_FIX_2D = 2, - GPS_FIX_3D = 3 -}; - -struct ekf_status_report { - bool velHealth; - bool posHealth; - bool hgtHealth; - bool velTimeout; - bool posTimeout; - bool hgtTimeout; - uint32_t velFailTime; - uint32_t posFailTime; - uint32_t hgtFailTime; - float states[n_states]; - bool statesNaN; - bool covarianceNaN; - bool kalmanGainsNaN; -}; - class AttPosEKF { public: @@ -141,7 +75,7 @@ public: accelProcessNoise = 0.5f; } - struct { + struct mag_state_struct { unsigned obsIndex; float MagPred[3]; float SH_MAG[9]; @@ -157,7 +91,12 @@ public: float magZbias; float R_MAG; Mat3f DCM; - } magstate; + }; + + struct mag_state_struct magstate; + struct mag_state_struct resetMagState; + + // Global variables @@ -166,6 +105,7 @@ public: float P[n_states][n_states]; // covariance matrix float Kfusion[n_states]; // Kalman gains float states[n_states]; // state matrix + float resetStates[n_states]; float storedStates[n_states][data_buffer_size]; // state vectors stored for the last 50 time steps uint32_t statetimeStamp[data_buffer_size]; // time stamp for each state vector stored @@ -183,6 +123,8 @@ public: float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + Vector3f lastGyroOffset; // Last gyro offset + Vector3f delAngTotal; Mat3f Tbn; // transformation matrix from body to NED coordinates Mat3f Tnb; // transformation amtrix from NED to body coordinates @@ -196,11 +138,11 @@ public: float varInnovVelPos[6]; // innovation variance output float velNED[3]; // North, East, Down velocity obs (m/s) + float accelGPSNED[3]; // Acceleration predicted by GPS in earth frame float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) float baroHgtOffset; ///< the baro (weather) offset from normalized altitude float rngMea; // Ground distance - float posNED[3]; // North, East Down position (m) float innovMag[3]; // innovation output float varInnovMag[3]; // innovation variance output @@ -243,6 +185,9 @@ public: bool useCompass; ///< boolean true if magnetometer data is being used bool useRangeFinder; ///< true when rangefinder is being used + bool ekfDiverged; + uint64_t lastReset; + struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; @@ -299,9 +244,9 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]); static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); +void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); -static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcLLH(float posNED[3], double &lat, double &lon, float &hgt, double latRef, double lonRef, float hgtRef); static void quat2Tnb(Mat3f &Tnb, const float (&quat)[4]); @@ -321,7 +266,7 @@ void ConstrainStates(); void ForceSymmetry(); -int CheckAndBound(); +int CheckAndBound(struct ekf_status_report *last_error); void ResetPosition(); @@ -333,8 +278,7 @@ void GetFilterState(struct ekf_status_report *state); void GetLastErrorState(struct ekf_status_report *last_error); -bool StatesNaN(struct ekf_status_report *err_report); -void FillErrorReport(struct ekf_status_report *err); +bool StatesNaN(); void InitializeDynamic(float (&initvelNED)[3], float declination); @@ -342,6 +286,10 @@ protected: bool FilterHealthy(); +bool GyroOffsetsDiverged(); + +bool VelNEDDiverged(); + void ResetHeight(void); void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp new file mode 100644 index 000000000..b4767a0d3 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.cpp @@ -0,0 +1,139 @@ + +#include "estimator_utilities.h" + +// Define EKF_DEBUG here to enable the debug print calls +// if the macro is not set, these will be completely +// optimized out by the compiler. +//#define EKF_DEBUG + +#ifdef EKF_DEBUG +#include <stdio.h> + +static void +ekf_debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[ekf]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +void +ekf_debug(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + ekf_debug_print(fmt, args); +} + +#else + +void ekf_debug(const char *fmt, ...) { while(0){} } +#endif + +float Vector3f::length(void) const +{ + return sqrt(x*x + y*y + z*z); +} + +void Vector3f::zero(void) +{ + x = 0.0f; + y = 0.0f; + z = 0.0f; +} + +Mat3f::Mat3f() { + identity(); +} + +void Mat3f::identity() { + x.x = 1.0f; + x.y = 0.0f; + x.z = 0.0f; + + y.x = 0.0f; + y.y = 1.0f; + y.z = 0.0f; + + z.x = 0.0f; + z.y = 0.0f; + z.z = 1.0f; +} + +Mat3f Mat3f::transpose(void) const +{ + Mat3f ret = *this; + swap_var(ret.x.y, ret.y.x); + swap_var(ret.x.z, ret.z.x); + swap_var(ret.y.z, ret.z.y); + return ret; +} + +// overload + operator to provide a vector addition +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x + vecIn2.x; + vecOut.y = vecIn1.y + vecIn2.y; + vecOut.z = vecIn1.z + vecIn2.z; + return vecOut; +} + +// overload - operator to provide a vector subtraction +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x - vecIn2.x; + vecOut.y = vecIn1.y - vecIn2.y; + vecOut.z = vecIn1.z - vecIn2.z; + return vecOut; +} + +// overload * operator to provide a matrix vector product +Vector3f operator*( Mat3f matIn, Vector3f vecIn) +{ + Vector3f vecOut; + vecOut.x = matIn.x.x*vecIn.x + matIn.x.y*vecIn.y + matIn.x.z*vecIn.z; + vecOut.y = matIn.y.x*vecIn.x + matIn.y.y*vecIn.y + matIn.y.z*vecIn.z; + vecOut.z = matIn.x.x*vecIn.x + matIn.z.y*vecIn.y + matIn.z.z*vecIn.z; + return vecOut; +} + +// overload % operator to provide a vector cross product +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) +{ + Vector3f vecOut; + vecOut.x = vecIn1.y*vecIn2.z - vecIn1.z*vecIn2.y; + vecOut.y = vecIn1.z*vecIn2.x - vecIn1.x*vecIn2.z; + vecOut.z = vecIn1.x*vecIn2.y - vecIn1.y*vecIn2.x; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(Vector3f vecIn1, float sclIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +// overload * operator to provide a vector scaler product +Vector3f operator*(float sclIn1, Vector3f vecIn1) +{ + Vector3f vecOut; + vecOut.x = vecIn1.x * sclIn1; + vecOut.y = vecIn1.y * sclIn1; + vecOut.z = vecIn1.z * sclIn1; + return vecOut; +} + +void swap_var(float &d1, float &d2) +{ + float tmp = d1; + d1 = d2; + d2 = tmp; +} diff --git a/src/modules/ekf_att_pos_estimator/estimator_utilities.h b/src/modules/ekf_att_pos_estimator/estimator_utilities.h new file mode 100644 index 000000000..d47568b62 --- /dev/null +++ b/src/modules/ekf_att_pos_estimator/estimator_utilities.h @@ -0,0 +1,82 @@ +#include <math.h> +#include <stdint.h> + +#pragma once + +#define GRAVITY_MSS 9.80665f +#define deg2rad 0.017453292f +#define rad2deg 57.295780f +#define pi 3.141592657f +#define earthRate 0.000072921f +#define earthRadius 6378145.0 +#define earthRadiusInv 1.5678540e-7 + +class Vector3f +{ +private: +public: + float x; + float y; + float z; + + float length(void) const; + void zero(void); +}; + +class Mat3f +{ +private: +public: + Vector3f x; + Vector3f y; + Vector3f z; + + Mat3f(); + + void identity(); + Mat3f transpose(void) const; +}; + +Vector3f operator*(float sclIn1, Vector3f vecIn1); +Vector3f operator+( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator-( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*( Mat3f matIn, Vector3f vecIn); +Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2); +Vector3f operator*(Vector3f vecIn1, float sclIn1); + +void swap_var(float &d1, float &d2); + +enum GPS_FIX { + GPS_FIX_NOFIX = 0, + GPS_FIX_2D = 2, + GPS_FIX_3D = 3 +}; + +struct ekf_status_report { + bool error; + bool velHealth; + bool posHealth; + bool hgtHealth; + bool velTimeout; + bool posTimeout; + bool hgtTimeout; + bool imuTimeout; + uint32_t velFailTime; + uint32_t posFailTime; + uint32_t hgtFailTime; + float states[32]; + unsigned n_states; + bool angNaN; + bool summedDelVelNaN; + bool KHNaN; + bool KHPNaN; + bool PNaN; + bool covarianceNaN; + bool kalmanGainsNaN; + bool statesNaN; + bool gyroOffsetsExcessive; + bool covariancesExcessive; + bool velOffsetExcessive; +}; + +void ekf_debug(const char *fmt, ...);
\ No newline at end of file diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 6fefec2c2..dc5220bf0 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -39,4 +39,5 @@ MODULE_COMMAND = ekf_att_pos_estimator SRCS = ekf_att_pos_estimator_main.cpp \ ekf_att_pos_estimator_params.c \ - estimator.cpp + estimator_23states.cpp \ + estimator_utilities.cpp diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 0e065211e..000c02e3d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -43,8 +43,8 @@ * Proceedings of the AIAA Guidance, Navigation and Control * Conference, Aug 2004. AIAA-2004-4900. * - * Original implementation for total energy control class: - * Paul Riseborough and Andrew Tridgell, 2013 (code in lib/external_lgpl) + * Implementation for total energy control class: + * Thomas Gubler * * More details and acknowledgements in the referenced library headers. * @@ -88,9 +88,9 @@ #include <mavlink/mavlink_log.h> #include <launchdetection/LaunchDetector.h> #include <ecl/l1/ecl_l1_pos_controller.h> -#include <external_lgpl/tecs/tecs.h> #include <drivers/drv_range_finder.h> #include "landingslope.h" +#include "mtecs/mTecs.h" /** @@ -153,8 +153,6 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - bool _setpoint_valid; /**< flag if the position control setpoint is valid */ - /** manual control states */ float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ double _loiter_hold_lat; @@ -200,7 +198,8 @@ private: math::Matrix<3, 3> _R_nb; ///< current attitude ECL_L1_Pos_Controller _l1_control; - TECS _tecs; + fwPosctrl::mTecs _mTecs; + bool _was_pos_control_mode; struct { float l1_period; @@ -343,11 +342,11 @@ private: /** * Control position. */ - bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed, + bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet); + void calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -368,6 +367,19 @@ private: * Reset landing state */ void reset_landing_state(); + + /* + * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter) + * XXX need to clean up/remove this function once mtecs fully replaces TECS + */ + void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + float altitude, + const math::Vector<3> &ground_speed, + tecs_mode mode = TECS_MODE_NORMAL); + }; namespace l1_control @@ -406,7 +418,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), /* states */ - _setpoint_valid(false), _loiter_hold(false), land_noreturn_horizontal(false), land_noreturn_vertical(false), @@ -431,6 +442,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos(), _pos_sp_triplet(), _sensor_combined(), + _mTecs(), + _was_pos_control_mode(false), _range_finder() { _nav_capabilities.turn_distance = 0.0f; @@ -550,23 +563,6 @@ FixedwingPositionControl::parameters_update() _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); - _tecs.set_time_const(_parameters.time_const); - _tecs.set_min_sink_rate(_parameters.min_sink_rate); - _tecs.set_max_sink_rate(_parameters.max_sink_rate); - _tecs.set_throttle_damp(_parameters.throttle_damp); - _tecs.set_integrator_gain(_parameters.integrator_gain); - _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); - _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); - _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); - _tecs.set_speed_weight(_parameters.speed_weight); - _tecs.set_pitch_damping(_parameters.pitch_damping); - _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); - _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); - _tecs.set_max_climb_rate(_parameters.max_climb_rate); - _tecs.set_heightrate_p(_parameters.heightrate_p); - _tecs.set_speedrate_p(_parameters.speedrate_p); - /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || _parameters.airspeed_max < 5.0f || @@ -589,6 +585,9 @@ FixedwingPositionControl::parameters_update() /* Update Launch Detector Parameters */ launchDetector.updateParams(); + /* Update the mTecs */ + _mTecs.updateParams(); + return OK; } @@ -635,9 +634,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() } } - /* update TECS state */ - _tecs.enable_airspeed(_airspeed_valid); - return airspeed_updated; } @@ -692,7 +688,6 @@ FixedwingPositionControl::vehicle_setpoint_poll() if (pos_sp_triplet_updated) { orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - _setpoint_valid = true; } } @@ -734,15 +729,15 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet) { - if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { + if (pos_sp_triplet.current.valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) { /* rotate ground speed vector with current attitude */ math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); - float ground_speed_body = yaw_vector * ground_speed; + float ground_speed_body = yaw_vector * ground_speed_2d; /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ float distance = 0.0f; @@ -801,12 +796,13 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, } bool -FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, +FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(current_position, ground_speed, pos_sp_triplet); + math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)}; + calculate_gndspeed_undershoot(current_position, ground_speed_2d, pos_sp_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -817,7 +813,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2); math::Vector<3> accel_earth = _R_nb * accel_body; - _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt; /* no throttle limit as default */ @@ -827,14 +822,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // XXX this should only execute if auto AND safety off (actuators active), // else integrators should be constantly reset. - if (_control_mode.flag_control_position_enabled) { + if (pos_sp_triplet.current.valid) { + + if (!_was_pos_control_mode) { + /* reset integrators */ + if (_mTecs.getEnabled()) { + _mTecs.resetIntegrators(); + _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } + } + + _was_pos_control_mode = true; /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); - /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ - _tecs.set_speed_weight(_parameters.speed_weight); - /* current waypoint (the one currently heading for) */ math::Vector<2> next_wp((float)pos_sp_triplet.current.lat, (float)pos_sp_triplet.current.lon); @@ -863,31 +865,29 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == SETPOINT_TYPE_NORMAL) { + if (pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER) { /* waypoint is a loiter waypoint */ _l1_control.navigate_loiter(curr_wp, current_position, pos_sp_triplet.current.loiter_radius, - pos_sp_triplet.current.loiter_direction, ground_speed); + pos_sp_triplet.current.loiter_direction, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { @@ -912,7 +912,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); - _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed_2d); /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); @@ -922,7 +922,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); } _att_sp.roll_body = _l1_control.nav_roll(); @@ -946,7 +946,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance); + float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); @@ -981,11 +981,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, flare_pitch_angle_rad, - 0.0f, throttle_max, throttle_land, - flare_pitch_angle_rad, math::radians(15.0f)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel, + calculate_target_airspeed(airspeed_land), eas2tas, + flare_pitch_angle_rad, math::radians(15.0f), + 0.0f, throttle_max, throttle_land, + false, flare_pitch_angle_rad, + _pos_sp_triplet.current.alt + relative_alt, ground_speed, + land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); if (!land_noreturn_vertical) { mavlink_log_info(_mavlink_fd, "#audio: Landing, flaring"); @@ -998,11 +1000,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* intersect glide slope: * minimize speed to approach speed - * if current position is higher or within 10m of slope follow the glide slope - * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * if current position is higher than the slope follow the glide slope (sink to the + * glide slope) + * also if the system captures the slope it should stay + * on the slope (bool land_onslope) + * if current position is below the slope continue at previous wp altitude + * until the intersection with slope * */ float altitude_desired_rel = relative_alt; - if (relative_alt > landing_slope_alt_rel_desired - 10.0f) { + if (relative_alt > landing_slope_alt_rel_desired || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { @@ -1011,14 +1017,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else { /* continue horizontally */ - altitude_desired_rel = math::max(relative_alt, L_altitude_rel); + altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, + calculate_target_airspeed(airspeed_approach), eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _pos_sp_triplet.current.alt + relative_alt, + ground_speed); } } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF) { @@ -1051,7 +1063,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } - _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -1062,22 +1074,36 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(1.3f * _parameters.airspeed_min), - _airspeed.indicated_airspeed_m_s, eas2tas, - true, math::max(math::radians(pos_sp_triplet.current.pitch_min), math::radians(10.0f)), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(1.3f * _parameters.airspeed_min), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, + _parameters.throttle_cruise, + true, + math::max(math::radians(pos_sp_triplet.current.pitch_min), + math::radians(10.0f)), + _global_pos.alt, + ground_speed, + TECS_MODE_TAKEOFF); /* limit roll motion to ensure enough lift */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f), math::radians(15.0f)); } else { - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), - _airspeed.indicated_airspeed_m_s, eas2tas, - false, math::radians(_parameters.pitch_limit_min), - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, + calculate_target_airspeed(_parameters.airspeed_trim), + eas2tas, + math::radians(_parameters.pitch_limit_min), + math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, + _parameters.throttle_max, + _parameters.throttle_cruise, + false, + math::radians(_parameters.pitch_limit_min), + _global_pos.alt, + ground_speed); } } else { @@ -1111,19 +1137,21 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (0/* posctrl mode enabled */) { + _was_pos_control_mode = false; + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to posctrl */) { - _altctrl_hold_heading = _att.yaw; - } + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; + } - if (0/* posctrl on and manual control yaw non-zero */) { - _altctrl_hold_heading = _att.yaw + _manual.r; - } + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.r; + } - //XXX not used + //XXX not used - /* climb out control */ + /* climb out control */ // bool climb_out = false; // // /* user wants to climb out */ @@ -1131,25 +1159,26 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in altctrl mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ - // XXX check if ground speed undershoot should be applied here - float altctrl_airspeed = _parameters.airspeed_min + - (_parameters.airspeed_max - _parameters.airspeed_min) * - _manual.z; + // XXX check if ground speed undershoot should be applied here + float altctrl_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.z; - _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, - altctrl_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - false, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + + tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed); } else if (0/* altctrl mode enabled */) { + _was_pos_control_mode = false; + /** ALTCTRL FLIGHT **/ if (0/* switched from another mode to altctrl */) { @@ -1170,8 +1199,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* user switched off throttle */ if (_manual.z < 0.1f) { throttle_max = 0.0f; - /* switch to pure pitch based altitude control, give up speed */ - _tecs.set_speed_weight(0.0f); } /* climb out control */ @@ -1182,18 +1209,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d); _att_sp.roll_body = _manual.y; _att_sp.yaw_body = _manual.r; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f, - altctrl_airspeed, - _airspeed.indicated_airspeed_m_s, eas2tas, - climb_out, _parameters.pitch_limit_min, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - _parameters.pitch_limit_min, _parameters.pitch_limit_max); + tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + climb_out, math::radians(_parameters.pitch_limit_min), + _global_pos.alt, ground_speed); } else { + _was_pos_control_mode = false; + /** MANUAL FLIGHT **/ /* no flight mode applies, do not publish an attitude setpoint */ @@ -1210,9 +1238,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); } else { - _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); + _att_sp.thrust = math::min(_mTecs.getThrottleSetpoint(), throttle_max); } - _att_sp.pitch_body = _tecs.get_pitch_demand(); + _att_sp.pitch_body = _mTecs.getPitchSetpoint(); if (_control_mode.flag_control_position_enabled) { last_manual = false; @@ -1326,7 +1354,7 @@ FixedwingPositionControl::task_main() range_finder_poll(); // vehicle_baro_poll(); - math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); + math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); math::Vector<2> current_position((float)_global_pos.lat, (float)_global_pos.lon); /* @@ -1388,6 +1416,30 @@ void FixedwingPositionControl::reset_landing_state() land_onslope = false; } +void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, + float pitch_min_rad, float pitch_max_rad, + float throttle_min, float throttle_max, float throttle_cruise, + bool climbout_mode, float climbout_pitch_min_rad, + float altitude, + const math::Vector<3> &ground_speed, + tecs_mode mode) +{ + /* Using mtecs library: prepare arguments for mtecs call */ + float flightPathAngle = 0.0f; + float ground_speed_length = ground_speed.length(); + if (ground_speed_length > FLT_EPSILON) { + flightPathAngle = -asinf(ground_speed(2)/ground_speed_length); + } + fwPosctrl::LimitOverride limitOverride; + if (climbout_mode) { + limitOverride.enablePitchMinOverride(M_RAD_TO_DEG_F * climbout_pitch_min_rad); + } else { + limitOverride.disablePitchMinOverride(); + } + _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, + limitOverride); +} + int FixedwingPositionControl::start() { diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index cf419ec7e..af155fe08 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -39,4 +39,7 @@ MODULE_COMMAND = fw_pos_control_l1 SRCS = fw_pos_control_l1_main.cpp \ fw_pos_control_l1_params.c \ - landingslope.cpp + landingslope.cpp \ + mtecs/mTecs.cpp \ + mtecs/limitoverride.cpp \ + mtecs/mTecs_params.c diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp new file mode 100644 index 000000000..58795edb6 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.cpp @@ -0,0 +1,71 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@gmail.com> + * + * 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 limitoverride.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include "limitoverride.h" + +namespace fwPosctrl { + +bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle, + BlockOutputLimiter &outputLimiterPitch) +{ + bool ret = false; + + if (overrideThrottleMinEnabled) { + outputLimiterThrottle.setMin(overrideThrottleMin); + ret = true; + } + if (overrideThrottleMaxEnabled) { + outputLimiterThrottle.setMax(overrideThrottleMax); + ret = true; + } + if (overridePitchMinEnabled) { + outputLimiterPitch.setMin(overridePitchMin); + ret = true; + } + if (overridePitchMaxEnabled) { + outputLimiterPitch.setMax(overridePitchMax); + ret = true; + } + + return ret; +} + +} /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/limitoverride.h b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h new file mode 100644 index 000000000..64c2e7bbd --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/limitoverride.h @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@gmail.com> + * + * 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 limitoverride.h + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + + +#ifndef LIMITOVERRIDE_H_ +#define LIMITOVERRIDE_H_ + +#include "mTecs_blocks.h" + +namespace fwPosctrl +{ + +/* A small class which provides helper functions to override control output limits which are usually set by +* parameters in special cases +*/ +class LimitOverride +{ +public: + LimitOverride() : + overrideThrottleMinEnabled(false), + overrideThrottleMaxEnabled(false), + overridePitchMinEnabled(false), + overridePitchMaxEnabled(false) + {}; + + ~LimitOverride() {}; + + /* + * Override the limits of the outputlimiter instances given by the arguments with the limits saved in + * this class (if enabled) + * @return true if the limit was applied + */ + bool applyOverride(BlockOutputLimiter &outputLimiterThrottle, + BlockOutputLimiter &outputLimiterPitch); + + /* Functions to enable or disable the override */ + void enableThrottleMinOverride(float value) { enable(&overrideThrottleMinEnabled, + &overrideThrottleMin, value); } + void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); } + void enableThrottleMaxOverride(float value) { enable(&overrideThrottleMaxEnabled, + &overrideThrottleMax, value); } + void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); } + void enablePitchMinOverride(float value) { enable(&overridePitchMinEnabled, + &overridePitchMin, value); } + void disablePitchMinOverride() { disable(&overridePitchMinEnabled); } + void enablePitchMaxOverride(float value) { enable(&overridePitchMaxEnabled, + &overridePitchMax, value); } + void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); } + +protected: + bool overrideThrottleMinEnabled; + float overrideThrottleMin; + bool overrideThrottleMaxEnabled; + float overrideThrottleMax; + bool overridePitchMinEnabled; + float overridePitchMin; //in degrees (replaces param values) + bool overridePitchMaxEnabled; + float overridePitchMax; //in degrees (replaces param values) + + /* Enable a specific limit override */ + void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; }; + + /* Disable a specific limit override */ + void disable(bool *flag) { *flag = false; }; +}; + +} /* namespace fwPosctrl */ + +#endif /* LIMITOVERRIDE_H_ */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp new file mode 100644 index 000000000..fc0a2551c --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -0,0 +1,313 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@gmail.com> + * + * 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 mTecs.cpp + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include "mTecs.h" + +#include <lib/geo/geo.h> +#include <stdio.h> + +namespace fwPosctrl { + +mTecs::mTecs() : + SuperBlock(NULL, "MT"), + /* Parameters */ + _mTecsEnabled(this, "ENABLED"), + _airspeedMin(this, "FW_AIRSPD_MIN", false), + /* Publications */ + _status(&getPublications(), ORB_ID(tecs_status)), + /* control blocks */ + _controlTotalEnergy(this, "THR"), + _controlEnergyDistribution(this, "PIT", true), + _controlAltitude(this, "FPA", true), + _controlAirSpeed(this, "ACC"), + _flightPathAngleLowpass(this, "FPA_LP"), + _airspeedLowpass(this, "A_LP"), + _airspeedDerivative(this, "AD"), + _throttleSp(0.0f), + _pitchSp(0.0f), + _BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"), + _BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true), + _BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"), + _BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true), + _BlockOutputLimiterLandThrottle(this, "LND_THR"), + _BlockOutputLimiterLandPitch(this, "LND_PIT", true), + timestampLastIteration(hrt_absolute_time()), + _firstIterationAfterReset(true), + _dtCalculated(false), + _counter(0), + _debug(false) +{ +} + +mTecs::~mTecs() +{ +} + +int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride) +{ + /* check if all input arguments are numbers and abort if not so */ + if (!isfinite(flightPathAngle) || !isfinite(altitude) || + !isfinite(altitudeSp) || !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + return -1; + } + + /* time measurement */ + updateTimeMeasurement(); + + /* calculate flight path angle setpoint from altitude setpoint */ + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); + + /* Debug output */ + if (_counter % 10 == 0) { + debug("***"); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + } + + /* Write part of the status message */ + _status.altitudeSp = altitudeSp; + _status.altitude = altitude; + + + /* use flightpath angle setpoint for total energy control */ + return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, + airspeedSp, mode, limitOverride); +} + +int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride) +{ + /* check if all input arguments are numbers and abort if not so */ + if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || + !isfinite(airspeed) || !isfinite(airspeedSp) || !isfinite(mode)) { + return -1; + } + + /* time measurement */ + updateTimeMeasurement(); + + /* Filter airspeed */ + float airspeedFiltered = _airspeedLowpass.update(airspeed); + + /* calculate longitudinal acceleration setpoint from airspeed setpoint*/ + float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered); + + /* Debug output */ + if (_counter % 10 == 0) { + debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f," + "accelerationLongitudinalSp%.4f", + (double)airspeedSp, (double)airspeed, + (double)airspeedFiltered, (double)accelerationLongitudinalSp); + } + + /* Write part of the status message */ + _status.airspeedSp = airspeedSp; + _status.airspeed = airspeed; + _status.airspeedFiltered = airspeedFiltered; + + + /* use longitudinal acceleration setpoint for total energy control */ + return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered, + accelerationLongitudinalSp, mode, limitOverride); +} + +int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, + float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride) +{ + /* check if all input arguments are numbers and abort if not so */ + if (!isfinite(flightPathAngle) || !isfinite(flightPathAngleSp) || + !isfinite(airspeedFiltered) || !isfinite(accelerationLongitudinalSp) || !isfinite(mode)) { + return -1; + } + /* time measurement */ + updateTimeMeasurement(); + + /* update parameters first */ + updateParams(); + + /* Filter flightpathangle */ + float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle); + + /* calculate values (energies) */ + float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered; + float airspeedDerivative = 0.0f; + if(_airspeedDerivative.getDt() > 0.0f) { + airspeedDerivative = _airspeedDerivative.update(airspeedFiltered); + } + float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G; + float airspeedDerivativeSp = accelerationLongitudinalSp; + float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G; + float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm; + + float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm; + float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError; + float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp; + float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate; + + float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm; + float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError; + float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp; + float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; + + /* Debug output */ + if (_counter % 10 == 0) { + debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", + (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); + debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", + (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + } + + /* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */ + if (mode != TECS_MODE_LAND && mode != TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) { + mode = TECS_MODE_UNDERSPEED; + } + + /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */ + BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter(); + BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter(); + if (mode == TECS_MODE_TAKEOFF) { + outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle; + outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch; + } else if (mode == TECS_MODE_LAND) { + // only limit pitch but do not limit throttle + outputLimiterPitch = &_BlockOutputLimiterLandPitch; + } else if (mode == TECS_MODE_LAND_THROTTLELIM) { + outputLimiterThrottle = &_BlockOutputLimiterLandThrottle; + outputLimiterPitch = &_BlockOutputLimiterLandPitch; + } else if (mode == TECS_MODE_UNDERSPEED) { + outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle; + outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch; + } + + /* Apply overrride given by the limitOverride argument (this is used for limits which are not given by + * parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector + * is running) */ + bool limitApplied = limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch); + + /* Write part of the status message */ + _status.flightPathAngleSp = flightPathAngleSp; + _status.flightPathAngle = flightPathAngle; + _status.flightPathAngleFiltered = flightPathAngleFiltered; + _status.airspeedDerivativeSp = airspeedDerivativeSp; + _status.airspeedDerivative = airspeedDerivative; + _status.totalEnergyRateSp = totalEnergyRateSp; + _status.totalEnergyRate = totalEnergyRate; + _status.energyDistributionRateSp = energyDistributionRateSp; + _status.energyDistributionRate = energyDistributionRate; + _status.mode = mode; + + /** update control blocks **/ + /* update total energy rate control block */ + _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle); + + /* update energy distribution rate control block */ + _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch); + + + if (_counter % 10 == 0) { + debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", + (double)_throttleSp, (double)_pitchSp, + (double)flightPathAngleSp, (double)flightPathAngle, + (double)accelerationLongitudinalSp, (double)airspeedDerivative); + } + + /* publish status messge */ + _status.update(); + + /* clean up */ + _firstIterationAfterReset = false; + _dtCalculated = false; + + _counter++; + + return 0; +} + +void mTecs::resetIntegrators() +{ + _controlTotalEnergy.getIntegral().setY(0.0f); + _controlEnergyDistribution.getIntegral().setY(0.0f); + timestampLastIteration = hrt_absolute_time(); + _firstIterationAfterReset = true; +} + +void mTecs::resetDerivatives(float airspeed) +{ + _airspeedDerivative.setU(airspeed); +} + + +void mTecs::updateTimeMeasurement() +{ + if (!_dtCalculated) { + float deltaTSeconds = 0.0f; + if (!_firstIterationAfterReset) { + hrt_abstime timestampNow = hrt_absolute_time(); + deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f; + timestampLastIteration = timestampNow; + } + setDt(deltaTSeconds); + + _dtCalculated = true; + } +} + +void mTecs::debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[mtecs]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +void mTecs::debug(const char *fmt, ...) { + + if (!_debug) { + return; + } + + va_list args; + + va_start(args, fmt); + debug_print(fmt, args); +} + +} /* namespace fwPosctrl */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h new file mode 100644 index 000000000..efa89a5d3 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -0,0 +1,155 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@gmail.com> + * + * 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 mTecs.h + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + + +#ifndef MTECS_H_ +#define MTECS_H_ + +#include "mTecs_blocks.h" +#include "limitoverride.h" + +#include <controllib/block/BlockParam.hpp> +#include <drivers/drv_hrt.h> +#include <uORB/Publication.hpp> +#include <uORB/topics/tecs_status.h> + +namespace fwPosctrl +{ + +/* Main class of the mTecs */ +class mTecs : public control::SuperBlock +{ +public: + mTecs(); + virtual ~mTecs(); + + /* + * Control in altitude setpoint and speed mode + */ + int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); + + /* + * Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode + */ + int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, + float airspeedSp, tecs_mode mode, LimitOverride limitOverride); + + /* + * Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case) + */ + int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered, + float accelerationLongitudinalSp, tecs_mode mode, LimitOverride limitOverride); + + /* + * Reset all integrators + */ + void resetIntegrators(); + + /* + * Reset all derivative calculations + */ + void resetDerivatives(float airspeed); + + /* Accessors */ + bool getEnabled() { return _mTecsEnabled.get() > 0; } + float getThrottleSetpoint() { return _throttleSp; } + float getPitchSetpoint() { return _pitchSp; } + float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); } + +protected: + /* parameters */ + control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */ + control::BlockParamFloat _airspeedMin; /**< minimal airspeed */ + + /* Publications */ + uORB::Publication<tecs_status_s> _status; /**< publish internal values for logging */ + + /* control blocks */ + BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output + is throttle */ + BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: + output is pitch */ + BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight + path angle setpoint */ + BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration + setpoint */ + + /* Other calculation Blocks */ + control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */ + control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */ + control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */ + + /* Output setpoints */ + float _throttleSp; /**< Throttle Setpoint from 0 to 1 */ + float _pitchSp; /**< Pitch Setpoint from -pi to pi */ + + /* Output Limits in special modes */ + BlockOutputLimiter _BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */ + BlockOutputLimiter _BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */ + BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in + last phase)*/ + BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */ + + /* Time measurements */ + hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */ + + bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + bool _dtCalculated; /**< True if dt has been calculated in this iteration */ + + int _counter; + bool _debug; ///< Set true to enable debug output + + + static void debug_print(const char *fmt, va_list args); + void debug(const char *fmt, ...); + + /* + * Measure and update the time step dt if this was not already done in the current iteration + */ + void updateTimeMeasurement(); +}; + +} /* namespace fwPosctrl */ + +#endif /* MTECS_H_ */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h new file mode 100644 index 000000000..e4e405227 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -0,0 +1,220 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@gmail.com> + * + * 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 mTecs_blocks.h + * + * Custom blocks for the mTecs + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#pragma once + +#include <controllib/blocks.hpp> +#include <systemlib/err.h> + +namespace fwPosctrl +{ + +using namespace control; + +/* An block which can be used to limit the output */ +class BlockOutputLimiter: public SuperBlock +{ +public: +// methods + BlockOutputLimiter(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _isAngularLimit(isAngularLimit), + _min(this, "MIN"), + _max(this, "MAX") + {}; + virtual ~BlockOutputLimiter() {}; + /* + * Imposes the limits given by _min and _max on value + * + * @param value is changed to be on the interval _min to _max + * @param difference if the value is changed this corresponds to the change of value * (-1) + * otherwise unchanged + * @return: true if the limit is applied, false otherwise + */ + bool limit(float& value, float& difference) { + float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); + float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); + if (value < minimum) { + difference = value - minimum; + value = minimum; + return true; + } else if (value > maximum) { + difference = value - maximum; + value = maximum; + return true; + } + return false; + } +//accessor: + bool isAngularLimit() {return _isAngularLimit ;} + float getMin() { return _min.get(); } + float getMax() { return _max.get(); } + void setMin(float value) { _min.set(value); } + void setMax(float value) { _max.set(value); } +protected: +//attributes + bool _isAngularLimit; + control::BlockParamFloat _min; + control::BlockParamFloat _max; +}; + + +/* A combination of feed forward, P and I gain using the output limiter*/ +class BlockFFPILimited: public SuperBlock +{ +public: +// methods + BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _outputLimiter(this, "", isAngularLimit), + _integral(this, "I"), + _kFF(this, "FF"), + _kP(this, "P"), + _kI(this, "I"), + _offset(this, "OFF") + {}; + virtual ~BlockFFPILimited() {}; + float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); } +// accessors + BlockIntegral &getIntegral() { return _integral; } + float getKFF() { return _kFF.get(); } + float getKP() { return _kP.get(); } + float getKI() { return _kI.get(); } + float getOffset() { return _offset.get(); } + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +protected: + BlockOutputLimiter _outputLimiter; + + float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);} + float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) { + float difference = 0.0f; + float integralYPrevious = _integral.getY(); + float output = calcUnlimitedOutput(inputValue, inputError); + if(outputLimiter.limit(output, difference) && + (((difference < 0) && (getKI() * getIntegral().getY() < 0)) || + ((difference > 0) && (getKI() * getIntegral().getY() > 0)))) { + getIntegral().setY(integralYPrevious); + } + return output; + } +private: + BlockIntegral _integral; + BlockParamFloat _kFF; + BlockParamFloat _kP; + BlockParamFloat _kI; + BlockParamFloat _offset; +}; + +/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/ +class BlockFFPILimitedCustom: public BlockFFPILimited +{ +public: +// methods + BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + BlockFFPILimited(parent, name, isAngularLimit) + {}; + virtual ~BlockFFPILimitedCustom() {}; + float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) { + return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter); + } +}; + +/* A combination of P gain and output limiter */ +class BlockPLimited: public SuperBlock +{ +public: +// methods + BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _kP(this, "P"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockPLimited() {}; + float update(float input) { + float difference = 0.0f; + float output = getKP() * input; + getOutputLimiter().limit(output, difference); + return output; + } +// accessors + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; + float getKP() { return _kP.get(); } +private: + control::BlockParamFloat _kP; + BlockOutputLimiter _outputLimiter; +}; + +/* A combination of P, D gains and output limiter */ +class BlockPDLimited: public SuperBlock +{ +public: +// methods + BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) : + SuperBlock(parent, name), + _kP(this, "P"), + _kD(this, "D"), + _derivative(this, "D"), + _outputLimiter(this, "", isAngularLimit) + {}; + virtual ~BlockPDLimited() {}; + float update(float input) { + float difference = 0.0f; + float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f); + getOutputLimiter().limit(output, difference); + + return output; + } +// accessors + float getKP() { return _kP.get(); } + float getKD() { return _kD.get(); } + BlockDerivative &getDerivative() { return _derivative; } + BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; }; +private: + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; + BlockDerivative _derivative; + BlockOutputLimiter _outputLimiter; +}; + +} + diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c new file mode 100644 index 000000000..5b9238780 --- /dev/null +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -0,0 +1,419 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: @author Thomas Gubler <thomasgubler@gmail.com> + * + * 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 mTecs_params.c + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +/* + * Controller parameters, accessible via MAVLink + */ + +/** + * mTECS enabled + * + * Set to 1 to enable mTECS + * + * @min 0 + * @max 1 + * @group mTECS + */ +PARAM_DEFINE_INT32(MT_ENABLED, 1); + +/** + * Total Energy Rate Control Feedforward + * Maps the total energy rate setpoint to the throttle setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_FF, 0.7f); + +/** + * Total Energy Rate Control P + * Maps the total energy rate error to the throttle setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f); + +/** + * Total Energy Rate Control I + * Maps the integrated total energy rate to the throttle setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_I, 0.25f); + +/** + * Total Energy Rate Control Offset (Cruise throttle sp) + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f); + +/** + * Energy Distribution Rate Control Feedforward + * Maps the energy distribution rate setpoint to the pitch setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.4f); + +/** + * Energy Distribution Rate Control P + * Maps the energy distribution rate error to the pitch setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f); + +/** + * Energy Distribution Rate Control I + * Maps the integrated energy distribution rate error to the pitch setpoint + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f); + + +/** + * Total Energy Distribution Offset (Cruise pitch sp) + * + * @min 0.0 + * @max 10.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f); + +/** + * Minimal Throttle Setpoint + * + * @min 0.0 + * @max 1.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f); + +/** + * Maximal Throttle Setpoint + * + * @min 0.0 + * @max 1.0 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f); + +/** + * Minimal Pitch Setpoint in Degrees + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f); + +/** + * Maximal Pitch Setpoint in Degrees + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f); + +/** + * Lowpass (cutoff freq.) for the flight path angle + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f); + +/** + * P gain for the altitude control + * Maps the altitude error to the flight path angle setpoint + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f); + +/** + * D gain for the altitude control + * Maps the change of altitude error to the flight path angle setpoint + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f); + +/** + * Lowpass for FPA error derivative calculation (see MT_FPA_D) + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f); + + +/** + * Minimal flight path angle setpoint + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f); + +/** + * Maximal flight path angle setpoint + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f); + +/** + * Lowpass (cutoff freq.) for airspeed + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_A_LP, 1.0f); + +/** + * P gain for the airspeed control + * Maps the airspeed error to the acceleration setpoint + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f); + +/** + * D gain for the airspeed control + * Maps the change of airspeed error to the acceleration setpoint + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f); + +/** + * Lowpass for ACC error derivative calculation (see MT_ACC_D) + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 1.0f); + +/** + * Minimal acceleration (air) + * + * @unit m/s^2 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); + +/** + * Maximal acceleration (air) + * +* @unit m/s^2 + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f); + +/** + * Airspeed derivative calculation lowpass + * + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f); + +/** + * Minimal throttle during takeoff + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f); + +/** + * Maximal throttle during takeoff + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f); + +/** + * Minimal pitch during takeoff + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f); + +/** + * Maximal pitch during takeoff + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f); + +/** + * Minimal throttle in underspeed mode + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f); + +/** + * Maximal throttle in underspeed mode + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f); + +/** + * Minimal pitch in underspeed mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f); + +/** + * Maximal pitch in underspeed mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f); + +/** + * Minimal throttle in landing mode (only last phase of landing) + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f); + +/** + * Maximal throttle in landing mode (only last phase of landing) + * + * @min 0.0f + * @max 1.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f); + +/** + * Minimal pitch in landing mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f); + +/** + * Maximal pitch in landing mode + * + * @min -90.0f + * @max 90.0f + * @unit deg + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f); + +/** + * Integrator Limit for Total Energy Rate Control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f); + +/** + * Integrator Limit for Energy Distribution Rate Control + * + * @min 0.0f + * @max 10.0f + * @group mTECS + */ +PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 6dfd22fdf..7758faed7 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -181,16 +181,13 @@ int gpio_led_main(int argc, char *argv[]) } else { gpio_led_started = true; warnx("start, using pin: %s", pin_name); + exit(0); } - - exit(0); - - } else if (!strcmp(argv[1], "stop")) { if (gpio_led_started) { gpio_led_started = false; warnx("stop"); - + exit(0); } else { errx(1, "not running"); } @@ -264,7 +261,7 @@ void gpio_led_cycle(FAR void *arg) pattern = 0x2A; // *_*_*_ fast blink (armed, error) } else if (priv->status.arming_state == ARMING_STATE_ARMED) { - if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && priv->status.failsafe_state == FAILSAFE_STATE_NORMAL) { + if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) { pattern = 0x3f; // ****** solid (armed) } else { diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp new file mode 100644 index 000000000..ca846a465 --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -0,0 +1,415 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +#include <crc32.h> +#include <unistd.h> +#include <stdio.h> +#include <fcntl.h> + +#include "mavlink_ftp.h" + +MavlinkFTP *MavlinkFTP::_server; + +MavlinkFTP * +MavlinkFTP::getServer() +{ + // XXX this really cries out for some locking... + if (_server == nullptr) { + _server = new MavlinkFTP; + } + return _server; +} + +MavlinkFTP::MavlinkFTP() +{ + // initialise the request freelist + dq_init(&_workFree); + sem_init(&_lock, 0, 1); + + // initialize session list + for (size_t i=0; i<kMaxSession; i++) { + _session_fds[i] = -1; + } + + // drop work entries onto the free list + for (unsigned i = 0; i < kRequestQueueSize; i++) { + _qFree(&_workBufs[i]); + } + +} + +void +MavlinkFTP::handle_message(Mavlink* mavlink, mavlink_message_t *msg) +{ + // get a free request + auto req = _dqFree(); + + // if we couldn't get a request slot, just drop it + if (req != nullptr) { + + // decode the request + if (req->decode(mavlink, msg)) { + + // and queue it for the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); + } else { + _qFree(req); + } + } +} + +void +MavlinkFTP::_workerTrampoline(void *arg) +{ + auto req = reinterpret_cast<Request *>(arg); + auto server = MavlinkFTP::getServer(); + + // call the server worker with the work item + server->_worker(req); +} + +void +MavlinkFTP::_worker(Request *req) +{ + auto hdr = req->header(); + ErrorCode errorCode = kErrNone; + uint32_t messageCRC; + + // basic sanity checks; must validate length before use + if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) { + errorCode = kErrNoRequest; + goto out; + } + + // check request CRC to make sure this is one of ours + messageCRC = hdr->crc32; + hdr->crc32 = 0; + if (crc32(req->rawData(), req->dataSize()) != messageCRC) { + errorCode = kErrNoRequest; + goto out; + printf("ftp: bad crc\n"); + } + + printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + + switch (hdr->opcode) { + case kCmdNone: + break; + + case kCmdTerminate: + errorCode = _workTerminate(req); + break; + + case kCmdReset: + errorCode = _workReset(); + break; + + case kCmdList: + errorCode = _workList(req); + break; + + case kCmdOpen: + errorCode = _workOpen(req, false); + break; + + case kCmdCreate: + errorCode = _workOpen(req, true); + break; + + case kCmdRead: + errorCode = _workRead(req); + break; + + case kCmdWrite: + errorCode = _workWrite(req); + break; + + case kCmdRemove: + errorCode = _workRemove(req); + break; + + default: + errorCode = kErrNoRequest; + break; + } + +out: + // handle success vs. error + if (errorCode == kErrNone) { + hdr->opcode = kRspAck; + printf("FTP: ack\n"); + } else { + printf("FTP: nak %u\n", errorCode); + hdr->opcode = kRspNak; + hdr->size = 1; + hdr->data[0] = errorCode; + } + + // respond to the request + _reply(req); + + // free the request buffer back to the freelist + _qFree(req); +} + +void +MavlinkFTP::_reply(Request *req) +{ + auto hdr = req->header(); + + // message is assumed to be already constructed in the request buffer, so generate the CRC + hdr->crc32 = 0; + hdr->crc32 = crc32(req->rawData(), req->dataSize()); + + // then pack and send the reply back to the request source + req->reply(); +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workList(Request *req) +{ + auto hdr = req->header(); + DIR *dp = opendir(req->dataAsCString()); + + if (dp == nullptr) { + printf("FTP: can't open path '%s'\n", req->dataAsCString()); + return kErrNotDir; + } + + ErrorCode errorCode = kErrNone; + struct dirent entry, *result = nullptr; + unsigned offset = 0; + + // move to the requested offset + seekdir(dp, hdr->offset); + + for (;;) { + // read the directory entry + if (readdir_r(dp, &entry, &result)) { + errorCode = kErrIO; + break; + } + + // no more entries? + if (result == nullptr) { + if (hdr->offset != 0 && offset == 0) { + // User is requesting subsequent dir entries but there were none. This means the user asked + // to seek past EOF. + errorCode = kErrEOF; + } + // Otherwise we are just at the last directory entry, so we leave the errorCode at kErrorNone to signal that + break; + } + + // name too big to fit? + if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) { + break; + } + + // store the type marker + switch (entry.d_type) { + case DTYPE_FILE: + hdr->data[offset++] = kDirentFile; + break; + case DTYPE_DIRECTORY: + hdr->data[offset++] = kDirentDir; + break; + default: + hdr->data[offset++] = kDirentUnknown; + break; + } + + // copy the name, which we know will fit + strcpy((char *)&hdr->data[offset], entry.d_name); + offset += strlen(entry.d_name) + 1; + printf("FTP: list %s\n", entry.d_name); + } + + closedir(dp); + hdr->size = offset; + + return errorCode; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workOpen(Request *req, bool create) +{ + auto hdr = req->header(); + + int session_index = _findUnusedSession(); + if (session_index < 0) { + return kErrNoSession; + } + + int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; + + int fd = ::open(req->dataAsCString(), oflag); + if (fd < 0) { + return create ? kErrPerm : kErrNotFile; + } + _session_fds[session_index] = fd; + + hdr->session = session_index; + hdr->size = 0; + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workRead(Request *req) +{ + auto hdr = req->header(); + + int session_index = hdr->session; + + if (!_validSession(session_index)) { + return kErrNoSession; + } + + // Seek to the specified position + printf("Seek %d\n", hdr->offset); + if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + return kErrEOF; + } + + int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); + if (bytes_read < 0) { + // Negative return indicates error other than eof + return kErrIO; + } + + printf("Read success %d\n", bytes_read); + hdr->size = bytes_read; + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workWrite(Request *req) +{ +#if 0 + // NYI: Coming soon + auto hdr = req->header(); + + // look up session + auto session = getSession(hdr->session); + if (session == nullptr) { + return kErrNoSession; + } + + // append to file + int result = session->append(hdr->offset, &hdr->data[0], hdr->size); + + if (result < 0) { + // XXX might also be no space, I/O, etc. + return kErrNotAppend; + } + + hdr->size = result; + return kErrNone; +#else + return kErrPerm; +#endif +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemove(Request *req) +{ + auto hdr = req->header(); + + // for now, send error reply + return kErrPerm; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workTerminate(Request *req) +{ + auto hdr = req->header(); + + if (!_validSession(hdr->session)) { + return kErrNoSession; + } + + ::close(_session_fds[hdr->session]); + + return kErrNone; +} + +MavlinkFTP::ErrorCode +MavlinkFTP::_workReset(void) +{ + for (size_t i=0; i<kMaxSession; i++) { + if (_session_fds[i] != -1) { + ::close(_session_fds[i]); + _session_fds[i] = -1; + } + } + + return kErrNone; +} + +bool +MavlinkFTP::_validSession(unsigned index) +{ + if ((index >= kMaxSession) || (_session_fds[index] < 0)) { + return false; + } + return true; +} + +int +MavlinkFTP::_findUnusedSession(void) +{ + for (size_t i=0; i<kMaxSession; i++) { + if (_session_fds[i] == -1) { + return i; + } + } + + return -1; +} + +char * +MavlinkFTP::Request::dataAsCString() +{ + // guarantee nul termination + if (header()->size < kMaxDataLength) { + requestData()[header()->size] = '\0'; + } else { + requestData()[kMaxDataLength - 1] = '\0'; + } + + // and return data + return (char *)&(header()->data[0]); +} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h new file mode 100644 index 000000000..e22e61553 --- /dev/null +++ b/src/modules/mavlink/mavlink_ftp.h @@ -0,0 +1,226 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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. + * + ****************************************************************************/ + +#pragma once + +/** + * @file mavlink_ftp.h + * + * MAVLink remote file server. + * + * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes + * a session ID and sequence number. + * + * A limited number of requests (currently 2) may be outstanding at a time. + * Additional messages will be discarded. + * + * Messages consist of a fixed header, followed by a data area. + * + */ + +#include <dirent.h> +#include <queue.h> + +#include <nuttx/wqueue.h> +#include <systemlib/err.h> + +#include "mavlink_messages.h" + +class MavlinkFTP +{ +public: + MavlinkFTP(); + + static MavlinkFTP *getServer(); + + // static interface + void handle_message(Mavlink* mavlink, + mavlink_message_t *msg); + +private: + + static const unsigned kRequestQueueSize = 2; + + static MavlinkFTP *_server; + + struct RequestHeader + { + uint8_t magic; + uint8_t session; + uint8_t opcode; + uint8_t size; + uint32_t crc32; + uint32_t offset; + uint8_t data[]; + }; + + enum Opcode : uint8_t + { + kCmdNone, // ignored, always acked + kCmdTerminate, // releases sessionID, closes file + kCmdReset, // terminates all sessions + kCmdList, // list files in <path> from <offset> + kCmdOpen, // opens <path> for reading, returns <session> + kCmdRead, // reads <size> bytes from <offset> in <session> + kCmdCreate, // creates <path> for writing, returns <session> + kCmdWrite, // appends <size> bytes at <offset> in <session> + kCmdRemove, // remove file (only if created by server?) + + kRspAck, + kRspNak + }; + + enum ErrorCode : uint8_t + { + kErrNone, + kErrNoRequest, + kErrNoSession, + kErrSequence, + kErrNotDir, + kErrNotFile, + kErrEOF, + kErrNotAppend, + kErrTooBig, + kErrIO, + kErrPerm + }; + + int _findUnusedSession(void); + bool _validSession(unsigned index); + + static const unsigned kMaxSession = 2; + int _session_fds[kMaxSession]; + + class Request + { + public: + union { + dq_entry_t entry; + work_s work; + }; + + bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { + if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + _mavlink = mavlink; + mavlink_msg_encapsulated_data_decode(fromMessage, &_message); + return true; + } + return false; + } + + void reply() { + + // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the + // flat memory architecture, as we're operating between threads here. + mavlink_message_t msg; + msg.checksum = 0; + unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), + _mavlink->get_channel(), &msg, sequence(), rawData()); + + _mavlink->lockMessageBufferMutex(); + bool fError = _mavlink->message_buffer_write(&msg, len); + _mavlink->unlockMessageBufferMutex(); + + if (!fError) { + warnx("FTP TX ERR"); + } else { + warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", + _mavlink->get_system_id(), + _mavlink->get_component_id(), + _mavlink->get_channel(), + len, + msg.checksum); + } + } + + uint8_t *rawData() { return &_message.data[0]; } + RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); } + uint8_t *requestData() { return &(header()->data[0]); } + unsigned dataSize() { return header()->size + sizeof(RequestHeader); } + uint16_t sequence() const { return _message.seqnr; } + mavlink_channel_t channel() { return _mavlink->get_channel(); } + + char *dataAsCString(); + + private: + Mavlink *_mavlink; + mavlink_encapsulated_data_t _message; + + }; + + static const uint8_t kProtocolMagic = 'f'; + static const char kDirentFile = 'F'; + static const char kDirentDir = 'D'; + static const char kDirentUnknown = 'U'; + static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader); + + /// Request worker; runs on the low-priority work queue to service + /// remote requests. + /// + static void _workerTrampoline(void *arg); + void _worker(Request *req); + + /// Reply to a request (XXX should be a Request method) + /// + void _reply(Request *req); + + ErrorCode _workList(Request *req); + ErrorCode _workOpen(Request *req, bool create); + ErrorCode _workRead(Request *req); + ErrorCode _workWrite(Request *req); + ErrorCode _workRemove(Request *req); + ErrorCode _workTerminate(Request *req); + ErrorCode _workReset(); + + // work freelist + Request _workBufs[kRequestQueueSize]; + dq_queue_t _workFree; + sem_t _lock; + + void _qLock() { do {} while (sem_wait(&_lock) != 0); } + void _qUnlock() { sem_post(&_lock); } + + void _qFree(Request *req) { + _qLock(); + dq_addlast(&req->entry, &_workFree); + _qUnlock(); + } + + Request *_dqFree() { + _qLock(); + auto req = reinterpret_cast<Request *>(dq_remfirst(&_workFree)); + _qUnlock(); + return req; + } + +}; diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a9b8323f3..9a5e31ef4 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -83,6 +83,10 @@ #include "mavlink_rate_limiter.h" #include "mavlink_commands.h" +#ifndef MAVLINK_CRC_EXTRA + #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems +#endif + /* oddly, ERROR is not defined for c++ */ #ifdef ERROR # undef ERROR @@ -114,6 +118,7 @@ static uint64_t last_write_try_times[6] = {0}; void mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length) { + Mavlink *instance; switch (channel) { @@ -192,13 +197,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (buf_free < desired) { /* we don't want to send anything just in half, so return */ + instance->count_txerr(); return; } } ssize_t ret = write(uart, ch, desired); if (ret != desired) { - warnx("TX FAIL"); + instance->count_txerr(); } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -230,6 +236,7 @@ Mavlink::Mavlink() : _verbose(false), _forwarding_on(false), _passing_on(false), + _ftp_on(false), _uart_fd(-1), _mavlink_param_queue_index(0), _subscribe_to_stream(nullptr), @@ -243,7 +250,8 @@ Mavlink::Mavlink() : _param_use_hil_gps(0), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { _wpm = &_wpm_s; mission.count = 0; @@ -296,6 +304,7 @@ Mavlink::Mavlink() : Mavlink::~Mavlink() { perf_free(_loop_perf); + perf_free(_txerr_perf); if (_task_running) { /* task wakes up every 10ms or so at the longest */ @@ -321,6 +330,12 @@ Mavlink::~Mavlink() } void +Mavlink::count_txerr() +{ + perf_count(_txerr_perf); +} + +void Mavlink::set_mode(enum MAVLINK_MODE mode) { _mode = mode; @@ -459,7 +474,7 @@ Mavlink::get_instance_id() return _instance_id; } -mavlink_channel_t +const mavlink_channel_t Mavlink::get_channel() { return _channel; @@ -537,6 +552,16 @@ void Mavlink::mavlink_update_system(void) _use_hil_gps = (bool)use_hil_gps; } +int Mavlink::get_system_id() +{ + return mavlink_system.sysid; +} + +int Mavlink::get_component_id() +{ + return mavlink_system.compid; +} + int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_config_original, bool *is_usb) { /* process baud rate */ @@ -899,7 +924,11 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item case MAV_CMD_NAV_TAKEOFF: mission_item->pitch_min = mavlink_mission_item->param1; break; - + case MAV_CMD_DO_JUMP: + mission_item->do_jump_mission_index = mavlink_mission_item->param1; + mission_item->do_jump_current_count = 0; + mission_item->do_jump_repeat_count = mavlink_mission_item->param2; + break; default: mission_item->acceptance_radius = mavlink_mission_item->param2; mission_item->time_inside = mavlink_mission_item->param1; @@ -915,6 +944,9 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item // mission_item->index = mavlink_mission_item->seq; mission_item->origin = ORIGIN_MAVLINK; + /* reset DO_JUMP count */ + mission_item->do_jump_current_count = 0; + return OK; } @@ -932,6 +964,11 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_ mavlink_mission_item->param1 = mission_item->pitch_min; break; + case NAV_CMD_DO_JUMP: + mavlink_mission_item->param1 = mission_item->do_jump_mission_index; + mavlink_mission_item->param2 = mission_item->do_jump_repeat_count; + break; + default: mavlink_mission_item->param2 = mission_item->acceptance_radius; mavlink_mission_item->param1 = mission_item->time_inside; @@ -1619,11 +1656,21 @@ Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate) int Mavlink::message_buffer_init(int size) { + _message_buffer.size = size; _message_buffer.write_ptr = 0; _message_buffer.read_ptr = 0; _message_buffer.data = (char*)malloc(_message_buffer.size); - return (_message_buffer.data == 0) ? ERROR : OK; + + int ret; + if (_message_buffer.data == 0) { + ret = ERROR; + _message_buffer.size = 0; + } else { + ret = OK; + } + + return ret; } void @@ -1751,7 +1798,7 @@ Mavlink::task_main(int argc, char *argv[]) * set error flag instead */ bool err_flag = false; - while ((ch = getopt(argc, argv, "b:r:d:m:fpvw")) != EOF) { + while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) { switch (ch) { case 'b': _baudrate = strtoul(optarg, NULL, 10); @@ -1807,6 +1854,10 @@ Mavlink::task_main(int argc, char *argv[]) _wait_to_transmit = true; break; + case 'x': + _ftp_on = true; + break; + default: err_flag = true; break; @@ -1872,9 +1923,12 @@ Mavlink::task_main(int argc, char *argv[]) mavlink_logbuffer_init(&_logbuffer, 5); /* if we are passing on mavlink messages, we need to prepare a buffer for this instance */ - if (_passing_on) { - /* initialize message buffer if multiplexing is on */ - if (OK != message_buffer_init(300)) { + if (_passing_on || _ftp_on) { + /* initialize message buffer if multiplexing is on or its needed for FTP. + * make space for two messages plus off-by-one space as we use the empty element + * marker ring buffer approach. + */ + if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) { errx(1, "can't allocate message buffer, exiting"); } @@ -2037,32 +2091,50 @@ Mavlink::task_main(int argc, char *argv[]) } } - /* pass messages from other UARTs */ - if (_passing_on) { + /* pass messages from other UARTs or FTP worker */ + if (_passing_on || _ftp_on) { bool is_part; - void *read_ptr; + uint8_t *read_ptr; + uint8_t *write_ptr; - /* guard get ptr by mutex */ pthread_mutex_lock(&_message_buffer_mutex); - int available = message_buffer_get_ptr(&read_ptr, &is_part); + int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); pthread_mutex_unlock(&_message_buffer_mutex); if (available > 0) { - /* write first part of buffer */ - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); - message_buffer_mark_read(available); + // Reconstruct message from buffer + + mavlink_message_t msg; + write_ptr = (uint8_t*)&msg; + + // Pull a single message from the buffer + int read_count = available; + if (read_count > sizeof(mavlink_message_t)) { + read_count = sizeof(mavlink_message_t); + } + + memcpy(write_ptr, read_ptr, read_count); + + // We hold the mutex until after we complete the second part of the buffer. If we don't + // we may end up breaking the empty slot overflow detection semantics when we mark the + // possibly partial read below. + pthread_mutex_lock(&_message_buffer_mutex); + + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ - if (is_part) { - /* guard get ptr by mutex */ - pthread_mutex_lock(&_message_buffer_mutex); - available = message_buffer_get_ptr(&read_ptr, &is_part); - pthread_mutex_unlock(&_message_buffer_mutex); - - _mavlink_resend_uart(_channel, (const mavlink_message_t*)read_ptr); + if (is_part && read_count < sizeof(mavlink_message_t)) { + write_ptr += read_count; + available = message_buffer_get_ptr((void**)&read_ptr, &is_part); + read_count = sizeof(mavlink_message_t) - read_count; + memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } + + pthread_mutex_unlock(&_message_buffer_mutex); + + _mavlink_resend_uart(_channel, &msg); } } @@ -2112,7 +2184,7 @@ Mavlink::task_main(int argc, char *argv[]) /* close mavlink logging device */ close(_mavlink_fd); - if (_passing_on) { + if (_passing_on || _ftp_on) { message_buffer_destroy(); pthread_mutex_destroy(&_message_buffer_mutex); } @@ -2130,11 +2202,20 @@ int Mavlink::start_helper(int argc, char *argv[]) /* create the instance in task context */ Mavlink *instance = new Mavlink(); - /* this will actually only return once MAVLink exits */ - int res = instance->task_main(argc, argv); + int res; - /* delete instance on main thread end */ - delete instance; + if (!instance) { + + /* out of memory */ + res = -ENOMEM; + warnx("OUT OF MEM"); + } else { + /* this will actually only return once MAVLink exits */ + res = instance->task_main(argc, argv); + + /* delete instance on main thread end */ + delete instance; + } return res; } @@ -2254,7 +2335,7 @@ Mavlink::stream(int argc, char *argv[]) static void usage() { - warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate] [-r rate] [-m mode] [-s stream] [-f] [-p] [-v] [-w]"); + warnx("usage: mavlink {start|stop-all|stream} [-d device] [-b baudrate]\n\t[-r rate][-m mode] [-s stream] [-f] [-p] [-v] [-w] [-x]"); } int mavlink_main(int argc, char *argv[]) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 85a88442c..6777d56c3 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -123,27 +123,41 @@ public: /** * Display the mavlink status. */ - void status(); + void status(); - static int stream(int argc, char *argv[]); + static int stream(int argc, char *argv[]); - static int instance_count(); + static int instance_count(); - static Mavlink *new_instance(); + static Mavlink *new_instance(); - static Mavlink *get_instance(unsigned instance); + static Mavlink *get_instance(unsigned instance); - static Mavlink *get_instance_for_device(const char *device_name); + static Mavlink *get_instance_for_device(const char *device_name); - static int destroy_all_instances(); + static int destroy_all_instances(); - static bool instance_exists(const char *device_name, Mavlink *self); + static bool instance_exists(const char *device_name, Mavlink *self); - static void forward_message(mavlink_message_t *msg, Mavlink *self); + static void forward_message(mavlink_message_t *msg, Mavlink *self); - static int get_uart_fd(unsigned index); + static int get_uart_fd(unsigned index); - int get_uart_fd(); + int get_uart_fd(); + + /** + * Get the MAVLink system id. + * + * @return The system ID of this vehicle + */ + int get_system_id(); + + /** + * Get the MAVLink component id. + * + * @return The component ID of this vehicle + */ + int get_component_id(); const char *_device_name; @@ -153,30 +167,30 @@ public: MAVLINK_MODE_CAMERA }; - void set_mode(enum MAVLINK_MODE); - enum MAVLINK_MODE get_mode() { return _mode; } + void set_mode(enum MAVLINK_MODE); + enum MAVLINK_MODE get_mode() { return _mode; } - bool get_hil_enabled() { return _hil_enabled; } + bool get_hil_enabled() { return _hil_enabled; } - bool get_use_hil_gps() { return _use_hil_gps; } + bool get_use_hil_gps() { return _use_hil_gps; } - bool get_flow_control_enabled() { return _flow_control_enabled; } + bool get_flow_control_enabled() { return _flow_control_enabled; } - bool get_forwarding_on() { return _forwarding_on; } + bool get_forwarding_on() { return _forwarding_on; } /** * Handle waypoint related messages. */ - void mavlink_wpm_message_handler(const mavlink_message_t *msg); + void mavlink_wpm_message_handler(const mavlink_message_t *msg); - static int start_helper(int argc, char *argv[]); + static int start_helper(int argc, char *argv[]); /** * Handle parameter related messages. */ - void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); + void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg); - void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); + void get_mavlink_mode_and_state(struct vehicle_status_s *status, struct position_setpoint_triplet_s *pos_sp_triplet, uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); /** * Enable / disable Hardware in the Loop simulation mode. @@ -186,90 +200,105 @@ public: * requested change could not be made or was * redundant. */ - int set_hil_enabled(bool hil_enabled); + int set_hil_enabled(bool hil_enabled); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); - int get_instance_id(); + int get_instance_id(); /** * Enable / disable hardware flow control. * * @param enabled True if hardware flow control should be enabled */ - int enable_flow_control(bool enabled); + int enable_flow_control(bool enabled); - mavlink_channel_t get_channel(); + const mavlink_channel_t get_channel(); + + void configure_stream_threadsafe(const char *stream_name, const float rate); - bool _task_should_exit; /**< if true, mavlink task should exit */ + bool _task_should_exit; /**< if true, mavlink task should exit */ - int get_mavlink_fd() { return _mavlink_fd; } + int get_mavlink_fd() { return _mavlink_fd; } + + MavlinkStream * get_streams() { return _streams; } const /* Functions for waiting to start transmission until message received. */ - void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } - bool get_has_received_messages() { return _received_messages; } - void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } - bool get_wait_to_transmit() { return _wait_to_transmit; } - bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } + bool get_has_received_messages() { return _received_messages; } + void set_wait_to_transmit(bool wait) { _wait_to_transmit = wait; } + bool get_wait_to_transmit() { return _wait_to_transmit; } + bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + + bool message_buffer_write(void *ptr, int size); + + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } + void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } + + /** + * Count a transmision error + */ + void count_txerr(); protected: - Mavlink *next; + Mavlink *next; private: - int _instance_id; + int _instance_id; - int _mavlink_fd; - bool _task_running; + int _mavlink_fd; + bool _task_running; /* states */ - bool _hil_enabled; /**< Hardware In the Loop mode */ - bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ - bool _is_usb_uart; /**< Port is USB */ - bool _wait_to_transmit; /**< Wait to transmit until received messages. */ - bool _received_messages; /**< Whether we've received valid mavlink messages. */ + bool _hil_enabled; /**< Hardware In the Loop mode */ + bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _is_usb_uart; /**< Port is USB */ + bool _wait_to_transmit; /**< Wait to transmit until received messages. */ + bool _received_messages; /**< Whether we've received valid mavlink messages. */ - unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ + unsigned _main_loop_delay; /**< mainloop delay, depends on data rate */ - MavlinkOrbSubscription *_subscriptions; - MavlinkStream *_streams; + MavlinkOrbSubscription *_subscriptions; + MavlinkStream *_streams; - orb_advert_t _mission_pub; - struct mission_s mission; - MAVLINK_MODE _mode; + orb_advert_t _mission_pub; + struct mission_s mission; + MAVLINK_MODE _mode; - uint8_t _mavlink_wpm_comp_id; - mavlink_channel_t _channel; + uint8_t _mavlink_wpm_comp_id; + mavlink_channel_t _channel; struct mavlink_logbuffer _logbuffer; - unsigned int _total_counter; + unsigned int _total_counter; - pthread_t _receive_thread; + pthread_t _receive_thread; /* Allocate storage space for waypoints */ - mavlink_wpm_storage _wpm_s; - mavlink_wpm_storage *_wpm; + mavlink_wpm_storage _wpm_s; + mavlink_wpm_storage *_wpm; - bool _verbose; - bool _forwarding_on; - bool _passing_on; - int _uart_fd; - int _baudrate; - int _datarate; + bool _verbose; + bool _forwarding_on; + bool _passing_on; + bool _ftp_on; + int _uart_fd; + int _baudrate; + int _datarate; /** * If the queue index is not at 0, the queue sending * logic will send parameters from the current index * to len - 1, the end of the param list. */ - unsigned int _mavlink_param_queue_index; + unsigned int _mavlink_param_queue_index; - bool mavlink_link_termination_allowed; + bool mavlink_link_termination_allowed; - char *_subscribe_to_stream; - float _subscribe_to_stream_rate; + char *_subscribe_to_stream; + float _subscribe_to_stream_rate; - bool _flow_control_enabled; + bool _flow_control_enabled; struct mavlink_message_buffer { int write_ptr; @@ -277,11 +306,14 @@ private: int size; char *data; }; + mavlink_message_buffer _message_buffer; pthread_mutex_t _message_buffer_mutex; - perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ + bool _param_initialized; param_t _param_system_id; param_t _param_component_id; @@ -294,7 +326,7 @@ private: * @param param The parameter id to send. * @return zero on success, nonzero on failure. */ - int mavlink_pm_send_param(param_t param); + int mavlink_pm_send_param(param_t param); /** * Send one parameter identified by index. @@ -302,7 +334,7 @@ private: * @param index The index of the parameter to send. * @return zero on success, nonzero else. */ - int mavlink_pm_send_param_for_index(uint16_t index); + int mavlink_pm_send_param_for_index(uint16_t index); /** * Send one parameter identified by name. @@ -310,14 +342,14 @@ private: * @param name The index of the parameter to send. * @return zero on success, nonzero else. */ - int mavlink_pm_send_param_for_name(const char *name); + int mavlink_pm_send_param_for_name(const char *name); /** * Send a queue of parameters, one parameter per function call. * * @return zero on success, nonzero on failure */ - int mavlink_pm_queued_send(void); + int mavlink_pm_queued_send(void); /** * Start sending the parameter queue. @@ -327,12 +359,12 @@ private: * mavlink_pm_queued_send(). * @see mavlink_pm_queued_send() */ - void mavlink_pm_start_queued_send(); + void mavlink_pm_start_queued_send(); - void mavlink_update_system(); + void mavlink_update_system(); - void mavlink_waypoint_eventloop(uint64_t now); - void mavlink_wpm_send_waypoint_reached(uint16_t seq); + void mavlink_waypoint_eventloop(uint64_t now); + void mavlink_wpm_send_waypoint_reached(uint16_t seq); void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq); void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq); void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count); @@ -349,7 +381,6 @@ private: int mavlink_open_uart(int baudrate, const char *uart_name, struct termios *uart_config_original, bool *is_usb); int configure_stream(const char *stream_name, const float rate); - void configure_stream_threadsafe(const char *stream_name, const float rate); int message_buffer_init(int size); @@ -359,8 +390,6 @@ private: int message_buffer_is_empty(); - bool message_buffer_write(void *ptr, int size); - int message_buffer_get_ptr(void **ptr, bool *is_part); void message_buffer_mark_read(int n); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index b295bf35f..e1ebc16cc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -120,50 +120,77 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set union px4_custom_mode custom_mode; custom_mode.data = 0; - if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { - /* use main state when navigator is not active */ - if (status->main_state == MAIN_STATE_MANUAL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + switch (status->nav_state) { + + case NAVIGATION_STATE_MANUAL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + + case NAVIGATION_STATE_ACRO: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; + break; - } else if (status->main_state == MAIN_STATE_ALTCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + case NAVIGATION_STATE_ALTCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL; + break; - } else if (status->main_state == MAIN_STATE_POSCTL) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + case NAVIGATION_STATE_POSCTL: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL; + break; - } else if (status->main_state == MAIN_STATE_AUTO) { - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + case NAVIGATION_STATE_AUTO_MISSION: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; - - } else if (status->main_state == MAIN_STATE_ACRO) { - *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; - } - - } else { - /* use navigation state when navigator is active */ - *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; - - if (pos_sp_triplet->nav_state == NAV_STATE_READY) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LOITER) { + case NAVIGATION_STATE_AUTO_LOITER: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_MISSION) { - custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION; - - } else if (pos_sp_triplet->nav_state == NAV_STATE_RTL) { + case NAVIGATION_STATE_AUTO_RTL: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL; + break; - } else if (pos_sp_triplet->nav_state == NAV_STATE_LAND) { + case NAVIGATION_STATE_LAND: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; - } + break; + + case NAVIGATION_STATE_AUTO_RTGS: + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED + | MAV_MODE_FLAG_STABILIZE_ENABLED + | MAV_MODE_FLAG_GUIDED_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTGS; + break; + + case NAVIGATION_STATE_TERMINATION: + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + break; + } *mavlink_custom_mode = custom_mode.data; @@ -205,6 +232,11 @@ public: return "HEARTBEAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HEARTBEAT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHeartbeat(); @@ -227,8 +259,15 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; /* always send the heartbeat, independent of the update status of the topics */ - (void)status_sub->update(&status); - (void)pos_sp_triplet_sub->update(&pos_sp_triplet); + if (!status_sub->update(&status)) { + /* if topic update failed fill it with defaults */ + memset(&status, 0, sizeof(status)); + } + + if (!pos_sp_triplet_sub->update(&pos_sp_triplet)) { + /* if topic update failed fill it with defaults */ + memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + } uint8_t mavlink_state = 0; uint8_t mavlink_base_mode = 0; @@ -258,6 +297,11 @@ public: return "SYS_STATUS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SYS_STATUS; + } + static MavlinkStream *new_instance() { return new MavlinkStreamSysStatus(); @@ -309,6 +353,11 @@ public: return "HIGHRES_IMU"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIGHRES_IMU; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHighresIMU(); @@ -394,6 +443,11 @@ public: return "ATTITUDE"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitude(); @@ -440,6 +494,11 @@ public: return "ATTITUDE_QUATERNION"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ATTITUDE_QUATERNION; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeQuaternion(); @@ -492,6 +551,11 @@ public: return "VFR_HUD"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_VFR_HUD; + } + static MavlinkStream *new_instance() { return new MavlinkStreamVFRHUD(); @@ -575,6 +639,11 @@ public: return "GPS_RAW_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_RAW_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGPSRawInt(); @@ -605,8 +674,8 @@ protected: gps.lat, gps.lon, gps.alt, - cm_uint16_from_m_float(gps.eph_m), - cm_uint16_from_m_float(gps.epv_m), + cm_uint16_from_m_float(gps.eph), + cm_uint16_from_m_float(gps.epv), gps.vel_m_s * 100.0f, _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, gps.satellites_visible); @@ -628,6 +697,11 @@ public: return "GLOBAL_POSITION_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionInt(); @@ -689,6 +763,11 @@ public: return "LOCAL_POSITION_NED"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_NED; + } + static MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionNED(); @@ -740,6 +819,11 @@ public: return "VICON_POSITION_ESTIMATE"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; + } + static MavlinkStream *new_instance() { return new MavlinkStreamViconPositionEstimate(); @@ -790,6 +874,11 @@ public: return "GPS_GLOBAL_ORIGIN"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGPSGlobalOrigin(); @@ -830,6 +919,11 @@ public: return MavlinkStreamServoOutputRaw<N>::get_name_static(); } + uint8_t get_id() + { + return MAVLINK_MSG_ID_SERVO_OUTPUT_RAW; + } + static const char *get_name_static() { switch (N) { @@ -907,6 +1001,11 @@ public: return "HIL_CONTROLS"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_HIL_CONTROLS; + } + static MavlinkStream *new_instance() { return new MavlinkStreamHILControls(); @@ -1044,6 +1143,11 @@ public: return "GLOBAL_POSITION_SETPOINT_INT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamGlobalPositionSetpointInt(); @@ -1087,6 +1191,11 @@ public: return "LOCAL_POSITION_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamLocalPositionSetpoint(); @@ -1135,6 +1244,11 @@ public: return "ROLL_PITCH_YAW_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawThrustSetpoint(); @@ -1183,6 +1297,11 @@ public: return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRollPitchYawRatesThrustSetpoint(); @@ -1231,6 +1350,11 @@ public: return "RC_CHANNELS_RAW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_RC_CHANNELS_RAW; + } + static MavlinkStream *new_instance() { return new MavlinkStreamRCChannelsRaw(); @@ -1315,6 +1439,11 @@ public: return "MANUAL_CONTROL"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_MANUAL_CONTROL; + } + static MavlinkStream *new_instance() { return new MavlinkStreamManualControl(); @@ -1364,6 +1493,11 @@ public: return "OPTICAL_FLOW"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_OPTICAL_FLOW; + } + static MavlinkStream *new_instance() { return new MavlinkStreamOpticalFlow(); @@ -1412,6 +1546,11 @@ public: return "ATTITUDE_CONTROLS"; } + uint8_t get_id() + { + return 0; + } + static MavlinkStream *new_instance() { return new MavlinkStreamAttitudeControls(); @@ -1470,6 +1609,11 @@ public: return "NAMED_VALUE_FLOAT"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_NAMED_VALUE_FLOAT; + } + static MavlinkStream *new_instance() { return new MavlinkStreamNamedValueFloat(); @@ -1518,6 +1662,11 @@ public: return "CAMERA_CAPTURE"; } + uint8_t get_id() + { + return 0; + } + static MavlinkStream *new_instance() { return new MavlinkStreamCameraCapture(); @@ -1563,6 +1712,11 @@ public: return "DISTANCE_SENSOR"; } + uint8_t get_id() + { + return MAVLINK_MSG_ID_DISTANCE_SENSOR; + } + static MavlinkStream *new_instance() { return new MavlinkStreamDistanceSensor(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9c528adbe..bb977d277 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -106,12 +106,17 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), + _telemetry_heartbeat_time(0), + _radio_status_available(false), _hil_frames(0), _old_timestamp(0), _hil_local_proj_inited(0), _hil_local_alt0(0.0) { memset(&hil_local_pos, 0, sizeof(hil_local_pos)); + + // make sure the FTP server is started + (void)MavlinkFTP::getServer(); } MavlinkReceiver::~MavlinkReceiver() @@ -150,6 +155,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_HEARTBEAT: + handle_message_heartbeat(msg); + break; + + case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: + handle_message_request_data_stream(msg); + break; + + case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + MavlinkFTP::getServer()->handle_message(_mavlink, msg); + break; + default: break; } @@ -411,6 +428,7 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) memset(&tstatus, 0, sizeof(tstatus)); tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = _telemetry_heartbeat_time; tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO; tstatus.rssi = rstatus.rssi; tstatus.remote_rssi = rstatus.remrssi; @@ -426,6 +444,9 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); } + + /* this means that heartbeats alone won't be published to the radio status no more */ + _radio_status_available = true; } void @@ -452,6 +473,54 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) +{ + mavlink_heartbeat_t hb; + mavlink_msg_heartbeat_decode(msg, &hb); + + /* ignore own heartbeats, accept only heartbeats from GCS */ + if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) { + _telemetry_heartbeat_time = hrt_absolute_time(); + + /* if no radio status messages arrive, lets at least publish that heartbeats were received */ + if (!_radio_status_available) { + + struct telemetry_status_s tstatus; + memset(&tstatus, 0, sizeof(tstatus)); + + tstatus.timestamp = _telemetry_heartbeat_time; + tstatus.heartbeat_time = _telemetry_heartbeat_time; + tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus); + + } else { + orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); + } + } + } +} + +void +MavlinkReceiver::handle_message_request_data_stream(mavlink_message_t *msg) +{ + mavlink_request_data_stream_t req; + mavlink_msg_request_data_stream_decode(msg, &req); + + if (req.target_system == mavlink_system.sysid && req.target_component == mavlink_system.compid) { + float rate = req.start_stop ? (1000.0f / req.req_message_rate) : 0.0f; + + MavlinkStream *stream; + LL_FOREACH(_mavlink->get_streams(), stream) { + if (req.req_stream_id == stream->get_id()) { + _mavlink->configure_stream_threadsafe(stream->get_name(), rate); + } + } + } +} + +void MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) { mavlink_hil_sensor_t imu; @@ -667,12 +736,12 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg) hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; - hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m - hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.eph = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv = (float)gps.epv * 1e-2f; // from cm to m hil_gps.timestamp_variance = timestamp; hil_gps.s_variance_m_s = 5.0f; - hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m; + hil_gps.p_variance_m = hil_gps.eph * hil_gps.eph; hil_gps.timestamp_velocity = timestamp; hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 9ab84b58a..040a07480 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -68,6 +68,8 @@ #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> +#include "mavlink_ftp.h" + class Mavlink; class MavlinkReceiver @@ -112,6 +114,8 @@ private: void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); + void handle_message_heartbeat(mavlink_message_t *msg); + void handle_message_request_data_stream(mavlink_message_t *msg); void handle_message_hil_sensor(mavlink_message_t *msg); void handle_message_hil_gps(mavlink_message_t *msg); void handle_message_hil_state_quaternion(mavlink_message_t *msg); @@ -138,6 +142,8 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; + hrt_abstime _telemetry_heartbeat_time; + bool _radio_status_available; int _hil_frames; uint64_t _old_timestamp; bool _hil_local_proj_inited; diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h index a41ace48e..69809a386 100644 --- a/src/modules/mavlink/mavlink_stream.h +++ b/src/modules/mavlink/mavlink_stream.h @@ -67,6 +67,7 @@ public: static const char *get_name_static(); virtual void subscribe(Mavlink *mavlink) = 0; virtual const char *get_name() const = 0; + virtual uint8_t get_id() = 0; protected: mavlink_channel_t _channel; diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index f532e26fe..a4d8bfbfb 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -43,7 +43,8 @@ SRCS += mavlink_main.cpp \ mavlink_messages.cpp \ mavlink_stream.cpp \ mavlink_rate_limiter.cpp \ - mavlink_commands.cpp + mavlink_commands.cpp \ + mavlink_ftp.cpp INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index bc8dbca50..266215308 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -78,7 +78,7 @@ bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { double lat = vehicle->lat / 1e7d; double lon = vehicle->lon / 1e7d; - float alt = vehicle->alt; + //float alt = vehicle->alt; return inside(lat, lon, vehicle->alt); } @@ -116,9 +116,9 @@ bool Geofence::inside(double lat, double lon, float altitude) } // skip vertex 0 (return point) - if (((temp_vertex_i.lon) >= lon != (temp_vertex_j.lon >= lon)) && - (lat <= (temp_vertex_j.lat - temp_vertex_i.lat) * (lon - temp_vertex_i.lon) / - (temp_vertex_j.lon - temp_vertex_i.lon) + temp_vertex_i.lat)) { + if (((double)temp_vertex_i.lon >= lon) != ((double)temp_vertex_j.lon >= lon) && + (lat <= (double)(temp_vertex_j.lat - temp_vertex_i.lat) * (lon - (double)temp_vertex_i.lon) / + (double)(temp_vertex_j.lon - temp_vertex_i.lon) + (double)temp_vertex_i.lat)) { c = !c; } @@ -294,4 +294,5 @@ Geofence::loadFromFile(const char *filename) int Geofence::clearDm() { dm_clear(DM_KEY_FENCE_POINTS); + return OK; } diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp new file mode 100644 index 000000000..542483fb1 --- /dev/null +++ b/src/modules/navigator/loiter.cpp @@ -0,0 +1,78 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 loiter.cpp + * + * Helper class to loiter + * + * @author Julian Oes <julian@oes.ch> + */ + +#include <string.h> +#include <stdlib.h> +#include <stdbool.h> +#include <math.h> +#include <fcntl.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> + +#include <uORB/uORB.h> +#include <uORB/topics/position_setpoint_triplet.h> + +#include "loiter.h" + +Loiter::Loiter(Navigator *navigator, const char *name) : + MissionBlock(navigator, name) +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +Loiter::~Loiter() +{ +} + +bool +Loiter::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + /* set loiter item, don't reuse an existing position setpoint */ + return set_loiter_item(pos_sp_triplet); +} + +void +Loiter::on_inactive() +{ +} diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h new file mode 100644 index 000000000..65ff5c31e --- /dev/null +++ b/src/modules/navigator/loiter.h @@ -0,0 +1,74 @@ +/*************************************************************************** + * + * Copyright (c) 2014 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 loiter.h + * + * Helper class to loiter + * + * @author Julian Oes <julian@oes.ch> + */ + +#ifndef NAVIGATOR_LOITER_H +#define NAVIGATOR_LOITER_H + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include "navigator_mode.h" +#include "mission_block.h" + +class Loiter : public MissionBlock +{ +public: + /** + * Constructor + */ + Loiter(Navigator *navigator, const char *name); + + /** + * Destructor + */ + ~Loiter(); + + /** + * This function is called while the mode is inactive + */ + virtual void on_inactive(); + + /** + * This function is called while the mode is active + */ + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); +}; + +#endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp new file mode 100644 index 000000000..72255103b --- /dev/null +++ b/src/modules/navigator/mission.cpp @@ -0,0 +1,461 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 navigator_mission.cpp + * + * Helper class to access missions + * + * @author Julian Oes <julian@oes.ch> + */ + +#include <sys/types.h> +#include <string.h> +#include <stdlib.h> +#include <unistd.h> + +#include <drivers/drv_hrt.h> + +#include <dataman/dataman.h> +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> +#include <geo/geo.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/mission_result.h> + +#include "navigator.h" +#include "mission.h" + +Mission::Mission(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _param_onboard_enabled(this, "ONBOARD_EN"), + _onboard_mission({0}), + _offboard_mission({0}), + _current_onboard_mission_index(-1), + _current_offboard_mission_index(-1), + _mission_result_pub(-1), + _mission_result({0}), + _mission_type(MISSION_TYPE_NONE) +{ + /* load initial params */ + updateParams(); + /* set initial mission items */ + on_inactive(); + +} + +Mission::~Mission() +{ +} + +void +Mission::on_inactive() +{ + _first_run = true; + + /* check anyway if missions have changed so that feedback to groundstation is given */ + bool onboard_updated; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } + + bool offboard_updated; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } +} + +bool +Mission::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + bool updated = false; + + /* check if anything has changed */ + bool onboard_updated; + orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); + if (onboard_updated) { + update_onboard_mission(); + } + + bool offboard_updated; + orb_check(_navigator->get_offboard_mission_sub(), &offboard_updated); + if (offboard_updated) { + update_offboard_mission(); + } + + /* reset mission items if needed */ + if (onboard_updated || offboard_updated || _first_run) { + set_mission_items(pos_sp_triplet); + updated = true; + _first_run = false; + } + + /* lets check if we reached the current mission item */ + if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { + advance_mission(); + set_mission_items(pos_sp_triplet); + updated = true; + } + + return updated; +} + +void +Mission::update_onboard_mission() +{ + if (orb_copy(ORB_ID(onboard_mission), _navigator->get_onboard_mission_sub(), &_onboard_mission) == OK) { + /* accept the current index set by the onboard mission if it is within bounds */ + if (_onboard_mission.current_index >=0 + && _onboard_mission.current_index < (int)_onboard_mission.count) { + _current_onboard_mission_index = _onboard_mission.current_index; + } else { + /* if less WPs available, reset to first WP */ + if (_current_onboard_mission_index >= (int)_onboard_mission.count) { + _current_onboard_mission_index = 0; + /* if not initialized, set it to 0 */ + } else if (_current_onboard_mission_index < 0) { + _current_onboard_mission_index = 0; + } + /* otherwise, just leave it */ + } + } else { + _onboard_mission.count = 0; + _onboard_mission.current_index = 0; + _current_onboard_mission_index = 0; + } +} + +void +Mission::update_offboard_mission() +{ + if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { + + /* determine current index */ + if (_offboard_mission.current_index >= 0 + && _offboard_mission.current_index < (int)_offboard_mission.count) { + _current_offboard_mission_index = _offboard_mission.current_index; + } else { + /* if less WPs available, reset to first WP */ + if (_current_offboard_mission_index >= (int)_offboard_mission.count) { + _current_offboard_mission_index = 0; + /* if not initialized, set it to 0 */ + } else if (_current_offboard_mission_index < 0) { + _current_offboard_mission_index = 0; + } + /* otherwise, just leave it */ + } + + /* Check mission feasibility, for now do not handle the return value, + * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ + dm_item_t dm_current; + + if (_offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + + missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, + (size_t)_offboard_mission.count, + _navigator->get_geofence(), + _navigator->get_home_position()->alt); + } else { + _offboard_mission.count = 0; + _offboard_mission.current_index = 0; + _current_offboard_mission_index = 0; + } + report_current_offboard_mission_item(); +} + + +void +Mission::advance_mission() +{ + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; + } +} + +void +Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + set_previous_pos_setpoint(pos_sp_triplet); + + /* try setting onboard mission item */ + if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_ONBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: onboard mission running"); + } + _mission_type = MISSION_TYPE_ONBOARD; + _navigator->set_can_loiter_at_sp(false); + + /* try setting offboard mission item */ + } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { + /* if mission type changed, notify */ + if (_mission_type != MISSION_TYPE_OFFBOARD) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: offboard mission running"); + } + _mission_type = MISSION_TYPE_OFFBOARD; + _navigator->set_can_loiter_at_sp(false); + } else { + if (_mission_type != MISSION_TYPE_NONE) { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: mission finished"); + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: no mission available"); + } + _mission_type = MISSION_TYPE_NONE; + _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.valid && _waypoint_position_reached); + + set_loiter_item(pos_sp_triplet); + reset_mission_item_reached(); + report_mission_finished(); + } +} + +bool +Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) +{ + /* make sure param is up to date */ + updateParams(); + if (_param_onboard_enabled.get() > 0 && + _current_onboard_mission_index >= 0&& + _current_onboard_mission_index < (int)_onboard_mission.count) { + struct mission_item_s new_mission_item; + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, + &new_mission_item)) { + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); + current_pos_sp->valid = true; + + reset_mission_item_reached(); + + /* TODO: report this somehow */ + memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); + return true; + } + } + return false; +} + +bool +Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) +{ + if (_current_offboard_mission_index >= 0 && + _current_offboard_mission_index < (int)_offboard_mission.count) { + dm_item_t dm_current; + if (_offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + struct mission_item_s new_mission_item; + if (read_mission_item(dm_current, true, &_current_offboard_mission_index, &new_mission_item)) { + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&new_mission_item, current_pos_sp); + current_pos_sp->valid = true; + + reset_mission_item_reached(); + + report_current_offboard_mission_item(); + memcpy(&_mission_item, &new_mission_item, sizeof(struct mission_item_s)); + return true; + } + } + return false; +} + +void +Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) +{ + int next_temp_mission_index = _onboard_mission.current_index + 1; + + /* try if there is a next onboard mission */ + if (_onboard_mission.current_index >= 0 && + next_temp_mission_index < (int)_onboard_mission.count) { + struct mission_item_s new_mission_item; + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { + /* convert next mission item to position setpoint */ + mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); + next_pos_sp->valid = true; + return; + } + } + + /* give up */ + next_pos_sp->valid = false; + return; +} + +void +Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) +{ + /* try if there is a next offboard mission */ + int next_temp_mission_index = _offboard_mission.current_index + 1; + warnx("next index: %d, count; %d", next_temp_mission_index, _offboard_mission.count); + if (_offboard_mission.current_index >= 0 && + next_temp_mission_index < (int)_offboard_mission.count) { + dm_item_t dm_current; + if (_offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + struct mission_item_s new_mission_item; + if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { + /* convert next mission item to position setpoint */ + mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); + next_pos_sp->valid = true; + return; + } + } + /* give up */ + next_pos_sp->valid = false; + return; +} + +bool +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, + struct mission_item_s *new_mission_item) +{ + /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ + for (int i=0; i<10; i++) { + const ssize_t len = sizeof(struct mission_item_s); + + /* read mission item from datamanager */ + if (dm_read(dm_item, *mission_index, new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR waypoint could not be read"); + return false; + } + + /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ + if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { + + /* do DO_JUMP as many times as requested */ + if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the + * dataman */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP waypoint could not be written"); + return false; + } + } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), + "#audio: DO JUMP repetitions completed"); + /* no more DO_JUMPS, therefore just try to continue with next mission item */ + (*mission_index)++; + } + + } else { + /* if it's not a DO_JUMP, then we were successful */ + return true; + } + } + + /* we have given up, we don't want to cycle forever */ + mavlink_log_critical(_navigator->get_mavlink_fd(), + "#audio: ERROR DO JUMP is cycling, giving up"); + return false; +} + +void +Mission::report_mission_item_reached() +{ + if (_mission_type == MISSION_TYPE_OFFBOARD) { + _mission_result.mission_reached = true; + _mission_result.mission_index_reached = _current_offboard_mission_index; + } + publish_mission_result(); +} + +void +Mission::report_current_offboard_mission_item() +{ + _mission_result.index_current_mission = _current_offboard_mission_index; + publish_mission_result(); +} + +void +Mission::report_mission_finished() +{ + _mission_result.mission_finished = true; + publish_mission_result(); +} + +void +Mission::publish_mission_result() +{ + /* lazily publish the mission result only once available */ + if (_mission_result_pub > 0) { + /* publish mission result */ + orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); + + } else { + /* advertise and publish */ + _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); + } + /* reset reached bool */ + _mission_result.mission_reached = false; + _mission_result.mission_finished = false; +} diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h new file mode 100644 index 000000000..6e4761946 --- /dev/null +++ b/src/modules/navigator/mission.h @@ -0,0 +1,178 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 mission.h + * + * Navigator mode to access missions + * + * @author Julian Oes <julian@oes.ch> + */ + +#ifndef NAVIGATOR_MISSION_H +#define NAVIGATOR_MISSION_H + +#include <drivers/drv_hrt.h> + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <dataman/dataman.h> + +#include <uORB/uORB.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/position_setpoint_triplet.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/mission_result.h> + +#include "navigator_mode.h" +#include "mission_block.h" +#include "mission_feasibility_checker.h" + +class Navigator; + +class Mission : public MissionBlock +{ +public: + /** + * Constructor + */ + Mission(Navigator *navigator, const char *name); + + /** + * Destructor + */ + virtual ~Mission(); + + /** + * This function is called while the mode is inactive + */ + virtual void on_inactive(); + + /** + * This function is called while the mode is active + */ + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + +private: + /** + * Update onboard mission topic + */ + void update_onboard_mission(); + + /** + * Update offboard mission topic + */ + void update_offboard_mission(); + + /** + * Move on to next mission item or switch to loiter + */ + void advance_mission(); + + /** + * Set new mission items + */ + void set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet); + + /** + * Try to set the current position setpoint from an onboard mission item + * @return true if mission item successfully set + */ + bool is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp); + + /** + * Try to set the current position setpoint from an offboard mission item + * @return true if mission item successfully set + */ + bool is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp); + + /** + * Try to set the next position setpoint from an onboard mission item + */ + void get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp); + + /** + * Try to set the next position setpoint from an offboard mission item + */ + void get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp); + + /** + * Read a mission item from the dataman and watch out for DO_JUMPS + * @return true if successful + */ + bool read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, + struct mission_item_s *new_mission_item); + + /** + * Report that a mission item has been reached + */ + void report_mission_item_reached(); + + /** + * Rport the current mission item + */ + void report_current_offboard_mission_item(); + + /** + * Report that the mission is finished if one exists or that none exists + */ + void report_mission_finished(); + + /** + * Publish the mission result so commander and mavlink know what is going on + */ + void publish_mission_result(); + + control::BlockParamFloat _param_onboard_enabled; + + struct mission_s _onboard_mission; + struct mission_s _offboard_mission; + + int _current_onboard_mission_index; + int _current_offboard_mission_index; + + orb_advert_t _mission_result_pub; + struct mission_result_s _mission_result; + + enum { + MISSION_TYPE_NONE, + MISSION_TYPE_ONBOARD, + MISSION_TYPE_OFFBOARD + } _mission_type; + + MissionFeasibilityChecker missionFeasiblityChecker; /**< class that checks if a mission is feasible */ +}; + +#endif diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp new file mode 100644 index 000000000..9b8d3d9c7 --- /dev/null +++ b/src/modules/navigator/mission_block.cpp @@ -0,0 +1,242 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 mission_block.cpp + * + * Helper class to use mission items + * + * @author Julian Oes <julian@oes.ch> + */ + +#include <sys/types.h> +#include <string.h> +#include <stdlib.h> +#include <unistd.h> + +#include <systemlib/err.h> +#include <geo/geo.h> + +#include <uORB/uORB.h> + +#include "navigator.h" +#include "mission_block.h" + + +MissionBlock::MissionBlock(Navigator *navigator, const char *name) : + NavigatorMode(navigator, name), + _waypoint_position_reached(false), + _waypoint_yaw_reached(false), + _time_first_inside_orbit(0), + _mission_item({0}), + _mission_item_valid(false) +{ +} + +MissionBlock::~MissionBlock() +{ +} + +bool +MissionBlock::is_mission_item_reached() +{ + if (_mission_item.nav_cmd == NAV_CMD_LAND) { + return _navigator->get_vstatus()->condition_landed; + } + + /* TODO: count turns */ + if ((/*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED)) { + return false; + } + + hrt_abstime now = hrt_absolute_time(); + + if (!_waypoint_position_reached) { + + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + float altitude_amsl = _mission_item.altitude_is_relative + ? _mission_item.altitude + _navigator->get_home_position()->alt + : _mission_item.altitude; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); + + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF && _navigator->get_vstatus()->is_rotary_wing) { + /* require only altitude for takeoff for multicopter */ + if (_navigator->get_global_position()->alt > + altitude_amsl - _navigator->get_acceptance_radius()) { + _waypoint_position_reached = true; + } + } else if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + /* for takeoff mission items use the parameter for the takeoff acceptance radius */ + if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { + _waypoint_position_reached = true; + } + } else { + /* for normal mission items used their acceptance radius */ + if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } + } + + if (_waypoint_position_reached && !_waypoint_yaw_reached) { + + /* TODO: removed takeoff, why? */ + if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { + + /* check yaw if defined only for rotary wing except takeoff */ + float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); + + if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ + _waypoint_yaw_reached = true; + } + + } else { + _waypoint_yaw_reached = true; + } + } + + /* check if the current waypoint was reached */ + if (_waypoint_position_reached && _waypoint_yaw_reached) { + + if (_time_first_inside_orbit == 0) { + _time_first_inside_orbit = now; + + // if (_mission_item.time_inside > 0.01f) { + // mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", + // (double)_mission_item.time_inside); + // } + } + + /* check if the MAV was long enough inside the waypoint orbit */ + if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { + return true; + } + } + return false; +} + +void +MissionBlock::reset_mission_item_reached() +{ + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; +} + +void +MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *item, struct position_setpoint_s *sp) +{ + sp->valid = true; + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; + + switch (item->nav_cmd) { + case NAV_CMD_IDLE: + sp->type = SETPOINT_TYPE_IDLE; + break; + + case NAV_CMD_TAKEOFF: + sp->type = SETPOINT_TYPE_TAKEOFF; + break; + + case NAV_CMD_LAND: + sp->type = SETPOINT_TYPE_LAND; + break; + + case NAV_CMD_LOITER_TIME_LIMIT: + case NAV_CMD_LOITER_TURN_COUNT: + case NAV_CMD_LOITER_UNLIMITED: + sp->type = SETPOINT_TYPE_LOITER; + break; + + default: + sp->type = SETPOINT_TYPE_POSITION; + break; + } +} + +void +MissionBlock::set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + /* reuse current setpoint as previous setpoint */ + if (pos_sp_triplet->current.valid) { + memcpy(&pos_sp_triplet->previous, &pos_sp_triplet->current, sizeof(struct position_setpoint_s)); + } +} + +bool +MissionBlock::set_loiter_item(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + /* don't change setpoint if 'can_loiter_at_sp' flag set */ + if (!(_navigator->get_can_loiter_at_sp() && pos_sp_triplet->current.valid)) { + /* use current position */ + pos_sp_triplet->current.lat = _navigator->get_global_position()->lat; + pos_sp_triplet->current.lon = _navigator->get_global_position()->lon; + pos_sp_triplet->current.alt = _navigator->get_global_position()->alt; + pos_sp_triplet->current.yaw = NAN; /* NAN means to use current yaw */ + + _navigator->set_can_loiter_at_sp(true); + } + + if (pos_sp_triplet->current.type != SETPOINT_TYPE_LOITER + || pos_sp_triplet->current.loiter_radius != _navigator->get_loiter_radius() + || pos_sp_triplet->current.loiter_direction != 1 + || pos_sp_triplet->previous.valid + || !pos_sp_triplet->current.valid + || pos_sp_triplet->next.valid) { + /* position setpoint triplet should be updated */ + pos_sp_triplet->current.type = SETPOINT_TYPE_LOITER; + pos_sp_triplet->current.loiter_radius = _navigator->get_loiter_radius(); + pos_sp_triplet->current.loiter_direction = 1; + + pos_sp_triplet->previous.valid = false; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; + return true; + } + + return false; +} diff --git a/src/modules/navigator/navigator_mission.h b/src/modules/navigator/mission_block.h index b0f88e016..f99002752 100644 --- a/src/modules/navigator/navigator_mission.h +++ b/src/modules/navigator/mission_block.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2014 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 @@ -31,75 +31,76 @@ * ****************************************************************************/ /** - * @file navigator_mission.h - * Helper class to access missions + * @file mission_block.h * - * @author Julian Oes <joes@student.ethz.ch> + * Helper class to use mission items + * + * @author Julian Oes <julian@oes.ch> */ -#ifndef NAVIGATOR_MISSION_H -#define NAVIGATOR_MISSION_H +#ifndef NAVIGATOR_MISSION_BLOCK_H +#define NAVIGATOR_MISSION_BLOCK_H + +#include <drivers/drv_hrt.h> #include <uORB/topics/mission.h> -#include <uORB/topics/mission_result.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/position_setpoint_triplet.h> + +#include "navigator_mode.h" +class Navigator; -class __EXPORT Mission +class MissionBlock : public NavigatorMode { public: /** * Constructor */ - Mission(); + MissionBlock(Navigator *navigator, const char *name); /** - * Destructor, also kills the sensors task. + * Destructor */ - ~Mission(); - - void set_offboard_dataman_id(int new_id); - void set_current_offboard_mission_index(int new_index); - void set_current_onboard_mission_index(int new_index); - void set_offboard_mission_count(unsigned new_count); - void set_onboard_mission_count(unsigned new_count); - - void set_onboard_mission_allowed(bool allowed); - - bool current_mission_available(); - bool next_mission_available(); - - int get_current_mission_item(struct mission_item_s *mission_item, bool *onboard, unsigned *index); - int get_next_mission_item(struct mission_item_s *mission_item); - - void move_to_next(); - - void report_mission_item_reached(); - void report_current_offboard_mission_item(); - void publish_mission_result(); + virtual ~MissionBlock(); -private: - bool current_onboard_mission_available(); - bool current_offboard_mission_available(); - bool next_onboard_mission_available(); - bool next_offboard_mission_available(); + /** + * Check if mission item has been reached + * @return true if successfully reached + */ + bool is_mission_item_reached(); + /** + * Reset all reached flags + */ + void reset_mission_item_reached(); - int _offboard_dataman_id; - unsigned _current_offboard_mission_index; - unsigned _current_onboard_mission_index; - unsigned _offboard_mission_item_count; /** number of offboard mission items available */ - unsigned _onboard_mission_item_count; /** number of onboard mission items available */ + /** + * Convert a mission item to a position setpoint + * + * @param the mission item to convert + * @param the position setpoint that needs to be set + */ + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); - bool _onboard_mission_allowed; + /** + * Set previous position setpoint to current setpoint + */ + void set_previous_pos_setpoint(struct position_setpoint_triplet_s *pos_sp_triplet); - enum { - MISSION_TYPE_NONE, - MISSION_TYPE_ONBOARD, - MISSION_TYPE_OFFBOARD, - } _current_mission_type; + /** + * Set a loiter item, if possible reuse the position setpoint, otherwise take the current position + * + * @param the position setpoint triplet to set + * @return true if setpoint has changed + */ + bool set_loiter_item(position_setpoint_triplet_s *pos_sp_triplet); - int _mission_result_pub; + bool _waypoint_position_reached; + bool _waypoint_yaw_reached; + hrt_abstime _time_first_inside_orbit; - struct mission_result_s _mission_result; + mission_item_s _mission_item; + bool _mission_item_valid; }; #endif diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index e1a6854b2..dd7f4c801 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -215,11 +215,12 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size // float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; + return false; } void MissionFeasibilityChecker::updateNavigationCapabilities() { - int res = orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); + (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } void MissionFeasibilityChecker::init() diff --git a/src/modules/navigator/navigator_state.h b/src/modules/navigator/mission_params.c index 476f93414..8692328db 100644 --- a/src/modules/navigator/navigator_state.h +++ b/src/modules/navigator/mission_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2014 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 @@ -32,24 +32,38 @@ ****************************************************************************/ /** - * @file navigator_state.h + * @file mission_params.c * - * Navigator state + * Parameters for mission. * - * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <joes@student.ethz.ch> */ -#ifndef NAVIGATOR_STATE_H_ -#define NAVIGATOR_STATE_H_ +#include <nuttx/config.h> -typedef enum { - NAV_STATE_NONE = 0, - NAV_STATE_READY, - NAV_STATE_LOITER, - NAV_STATE_MISSION, - NAV_STATE_RTL, - NAV_STATE_LAND, - NAV_STATE_MAX -} nav_state_t; +#include <systemlib/param/param.h> -#endif /* NAVIGATOR_STATE_H_ */ +/* + * Mission parameters, accessible via MAVLink + */ + +/** + * Take-off altitude + * + * Even if first waypoint has altitude less then MIS_TAKEOFF_ALT above home position, system will climb to + * MIS_TAKEOFF_ALT on takeoff, then go to waypoint. + * + * @unit meters + * @group Mission + */ +PARAM_DEFINE_FLOAT(MIS_TAKEOFF_ALT, 10.0f); + + +/** + * Enable onboard mission + * + * @min 0 + * @max 1 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 0); diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index 6ea9dec2b..a1e109030 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2014 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 @@ -39,7 +39,13 @@ MODULE_COMMAND = navigator SRCS = navigator_main.cpp \ navigator_params.c \ - navigator_mission.cpp \ + navigator_mode.cpp \ + mission_block.cpp \ + mission.cpp \ + mission_params.c \ + loiter.cpp \ + rtl.cpp \ + rtl_params.c \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h new file mode 100644 index 000000000..184ecd365 --- /dev/null +++ b/src/modules/navigator/navigator.h @@ -0,0 +1,219 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 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 navigator.h + * Helper class to access missions + * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef NAVIGATOR_H +#define NAVIGATOR_H + +#include <systemlib/perf_counter.h> + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/position_setpoint_triplet.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/parameter_update.h> + +#include "navigator_mode.h" +#include "mission.h" +#include "loiter.h" +#include "rtl.h" +#include "geofence.h" + +/** + * Number of navigation modes that need on_active/on_inactive calls + * Currently: mission, loiter, and rtl + */ +#define NAVIGATOR_MODE_ARRAY_SIZE 3 + +class Navigator : public control::SuperBlock +{ +public: + /** + * Constructor + */ + Navigator(); + + /** + * Destructor, also kills the navigators task. + */ + ~Navigator(); + + /** + * Start the navigator task. + * + * @return OK on success. + */ + int start(); + + /** + * Display the navigator status. + */ + void status(); + + /** + * Add point to geofence + */ + void add_fence_point(int argc, char *argv[]); + + /** + * Load fence from file + */ + void load_fence_from_file(const char *filename); + + /** + * Setters + */ + void set_can_loiter_at_sp(bool can_loiter) { _can_loiter_at_sp = can_loiter; } + + /** + * Getters + */ + struct vehicle_status_s* get_vstatus() { return &_vstatus; } + struct vehicle_global_position_s* get_global_position() { return &_global_pos; } + struct home_position_s* get_home_position() { return &_home_pos; } + int get_onboard_mission_sub() { return _onboard_mission_sub; } + int get_offboard_mission_sub() { return _offboard_mission_sub; } + Geofence& get_geofence() { return _geofence; } + bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } + float get_loiter_radius() { return _param_loiter_radius.get(); } + float get_acceptance_radius() { return _param_acceptance_radius.get(); } + int get_mavlink_fd() { return _mavlink_fd; } + +private: + + bool _task_should_exit; /**< if true, sensor task should exit */ + int _navigator_task; /**< task handle for sensor task */ + + int _mavlink_fd; /**< the file descriptor to send messages over mavlink */ + + int _global_pos_sub; /**< global position subscription */ + int _home_pos_sub; /**< home position subscription */ + int _vstatus_sub; /**< vehicle status subscription */ + int _capabilities_sub; /**< notification of vehicle capabilities updates */ + int _control_mode_sub; /**< vehicle control mode subscription */ + int _onboard_mission_sub; /**< onboard mission subscription */ + int _offboard_mission_sub; /**< offboard mission subscription */ + int _param_update_sub; /**< param update subscription */ + + orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ + + vehicle_status_s _vstatus; /**< vehicle status */ + vehicle_control_mode_s _control_mode; /**< vehicle control mode */ + vehicle_global_position_s _global_pos; /**< global vehicle position */ + home_position_s _home_pos; /**< home position for RTL */ + mission_item_s _mission_item; /**< current mission item */ + navigation_capabilities_s _nav_caps; /**< navigation capabilities */ + position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ + + bool _mission_item_valid; /**< flags if the current mission item is valid */ + + perf_counter_t _loop_perf; /**< loop performance counter */ + + Geofence _geofence; /**< class that handles the geofence */ + bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ + + bool _fence_valid; /**< flag if fence is valid */ + bool _inside_fence; /**< vehicle is inside fence */ + + NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ + Mission _mission; /**< class that handles the missions */ + Loiter _loiter; /**< class that handles loiter */ + RTL _rtl; /**< class that handles RTL */ + + NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ + + bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */ + bool _update_triplet; /**< flags if position SP triplet needs to be published */ + + control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */ + control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */ + /** + * Retrieve global position + */ + void global_position_update(); + + /** + * Retrieve home position + */ + void home_position_update(); + + /** + * Retreive navigation capabilities + */ + void navigation_capabilities_update(); + + /** + * Retrieve vehicle status + */ + void vehicle_status_update(); + + /** + * Retrieve vehicle control mode + */ + void vehicle_control_mode_update(); + + /** + * Update parameters + */ + void params_update(); + + /** + * Shim for calling task_main from task_create. + */ + static void task_main_trampoline(int argc, char *argv[]); + + /** + * Main task. + */ + void task_main(); + + /** + * Translate mission item to a position setpoint. + */ + void mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp); + + /** + * Publish a new position setpoint triplet for position controllers + */ + void publish_position_setpoint_triplet(); +}; +#endif diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 06e0c5904..546602741 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -32,18 +32,18 @@ ****************************************************************************/ /** * @file navigator_main.cpp - * Implementation of the main navigation state machine. * - * Handles missions, geo fencing and failsafe navigation behavior. - * Published the mission item triplet for the position controller. + * Handles mission items, geo fencing and failsafe navigation behavior. + * Published the position setpoint triplet for the position controller. * * @author Lorenz Meier <lm@inf.ethz.ch> * @author Jean Cyr <jean.m.cyr@gmail.com> - * @author Julian Oes <joes@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> * @author Anton Babushkin <anton.babushkin@me.com> */ #include <nuttx/config.h> + #include <stdio.h> #include <stdlib.h> #include <string.h> @@ -54,42 +54,28 @@ #include <poll.h> #include <time.h> #include <sys/ioctl.h> +#include <sys/types.h> +#include <sys/stat.h> + #include <drivers/device/device.h> #include <drivers/drv_hrt.h> #include <arch/board/board.h> + #include <uORB/uORB.h> -#include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/home_position.h> -#include <uORB/topics/position_setpoint_triplet.h> #include <uORB/topics/vehicle_status.h> -#include <uORB/topics/vehicle_control_mode.h> -#include <uORB/topics/parameter_update.h> #include <uORB/topics/mission.h> #include <uORB/topics/fence.h> #include <uORB/topics/navigation_capabilities.h> -#include <systemlib/param/param.h> + #include <systemlib/err.h> -#include <systemlib/state_table.h> -#include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <geo/geo.h> -#include <mathlib/mathlib.h> #include <dataman/dataman.h> +#include <mathlib/mathlib.h> #include <mavlink/mavlink_log.h> -#include <sys/types.h> -#include <sys/stat.h> - -#include "navigator_state.h" -#include "navigator_mission.h" -#include "mission_feasibility_checker.h" -#include "geofence.h" - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; +#include "navigator.h" /** * navigator app start / stop handling function @@ -98,342 +84,53 @@ static const int ERROR = -1; */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); -class Navigator : public StateTable -{ -public: - /** - * Constructor - */ - Navigator(); - - /** - * Destructor, also kills the navigators task. - */ - ~Navigator(); - - /** - * Start the navigator task. - * - * @return OK on success. - */ - int start(); - - /** - * Display the navigator status. - */ - void status(); - - /** - * Add point to geofence - */ - void add_fence_point(int argc, char *argv[]); - - /** - * Load fence from file - */ - void load_fence_from_file(const char *filename); - -private: - - bool _task_should_exit; /**< if true, sensor task should exit */ - int _navigator_task; /**< task handle for sensor task */ - - int _mavlink_fd; - - int _global_pos_sub; /**< global position subscription */ - int _home_pos_sub; /**< home position subscription */ - int _vstatus_sub; /**< vehicle status subscription */ - int _params_sub; /**< notification of parameter updates */ - int _offboard_mission_sub; /**< notification of offboard mission updates */ - int _onboard_mission_sub; /**< notification of onboard mission updates */ - int _capabilities_sub; /**< notification of vehicle capabilities updates */ - int _control_mode_sub; /**< vehicle control mode subscription */ - - orb_advert_t _pos_sp_triplet_pub; /**< publish position setpoint triplet */ - orb_advert_t _mission_result_pub; /**< publish mission result topic */ - - struct vehicle_status_s _vstatus; /**< vehicle status */ - struct vehicle_control_mode_s _control_mode; /**< vehicle control mode */ - struct vehicle_global_position_s _global_pos; /**< global vehicle position */ - struct home_position_s _home_pos; /**< home position for RTL */ - struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of position setpoints */ - struct mission_result_s _mission_result; /**< mission result for commander/mavlink */ - struct mission_item_s _mission_item; /**< current mission item */ - - perf_counter_t _loop_perf; /**< loop performance counter */ - - Geofence _geofence; - bool _geofence_violation_warning_sent; - - bool _fence_valid; /**< flag if fence is valid */ - bool _inside_fence; /**< vehicle is inside fence */ - - struct navigation_capabilities_s _nav_caps; - - class Mission _mission; - - bool _mission_item_valid; /**< current mission item valid */ - bool _global_pos_valid; /**< track changes of global_position */ - bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */ - bool _waypoint_position_reached; - bool _waypoint_yaw_reached; - uint64_t _time_first_inside_orbit; - bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ - bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ - - MissionFeasibilityChecker missionFeasiblityChecker; - - uint64_t _set_nav_state_timestamp; /**< timestamp of last handled navigation state request */ - - bool _pos_sp_triplet_updated; - - const char *nav_states_str[NAV_STATE_MAX]; - - struct { - float min_altitude; - float acceptance_radius; - float loiter_radius; - int onboard_mission_enabled; - float takeoff_alt; - float land_alt; - float rtl_alt; - float rtl_land_delay; - } _parameters; /**< local copies of parameters */ - - struct { - param_t min_altitude; - param_t acceptance_radius; - param_t loiter_radius; - param_t onboard_mission_enabled; - param_t takeoff_alt; - param_t land_alt; - param_t rtl_alt; - param_t rtl_land_delay; - } _parameter_handles; /**< handles for parameters */ - - enum Event { - EVENT_NONE_REQUESTED, - EVENT_READY_REQUESTED, - EVENT_LOITER_REQUESTED, - EVENT_MISSION_REQUESTED, - EVENT_RTL_REQUESTED, - EVENT_LAND_REQUESTED, - EVENT_MISSION_CHANGED, - EVENT_HOME_POSITION_CHANGED, - MAX_EVENT - }; - - /** - * State machine transition table - */ - static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; - - enum RTLState { - RTL_STATE_NONE = 0, - RTL_STATE_CLIMB, - RTL_STATE_RETURN, - RTL_STATE_DESCEND - }; - - enum RTLState _rtl_state; - - /** - * Update our local parameter cache. - */ - void parameters_update(); - - /** - * Retrieve global position - */ - void global_position_update(); - - /** - * Retrieve home position - */ - void home_position_update(); - - /** - * Retreive navigation capabilities - */ - void navigation_capabilities_update(); - - /** - * Retrieve offboard mission. - */ - void offboard_mission_update(bool isrotaryWing); - - /** - * Retrieve onboard mission. - */ - void onboard_mission_update(); - - /** - * Retrieve vehicle status - */ - void vehicle_status_update(); - - /** - * Retrieve vehicle control mode - */ - void vehicle_control_mode_update(); - - /** - * Shim for calling task_main from task_create. - */ - static void task_main_trampoline(int argc, char *argv[]); - - /** - * Main task. - */ - void task_main(); - - void publish_safepoints(unsigned points); - - /** - * Functions that are triggered when a new state is entered. - */ - void start_none(); - void start_ready(); - void start_loiter(); - void start_mission(); - void start_rtl(); - void start_land(); - void start_land_home(); - - /** - * Fork for state transitions - */ - void request_loiter_or_ready(); - void request_mission_if_available(); - - /** - * Guards offboard mission - */ - bool offboard_mission_available(unsigned relative_index); - - /** - * Guards onboard mission - */ - bool onboard_mission_available(unsigned relative_index); - - /** - * Reset all "reached" flags. - */ - void reset_reached(); - - /** - * Check if current mission item has been reached. - */ - bool check_mission_item_reached(); - - /** - * Perform actions when current mission item reached. - */ - void on_mission_item_reached(); - - /** - * Move to next waypoint - */ - void set_mission_item(); - - /** - * Switch to next RTL state - */ - void set_rtl_item(); - - /** - * Set position setpoint for mission item - */ - void position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item); - - /** - * Helper function to get a takeoff item - */ - void get_takeoff_setpoint(position_setpoint_s *pos_sp); - - /** - * Publish a new mission item triplet for position controller - */ - void publish_position_setpoint_triplet(); -}; namespace navigator { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - Navigator *g_navigator; } Navigator::Navigator() : - -/* state machine transition table */ - StateTable(&myTable[0][0], NAV_STATE_MAX, MAX_EVENT), - + SuperBlock(NULL, "NAV"), _task_should_exit(false), _navigator_task(-1), _mavlink_fd(-1), - -/* subscriptions */ _global_pos_sub(-1), _home_pos_sub(-1), _vstatus_sub(-1), - _params_sub(-1), - _offboard_mission_sub(-1), - _onboard_mission_sub(-1), _capabilities_sub(-1), _control_mode_sub(-1), - -/* publications */ + _onboard_mission_sub(-1), + _offboard_mission_sub(-1), _pos_sp_triplet_pub(-1), - -/* performance counters */ + _vstatus({}), + _control_mode({}), + _global_pos({}), + _home_pos({}), + _mission_item({}), + _nav_caps({}), + _pos_sp_triplet({}), + _mission_item_valid(false), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), - + _geofence({}), _geofence_violation_warning_sent(false), _fence_valid(false), _inside_fence(true), - _mission(), - _mission_item_valid(false), - _global_pos_valid(false), - _reset_loiter_pos(true), - _waypoint_position_reached(false), - _waypoint_yaw_reached(false), - _time_first_inside_orbit(0), - _need_takeoff(true), - _do_takeoff(false), - _set_nav_state_timestamp(0), - _pos_sp_triplet_updated(false), -/* states */ - _rtl_state(RTL_STATE_NONE) + _navigation_mode(nullptr), + _mission(this, "MIS"), + _loiter(this, "LOI"), + _rtl(this, "RTL"), + _update_triplet(false), + _param_loiter_radius(this, "LOITER_RAD"), + _param_acceptance_radius(this, "ACC_RAD") { - _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); - _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); - _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); - _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); - _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); - _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); - _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); - _parameter_handles.rtl_land_delay = param_find("NAV_RTL_LAND_T"); - - memset(&_pos_sp_triplet, 0, sizeof(struct position_setpoint_triplet_s)); - memset(&_mission_item, 0, sizeof(struct mission_item_s)); - - memset(&nav_states_str, 0, sizeof(nav_states_str)); - nav_states_str[0] = "NONE"; - nav_states_str[1] = "READY"; - nav_states_str[2] = "LOITER"; - nav_states_str[3] = "MISSION"; - nav_states_str[4] = "RTL"; - nav_states_str[5] = "LAND"; - - /* Initialize state machine */ - myState = NAV_STATE_NONE; - start_none(); + /* Create a list of our possible navigation types */ + _navigation_mode_array[0] = &_mission; + _navigation_mode_array[1] = &_loiter; + _navigation_mode_array[2] = &_rtl; + + updateParams(); } Navigator::~Navigator() @@ -462,27 +159,6 @@ Navigator::~Navigator() } void -Navigator::parameters_update() -{ - /* read from param to clear updated flag */ - struct parameter_update_s update; - orb_copy(ORB_ID(parameter_update), _params_sub, &update); - - param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); - param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); - param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); - param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); - param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); - param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); - param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); - param_get(_parameter_handles.rtl_land_delay, &(_parameters.rtl_land_delay)); - - _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); - - _geofence.updateParams(); -} - -void Navigator::global_position_update() { orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos); @@ -500,56 +176,6 @@ Navigator::navigation_capabilities_update() orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); } - -void -Navigator::offboard_mission_update(bool isrotaryWing) -{ - struct mission_s offboard_mission; - - if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &offboard_mission) == OK) { - - /* Check mission feasibility, for now do not handle the return value, - * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ - dm_item_t dm_current; - - if (offboard_mission.dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt); - - _mission.set_offboard_dataman_id(offboard_mission.dataman_id); - - _mission.set_offboard_mission_count(offboard_mission.count); - _mission.set_current_offboard_mission_index(offboard_mission.current_index); - - } else { - _mission.set_offboard_mission_count(0); - _mission.set_current_offboard_mission_index(0); - } - - _mission.publish_mission_result(); -} - -void -Navigator::onboard_mission_update() -{ - struct mission_s onboard_mission; - - if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) { - - _mission.set_onboard_mission_count(onboard_mission.count); - _mission.set_current_onboard_mission_index(onboard_mission.current_index); - - } else { - _mission.set_onboard_mission_count(0); - _mission.set_current_onboard_mission_index(0); - } -} - void Navigator::vehicle_status_update() { @@ -570,6 +196,13 @@ Navigator::vehicle_control_mode_update() } void +Navigator::params_update() +{ + parameter_update_s param_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶m_update); +} + +void Navigator::task_main_trampoline(int argc, char *argv[]) { navigator::g_navigator->task_main(); @@ -580,16 +213,12 @@ Navigator::task_main() { /* inform about start */ warnx("Initializing.."); - fflush(stdout); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); - mavlink_log_info(_mavlink_fd, "[navigator] started"); - /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file - * else clear geofence data in datamanager - */ + * else clear geofence data in datamanager */ struct stat buffer; if (stat(GEOFENCE_FILENAME, &buffer) == 0) { @@ -603,68 +232,58 @@ Navigator::task_main() warnx("Could not clear geofence"); } - /* - * do subscriptions - */ + /* do subscriptions */ _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); - _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); _home_pos_sub = orb_subscribe(ORB_ID(home_position)); + _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); + _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); + _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); /* copy all topics first time */ vehicle_status_update(); vehicle_control_mode_update(); - parameters_update(); global_position_update(); home_position_update(); navigation_capabilities_update(); - offboard_mission_update(_vstatus.is_rotary_wing); - onboard_mission_update(); + params_update(); /* rate limit position updates to 50 Hz */ orb_set_interval(_global_pos_sub, 20); - unsigned prevState = NAV_STATE_NONE; hrt_abstime mavlink_open_time = 0; const hrt_abstime mavlink_open_interval = 500000; /* wakeup source(s) */ - struct pollfd fds[8]; + struct pollfd fds[6]; /* Setup of loop */ - fds[0].fd = _params_sub; + fds[0].fd = _global_pos_sub; fds[0].events = POLLIN; - fds[1].fd = _global_pos_sub; + fds[1].fd = _home_pos_sub; fds[1].events = POLLIN; - fds[2].fd = _home_pos_sub; + fds[2].fd = _capabilities_sub; fds[2].events = POLLIN; - fds[3].fd = _capabilities_sub; + fds[3].fd = _vstatus_sub; fds[3].events = POLLIN; - fds[4].fd = _offboard_mission_sub; + fds[4].fd = _control_mode_sub; fds[4].events = POLLIN; - fds[5].fd = _onboard_mission_sub; + fds[5].fd = _param_update_sub; fds[5].events = POLLIN; - fds[6].fd = _vstatus_sub; - fds[6].events = POLLIN; - fds[7].fd = _control_mode_sub; - fds[7].events = POLLIN; while (!_task_should_exit) { /* wait for up to 100ms for data */ int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - /* timed out - periodic check for _task_should_exit, etc. */ if (pret == 0) { + /* timed out - periodic check for _task_should_exit, etc. */ continue; - } - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { + } else if (pret < 0) { + /* this is undesirable but not much we can do - might want to flag unhappy status */ warn("poll error %d, %d", pret, errno); continue; } @@ -677,162 +296,103 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + /* parameters updated */ + if (fds[5].revents & POLLIN) { + params_update(); + updateParams(); + } + /* vehicle control mode updated */ - if (fds[7].revents & POLLIN) { + if (fds[4].revents & POLLIN) { vehicle_control_mode_update(); } /* vehicle status updated */ - if (fds[6].revents & POLLIN) { + if (fds[3].revents & POLLIN) { vehicle_status_update(); - - /* evaluate state requested by commander */ - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - /* publish position setpoint triplet on each status update if navigator active */ - _pos_sp_triplet_updated = true; - - if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) { - /* commander requested new navigation mode, try to set it */ - switch (_vstatus.set_nav_state) { - case NAV_STATE_NONE: - /* nothing to do */ - break; - - case NAV_STATE_LOITER: - request_loiter_or_ready(); - break; - - case NAV_STATE_MISSION: - request_mission_if_available(); - break; - - case NAV_STATE_RTL: - if (!(_rtl_state == RTL_STATE_DESCEND && - (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) && - _vstatus.condition_home_position_valid) { - dispatch(EVENT_RTL_REQUESTED); - } - - break; - - case NAV_STATE_LAND: - dispatch(EVENT_LAND_REQUESTED); - - break; - - default: - warnx("ERROR: Requested navigation state not supported"); - break; - } - - } else { - /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ - if (myState == NAV_STATE_NONE) { - request_mission_if_available(); - } - } - - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { - if (check_mission_item_reached()) { - on_mission_item_reached(); - } - } - - } else { - /* navigator shouldn't act */ - dispatch(EVENT_NONE_REQUESTED); - } - - _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp; - } - - /* parameters updated */ - if (fds[0].revents & POLLIN) { - parameters_update(); - /* note that these new parameters won't be in effect until a mission triplet is published again */ } /* navigation capabilities updated */ - if (fds[3].revents & POLLIN) { + if (fds[2].revents & POLLIN) { navigation_capabilities_update(); } - /* offboard mission updated */ - if (fds[4].revents & POLLIN) { - offboard_mission_update(_vstatus.is_rotary_wing); - // XXX check if mission really changed - dispatch(EVENT_MISSION_CHANGED); - } - - /* onboard mission updated */ - if (fds[5].revents & POLLIN) { - onboard_mission_update(); - // XXX check if mission really changed - dispatch(EVENT_MISSION_CHANGED); - } - /* home position updated */ - if (fds[2].revents & POLLIN) { + if (fds[1].revents & POLLIN) { home_position_update(); - // XXX check if home position really changed - dispatch(EVENT_HOME_POSITION_CHANGED); } /* global position updated */ - if (fds[1].revents & POLLIN) { + if (fds[0].revents & POLLIN) { global_position_update(); - if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) { - /* publish position setpoint triplet on each position update if navigator active */ - _pos_sp_triplet_updated = true; - - if (myState == NAV_STATE_LAND && !_global_pos_valid) { - /* got global position when landing, update setpoint */ - start_land(); - } - - /* check if waypoint has been reached in MISSION, RTL and LAND modes */ - if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) { - if (check_mission_item_reached()) { - on_mission_item_reached(); - } - } - } - /* Check geofence violation */ if (!_geofence.inside(&_global_pos)) { - //xxx: publish geofence violation here (or change local flag depending on which app handles the flight termination) /* Issue a warning about the geofence violation once */ if (!_geofence_violation_warning_sent) { mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation"); _geofence_violation_warning_sent = true; } - } else { /* Reset the _geofence_violation_warning_sent field */ _geofence_violation_warning_sent = false; } } - _global_pos_valid = _vstatus.condition_global_position_valid; + /* Do stuff according to navigation state set by commander */ + switch (_vstatus.nav_state) { + case NAVIGATION_STATE_MANUAL: + case NAVIGATION_STATE_ACRO: + case NAVIGATION_STATE_ALTCTL: + case NAVIGATION_STATE_POSCTL: + _navigation_mode = nullptr; + _can_loiter_at_sp = false; + break; + case NAVIGATION_STATE_AUTO_MISSION: + _navigation_mode = &_mission; + break; + case NAVIGATION_STATE_AUTO_LOITER: + _navigation_mode = &_loiter; + break; + case NAVIGATION_STATE_AUTO_RTL: + _navigation_mode = &_rtl; + break; + case NAVIGATION_STATE_AUTO_RTGS: + _navigation_mode = &_rtl; /* TODO: change this to something else */ + break; + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: + default: + _navigation_mode = nullptr; + _can_loiter_at_sp = false; + break; + } - /* publish position setpoint triplet if updated */ - if (_pos_sp_triplet_updated) { - _pos_sp_triplet_updated = false; - publish_position_setpoint_triplet(); + /* iterate through navigation modes and set active/inactive for each */ + for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) { + if (_navigation_mode == _navigation_mode_array[i]) { + _update_triplet = _navigation_mode_array[i]->on_active(&_pos_sp_triplet); + } else { + _navigation_mode_array[i]->on_inactive(); + } } - /* notify user about state changes */ - if (myState != prevState) { - mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]); - prevState = myState; + /* if nothing is running, set position setpoint triplet invalid */ + if (_navigation_mode == nullptr) { + _pos_sp_triplet.previous.valid = false; + _pos_sp_triplet.current.valid = false; + _pos_sp_triplet.next.valid = false; + _update_triplet = true; + } + + if (_update_triplet) { + publish_position_setpoint_triplet(); + _update_triplet = false; } perf_end(_loop_perf); } - warnx("exiting."); _navigator_task = -1; @@ -863,19 +423,21 @@ Navigator::start() void Navigator::status() { - warnx("Global position: %svalid", _global_pos_valid ? "" : "in"); - - if (_global_pos_valid) { - warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); - warnx("Altitude %5.5f meters, altitude above home %5.5f meters", - (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); - warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", - (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); - warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); - } + /* TODO: add this again */ + // warnx("Global position is %svalid", _global_pos_valid ? "" : "in"); + + // if (_global_pos.global_valid) { + // warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat); + // warnx("Altitude %5.5f meters, altitude above home %5.5f meters", + // (double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt)); + // warnx("Ground velocity in m/s, N %5.5f, E %5.5f, D %5.5f", + // (double)_global_pos.vel_n, (double)_global_pos.vel_e, (double)_global_pos.vel_d); + // warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); + // } if (_fence_valid) { warnx("Geofence is valid"); + /* TODO: needed? */ // warnx("Vertex longitude latitude"); // for (unsigned i = 0; i < _fence.count; i++) // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); @@ -883,747 +445,19 @@ Navigator::status() } else { warnx("Geofence not set"); } - - switch (myState) { - case NAV_STATE_NONE: - warnx("State: None"); - break; - - case NAV_STATE_LOITER: - warnx("State: Loiter"); - break; - - case NAV_STATE_MISSION: - warnx("State: Mission"); - break; - - case NAV_STATE_RTL: - warnx("State: RTL"); - break; - - default: - warnx("State: Unknown"); - break; - } -} - -StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = { - { - /* NAV_STATE_NONE */ - /* EVENT_NONE_REQUESTED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_NONE}, - }, - { - /* NAV_STATE_READY */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_READY}, - }, - { - /* NAV_STATE_LOITER */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_LOITER_REQUESTED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LOITER}, - }, - { - /* NAV_STATE_MISSION */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {NO_ACTION, NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_MISSION}, - }, - { - /* NAV_STATE_RTL */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {ACTION(&Navigator::start_land_home), NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_RTL}, - /* EVENT_HOME_POSITION_CHANGED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, // TODO need to reset rtl_state - }, - { - /* NAV_STATE_LAND */ - /* EVENT_NONE_REQUESTED */ {ACTION(&Navigator::start_none), NAV_STATE_NONE}, - /* EVENT_READY_REQUESTED */ {ACTION(&Navigator::start_ready), NAV_STATE_READY}, - /* EVENT_LOITER_REQUESTED */ {ACTION(&Navigator::start_loiter), NAV_STATE_LOITER}, - /* EVENT_MISSION_REQUESTED */ {ACTION(&Navigator::start_mission), NAV_STATE_MISSION}, - /* EVENT_RTL_REQUESTED */ {ACTION(&Navigator::start_rtl), NAV_STATE_RTL}, - /* EVENT_LAND_REQUESTED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_MISSION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - /* EVENT_HOME_POSITION_CHANGED */ {NO_ACTION, NAV_STATE_LAND}, - }, -}; - -void -Navigator::start_none() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = false; - _pos_sp_triplet.next.valid = false; - _mission_item_valid = false; - - _reset_loiter_pos = true; - _do_takeoff = false; - _rtl_state = RTL_STATE_NONE; - - _pos_sp_triplet_updated = true; -} - -void -Navigator::start_ready() -{ - reset_reached(); - - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = false; - - _pos_sp_triplet.current.type = SETPOINT_TYPE_IDLE; - - _mission_item_valid = false; - - _reset_loiter_pos = true; - _do_takeoff = false; - - if (_rtl_state != RTL_STATE_DESCEND) { - /* reset RTL state if landed not at home */ - _rtl_state = RTL_STATE_NONE; - } - - _pos_sp_triplet_updated = true; -} - -void -Navigator::start_loiter() -{ - reset_reached(); - - _do_takeoff = false; - - /* set loiter position if needed */ - if (_reset_loiter_pos || !_pos_sp_triplet.current.valid) { - _reset_loiter_pos = false; - - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.yaw = NAN; // NAN means to use current yaw - - float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; - - /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { - _pos_sp_triplet.current.alt = min_alt_amsl; - mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); - - } else { - _pos_sp_triplet.current.alt = _global_pos.alt; - mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude"); - } - - } - _pos_sp_triplet.current.type = SETPOINT_TYPE_LOITER; - _pos_sp_triplet.current.loiter_radius = _parameters.loiter_radius; - _pos_sp_triplet.current.loiter_direction = 1; - _pos_sp_triplet.previous.valid = false; - _pos_sp_triplet.current.valid = true; - _pos_sp_triplet.next.valid = false; - _mission_item_valid = false; - - _pos_sp_triplet_updated = true; -} - -void -Navigator::start_mission() -{ - _need_takeoff = true; - - set_mission_item(); -} - -void -Navigator::set_mission_item() -{ - reset_reached(); - - /* copy current mission to previous item */ - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _reset_loiter_pos = true; - _do_takeoff = false; - - int ret; - bool onboard; - unsigned index; - - ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index); - - if (ret == OK) { - _mission.report_current_offboard_mission_item(); - - _mission_item_valid = true; - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - if (_mission_item.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && - _mission_item.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && - _mission_item.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && - _mission_item.nav_cmd != NAV_CMD_LOITER_UNLIMITED) { - /* don't reset RTL state on RTL or LOITER items */ - _rtl_state = RTL_STATE_NONE; - } - - if (_vstatus.is_rotary_wing) { - if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED - )) { - /* do special TAKEOFF handling for VTOL */ - _need_takeoff = false; - - /* calculate desired takeoff altitude AMSL */ - float takeoff_alt_amsl = _pos_sp_triplet.current.alt; - - if (_vstatus.condition_landed) { - /* takeoff to at least NAV_TAKEOFF_ALT from ground if landed */ - takeoff_alt_amsl = fmaxf(takeoff_alt_amsl, _global_pos.alt + _parameters.takeoff_alt); - } - - /* check if we really need takeoff */ - if (_vstatus.condition_landed || _global_pos.alt < takeoff_alt_amsl - _mission_item.acceptance_radius) { - /* force TAKEOFF if landed or waypoint altitude is more than current */ - _do_takeoff = true; - - /* move current position setpoint to next */ - memcpy(&_pos_sp_triplet.next, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - /* set current setpoint to takeoff */ - - _pos_sp_triplet.current.lat = _global_pos.lat; - _pos_sp_triplet.current.lon = _global_pos.lon; - _pos_sp_triplet.current.alt = takeoff_alt_amsl; - _pos_sp_triplet.current.yaw = NAN; - _pos_sp_triplet.current.type = SETPOINT_TYPE_TAKEOFF; - } - - } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { - /* will need takeoff after landing */ - _need_takeoff = true; - } - } - - if (_do_takeoff) { - mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt)); - - } else { - if (onboard) { - mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index); - - } else { - mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index); - } - } - - } else { - /* since a mission is not advanced without WPs available, this is not supposed to happen */ - _mission_item_valid = false; - _pos_sp_triplet.current.valid = false; - warnx("ERROR: current WP can't be set"); - } - - if (!_do_takeoff) { - mission_item_s item_next; - ret = _mission.get_next_mission_item(&item_next); - - if (ret == OK) { - position_setpoint_from_mission_item(&_pos_sp_triplet.next, &item_next); - - } else { - /* this will fail for the last WP */ - _pos_sp_triplet.next.valid = false; - } - } - - _pos_sp_triplet_updated = true; -} - -void -Navigator::start_rtl() -{ - _do_takeoff = false; - - /* decide if we need climb */ - if (_rtl_state == RTL_STATE_NONE) { - if (_global_pos.alt < _home_pos.alt + _parameters.rtl_alt) { - _rtl_state = RTL_STATE_CLIMB; - - } else { - _rtl_state = RTL_STATE_RETURN; - } - } - - /* if switching directly to return state, reset altitude setpoint */ - if (_rtl_state == RTL_STATE_RETURN) { - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; - } - - _reset_loiter_pos = true; - set_rtl_item(); -} - -void -Navigator::start_land() -{ - reset_reached(); - - /* this state can be requested by commander even if no global position available, - * in his case controller must perform landing without position control */ - _do_takeoff = false; - _reset_loiter_pos = true; - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _global_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; -} - -void -Navigator::start_land_home() -{ - reset_reached(); - - /* land to home position, should be called when hovering above home, from RTL state */ - _do_takeoff = false; - _reset_loiter_pos = true; - - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LAND; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; -} - -void -Navigator::set_rtl_item() -{ - reset_reached(); - - switch (_rtl_state) { - case RTL_STATE_CLIMB: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - float climb_alt = _home_pos.alt + _parameters.rtl_alt; - - if (_vstatus.condition_landed) { - climb_alt = fmaxf(climb_alt, _global_pos.alt + _parameters.rtl_alt); - } - - _mission_item_valid = true; - - _mission_item.lat = _global_pos.lat; - _mission_item.lon = _global_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = climb_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt)); - break; - } - - case RTL_STATE_RETURN: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - // don't change altitude - if (_pos_sp_triplet.previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - _mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon); - - } else { - /* else use current position */ - _mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon); - } - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = 0.0f; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = true; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - case RTL_STATE_DESCEND: { - memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s)); - - _mission_item_valid = true; - - _mission_item.lat = _home_pos.lat; - _mission_item.lon = _home_pos.lon; - _mission_item.altitude_is_relative = false; - _mission_item.altitude = _home_pos.alt + _parameters.land_alt; - _mission_item.yaw = NAN; - _mission_item.loiter_radius = _parameters.loiter_radius; - _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - _mission_item.acceptance_radius = _parameters.acceptance_radius; - _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; - _mission_item.pitch_min = 0.0f; - _mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f; - _mission_item.origin = ORIGIN_ONBOARD; - - position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item); - - _pos_sp_triplet.next.valid = false; - - mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt)); - break; - } - - default: { - mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state); - start_loiter(); - break; - } - } - - _pos_sp_triplet_updated = true; -} - -void -Navigator::request_loiter_or_ready() -{ - /* XXX workaround: no landing detector for fixedwing yet */ - if (_vstatus.condition_landed && _vstatus.is_rotary_wing) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } -} - -void -Navigator::request_mission_if_available() -{ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - request_loiter_or_ready(); - } -} - -void -Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) -{ - sp->valid = true; - - if (item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* set home position for RTL item */ - sp->lat = _home_pos.lat; - sp->lon = _home_pos.lon; - sp->alt = _home_pos.alt + _parameters.rtl_alt; - - if (_pos_sp_triplet.previous.valid) { - /* if previous setpoint is valid then use it to calculate heading to home */ - sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon); - - } else { - /* else use current position */ - sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon); - } - sp->loiter_radius = _parameters.loiter_radius; - sp->loiter_direction = 1; - sp->pitch_min = 0.0f; - - } else { - sp->lat = item->lat; - sp->lon = item->lon; - sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude; - sp->yaw = item->yaw; - sp->loiter_radius = item->loiter_radius; - sp->loiter_direction = item->loiter_direction; - sp->pitch_min = item->pitch_min; - } - - if (item->nav_cmd == NAV_CMD_TAKEOFF) { - sp->type = SETPOINT_TYPE_TAKEOFF; - - } else if (item->nav_cmd == NAV_CMD_LAND) { - sp->type = SETPOINT_TYPE_LAND; - - } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { - sp->type = SETPOINT_TYPE_LOITER; - - } else { - sp->type = SETPOINT_TYPE_NORMAL; - } -} - -bool -Navigator::check_mission_item_reached() -{ - /* only check if there is actually a mission item to check */ - if (!_mission_item_valid) { - return false; - } - - if (_mission_item.nav_cmd == NAV_CMD_LAND) { - return _vstatus.condition_landed; - } - - /* XXX TODO count turns */ - if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && - _mission_item.loiter_radius > 0.01f) { - - return false; - } - - uint64_t now = hrt_absolute_time(); - - if (!_waypoint_position_reached) { - float acceptance_radius; - - if (_mission_item.nav_cmd == NAV_CMD_WAYPOINT && _mission_item.acceptance_radius > 0.01f) { - acceptance_radius = _mission_item.acceptance_radius; - - } else { - acceptance_radius = _parameters.acceptance_radius; - } - - if (_do_takeoff) { - /* require only altitude for takeoff */ - if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) { - _waypoint_position_reached = true; - } - - } else { - float dist = -1.0f; - float dist_xy = -1.0f; - float dist_z = -1.0f; - - /* calculate AMSL altitude for this waypoint */ - float wp_alt_amsl = _mission_item.altitude; - - if (_mission_item.altitude_is_relative) - wp_alt_amsl += _home_pos.alt; - - dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl, - (double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt, - &dist_xy, &dist_z); - - if (dist >= 0.0f && dist <= acceptance_radius) { - _waypoint_position_reached = true; - } - } - } - - if (_waypoint_position_reached && !_waypoint_yaw_reached) { - if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) { - /* check yaw if defined only for rotary wing except takeoff */ - float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw); - - if (fabsf(yaw_err) < 0.2f) { /* XXX get rid of magic number */ - _waypoint_yaw_reached = true; - } - - } else { - _waypoint_yaw_reached = true; - } - } - - /* check if the current waypoint was reached */ - if (_waypoint_position_reached && _waypoint_yaw_reached) { - if (_time_first_inside_orbit == 0) { - _time_first_inside_orbit = now; - - if (_mission_item.time_inside > 0.01f) { - mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside); - } - } - - /* check if the MAV was long enough inside the waypoint orbit */ - if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6) - || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - reset_reached(); - return true; - } - } - - return false; - -} - -void -Navigator::reset_reached() -{ - _time_first_inside_orbit = 0; - _waypoint_position_reached = false; - _waypoint_yaw_reached = false; - -} - -void -Navigator::on_mission_item_reached() -{ - if (myState == NAV_STATE_MISSION) { - - _mission.report_mission_item_reached(); - - if (_do_takeoff) { - /* takeoff completed */ - _do_takeoff = false; - mavlink_log_info(_mavlink_fd, "#audio: takeoff completed"); - - } else { - /* advance by one mission item */ - _mission.move_to_next(); - } - - if (_mission.current_mission_available()) { - if (_mission_item.autocontinue) { - /* continue mission */ - set_mission_item(); - - } else { - /* autocontinue disabled for this item */ - request_loiter_or_ready(); - } - - } else { - /* if no more mission items available then finish mission */ - /* loiter at last waypoint */ - _reset_loiter_pos = false; - mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - request_loiter_or_ready(); - } - - } else if (myState == NAV_STATE_RTL) { - /* RTL completed */ - if (_rtl_state == RTL_STATE_DESCEND) { - /* hovering above home position, land if needed or loiter */ - mavlink_log_info(_mavlink_fd, "[navigator] RTL completed"); - - if (_mission_item.autocontinue) { - dispatch(EVENT_LAND_REQUESTED); - - } else { - _reset_loiter_pos = false; - dispatch(EVENT_LOITER_REQUESTED); - } - - } else { - /* next RTL step */ - _rtl_state = (RTLState)(_rtl_state + 1); - set_rtl_item(); - } - - } else if (myState == NAV_STATE_LAND) { - /* landing completed */ - mavlink_log_info(_mavlink_fd, "[navigator] landing completed"); - dispatch(EVENT_READY_REQUESTED); - } - _mission.publish_mission_result(); } void Navigator::publish_position_setpoint_triplet() { /* update navigation state */ - _pos_sp_triplet.nav_state = static_cast<nav_state_t>(myState); + /* TODO: set nav_state */ /* lazily publish the position setpoint triplet only once available */ if (_pos_sp_triplet_pub > 0) { - /* publish the position setpoint triplet */ orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, &_pos_sp_triplet); } else { - /* advertise and publish */ _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), &_pos_sp_triplet); } } diff --git a/src/modules/navigator/navigator_mission.cpp b/src/modules/navigator/navigator_mission.cpp deleted file mode 100644 index 49fc62785..000000000 --- a/src/modules/navigator/navigator_mission.cpp +++ /dev/null @@ -1,319 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ -/** - * @file navigator_mission.cpp - * Helper class to access missions - * - * @author Julian Oes <joes@student.ethz.ch> - */ - -#include <string.h> -#include <stdlib.h> -#include <dataman/dataman.h> -#include <systemlib/err.h> -#include <uORB/uORB.h> -#include <uORB/topics/mission_result.h> -#include "navigator_mission.h" - -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - - -Mission::Mission() : - - _offboard_dataman_id(-1), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), - _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE), - _mission_result_pub(-1) -{ - memset(&_mission_result, 0, sizeof(struct mission_result_s)); -} - -Mission::~Mission() -{ - -} - -void -Mission::set_offboard_dataman_id(int new_id) -{ - _offboard_dataman_id = new_id; -} - -void -Mission::set_current_offboard_mission_index(int new_index) -{ - if (new_index != -1) { - warnx("specifically set to %d", new_index); - _current_offboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_offboard_mission_index >= _offboard_mission_item_count) { - _current_offboard_mission_index = 0; - } - } - report_current_offboard_mission_item(); -} - -void -Mission::set_current_onboard_mission_index(int new_index) -{ - if (new_index != -1) { - _current_onboard_mission_index = (unsigned)new_index; - } else { - - /* if less WPs available, reset to first WP */ - if (_current_onboard_mission_index >= _onboard_mission_item_count) { - _current_onboard_mission_index = 0; - } - } - // TODO: implement this for onboard missions as well - // report_current_mission_item(); -} - -void -Mission::set_offboard_mission_count(unsigned new_count) -{ - _offboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_count(unsigned new_count) -{ - _onboard_mission_item_count = new_count; -} - -void -Mission::set_onboard_mission_allowed(bool allowed) -{ - _onboard_mission_allowed = allowed; -} - -bool -Mission::current_mission_available() -{ - return (current_onboard_mission_available() || current_offboard_mission_available()); - -} - -bool -Mission::next_mission_available() -{ - return (next_onboard_mission_available() || next_offboard_mission_available()); -} - -int -Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, unsigned *index) -{ - /* try onboard mission first */ - if (current_onboard_mission_available()) { - - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - - /* otherwise fallback to offboard */ - - } else if (current_offboard_mission_available()) { - - dm_item_t dm_current; - - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(dm_current, _current_offboard_mission_index, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - _current_mission_type = MISSION_TYPE_NONE; - return ERROR; - } - - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; - - } else { - /* happens when no more mission items can be added as a next item */ - _current_mission_type = MISSION_TYPE_NONE; - return ERROR; - } - - return OK; -} - -int -Mission::get_next_mission_item(struct mission_item_s *new_mission_item) -{ - /* try onboard mission first */ - if (next_onboard_mission_available()) { - - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(DM_KEY_WAYPOINTS_ONBOARD, _current_onboard_mission_index + 1, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - /* otherwise fallback to offboard */ - - } else if (next_offboard_mission_available()) { - - dm_item_t dm_current; - - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - - const ssize_t len = sizeof(struct mission_item_s); - - if (dm_read(dm_current, _current_offboard_mission_index + 1, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return ERROR; - } - - } else { - /* happens when no more mission items can be added as a next item */ - return ERROR; - } - - return OK; -} - - -bool -Mission::current_onboard_mission_available() -{ - return _onboard_mission_item_count > _current_onboard_mission_index && _onboard_mission_allowed; -} - -bool -Mission::current_offboard_mission_available() -{ - return _offboard_mission_item_count > _current_offboard_mission_index; -} - -bool -Mission::next_onboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_ONBOARD) { - next = 1; - } - - return _onboard_mission_item_count > (_current_onboard_mission_index + next) && _onboard_mission_allowed; -} - -bool -Mission::next_offboard_mission_available() -{ - unsigned next = 0; - - if (_current_mission_type != MISSION_TYPE_OFFBOARD) { - next = 1; - } - - return _offboard_mission_item_count > (_current_offboard_mission_index + next); -} - -void -Mission::move_to_next() -{ - switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - -void -Mission::report_mission_item_reached() -{ - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; - } -} - -void -Mission::report_current_offboard_mission_item() -{ - _mission_result.index_current_mission = _current_offboard_mission_index; -} - -void -Mission::publish_mission_result() -{ - /* lazily publish the mission result only once available */ - if (_mission_result_pub > 0) { - /* publish mission result */ - orb_publish(ORB_ID(mission_result), _mission_result_pub, &_mission_result); - - } else { - /* advertise and publish */ - _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); - } - /* reset reached bool */ - _mission_result.mission_reached = false; -} diff --git a/src/modules/navigator/navigator_mode.cpp b/src/modules/navigator/navigator_mode.cpp new file mode 100644 index 000000000..25e767c2b --- /dev/null +++ b/src/modules/navigator/navigator_mode.cpp @@ -0,0 +1,70 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 navigator_mode.cpp + * + * Helper class for different modes in navigator + * + * @author Julian Oes <julian@oes.ch> + */ + +#include "navigator_mode.h" + +NavigatorMode::NavigatorMode(Navigator *navigator, const char *name) : + SuperBlock(NULL, name), + _navigator(navigator), + _first_run(true) +{ + /* load initial params */ + updateParams(); + /* set initial mission items */ + on_inactive(); +} + +NavigatorMode::~NavigatorMode() +{ +} + +void +NavigatorMode::on_inactive() +{ + _first_run = true; +} + +bool +NavigatorMode::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + pos_sp_triplet->current.valid = false; + _first_run = false; + return false; +} diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h new file mode 100644 index 000000000..cbb53d91b --- /dev/null +++ b/src/modules/navigator/navigator_mode.h @@ -0,0 +1,86 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 navigator_mode.h + * + * Helper class for different modes in navigator + * + * @author Julian Oes <julian@oes.ch> + */ + +#ifndef NAVIGATOR_MODE_H +#define NAVIGATOR_MODE_H + +#include <drivers/drv_hrt.h> + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <dataman/dataman.h> + +#include <uORB/topics/position_setpoint_triplet.h> + +class Navigator; + +class NavigatorMode : public control::SuperBlock +{ +public: + /** + * Constructor + */ + NavigatorMode(Navigator *navigator, const char *name); + + /** + * Destructor + */ + virtual ~NavigatorMode(); + + /** + * This function is called while the mode is inactive + */ + virtual void on_inactive(); + + /** + * This function is called while the mode is active + * + * @param position setpoint triplet to set + * @return true if position setpoint triplet has been changed + */ + virtual bool on_active(struct position_setpoint_triplet_s *pos_sp_triplet); + +protected: + Navigator *_navigator; + bool _first_run; +}; + +#endif diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 5139283b6..084afe340 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Copyright (c) 2014 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 @@ -17,7 +17,7 @@ * 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 + * 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, @@ -34,104 +34,33 @@ /** * @file navigator_params.c * - * Parameters defined by the navigator task. + * Parameters for navigator in general * - * @author Lorenz Meier <lm@inf.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Anton Babushkin <anton.babushkin@me.com> + * @author Julian Oes <julian@oes.ch> */ #include <nuttx/config.h> #include <systemlib/param/param.h> -/* - * Navigator parameters, accessible via MAVLink - */ - -/** - * Minimum altitude (fixed wing only) - * - * Minimum altitude above home for LOITER. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); - -/** - * Waypoint acceptance radius - * - * Default value of acceptance radius (if not specified in mission item). - * - * @unit meters - * @min 0.0 - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); - /** - * Loiter radius (fixed wing only) + * Loiter radius (FW only) * - * Default value of loiter radius (if not specified in mission item). + * Default value of loiter radius for missions, loiter, RTL, etc. (fixedwing only). * * @unit meters * @min 0.0 - * @group Navigation + * @group Mission */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); /** - * Enable onboard mission - * - * @group Navigation - */ -PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); - -/** - * Take-off altitude - * - * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); - -/** - * Landing altitude - * - * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. - * - * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); - -/** - * Return-To-Launch altitude + * Acceptance Radius * - * Minimum altitude above home position for going home in RTL mode. + * Default acceptance radius, overridden by acceptance radius of waypoint if set. * * @unit meters - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); - -/** - * Return-To-Launch delay - * - * Delay after descend before landing in RTL mode. - * If set to -1 the system will not land but loiter at NAV_LAND_ALT. - * - * @unit seconds - * @group Navigation - */ -PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); - -/** - * Enable parachute deployment - * - * @group Navigation + * @min 1.0 + * @group Mission */ -PARAM_DEFINE_INT32(NAV_PARACHUTE_EN, 0); +PARAM_DEFINE_FLOAT(NAV_ACC_RAD, 25.0f); diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp new file mode 100644 index 000000000..043f773a4 --- /dev/null +++ b/src/modules/navigator/rtl.cpp @@ -0,0 +1,320 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 navigator_rtl.cpp + * Helper class to access RTL + * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#include <string.h> +#include <stdlib.h> +#include <math.h> +#include <fcntl.h> + +#include <mavlink/mavlink_log.h> +#include <systemlib/err.h> +#include <geo/geo.h> + +#include <uORB/uORB.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/home_position.h> + +#include "navigator.h" +#include "rtl.h" + +#define DELAY_SIGMA 0.01f + +RTL::RTL(Navigator *navigator, const char *name) : + MissionBlock(navigator, name), + _rtl_state(RTL_STATE_NONE), + _param_return_alt(this, "RETURN_ALT"), + _param_descend_alt(this, "DESCEND_ALT"), + _param_land_delay(this, "LAND_DELAY") +{ + /* load initial params */ + updateParams(); + /* initial reset */ + on_inactive(); +} + +RTL::~RTL() +{ +} + +void +RTL::on_inactive() +{ + _first_run = true; + + /* reset RTL state only if setpoint moved */ + if (!_navigator->get_can_loiter_at_sp()) { + _rtl_state = RTL_STATE_NONE; + } +} + +bool +RTL::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + bool updated = false; + + if (_first_run) { + _first_run = false; + + /* decide where to enter the RTL procedure when we switch into it */ + if (_rtl_state == RTL_STATE_NONE) { + /* for safety reasons don't go into RTL if landed */ + if (_navigator->get_vstatus()->condition_landed) { + _rtl_state = RTL_STATE_LANDED; + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no RTL when landed"); + + /* if lower than return altitude, climb up first */ + } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + + _param_return_alt.get()) { + _rtl_state = RTL_STATE_CLIMB; + + /* otherwise go straight to return */ + } else { + /* set altitude setpoint to current altitude */ + _rtl_state = RTL_STATE_RETURN; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_global_position()->alt; + } + } + + set_rtl_item(pos_sp_triplet); + updated = true; + + } else if (_rtl_state != RTL_STATE_LANDED && is_mission_item_reached()) { + advance_rtl(); + set_rtl_item(pos_sp_triplet); + updated = true; + } + + return updated; +} + +void +RTL::set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet) +{ + /* make sure we have the latest params */ + updateParams(); + + set_previous_pos_setpoint(pos_sp_triplet); + _navigator->set_can_loiter_at_sp(false); + + switch (_rtl_state) { + case RTL_STATE_CLIMB: { + float climb_alt = _navigator->get_home_position()->alt + _param_return_alt.get(); + + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = climb_alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: climb to %d meters above home", + (int)(climb_alt - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_RETURN: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + // don't change altitude + + if (pos_sp_triplet->previous.valid) { + /* if previous setpoint is valid then use it to calculate heading to home */ + _mission_item.yaw = get_bearing_to_next_waypoint( + pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon, + _mission_item.lat, _mission_item.lon); + + } else { + /* else use current position */ + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _mission_item.lat, _mission_item.lon); + } + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: return at %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_DESCEND: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = false; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: descend to %d meters above home", + (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + break; + } + + case RTL_STATE_LOITER: { + bool autoland = _param_land_delay.get() > -DELAY_SIGMA; + + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = _param_land_delay.get() < 0.0f ? 0.0f : _param_land_delay.get(); + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = autoland; + _mission_item.origin = ORIGIN_ONBOARD; + + _navigator->set_can_loiter_at_sp(true); + + if (autoland) { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: loiter %.1fs", _mission_item.time_inside); + + } else { + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, loiter"); + } + break; + } + + case RTL_STATE_LAND: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_LAND; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: land at home"); + break; + } + + case RTL_STATE_LANDED: { + _mission_item.lat = _navigator->get_home_position()->lat; + _mission_item.lon = _navigator->get_home_position()->lon; + _mission_item.altitude_is_relative = false; + _mission_item.altitude = _navigator->get_home_position()->alt; + _mission_item.yaw = NAN; + _mission_item.loiter_radius = _navigator->get_loiter_radius(); + _mission_item.loiter_direction = 1; + _mission_item.nav_cmd = NAV_CMD_IDLE; + _mission_item.acceptance_radius = _navigator->get_acceptance_radius(); + _mission_item.time_inside = 0.0f; + _mission_item.pitch_min = 0.0f; + _mission_item.autocontinue = true; + _mission_item.origin = ORIGIN_ONBOARD; + + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RTL: completed, landed"); + break; + } + + default: + break; + } + + /* convert mission item to current position setpoint and make it valid */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + reset_mission_item_reached(); + pos_sp_triplet->current.valid = true; + pos_sp_triplet->next.valid = false; +} + +void +RTL::advance_rtl() +{ + switch (_rtl_state) { + case RTL_STATE_CLIMB: + _rtl_state = RTL_STATE_RETURN; + break; + + case RTL_STATE_RETURN: + _rtl_state = RTL_STATE_DESCEND; + break; + + case RTL_STATE_DESCEND: + /* only go to land if autoland is enabled */ + if (_param_land_delay.get() < -DELAY_SIGMA || _param_land_delay.get() > DELAY_SIGMA) { + _rtl_state = RTL_STATE_LOITER; + + } else { + _rtl_state = RTL_STATE_LAND; + } + break; + + case RTL_STATE_LOITER: + _rtl_state = RTL_STATE_LAND; + break; + + case RTL_STATE_LAND: + _rtl_state = RTL_STATE_LANDED; + break; + + default: + break; + } +} diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h new file mode 100644 index 000000000..b4b729e89 --- /dev/null +++ b/src/modules/navigator/rtl.h @@ -0,0 +1,110 @@ +/*************************************************************************** + * + * Copyright (c) 2013-2014 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 navigator_rtl.h + * Helper class for RTL + * + * @author Julian Oes <julian@oes.ch> + * @author Anton Babushkin <anton.babushkin@me.com> + */ + +#ifndef NAVIGATOR_RTL_H +#define NAVIGATOR_RTL_H + +#include <controllib/blocks.hpp> +#include <controllib/block/BlockParam.hpp> + +#include <uORB/topics/mission.h> +#include <uORB/topics/mission.h> +#include <uORB/topics/home_position.h> +#include <uORB/topics/vehicle_global_position.h> + +#include "navigator_mode.h" +#include "mission_block.h" + +class Navigator; + +class RTL : public MissionBlock +{ +public: + /** + * Constructor + */ + RTL(Navigator *navigator, const char *name); + + /** + * Destructor + */ + ~RTL(); + + /** + * This function is called while the mode is inactive + */ + void on_inactive(); + + /** + * This function is called while the mode is active + * + * @param position setpoint triplet that needs to be set + * @return true if updated + */ + bool on_active(position_setpoint_triplet_s *pos_sp_triplet); + + +private: + /** + * Set the RTL item + */ + void set_rtl_item(position_setpoint_triplet_s *pos_sp_triplet); + + /** + * Move to next RTL item + */ + void advance_rtl(); + + enum RTLState { + RTL_STATE_NONE = 0, + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_DESCEND, + RTL_STATE_LOITER, + RTL_STATE_LAND, + RTL_STATE_LANDED, + } _rtl_state; + + control::BlockParamFloat _param_return_alt; + control::BlockParamFloat _param_descend_alt; + control::BlockParamFloat _param_land_delay; +}; + +#endif diff --git a/src/modules/navigator/rtl_params.c b/src/modules/navigator/rtl_params.c new file mode 100644 index 000000000..bfe6ce7e1 --- /dev/null +++ b/src/modules/navigator/rtl_params.c @@ -0,0 +1,98 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 rtl_params.c + * + * Parameters for RTL + * + * @author Julian Oes <julian@oes.ch> + */ + +#include <nuttx/config.h> + +#include <systemlib/param/param.h> + +/* + * RTL parameters, accessible via MAVLink + */ + +/** + * Loiter radius after RTL (FW only) + * + * Default value of loiter radius after RTL (fixedwing only). + * + * @unit meters + * @min 0.0 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LOITER_RAD, 50.0f); + +/** + * RTL altitude + * + * Altitude to fly back in RTL in meters + * + * @unit meters + * @min 0 + * @max 1 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_RETURN_ALT, 100); + + +/** + * RTL loiter altitude + * + * Stay at this altitude above home position after RTL descending. + * Land (i.e. slowly descend) from this altitude if autolanding allowed. + * + * @unit meters + * @min 0 + * @max 100 + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_DESCEND_ALT, 20); + +/** + * RTL delay + * + * Delay after descend before landing in RTL mode. + * If set to -1 the system will not land but loiter at NAV_LAND_ALT. + * + * @unit seconds + * @min -1 + * @max + * @group RTL + */ +PARAM_DEFINE_FLOAT(RTL_LAND_DELAY, -1.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 f908d7a3b..62cee145e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -619,13 +619,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) { + if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) { + if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); @@ -705,8 +705,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* save rotation matrix at this moment */ memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); - w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m); - w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m); + w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph); + w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv); } } else { @@ -859,7 +859,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); if (use_gps_z) { - epv = fminf(epv, gps.epv_m); + epv = fminf(epv, gps.epv); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); } @@ -894,7 +894,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } if (use_gps_xy) { - eph = fminf(eph, gps.eph_m); + eph = fminf(eph, gps.eph); inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 577cadfbb..e483b35e8 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -84,8 +84,10 @@ #include <uORB/topics/esc_status.h> #include <uORB/topics/telemetry_status.h> #include <uORB/topics/estimator_status.h> +#include <uORB/topics/tecs_status.h> #include <uORB/topics/system_power.h> #include <uORB/topics/servorail_status.h> +#include <uORB/topics/wind_estimate.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> @@ -939,8 +941,10 @@ int sdlog2_thread_main(int argc, char *argv[]) struct telemetry_status_s telemetry; struct range_finder_report range_finder; struct estimator_status_report estimator_status; + struct tecs_status_s tecs_status; struct system_power_s system_power; struct servorail_status_s servorail_status; + struct wind_estimate_s wind_estimate; } buf; memset(&buf, 0, sizeof(buf)); @@ -972,13 +976,16 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_BATT_s log_BATT; struct log_DIST_s log_DIST; struct log_TELE_s log_TELE; - struct log_ESTM_s log_ESTM; + struct log_EST0_s log_EST0; + struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; struct log_GS0A_s log_GS0A; struct log_GS0B_s log_GS0B; struct log_GS1A_s log_GS1A; struct log_GS1B_s log_GS1B; + struct log_TECS_s log_TECS; + struct log_WIND_s log_WIND; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1010,8 +1017,10 @@ int sdlog2_thread_main(int argc, char *argv[]) int telemetry_sub; int range_finder_sub; int estimator_status_sub; + int tecs_status_sub; int system_power_sub; int servorail_status_sub; + int wind_sub; } subs; subs.cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); @@ -1037,8 +1046,12 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.telemetry_sub = orb_subscribe(ORB_ID(telemetry_status)); subs.range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); subs.estimator_status_sub = orb_subscribe(ORB_ID(estimator_status)); + subs.tecs_status_sub = orb_subscribe(ORB_ID(tecs_status)); subs.system_power_sub = orb_subscribe(ORB_ID(system_power)); subs.servorail_status_sub = orb_subscribe(ORB_ID(servorail_status)); + subs.wind_sub = orb_subscribe(ORB_ID(wind_estimate)); + /* we need to rate-limit wind, as we do not need the full update rate */ + orb_set_interval(subs.wind_sub, 90); thread_running = true; @@ -1105,7 +1118,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_STAT_MSG; log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state; log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state; - log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe_state; + log_msg.body.log_STAT.failsafe_state = (uint8_t) buf_status.failsafe; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning; log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed; @@ -1126,8 +1139,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.msg_type = LOG_GPS_MSG; log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec; log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type; - log_msg.body.log_GPS.eph = buf_gps_pos.eph_m; - log_msg.body.log_GPS.epv = buf_gps_pos.epv_m; + log_msg.body.log_GPS.eph = buf_gps_pos.eph; + log_msg.body.log_GPS.epv = buf_gps_pos.epv; log_msg.body.log_GPS.lat = buf_gps_pos.lat; log_msg.body.log_GPS.lon = buf_gps_pos.lon; log_msg.body.log_GPS.alt = buf_gps_pos.alt * 0.001f; @@ -1340,7 +1353,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- GLOBAL POSITION SETPOINT --- */ if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) { log_msg.msg_type = LOG_GPSP_MSG; - log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state; + log_msg.body.log_GPSP.nav_state = 0; /* TODO: Fix this */ log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d); log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d); log_msg.body.log_GPSP.alt = buf.triplet.current.alt; @@ -1477,15 +1490,52 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ESTIMATOR STATUS --- */ if (copy_if_updated(ORB_ID(estimator_status), subs.estimator_status_sub, &buf.estimator_status)) { - log_msg.msg_type = LOG_ESTM_MSG; - unsigned maxcopy = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_ESTM.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_ESTM.s); - memset(&(log_msg.body.log_ESTM.s), 0, sizeof(log_msg.body.log_ESTM.s)); - memcpy(&(log_msg.body.log_ESTM.s), buf.estimator_status.states, maxcopy); - log_msg.body.log_ESTM.n_states = buf.estimator_status.n_states; - log_msg.body.log_ESTM.states_nan = buf.estimator_status.states_nan; - log_msg.body.log_ESTM.covariance_nan = buf.estimator_status.covariance_nan; - log_msg.body.log_ESTM.kalman_gain_nan = buf.estimator_status.kalman_gain_nan; - LOGBUFFER_WRITE_AND_COUNT(ESTM); + log_msg.msg_type = LOG_EST0_MSG; + unsigned maxcopy0 = (sizeof(buf.estimator_status.states) < sizeof(log_msg.body.log_EST0.s)) ? sizeof(buf.estimator_status.states) : sizeof(log_msg.body.log_EST0.s); + memset(&(log_msg.body.log_EST0.s), 0, sizeof(log_msg.body.log_EST0.s)); + memcpy(&(log_msg.body.log_EST0.s), buf.estimator_status.states, maxcopy0); + log_msg.body.log_EST0.n_states = buf.estimator_status.n_states; + log_msg.body.log_EST0.nan_flags = buf.estimator_status.nan_flags; + log_msg.body.log_EST0.health_flags = buf.estimator_status.health_flags; + log_msg.body.log_EST0.timeout_flags = buf.estimator_status.timeout_flags; + LOGBUFFER_WRITE_AND_COUNT(EST0); + + log_msg.msg_type = LOG_EST1_MSG; + unsigned maxcopy1 = ((sizeof(buf.estimator_status.states) - maxcopy0) < sizeof(log_msg.body.log_EST1.s)) ? (sizeof(buf.estimator_status.states) - maxcopy0) : sizeof(log_msg.body.log_EST1.s); + memset(&(log_msg.body.log_EST1.s), 0, sizeof(log_msg.body.log_EST1.s)); + memcpy(&(log_msg.body.log_EST1.s), buf.estimator_status.states + maxcopy0, maxcopy1); + LOGBUFFER_WRITE_AND_COUNT(EST1); + } + + /* --- TECS STATUS --- */ + if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) { + log_msg.msg_type = LOG_TECS_MSG; + log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp; + log_msg.body.log_TECS.altitude = buf.tecs_status.altitude; + log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp; + log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle; + log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered; + log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp; + log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed; + log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered; + log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp; + log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative; + log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp; + log_msg.body.log_TECS.totalEnergyRate = buf.tecs_status.totalEnergyRate; + log_msg.body.log_TECS.energyDistributionRateSp = buf.tecs_status.energyDistributionRateSp; + log_msg.body.log_TECS.energyDistributionRate = buf.tecs_status.energyDistributionRate; + log_msg.body.log_TECS.mode = (uint8_t)buf.tecs_status.mode; + LOGBUFFER_WRITE_AND_COUNT(TECS); + } + + /* --- WIND ESTIMATE --- */ + if (copy_if_updated(ORB_ID(wind_estimate), subs.wind_sub, &buf.wind_estimate)) { + log_msg.msg_type = LOG_WIND_MSG; + log_msg.body.log_WIND.x = buf.wind_estimate.windspeed_north; + log_msg.body.log_WIND.y = buf.wind_estimate.windspeed_east; + log_msg.body.log_WIND.cov_x = buf.wind_estimate.covariance_north; + log_msg.body.log_WIND.cov_y = buf.wind_estimate.covariance_east; + LOGBUFFER_WRITE_AND_COUNT(WIND); } /* signal the other thread new data, but not yet unlock */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index f4d88f079..8c05e87c5 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -288,15 +288,7 @@ struct log_TELE_s { uint8_t txbuf; }; -/* --- ESTM - ESTIMATOR STATUS --- */ -#define LOG_ESTM_MSG 23 -struct log_ESTM_s { - float s[10]; - uint8_t n_states; - uint8_t states_nan; - uint8_t covariance_nan; - uint8_t kalman_gain_nan; -}; +// ID 23 available /* --- PWR - ONBOARD POWER SYSTEM --- */ #define LOG_PWR_MSG 24 @@ -346,6 +338,53 @@ struct log_GS1B_s { uint8_t satellite_snr[16]; /**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */ }; +/* --- TECS - TECS STATUS --- */ +#define LOG_TECS_MSG 30 +struct log_TECS_s { + float altitudeSp; + float altitude; + float flightPathAngleSp; + float flightPathAngle; + float flightPathAngleFiltered; + float airspeedSp; + float airspeed; + float airspeedFiltered; + float airspeedDerivativeSp; + float airspeedDerivative; + + float totalEnergyRateSp; + float totalEnergyRate; + float energyDistributionRateSp; + float energyDistributionRate; + + uint8_t mode; +}; + +/* --- WIND - WIND ESTIMATE --- */ +#define LOG_WIND_MSG 31 +struct log_WIND_s { + float x; + float y; + float cov_x; + float cov_y; +}; + +/* --- EST0 - ESTIMATOR STATUS --- */ +#define LOG_EST0_MSG 32 +struct log_EST0_s { + float s[12]; + uint8_t n_states; + uint8_t nan_flags; + uint8_t health_flags; + uint8_t timeout_flags; +}; + +/* --- EST1 - ESTIMATOR STATUS --- */ +#define LOG_EST1_MSG 33 +struct log_EST1_s { + float s[16]; +}; + /********** SYSTEM MESSAGES, ID > 0x80 **********/ /* --- TIME - TIME STAMP --- */ @@ -394,13 +433,16 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(BATT, "ffff", "V,VFilt,C,Discharged"), LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"), LOG_FORMAT(TELE, "BBBBHHB", "RSSI,RemRSSI,Noise,RemNoise,RXErr,Fixed,TXBuf"), - LOG_FORMAT(ESTM, "ffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"), + LOG_FORMAT(EST0, "ffffffffffffBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,nStat,fNaN,fHealth,fTOut"), + LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), + LOG_FORMAT(TECS, "ffffffffffffffB", "AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"), + LOG_FORMAT(WIND, "ffff", "X,Y,CovX,CovY"), /* system-level messages, ID >= 0x80 */ /* FMT: don't write format of format message, it's useless */ diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 999cf8bb3..c8a3ec8f0 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -243,6 +243,36 @@ PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0); PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** + * Board rotation Y (Pitch) offset + * + * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ + PARAM_DEFINE_FLOAT(SENS_BOARD_Y_OFF, 0.0f); + +/** + * Board rotation X (Roll) offset + * + * This parameter defines a rotational offset in degrees around the X (Roll) axis It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_X_OFF, 0.0f); + +/** + * Board rotation Z (YAW) offset + * + * This parameter defines a rotational offset in degrees around the Z (Yaw) axis. It allows the user + * to fine tune the board offset in the event of misalignment. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_FLOAT(SENS_BOARD_Z_OFF, 0.0f); + +/** * External magnetometer rotation * * This parameter defines the rotation of the external magnetometer relative diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b268b1b36..16fcb4c26 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,7 +229,7 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _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 */ - + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -252,6 +252,8 @@ private: int board_rotation; int external_mag_rotation; + + float board_offset[3]; int rc_map_roll; int rc_map_pitch; @@ -341,6 +343,8 @@ private: param_t board_rotation; param_t external_mag_rotation; + + param_t board_offset[3]; } _parameter_handles; /**< handles for interesting parameters */ @@ -587,6 +591,11 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); + + /* rotation offsets */ + _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); + _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); + _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); /* fetch initial parameter values */ parameters_update(); @@ -791,6 +800,18 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); + + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); + param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); + param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); + + /** fine tune board offset on parameter update **/ + math::Matrix<3, 3> board_rotation_offset; + board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], + M_DEG_TO_RAD_F * _parameters.board_offset[1], + M_DEG_TO_RAD_F * _parameters.board_offset[2]); + + _board_rotation = _board_rotation * board_rotation_offset; return OK; } diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 6835ee4a2..668d9dfdf 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -75,7 +75,7 @@ __EXPORT extern void perf_free(perf_counter_t handle); /** * Count a performance event. * - * This call only affects counters that take single events; PC_COUNT etc. + * This call only affects counters that take single events; PC_COUNT, PC_INTERVAL etc. * * @param handle The handle returned from perf_alloc. */ diff --git a/src/modules/systemlib/state_table.h b/src/modules/systemlib/state_table.h index f2709d29f..e6011fdef 100644 --- a/src/modules/systemlib/state_table.h +++ b/src/modules/systemlib/state_table.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013-2014 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 @@ -33,8 +33,9 @@ /** * @file state_table.h - * + * * Finite-State-Machine helper class for state table + * @author: Julian Oes <julian@oes.ch> */ #ifndef __SYSTEMLIB_STATE_TABLE_H @@ -48,22 +49,28 @@ public: Action action; unsigned nextState; }; - + StateTable(Tran const *table, unsigned nStates, unsigned nSignals) : myTable(table), myNsignals(nSignals), myNstates(nStates) {} - + #define NO_ACTION &StateTable::doNothing - #define ACTION(_target) static_cast<StateTable::Action>(_target) + #define ACTION(_target) StateTable::Action(_target) virtual ~StateTable() {} - + void dispatch(unsigned const sig) { - register Tran const *t = myTable + myState*myNsignals + sig; - (this->*(t->action))(); + /* get transition using state table */ + Tran const *t = myTable + myState*myNsignals + sig; + /* accept new state */ myState = t->nextState; + + /* */ + (this->*(t->action))(); + } + void doNothing() { + return; } - void doNothing() {} protected: unsigned myState; private: @@ -72,4 +79,4 @@ private: unsigned myNstates; }; -#endif
\ No newline at end of file +#endif diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 5a5981617..cd0b30dd6 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -47,6 +47,7 @@ #include "topics/vehicle_rates_setpoint.h" #include "topics/actuator_outputs.h" #include "topics/encoders.h" +#include "topics/tecs_status.h" namespace uORB { @@ -76,5 +77,6 @@ template class __EXPORT Publication<vehicle_attitude_setpoint_s>; template class __EXPORT Publication<vehicle_rates_setpoint_s>; template class __EXPORT Publication<actuator_outputs_s>; template class __EXPORT Publication<encoders_s>; +template class __EXPORT Publication<tecs_status_s>; } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 90675bb2e..7c3bb0009 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -199,3 +199,9 @@ ORB_DEFINE(encoders, struct encoders_s); #include "topics/estimator_status.h" ORB_DEFINE(estimator_status, struct estimator_status_report); + +#include "topics/tecs_status.h" +ORB_DEFINE(tecs_status, struct tecs_status_s); + +#include "topics/wind_estimate.h" +ORB_DEFINE(wind_estimate, struct wind_estimate_s); diff --git a/src/modules/uORB/topics/estimator_status.h b/src/modules/uORB/topics/estimator_status.h index 5530cdb21..7f26b505b 100644 --- a/src/modules/uORB/topics/estimator_status.h +++ b/src/modules/uORB/topics/estimator_status.h @@ -64,9 +64,9 @@ struct estimator_status_report { uint64_t timestamp; /**< Timestamp in microseconds since boot */ float states[32]; /**< Internal filter states */ float n_states; /**< Number of states effectively used */ - bool states_nan; /**< If set to true, one of the states is NaN */ - bool covariance_nan; /**< If set to true, the covariance matrix went NaN */ - bool kalman_gain_nan; /**< If set to true, the Kalman gain matrix went NaN */ + uint8_t nan_flags; /**< Bitmask to indicate NaN states */ + uint8_t health_flags; /**< Bitmask to indicate sensor health states (vel, pos, hgt) */ + uint8_t timeout_flags; /**< Bitmask to indicate timeout flags (vel, pos, hgt) */ }; diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h index 08d11abae..70071130d 100644 --- a/src/modules/uORB/topics/home_position.h +++ b/src/modules/uORB/topics/home_position.h @@ -59,10 +59,13 @@ struct home_position_s { uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - //bool altitude_is_relative; // TODO what means home relative altitude? we need clear definition of reference altitude then double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude in meters */ + + float x; /**< X coordinate in meters */ + float y; /**< Y coordinate in meters */ + float z; /**< Z coordinate in meters */ }; /** diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index ef4bc1def..d9dd61df1 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,9 @@ /** * @file mission.h * Definition of a mission consisting of mission items. + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #ifndef TOPIC_MISSION_H_ @@ -50,6 +50,7 @@ /* compatible to mavlink MAV_CMD */ enum NAV_CMD { + NAV_CMD_IDLE=0, NAV_CMD_WAYPOINT=16, NAV_CMD_LOITER_UNLIMITED=17, NAV_CMD_LOITER_TURN_COUNT=18, @@ -58,7 +59,8 @@ enum NAV_CMD { NAV_CMD_LAND=21, NAV_CMD_TAKEOFF=22, NAV_CMD_ROI=80, - NAV_CMD_PATHPLANNING=81 + NAV_CMD_PATHPLANNING=81, + NAV_CMD_DO_JUMP=177 }; enum ORIGIN { @@ -91,6 +93,9 @@ struct mission_item_s { float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ bool autocontinue; /**< true if next waypoint should follow after this one */ enum ORIGIN origin; /**< where the waypoint has been generated */ + int do_jump_mission_index; /**< index where the do jump will go to */ + unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */ + unsigned do_jump_current_count; /**< count how many times the jump has been done */ }; struct mission_s diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h index 7c3921198..ad654a9ff 100644 --- a/src/modules/uORB/topics/mission_result.h +++ b/src/modules/uORB/topics/mission_result.h @@ -56,6 +56,7 @@ struct mission_result_s bool mission_reached; /**< true if mission has been reached */ unsigned mission_index_reached; /**< index of the mission which has been reached */ unsigned index_current_mission; /**< index of the current mission */ + bool mission_finished; /**< true if mission has been completed */ }; /** diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 34aaa30dd..ce42035ba 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -1,9 +1,6 @@ /**************************************************************************** * * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,10 @@ /** * @file mission_item_triplet.h * Definition of the global WGS84 position setpoint uORB topic. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #ifndef TOPIC_MISSION_ITEM_TRIPLET_H_ @@ -45,7 +46,6 @@ #include <stdint.h> #include <stdbool.h> #include "../uORB.h" -#include <navigator/navigator_state.h> /** * @addtogroup topics @@ -54,11 +54,12 @@ enum SETPOINT_TYPE { - SETPOINT_TYPE_NORMAL = 0, /**< normal setpoint */ - SETPOINT_TYPE_LOITER, /**< loiter setpoint */ - SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ - SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, vehicle must descend until landing */ - SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ + SETPOINT_TYPE_POSITION = 0, /**< position setpoint */ + SETPOINT_TYPE_VELOCITY, /**< velocity setpoint */ + SETPOINT_TYPE_LOITER, /**< loiter setpoint */ + SETPOINT_TYPE_TAKEOFF, /**< takeoff setpoint */ + SETPOINT_TYPE_LAND, /**< land setpoint, altitude must be ignored, descend until landing */ + SETPOINT_TYPE_IDLE, /**< do nothing, switch off motors or keep at idle speed (MC) */ }; struct position_setpoint_s @@ -84,8 +85,6 @@ struct position_setpoint_triplet_s struct position_setpoint_s previous; struct position_setpoint_s current; struct position_setpoint_s next; - - nav_state_t nav_state; /**< navigation state */ }; /** diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h new file mode 100644 index 000000000..c4d0c1874 --- /dev/null +++ b/src/modules/uORB/topics/tecs_status.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 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 vehicle_global_position.h + * Definition of the global fused WGS84 position uORB topic. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + */ + +#ifndef TECS_STATUS_T_H_ +#define TECS_STATUS_T_H_ + +#include <stdint.h> +#include <stdbool.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +typedef enum { + TECS_MODE_NORMAL, + TECS_MODE_UNDERSPEED, + TECS_MODE_TAKEOFF, + TECS_MODE_LAND, + TECS_MODE_LAND_THROTTLELIM +} tecs_mode; + + /** + * Internal values of the (m)TECS fixed wing speed alnd altitude control system + */ +struct tecs_status_s { + uint64_t timestamp; /**< timestamp, in microseconds since system start */ + + float altitudeSp; + float altitude; + float flightPathAngleSp; + float flightPathAngle; + float flightPathAngleFiltered; + float airspeedSp; + float airspeed; + float airspeedFiltered; + float airspeedDerivativeSp; + float airspeedDerivative; + + float totalEnergyRateSp; + float totalEnergyRate; + float energyDistributionRateSp; + float energyDistributionRate; + + tecs_mode mode; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(tecs_status); + +#endif diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h index 76693c46e..e9e00d76c 100644 --- a/src/modules/uORB/topics/telemetry_status.h +++ b/src/modules/uORB/topics/telemetry_status.h @@ -57,6 +57,7 @@ enum TELEMETRY_STATUS_RADIO_TYPE { struct telemetry_status_s { uint64_t timestamp; + uint64_t heartbeat_time; /**< Time of last received heartbeat from remote system */ enum TELEMETRY_STATUS_RADIO_TYPE type; /**< type of the radio hardware */ uint8_t rssi; /**< local signal strength */ uint8_t remote_rssi; /**< remote signal strength */ diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h index 4897ca737..e32529cb4 100644 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ b/src/modules/uORB/topics/vehicle_global_position.h @@ -36,7 +36,7 @@ * Definition of the global fused WGS84 position uORB topic. * * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> * @author Lorenz Meier <lm@inf.ethz.ch> */ @@ -61,15 +61,14 @@ * e.g. control inputs of the vehicle in a Kalman-filter implementation. */ struct vehicle_global_position_s { - uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - - uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ + uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ + uint64_t time_gps_usec; /**< GPS timestamp in microseconds */ double lat; /**< Latitude in degrees */ double lon; /**< Longitude in degrees */ float alt; /**< Altitude AMSL in meters */ - float vel_n; /**< Ground north velocity, m/s */ - float vel_e; /**< Ground east velocity, m/s */ - float vel_d; /**< Ground downside velocity, m/s */ + float vel_n; /**< Ground north velocity, m/s */ + float vel_e; /**< Ground east velocity, m/s */ + float vel_d; /**< Ground downside velocity, m/s */ float yaw; /**< Yaw in radians -PI..+PI. */ float eph; float epv; diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h index 5924a324d..bbacb733a 100644 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ b/src/modules/uORB/topics/vehicle_gps_position.h @@ -65,8 +65,8 @@ struct vehicle_gps_position_s { float c_variance_rad; /**< course accuracy estimate rad */ uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - float eph_m; /**< GPS HDOP horizontal dilution of position in m */ - float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + float eph; /**< GPS HDOP horizontal dilution of position in m */ + float epv; /**< GPS VDOP horizontal dilution of position in m */ unsigned noise_per_ms; /**< */ unsigned jamming_indicator; /**< */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 85962883d..56590047f 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -1,10 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier <lm@inf.ethz.ch> - * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> - * @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> + * Copyright (C) 2012 - 2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -45,6 +41,11 @@ * All apps should write to subsystem_info: * * (any app) --> subsystem_info (published) --> (commander app state machine) --> vehicle_status --> (mavlink app) + * + * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <julian@oes.ch> */ #ifndef VEHICLE_STATUS_H_ @@ -54,20 +55,22 @@ #include <stdbool.h> #include "../uORB.h" -#include <navigator/navigator_state.h> - /** * @addtogroup topics @{ */ -/* main state machine */ +/** + * Main state, i.e. what user wants. Controlled by RC or from ground station via telemetry link. + */ typedef enum { MAIN_STATE_MANUAL = 0, MAIN_STATE_ALTCTL, MAIN_STATE_POSCTL, - MAIN_STATE_AUTO, + MAIN_STATE_AUTO_MISSION, + MAIN_STATE_AUTO_LOITER, + MAIN_STATE_AUTO_RTL, MAIN_STATE_ACRO, - MAIN_STATE_MAX + MAIN_STATE_MAX, } main_state_t; // If you change the order, add or remove arming_state_t states make sure to update the arrays @@ -80,7 +83,7 @@ typedef enum { ARMING_STATE_STANDBY_ERROR, ARMING_STATE_REBOOT, ARMING_STATE_IN_AIR_RESTORE, - ARMING_STATE_MAX + ARMING_STATE_MAX, } arming_state_t; typedef enum { @@ -88,13 +91,23 @@ typedef enum { HIL_STATE_ON } hil_state_t; +/** + * Navigation state, i.e. "what should vehicle do". + */ typedef enum { - FAILSAFE_STATE_NORMAL = 0, /**< Normal operation */ - FAILSAFE_STATE_RTL, /**< Return To Launch */ - FAILSAFE_STATE_LAND, /**< Land without position control */ - FAILSAFE_STATE_TERMINATION, /**< Disable motors and use parachute, can't be recovered */ - FAILSAFE_STATE_MAX -} failsafe_state_t; + NAVIGATION_STATE_MANUAL = 0, /**< Manual mode */ + NAVIGATION_STATE_ALTCTL, /**< Altitude control mode */ + NAVIGATION_STATE_POSCTL, /**< Position control mode */ + NAVIGATION_STATE_AUTO_MISSION, /**< Auto mission mode */ + NAVIGATION_STATE_AUTO_LOITER, /**< Auto loiter mode */ + NAVIGATION_STATE_AUTO_RTL, /**< Auto return to launch mode */ + NAVIGATION_STATE_AUTO_RTGS, /**< Auto return to groundstation on data link loss */ + NAVIGATION_STATE_ACRO, /**< Acro mode */ + NAVIGATION_STATE_LAND, /**< Land mode */ + NAVIGATION_STATE_DESCEND, /**< Descend mode (no position control) */ + NAVIGATION_STATE_TERMINATION, /**< Termination mode */ + NAVIGATION_STATE_MAX, +} navigation_state_t; enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, @@ -154,12 +167,11 @@ struct vehicle_status_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 */ - main_state_t main_state; /**< main state machine */ - unsigned int set_nav_state; /**< set navigation state machine to specified value */ - uint64_t set_nav_state_timestamp; /**< timestamp of latest change of set_nav_state */ + main_state_t main_state; /**< main state machine */ + navigation_state_t nav_state; /**< set navigation state machine to specified value */ arming_state_t arming_state; /**< current arming state */ - hil_state_t hil_state; /**< current hil state */ - failsafe_state_t failsafe_state; /**< current failsafe state */ + hil_state_t hil_state; /**< current hil state */ + bool failsafe; /**< true if system is in failsafe state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ @@ -186,6 +198,8 @@ struct vehicle_status_s { bool rc_signal_lost; /**< true if RC reception lost */ bool rc_input_blocked; /**< set if RC input should be ignored */ + bool data_link_lost; /**< datalink to GCS lost */ + bool offboard_control_signal_found_once; bool offboard_control_signal_lost; bool offboard_control_signal_weak; diff --git a/src/modules/uORB/topics/wind_estimate.h b/src/modules/uORB/topics/wind_estimate.h new file mode 100644 index 000000000..58333a64f --- /dev/null +++ b/src/modules/uORB/topics/wind_estimate.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (c) 2014 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 wind_estimate.h + * + * Wind estimate topic topic + * + */ + +#ifndef TOPIC_WIND_ESTIMATE_H +#define TOPIC_WIND_ESTIMATE_H + +#include <stdint.h> +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ + +/** Wind estimate */ +struct wind_estimate_s { + + uint64_t timestamp; /**< Microseconds since system boot */ + float windspeed_north; /**< Wind component in north / X direction */ + float windspeed_east; /**< Wind component in east / Y direction */ + float covariance_north; /**< Uncertainty - set to zero (no uncertainty) if not estimated */ + float covariance_east; /**< Uncertainty - set to zero (no uncertainty) if not estimated */ +}; + +/** + * @} + */ + +ORB_DECLARE(wind_estimate); + +#endif
\ No newline at end of file diff --git a/src/systemcmds/mtd/mtd.c b/src/systemcmds/mtd/mtd.c index a57eaafe7..fcc9b8366 100644 --- a/src/systemcmds/mtd/mtd.c +++ b/src/systemcmds/mtd/mtd.c @@ -193,8 +193,12 @@ ramtron_attach(void) errx(1, "failed to initialize mtd driver"); int ret = mtd_dev->ioctl(mtd_dev, MTDIOC_SETSPEED, (unsigned long)10*1000*1000); - if (ret != OK) - warnx(1, "failed to set bus speed"); + if (ret != OK) { + // FIXME: From the previous warnx call, it looked like this should have been an errx instead. Tried + // that but setting the bug speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to warnx. + warnx("failed to set bus speed"); + } attached = true; } diff --git a/src/systemcmds/tests/test_mixer.cpp b/src/systemcmds/tests/test_mixer.cpp index df382e2c6..0b826b826 100644 --- a/src/systemcmds/tests/test_mixer.cpp +++ b/src/systemcmds/tests/test_mixer.cpp @@ -372,6 +372,7 @@ int test_mixer(int argc, char *argv[]) } warnx("SUCCESS: No errors in mixer test"); + return 0; } static int |