From adbccfaa1cd100609490b61f2081a0619b0a36c8 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 3 Jun 2015 09:32:02 -0400 Subject: [PATCH 01/34] Saturate velocity command for mc_pos_control. --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 7a3a5a679b..2509f2b8c4 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1032,7 +1032,14 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; + /* make sure velocity setpoint is saturated */ _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; + for (int i=0; i<3; i++) { + if (_vel_sp(i) > _params.vel_max(i)) { + _vel_sp(i) = _params.vel_max(i); + } else if (_vel_sp(i) < -_params.vel_max(i)) + _vel_sp(i) = -_params.vel_max(i); + } if (!_control_mode.flag_control_altitude_enabled) { _reset_alt_sp = true; From dedd16e36e4f0690f8662b93f2aa8144cc8a57bf Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 3 Jun 2015 21:15:17 -0400 Subject: [PATCH 02/34] Modified velocity saturation to maintain direction. --- .../mc_pos_control/mc_pos_control_main.cpp | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2509f2b8c4..995937aa6c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1032,13 +1032,21 @@ MulticopterPositionControl::task_main() /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err = _pos_sp - _pos; - /* make sure velocity setpoint is saturated */ _vel_sp = pos_err.emult(_params.pos_p) + _vel_ff; - for (int i=0; i<3; i++) { - if (_vel_sp(i) > _params.vel_max(i)) { - _vel_sp(i) = _params.vel_max(i); - } else if (_vel_sp(i) < -_params.vel_max(i)) - _vel_sp(i) = -_params.vel_max(i); + + /* make sure velocity setpoint is saturated in xy*/ + float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) + + _vel_sp(1)*_vel_sp(1)); + if (vel_norm_xy > _params.vel_max(0)) { + /* note assumes vel_max(0) == vel_max(1) */ + _vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy; + _vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy; + } + + /* make sure velocity setpoint is saturated in z*/ + float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2)); + if (vel_norm_z > _params.vel_max(2)) { + _vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z; } if (!_control_mode.flag_control_altitude_enabled) { From 0bfc727584d09bd910129ce3c03551b5ec2a5b35 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 13:30:44 +0200 Subject: [PATCH 03/34] Add more functionality to HIL driver --- src/drivers/hil/hil.cpp | 126 +++++++++++++++++++++++++++++++--------- 1 file changed, 97 insertions(+), 29 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index a7c2e83e34..0fbabaf2f1 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -264,36 +264,42 @@ HIL::set_mode(Mode mode) debug("MODE_2PWM"); /* multi-port with flow control lines as PWM */ _update_rate = 50; /* default output rate */ + _num_outputs = 2; break; case MODE_4PWM: debug("MODE_4PWM"); /* multi-port as 4 PWM outs */ _update_rate = 50; /* default output rate */ + _num_outputs = 4; break; case MODE_8PWM: - debug("MODE_8PWM"); - /* multi-port as 8 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + debug("MODE_8PWM"); + /* multi-port as 8 PWM outs */ + _update_rate = 50; /* default output rate */ + _num_outputs = 8; + break; - case MODE_12PWM: - debug("MODE_12PWM"); - /* multi-port as 12 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + case MODE_12PWM: + debug("MODE_12PWM"); + /* multi-port as 12 PWM outs */ + _update_rate = 50; /* default output rate */ + _num_outputs = 12; + break; - case MODE_16PWM: - debug("MODE_16PWM"); - /* multi-port as 16 PWM outs */ - _update_rate = 50; /* default output rate */ - break; + case MODE_16PWM: + debug("MODE_16PWM"); + /* multi-port as 16 PWM outs */ + _update_rate = 50; /* default output rate */ + _num_outputs = 16; + break; case MODE_NONE: debug("MODE_NONE"); /* disable servo outputs and set a very low update rate */ _update_rate = 10; + _num_outputs = 0; break; default: @@ -468,13 +474,6 @@ HIL::ioctl(file *filp, int cmd, unsigned long arg) { int ret; - debug("ioctl 0x%04x 0x%08x", cmd, arg); - - // /* try it as a GPIO ioctl first */ - // ret = HIL::gpio_ioctl(filp, cmd, arg); - // if (ret != -ENOTTY) - // return ret; - /* if we are in valid PWM mode, try it as a PWM ioctl as well */ switch(_mode) { case MODE_2PWM: @@ -523,6 +522,62 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) // HIL always outputs at the alternate (usually faster) rate break; + case PWM_SERVO_GET_DEFAULT_UPDATE_RATE: + *(uint32_t *)arg = 400; + break; + + case PWM_SERVO_GET_UPDATE_RATE: + *(uint32_t *)arg = 400; + break; + + case PWM_SERVO_GET_SELECT_UPDATE_RATE: + *(uint32_t *)arg = 0; + break; + + case PWM_SERVO_GET_FAILSAFE_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 850; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_DISARMED_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 900; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_MIN_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 1000; + } + + pwm->channel_count = _num_outputs; + break; + } + + case PWM_SERVO_GET_MAX_PWM: { + struct pwm_output_values *pwm = (struct pwm_output_values *)arg; + + for (unsigned i = 0; i < _num_outputs; i++) { + pwm->values[i] = 2000; + } + + pwm->channel_count = _num_outputs; + break; + } + case PWM_SERVO_SET(2): case PWM_SERVO_SET(3): if (_mode != MODE_4PWM) { @@ -543,18 +598,26 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; - case PWM_SERVO_GET(2): + case PWM_SERVO_GET(7): + case PWM_SERVO_GET(6): + case PWM_SERVO_GET(5): + case PWM_SERVO_GET(4): + if (_num_outputs < 8) { + ret = -EINVAL; + break; + } + case PWM_SERVO_GET(3): - if (_mode != MODE_4PWM) { + case PWM_SERVO_GET(2): + if (_num_outputs < 4) { ret = -EINVAL; break; } /* FALLTHROUGH */ - case PWM_SERVO_GET(0): - case PWM_SERVO_GET(1): { - // channel = cmd - PWM_SERVO_SET(0); - // *(servo_position_t *)arg = up_pwm_servo_get(channel); + case PWM_SERVO_GET(1): + case PWM_SERVO_GET(0): { + *(servo_position_t *)arg = 1500; break; } @@ -566,11 +629,16 @@ HIL::pwm_ioctl(file *filp, int cmd, unsigned long arg) break; } + case PWM_SERVO_GET_COUNT: case MIXERIOCGETOUTPUTCOUNT: - if (_mode == MODE_4PWM) { - *(unsigned *)arg = 4; + if (_mode == MODE_8PWM) { + *(unsigned *)arg = 8; + } else if (_mode == MODE_4PWM) { + + *(unsigned *)arg = 4; } else { + *(unsigned *)arg = 2; } From 6c0539c243a13ed0cb2c4c508b4770bdff57add6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 15:55:55 +0200 Subject: [PATCH 04/34] FW position controller: Do handle idle mission items correctly --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) 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 b5861d0f16..90cf391536 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 @@ -1085,7 +1085,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { + if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _att_sp.thrust = 0.0f; + _att_sp.roll_body = 0.0f; + _att_sp.pitch_body = 0.0f; + + } else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) { /* waypoint is a plain navigation waypoint */ _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d); _att_sp.roll_body = _l1_control.nav_roll(); @@ -1544,6 +1549,9 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* making sure again that the correct thrust is used, * without depending on library calls for safety reasons */ _att_sp.thrust = launchDetector.getThrottlePreTakeoff(); + } else if (_control_mode_current == FW_POSCTRL_MODE_AUTO && + pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { + _att_sp.thrust = 0.0f; } else { /* Copy thrust and pitch values from tecs */ _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : From 05993bee6fa523d6d8ecfcceb614fa45fe669956 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 15:57:27 +0200 Subject: [PATCH 05/34] Navigator: Provide better feedback if no mission present, enforce minimum altitude in loiter and in auto modes --- src/modules/navigator/loiter.cpp | 5 ++-- src/modules/navigator/loiter.h | 3 +++ src/modules/navigator/mission.cpp | 32 ++++++++++++++++++------- src/modules/navigator/mission_block.cpp | 8 +++++-- src/modules/navigator/mission_block.h | 2 +- 5 files changed, 37 insertions(+), 13 deletions(-) diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp index a744d58cf0..aabdb2b075 100644 --- a/src/modules/navigator/loiter.cpp +++ b/src/modules/navigator/loiter.cpp @@ -55,7 +55,8 @@ #include "navigator.h" Loiter::Loiter(Navigator *navigator, const char *name) : - MissionBlock(navigator, name) + MissionBlock(navigator, name), + _param_min_alt(this, "MIS_TAKEOFF_ALT", false) { /* load initial params */ updateParams(); @@ -74,7 +75,7 @@ void Loiter::on_activation() { /* set current mission item to loiter */ - set_loiter_item(&_mission_item); + set_loiter_item(&_mission_item, _param_min_alt.get()); /* convert mission item to current setpoint */ struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); diff --git a/src/modules/navigator/loiter.h b/src/modules/navigator/loiter.h index 37ab57a078..0627c54129 100644 --- a/src/modules/navigator/loiter.h +++ b/src/modules/navigator/loiter.h @@ -59,6 +59,9 @@ public: virtual void on_activation(); virtual void on_active(); + +private: + control::BlockParamFloat _param_min_alt; }; #endif diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index fe876ee8b1..a74e042a91 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -377,6 +377,7 @@ Mission::set_mission_items() /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); + user_feedback_done = true; } _mission_type = MISSION_TYPE_ONBOARD; @@ -385,6 +386,7 @@ Mission::set_mission_items() /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); + user_feedback_done = true; } _mission_type = MISSION_TYPE_OFFBOARD; } else { @@ -392,21 +394,17 @@ Mission::set_mission_items() if (_mission_type != MISSION_TYPE_NONE) { /* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */ mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + user_feedback_done = true; /* use last setpoint for loiter */ _navigator->set_can_loiter_at_sp(true); - } else if (!user_feedback_done) { - /* only tell users that we got no mission if there has not been any - * better, more specific feedback yet - * https://en.wikipedia.org/wiki/Loiter_(aeronautics) - */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering"); } + _mission_type = MISSION_TYPE_NONE; - /* set loiter mission item */ - set_loiter_item(&_mission_item); + /* set loiter mission item and ensure that there is a minimum clearance from home */ + set_loiter_item(&_mission_item, _param_takeoff_alt.get()); /* update position setpoint triplet */ pos_sp_triplet->previous.valid = false; @@ -418,6 +416,24 @@ Mission::set_mission_items() set_mission_finished(); + if (!user_feedback_done) { + /* only tell users that we got no mission if there has not been any + * better, more specific feedback yet + * https://en.wikipedia.org/wiki/Loiter_(aeronautics) + */ + + if (_navigator->get_vstatus()->condition_landed) { + /* landed, refusing to take off without a mission */ + + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff"); + } else { + mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering"); + } + + user_feedback_done = true; + + } + _navigator->set_position_setpoint_triplet_updated(); return; } diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 42c74428ad..8e83a3329d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -228,7 +228,7 @@ MissionBlock::set_previous_pos_setpoint() } void -MissionBlock::set_loiter_item(struct mission_item_s *item) +MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance) { if (_navigator->get_vstatus()->condition_landed) { /* landed, don't takeoff, but switch to IDLE mode */ @@ -246,10 +246,14 @@ MissionBlock::set_loiter_item(struct mission_item_s *item) item->altitude = pos_sp_triplet->current.alt; } else { - /* use current position */ + /* use current position and use return altitude as clearance */ item->lat = _navigator->get_global_position()->lat; item->lon = _navigator->get_global_position()->lon; item->altitude = _navigator->get_global_position()->alt; + + if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) { + item->altitude = _navigator->get_home_position()->alt + min_clearance; + } } item->altitude_is_relative = false; diff --git a/src/modules/navigator/mission_block.h b/src/modules/navigator/mission_block.h index ec3e305825..4e6e99acb0 100644 --- a/src/modules/navigator/mission_block.h +++ b/src/modules/navigator/mission_block.h @@ -91,7 +91,7 @@ protected: /** * Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position */ - void set_loiter_item(struct mission_item_s *item); + void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f); mission_item_s _mission_item; bool _waypoint_position_reached; From 92aeef2b846661a9b6f41347ea29ae1e0bb2e48b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 15:57:57 +0200 Subject: [PATCH 06/34] commander: Better text feedback --- src/modules/commander/state_machine_helper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7a379612da..e975cf87ef 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -375,7 +375,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, switch (new_state) { case vehicle_status_s::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)"); + mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); ret = TRANSITION_DENIED; break; From 3f77455dd85870caacc5dfa76aecd669a43de2e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 15:58:21 +0200 Subject: [PATCH 07/34] commander: Condition HIL arming check properly --- src/modules/commander/commander.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7c15992f40..67e17aae08 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -450,7 +450,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char transition_result_t arming_res = TRANSITION_NOT_CHANGED; // For HIL platforms, require that simulated sensors are connected - if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) { + if (arm && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL && + is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) { mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming"); return TRANSITION_DENIED; } From 8838b18da75d6f4354f73b38152c2ca98f9197aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 18:53:38 +0200 Subject: [PATCH 08/34] FW attitude control: Run attitude controller as fast as we can to minimize latency --- src/modules/fw_att_control/fw_att_control_main.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index c44f29a404..fe27de14f5 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -634,8 +634,9 @@ FixedwingAttitudeControl::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */ - orb_set_interval(_att_sub, 17); + /* do not limit the attitude updates in order to minimize latency. + * actuator outputs are still limited by the individual drivers + * properly to not saturate IO or physical limitations */ parameters_update(); From 55ed9e96126cab150dbad1d9bd9db392b75781d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 4 Jun 2015 18:54:28 +0200 Subject: [PATCH 09/34] ECL: Run TECS filter faster, adjust gains accordingly --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index cfcc48b62a..d13673ec9b 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -89,7 +89,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3 // take 5 point moving average //_vel_dot = _vdot_filter.apply(temp); // XXX resolve this properly - _vel_dot = 0.9f * _vel_dot + 0.1f * temp; + _vel_dot = 0.95f * _vel_dot + 0.05f * temp; } else { _vel_dot = 0.0f; From f9f34078d15281f3edfc0a1e0d49ee1676ee2d33 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 00:16:25 +0200 Subject: [PATCH 10/34] commander: Ensure RTL can be triggered in all modes --- src/modules/commander/commander.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7c15992f40..f6780e2af1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2504,12 +2504,15 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: - case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL: case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER: + /* override is not ok for the RTL and recovery mode */ + control_mode.flag_external_manual_override_ok = false; + /* fallthrough */ case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS: case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL: + case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION: + case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; control_mode.flag_control_rates_enabled = true; From 7deeda726cbeff7259dc671244990b8532146b99 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 12:07:32 +0200 Subject: [PATCH 11/34] airspeed topic: Add unfiltered airspeed --- msg/airspeed.msg | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/msg/airspeed.msg b/msg/airspeed.msg index 8d6af2138d..525bfd7f88 100644 --- a/msg/airspeed.msg +++ b/msg/airspeed.msg @@ -1,4 +1,5 @@ uint64 timestamp # microseconds since system boot, needed to integrate float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown -float32 true_airspeed_m_s # true airspeed in meters per second, -1 if unknown +float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown +float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown From 0916e6fc199fee2acbefa924b55082eb483ef3bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 12:09:21 +0200 Subject: [PATCH 12/34] sensors app: Populate unfiltered airspeed field --- src/modules/sensors/sensors.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 203564ec97..1e831becd9 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1288,6 +1288,10 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) _airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + _airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f, + calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); + _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ From e76bdc3cace535108aa90ca89eadfbaef1f13b01 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 12:10:36 +0200 Subject: [PATCH 13/34] EKF: Use unfiltered airspeed if airspeed is large enough - rely for better stability on the filtered speed for the threshold. Lower the threshold to 5 m/s to ensure airspeed fusion even on small wings --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 b9897ffcfc..84da033adb 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 @@ -1062,7 +1062,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const } // Fuse Airspeed Measurements - if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) { + if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) { _ekf->fuseVtasData = true; _ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data @@ -1320,7 +1320,7 @@ void AttitudePositionEstimatorEKF::pollData() orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed); perf_count(_perf_airspeed); - _ekf->VtasMeas = _airspeed.true_airspeed_m_s; + _ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s; } From 44441ab501be45165d4eeb2e0f138e2153f9f66e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 14:05:17 +0200 Subject: [PATCH 14/34] FW pos control: Perform climbout if user requests more than 85% pitch up --- .../fw_pos_control_l1_main.cpp | 45 +++++++++++-------- 1 file changed, 27 insertions(+), 18 deletions(-) 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 90cf391536..01d94a8881 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 @@ -100,8 +100,8 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode #define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading -#define THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes - +static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes +static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode /** * L1 control app start / stop handling function @@ -370,7 +370,7 @@ private: /** * Publish navigation capabilities */ - void navigation_capabilities_publish(); + void navigation_capabilities_publish(); /** * Get a new waypoint based on heading and distance from current position @@ -386,27 +386,30 @@ private: /** * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available */ - float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); + float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** * Control position. */ /** - * Do takeoff help when in altitude controlled modes + * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + void do_takeoff_help(); /** - * Update desired altitude base on user pitch stick input + * Update desired altitude base on user pitch stick input + * + * @param dt Time step + * @return true if climbout mode was requested by user (climb with max rate and min airspeed) */ - void update_desired_altitude(float dt); + bool update_desired_altitude(float dt); 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_2d, 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_2d, const struct position_setpoint_triplet_s &pos_sp_triplet); /** * Shim for calling task_main from task_create. @@ -421,12 +424,12 @@ private: /* * Reset takeoff state */ - void reset_takeoff_state(); + void reset_takeoff_state(); /* * Reset landing state */ - void 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) @@ -955,16 +958,20 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint } } -void FixedwingPositionControl::update_desired_altitude(float dt) +bool FixedwingPositionControl::update_desired_altitude(float dt) { const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; static bool was_in_deadband = false; + bool climbout_mode = false; + + // XXX the sign magic in this function needs to be fixed if (_manual.x > deadBand) { float pitch = (_manual.x - deadBand) / factor; _hold_alt -= (_parameters.max_climb_rate * dt) * pitch; was_in_deadband = false; + climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH); } else if (_manual.x < - deadBand) { float pitch = (_manual.x + deadBand) / factor; _hold_alt -= (_parameters.max_sink_rate * dt) * pitch; @@ -976,6 +983,8 @@ void FixedwingPositionControl::update_desired_altitude(float dt) _hold_alt = _global_pos.alt; was_in_deadband = true; } + + return climbout_mode; } void FixedwingPositionControl::do_takeoff_help() @@ -1275,7 +1284,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Inform user that launchdetection is running */ static hrt_abstime last_sent = 0; if(hrt_absolute_time() - last_sent > 4e6) { - mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running"); + mavlink_log_critical(_mavlink_fd, "Launchdetection running"); last_sent = hrt_absolute_time(); } @@ -1404,7 +1413,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _manual.z; /* update desired altitude based on user pitch stick input */ - update_desired_altitude(dt); + bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); @@ -1423,7 +1432,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, throttle_max, _parameters.throttle_cruise, - false, + climbout_requested, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, @@ -1497,7 +1506,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _manual.z; /* update desired altitude based on user pitch stick input */ - update_desired_altitude(dt); + bool climbout_requested = update_desired_altitude(dt); /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ do_takeoff_help(); @@ -1516,7 +1525,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_min, throttle_max, _parameters.throttle_cruise, - false, + climbout_requested, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, From 9e3e43c49ee7f3e8b482b401f262a0bbd1868db4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 15:27:24 +0200 Subject: [PATCH 15/34] Update comments in attitude controller. Fixes #2369 --- src/modules/mc_att_control/mc_att_control_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 4136107414..1b5af55b19 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -599,10 +599,10 @@ MulticopterAttitudeControl::vehicle_motor_limits_poll() } } -/* +/** * Attitude controller. - * Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode) - * Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes) + * Input: 'vehicle_attitude_setpoint' topics (depending on mode) + * Output: '_rates_sp' vector, '_thrust_sp' */ void MulticopterAttitudeControl::control_attitude(float dt) From 82352a64aaf341cddad5fc8e617a203ed6cf7285 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 19:36:29 +0200 Subject: [PATCH 16/34] commander: Remove unused param handles --- src/modules/commander/commander.cpp | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index fa042ff47c..73550e41e9 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -180,8 +180,6 @@ static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; -static float takeoff_alt = 5.0f; -static int parachute_enabled = 0; static float eph_threshold = 5.0f; static float epv_threshold = 10.0f; @@ -860,8 +858,6 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); param_t _param_system_id = param_find("MAV_SYS_ID"); 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"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); @@ -1279,11 +1275,7 @@ int commander_thread_main(int argc, char *argv[]) rc_calibration_check(mavlink_fd); } - /* navigation parameters */ - param_get(_param_takeoff_alt, &takeoff_alt); - /* Safety parameters */ - param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); From 2ba8ac44382d7e88cc22b73471733c2d01d9305e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 17:31:31 +0200 Subject: [PATCH 17/34] Move mission result to generated topics --- msg/mission_result.msg | 11 ++++ src/modules/uORB/topics/mission_result.h | 75 ------------------------ 2 files changed, 11 insertions(+), 75 deletions(-) create mode 100644 msg/mission_result.msg delete mode 100644 src/modules/uORB/topics/mission_result.h diff --git a/msg/mission_result.msg b/msg/mission_result.msg new file mode 100644 index 0000000000..532db6f73d --- /dev/null +++ b/msg/mission_result.msg @@ -0,0 +1,11 @@ +uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified +uint32 seq_reached # Sequence of the mission item which has been reached +uint32 seq_current # Sequence of the current mission item +bool valid # true if mission is valid +bool reached # true if mission has been reached +bool finished # true if mission has been completed +bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode +bool flight_termination # true if the navigator demands a flight termination from the commander app +bool item_do_jump_changed # true if the number of do jumps remaining has changed +uint32 item_changed_index # indicate which item has changed +uint32 item_do_jump_remaining # set to the number of do jumps remaining for that item diff --git a/src/modules/uORB/topics/mission_result.h b/src/modules/uORB/topics/mission_result.h deleted file mode 100644 index 16e7f2f126..0000000000 --- a/src/modules/uORB/topics/mission_result.h +++ /dev/null @@ -1,75 +0,0 @@ -/**************************************************************************** - * - * 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 mission_result.h - * Mission results that navigator needs to pass on to commander and mavlink. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * @author Ban Siesta - */ - -#ifndef TOPIC_MISSION_RESULT_H -#define TOPIC_MISSION_RESULT_H - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -struct mission_result_s { - unsigned seq_reached; /**< Sequence of the mission item which has been reached */ - unsigned seq_current; /**< Sequence of the current mission item */ - bool reached; /**< true if mission has been reached */ - bool finished; /**< true if mission has been completed */ - bool stay_in_failsafe; /**< true if the commander should not switch out of the failsafe mode*/ - bool flight_termination; /**< true if the navigator demands a flight termination from the commander app */ - bool item_do_jump_changed; /**< true if the number of do jumps remaining has changed */ - unsigned item_changed_index; /**< indicate which item has changed */ - unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(mission_result); - -#endif From 2cf10a5e999ce1e5c2579a202755346a6ad18046 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 17:31:58 +0200 Subject: [PATCH 18/34] Navigator: Publish mission validity in mission result --- src/modules/navigator/mission.cpp | 9 +++++++++ src/modules/navigator/navigator.h | 3 +++ src/modules/navigator/navigator_main.cpp | 4 ++++ 3 files changed, 16 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index a74e042a91..3848d16e5d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -197,6 +197,11 @@ Mission::update_onboard_mission() /* otherwise, just leave it */ } + // XXX check validity here as well + _navigator->get_mission_result()->valid = true; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + } else { _onboard_mission.count = 0; _onboard_mission.current_seq = 0; @@ -234,6 +239,10 @@ Mission::update_offboard_mission() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid()); + _navigator->get_mission_result()->valid = !failed; + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + } else { warnx("offboard mission update failed"); } diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 782a297fbb..093e1be3c2 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -163,6 +163,8 @@ public: float get_acceptance_radius(float mission_item_radius); int get_mavlink_fd() { return _mavlink_fd; } + void increment_mission_instance_count() { _mission_instance_count++; } + private: bool _task_should_exit; /**< if true, sensor task should exit */ @@ -205,6 +207,7 @@ private: bool _home_position_set; bool _mission_item_valid; /**< flags if the current mission item is valid */ + int _mission_instance_count; /**< instance count for the current mission */ perf_counter_t _loop_perf; /**< loop performance counter */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 8cfce50879..1460972cc2 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -125,6 +125,7 @@ Navigator::Navigator() : _att_sp{}, _home_position_set(false), _mission_item_valid(false), + _mission_instance_count(0), _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), @@ -663,6 +664,8 @@ int navigator_main(int argc, char *argv[]) void Navigator::publish_mission_result() { + _mission_result.instance_count = _mission_instance_count; + /* lazily publish the mission result only once available */ if (_mission_result_pub > 0) { /* publish mission result */ @@ -679,6 +682,7 @@ Navigator::publish_mission_result() _mission_result.item_do_jump_changed = false; _mission_result.item_changed_index = 0; _mission_result.item_do_jump_remaining = 0; + _mission_result.valid = true; } void From a4b238946070dfa2094b87d8fff61a987a6bec03 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 18:37:19 +0200 Subject: [PATCH 19/34] Commander: Support new mission status --- src/modules/commander/commander.cpp | 26 ++++++++++++++++- src/modules/commander/commander_helper.cpp | 33 ++++++++++++++++++++++ src/modules/commander/commander_helper.h | 3 ++ 3 files changed, 61 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 73550e41e9..edeb31e888 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -190,6 +190,8 @@ static struct vehicle_control_mode_s control_mode; static struct offboard_control_mode_s offboard_control_mode; static struct home_position_s _home; +static unsigned _last_mission_instance = 0; + /** * The daemon app only briefly exists to start * the background job. The stack size assigned in the @@ -839,7 +841,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & //Play tune first time we initialize HOME if (!status.condition_home_position_valid) { - tune_positive(true); + tune_home_set(true); } /* mark home position as set */ @@ -1764,6 +1766,28 @@ int commander_thread_main(int argc, char *argv[]) } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination + /* Only evaluate mission state if home is set, + * this prevents false positives for the mission + * rejection. Back off 2 seconds to not overlay + * home tune. + */ + if (status.condition_home_position_valid && + (hrt_elapsed_time(&_home.timestamp) > 2000000) && + _last_mission_instance != mission_result.instance_count) { + if (mission_result.valid) { + /* the mission is valid */ + tune_mission_ok(true); + warnx("mission ok"); + } else { + /* the mission is not valid */ + tune_mission_fail(true); + warnx("mission fail"); + } + + /* prevent further feedback until the mission changes */ + _last_mission_instance = mission_result.instance_count; + } + /* RC input check */ if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index c0f8561fda..cbf11de1b0 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -172,6 +172,39 @@ void set_tune(int tune) } } +void tune_home_set(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } +} + +void tune_mission_ok(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } +} + +void tune_mission_fail(bool use_buzzer) +{ + blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME; + rgbled_set_color(RGBLED_COLOR_GREEN); + rgbled_set_mode(RGBLED_MODE_BLINK_FAST); + + if (use_buzzer) { + set_tune(TONE_NOTIFY_POSITIVE_TUNE); + } +} + /** * Blink green LED and play positive tune (if use_buzzer == true). */ diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index d2aace2a40..d2ab41f887 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -58,6 +58,9 @@ void buzzer_deinit(void); void set_tune_override(int tune); void set_tune(int tune); +void tune_home_set(bool use_buzzer); +void tune_mission_ok(bool use_buzzer); +void tune_mission_fail(bool use_buzzer); void tune_positive(bool use_buzzer); void tune_neutral(bool use_buzzer); void tune_negative(bool use_buzzer); From 174f4d27f3e65454808e073023ee14833f3d7ff2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 18:37:32 +0200 Subject: [PATCH 20/34] Navigator: output new mission status --- src/modules/navigator/mission.cpp | 18 ++++++++++++++++++ src/modules/navigator/mission.h | 1 + 2 files changed, 19 insertions(+) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 3848d16e5d..be27e6208a 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -78,6 +78,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _takeoff(false), _mission_type(MISSION_TYPE_NONE), _inited(false), + _home_inited(false), _dist_1wp_ok(false), _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), @@ -110,6 +111,22 @@ Mission::on_inactive() update_offboard_mission(); } + /* check if the home position became valid in the meantime */ + if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) && + !_home_inited && _navigator->home_position_valid()) { + + dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); + + _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, + dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), + _navigator->get_home_position()->alt, _navigator->home_position_valid()); + + _navigator->increment_mission_instance_count(); + _navigator->set_mission_result_updated(); + + _home_inited = true; + } + } else { /* read mission topics on initialization */ _inited = true; @@ -176,6 +193,7 @@ Mission::on_active() && _mission_type != MISSION_TYPE_NONE) { heading_sp_update(); } + } void diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index bc9a2c6c82..6cfae49598 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -186,6 +186,7 @@ private: } _mission_type; bool _inited; + bool _home_inited; bool _dist_1wp_ok; MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ From 21ca431131e5c50854a75d55655d4c87b268cea8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 15:12:17 +0200 Subject: [PATCH 21/34] Tone alarm: Add home set tune --- src/drivers/drv_tone_alarm.h | 1 + src/drivers/stm32/tone_alarm/tone_alarm.cpp | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h index 31aee266e1..3506d832d0 100644 --- a/src/drivers/drv_tone_alarm.h +++ b/src/drivers/drv_tone_alarm.h @@ -152,6 +152,7 @@ enum { TONE_EKF_WARNING_TUNE, TONE_BARO_WARNING_TUNE, TONE_SINGLE_BEEP_TUNE, + TONE_HOME_SET, TONE_NUMBER_OF_TUNES }; diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index a18b54981f..bf8418bf9f 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -339,6 +339,7 @@ ToneAlarm::ToneAlarm() : _default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning _default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning _default_tunes[TONE_SINGLE_BEEP_TUNE] = "MFT100a8"; // single beep + _default_tunes[TONE_HOME_SET] = "MFT100L4>G#6A#6B#4"; _tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune _tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone @@ -354,6 +355,7 @@ ToneAlarm::ToneAlarm() : _tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning _tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning _tune_names[TONE_SINGLE_BEEP_TUNE] = "beep"; // single beep + _tune_names[TONE_HOME_SET] = "home_set"; } ToneAlarm::~ToneAlarm() From b5a79bbc0b22a83c7a4b3cefaf7f1195f5b05f32 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 14 Jun 2015 15:12:35 +0200 Subject: [PATCH 22/34] commander: Use distinct tunes for home set and mission ok / failed --- src/modules/commander/commander_helper.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index cbf11de1b0..362a707c03 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -179,7 +179,7 @@ void tune_home_set(bool use_buzzer) rgbled_set_mode(RGBLED_MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(TONE_HOME_SET); } } @@ -190,7 +190,7 @@ void tune_mission_ok(bool use_buzzer) rgbled_set_mode(RGBLED_MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(TONE_NOTIFY_NEUTRAL_TUNE); } } @@ -201,7 +201,7 @@ void tune_mission_fail(bool use_buzzer) rgbled_set_mode(RGBLED_MODE_BLINK_FAST); if (use_buzzer) { - set_tune(TONE_NOTIFY_POSITIVE_TUNE); + set_tune(TONE_NOTIFY_NEGATIVE_TUNE); } } From eb3cc8b41ab587f7cd6240abb9dcba918888218c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 17:02:55 +0200 Subject: [PATCH 23/34] mission result topic: Add warnings --- msg/mission_result.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/mission_result.msg b/msg/mission_result.msg index 532db6f73d..ac4d32f559 100644 --- a/msg/mission_result.msg +++ b/msg/mission_result.msg @@ -2,6 +2,7 @@ uint32 instance_count # Instance count of this mission. Increments monotonically uint32 seq_reached # Sequence of the mission item which has been reached uint32 seq_current # Sequence of the current mission item bool valid # true if mission is valid +bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings bool reached # true if mission has been reached bool finished # true if mission has been completed bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode From b11e13331882272c6b6bd5d200ec938070ade632 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 17:03:12 +0200 Subject: [PATCH 24/34] Evaluate warning field from mission result --- src/modules/commander/commander.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index edeb31e888..5ac8e9ca8b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1774,14 +1774,18 @@ int commander_thread_main(int argc, char *argv[]) if (status.condition_home_position_valid && (hrt_elapsed_time(&_home.timestamp) > 2000000) && _last_mission_instance != mission_result.instance_count) { - if (mission_result.valid) { + if (!mission_result.valid) { + /* the mission is invalid */ + tune_mission_fail(true); + warnx("mission fail"); + } else if (mission_result.warning) { + /* the mission has a warning */ + tune_mission_fail(true); + warnx("mission warning"); + } else { /* the mission is valid */ tune_mission_ok(true); warnx("mission ok"); - } else { - /* the mission is not valid */ - tune_mission_fail(true); - warnx("mission fail"); } /* prevent further feedback until the mission changes */ From 41f535ae262d7a6e5f4a2fe043b66d86dde9993e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 17:03:34 +0200 Subject: [PATCH 25/34] navigator: Include distance to first waypoint in mission check, provide warning feedback --- src/modules/navigator/mission.cpp | 82 ++----------- src/modules/navigator/mission.h | 1 - .../navigator/mission_feasibility_checker.cpp | 113 ++++++++++++++---- .../navigator/mission_feasibility_checker.h | 9 +- 4 files changed, 107 insertions(+), 98 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index be27e6208a..4944ebe789 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -79,7 +79,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _mission_type(MISSION_TYPE_NONE), _inited(false), _home_inited(false), - _dist_1wp_ok(false), _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), @@ -119,7 +118,9 @@ Mission::on_inactive() _navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid()); + _navigator->get_home_position()->alt, _navigator->home_position_valid(), + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _param_dist_1wp.get(), _navigator->get_mission_result()->warning); _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); @@ -255,7 +256,9 @@ Mission::update_offboard_mission() failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), - _navigator->get_home_position()->alt, _navigator->home_position_valid()); + _navigator->get_home_position()->alt, _navigator->home_position_valid(), + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, + _param_dist_1wp.get(), _navigator->get_mission_result()->warning); _navigator->get_mission_result()->valid = !failed; _navigator->increment_mission_instance_count(); @@ -308,73 +311,6 @@ Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item) } } -bool -Mission::check_dist_1wp() -{ - if (_dist_1wp_ok) { - /* always return true after at least one successful check */ - return true; - } - - /* check if first waypoint is not too far from home */ - if (_param_dist_1wp.get() > 0.0f) { - if (_navigator->get_vstatus()->condition_home_position_valid) { - struct mission_item_s mission_item; - - /* find first waypoint (with lat/lon) item in datamanager */ - for (unsigned i = 0; i < _offboard_mission.count; i++) { - if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i, - &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { - - /* check only items with valid lat/lon */ - if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || - 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 || - mission_item.nav_cmd == NAV_CMD_TAKEOFF || - mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { - - /* check distance from current position to item */ - float dist_to_1wp = get_distance_to_next_waypoint( - mission_item.lat, mission_item.lon, - _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); - - if (dist_to_1wp < _param_dist_1wp.get()) { - _dist_1wp_ok = true; - if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) { - /* allow at 2/3 distance, but warn */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp); - } - return true; - - } else { - /* item is too far from home */ - mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get()); - return false; - } - } - - } else { - /* error reading, mission is invalid */ - mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission"); - return false; - } - } - - /* no waypoints found in mission, then we will not fly far away */ - _dist_1wp_ok = true; - return true; - - } else { - mavlink_log_info(_navigator->get_mavlink_fd(), "no home position"); - return false; - } - - } else { - return true; - } -} - void Mission::set_mission_items() { @@ -394,10 +330,8 @@ Mission::set_mission_items() _mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item); } - /* get home distance state */ - bool home_dist_ok = check_dist_1wp(); /* the home dist check provides user feedback, so we initialize it to this */ - bool user_feedback_done = !home_dist_ok; + bool user_feedback_done = false; /* try setting onboard mission item */ if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) { @@ -409,7 +343,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) { + } else if (read_mission_item(false, true, &_mission_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 6cfae49598..d77f461574 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -187,7 +187,6 @@ private: bool _inited; bool _home_inited; - bool _dist_1wp_ok; MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */ diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 9d1dc7c7e6..05019bf8aa 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -57,23 +57,40 @@ #endif static const int ERROR = -1; -MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false) +MissionFeasibilityChecker::MissionFeasibilityChecker() : + _mavlink_fd(-1), + _capabilities_sub(-1), + _initDone(false), + _dist_1wp_ok(false) { _nav_caps = {0}; } -bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) +bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, + dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, + float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued) { bool failed = false; + bool warned = false; /* Init if not done yet */ init(); _mavlink_fd = mavlink_fd; + // first check if we have a valid position + if (!home_valid /* can later use global / local pos for finer granularity */) { + failed = true; + warned = true; + mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock."); + } else { + failed |= !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); + } + // check if all mission item commands are supported failed |= !checkMissionItemValidity(dm_current, nMissionItems); - + failed |= !checkGeofence(dm_current, nMissionItems, geofence); + failed |= !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); @@ -90,28 +107,20 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { - - /* Perform checks and issue feedback to the user for all checks */ - bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); - - /* Mission is only marked as feasible if all checks return true */ - return (resGeofence && resHomeAltitude); + /* no custom rotary wing checks yet */ + return true; } bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); -// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); /* Perform checks and issue feedback to the user for all checks */ bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); - bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); - bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid); /* Mission is only marked as feasible if all checks return true */ - return (resLanding && resGeofence && resHomeAltitude); + return resLanding; } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -137,7 +146,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } -bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error) +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, + float home_alt, bool home_valid, bool &warning_issued, bool throw_error) { /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ for (size_t i = 0; i < nMissionItems; i++) { @@ -145,17 +155,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, const ssize_t len = sizeof(struct mission_item_s); if (dm_read(dm_current, i, &missionitem, len) != len) { + warning_issued = true; /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - if (throw_error) { - return false; - } else { - return true; - } + return false; } /* always reject relative alt without home set */ if (missionitem.altitude_is_relative && !home_valid) { mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i); + warning_issued = true; return false; } @@ -163,6 +171,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude; if (home_alt > wp_alt) { + + warning_issued = true; + if (throw_error) { mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i); return false; @@ -275,6 +286,68 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size return true; } +bool +MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued) +{ + if (_dist_1wp_ok) { + /* always return true after at least one successful check */ + return true; + } + + /* check if first waypoint is not too far from home */ + if (dist_first_wp > 0.0f) { + struct mission_item_s mission_item; + + /* find first waypoint (with lat/lon) item in datamanager */ + for (unsigned i = 0; i < nMissionItems; i++) { + if (dm_read(dm_current, i, + &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { + + /* check only items with valid lat/lon */ + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + 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 || + mission_item.nav_cmd == NAV_CMD_TAKEOFF || + mission_item.nav_cmd == NAV_CMD_PATHPLANNING) { + + /* check distance from current position to item */ + float dist_to_1wp = get_distance_to_next_waypoint( + mission_item.lat, mission_item.lon, curr_lat, curr_lon); + + if (dist_to_1wp < dist_first_wp) { + _dist_1wp_ok = true; + if (dist_to_1wp > ((dist_first_wp * 3) / 2)) { + /* allow at 2/3 distance, but warn */ + mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp); + warning_issued = true; + } + return true; + + } else { + /* item is too far from home */ + mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp); + warning_issued = true; + return false; + } + } + + } else { + /* error reading, mission is invalid */ + mavlink_log_info(_mavlink_fd, "error reading offboard mission"); + return false; + } + } + + /* no waypoints found in mission, then we will not fly far away */ + _dist_1wp_ok = true; + return true; + + } else { + return true; + } +} + void MissionFeasibilityChecker::updateNavigationCapabilities() { (void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps); diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 9c9511be3d..4586f75a47 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -57,12 +57,14 @@ private: struct navigation_capabilities_s _nav_caps; bool _initDone; + bool _dist_1wp_ok; void init(); /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); - bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false); bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); + bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued); /* Checks specific to fixedwing airframes */ bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); @@ -79,8 +81,9 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt, bool home_valid); + bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, + size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, + double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued); }; From 73d179fb59eb996d7cf95ab524b399c8efc8d407 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Apr 2015 01:29:51 +0200 Subject: [PATCH 26/34] MS5611 driver: Fix reeset logic via I2C, minor code style fixes. Fixes #2007, identified by Kirill-ka --- src/drivers/ms5611/ms5611.cpp | 28 ++++++++++++++++++---------- 1 file changed, 18 insertions(+), 10 deletions(-) diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index ef94d03633..18b02228c2 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -154,10 +154,12 @@ protected: /** * Initialize the automatic measurement state machine and start it. * + * @param delay_ticks the number of queue ticks before executing the next cycle + * * @note This function is called at open and error time. It might make sense * to make it more aggressive about resetting the bus in case of errors. */ - void start_cycle(); + void start_cycle(unsigned delay_ticks = 1); /** * Stop the automatic measurement state machine. @@ -515,7 +517,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) } void -MS5611::start_cycle() +MS5611::start_cycle(unsigned delay_ticks) { /* reset the report ring and state machine */ @@ -524,7 +526,7 @@ MS5611::start_cycle() _reports->flush(); /* schedule a cycle to start things */ - work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1); + work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, delay_ticks); } void @@ -564,8 +566,11 @@ MS5611::cycle() } /* issue a reset command to the sensor */ _interface->ioctl(IOCTL_RESET, dummy); - /* reset the collection state machine and try again */ - start_cycle(); + /* reset the collection state machine and try again - we need + * to wait 2.8 ms after issuing the sensor reset command + * according to the MS5611 datasheet + */ + start_cycle(USEC2TICK(2800)); return; } @@ -594,7 +599,6 @@ MS5611::cycle() /* measurement phase */ ret = measure(); if (ret != OK) { - //log("measure error %d", ret); /* issue a reset command to the sensor */ _interface->ioctl(IOCTL_RESET, dummy); /* reset the collection state machine and try again */ @@ -1182,26 +1186,30 @@ ms5611_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(verb, "start")) + if (!strcmp(verb, "start")) { ms5611::start(busid); + } /* * Test the driver/device. */ - if (!strcmp(verb, "test")) + if (!strcmp(verb, "test")) { ms5611::test(busid); + } /* * Reset the driver. */ - if (!strcmp(verb, "reset")) + if (!strcmp(verb, "reset")) { ms5611::reset(busid); + } /* * Print driver information. */ - if (!strcmp(verb, "info")) + if (!strcmp(verb, "info")) { ms5611::info(); + } /* * Perform MSL pressure calibration given an altitude in metres From 677aef6673e96f7227db981a2e464898c86290ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 21:55:02 +0200 Subject: [PATCH 27/34] navigator: Fixed bitwise or --- .../navigator/mission_feasibility_checker.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 05019bf8aa..c57a12aefb 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -84,18 +84,18 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota warned = true; mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock."); } else { - failed |= !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); + failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued); } // check if all mission item commands are supported - failed |= !checkMissionItemValidity(dm_current, nMissionItems); - failed |= !checkGeofence(dm_current, nMissionItems, geofence); - failed |= !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); + failed = failed || !checkMissionItemValidity(dm_current, nMissionItems); + failed = failed || !checkGeofence(dm_current, nMissionItems, geofence); + failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { - failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); } else { - failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } if (!failed) { From 460c6bcf5715aac96bbb92ab225676b6b62a8c0c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 15 Jun 2015 21:56:44 +0200 Subject: [PATCH 28/34] MC att control demand: Require a higher minimum throttle --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 ++++--- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 995937aa6c..6eaca26b17 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1396,14 +1396,15 @@ MulticopterPositionControl::task_main() } //Control roll and pitch directly if we no aiding velocity controller is active - if(!_control_mode.flag_control_velocity_enabled) { + if (!_control_mode.flag_control_velocity_enabled) { _att_sp.roll_body = _manual.y * _params.man_roll_max; _att_sp.pitch_body = -_manual.x * _params.man_pitch_max; } //Control climb rate directly if no aiding altitude controller is active - if(!_control_mode.flag_control_climb_rate_enabled) { - _att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER); + if (!_control_mode.flag_control_climb_rate_enabled) { + _att_sp.thrust = math::min(_manual.z, _params.thr_max); + _att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min); } //Construct attitude setpoint rotation matrix diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index ade43ffb91..4a58dfdd0c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -50,7 +50,7 @@ * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.18f); /** * Maximum thrust From 79944b2c354e15ebbb47901d01d6542060b84c05 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 16 Jun 2015 11:21:36 +0200 Subject: [PATCH 29/34] Update pitch and yaw gains to flight tested values. --- ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d index bd99e0e554..36d387c759 100644 --- a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -9,16 +9,15 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then - # TODO tune roll/pitch separately param set MC_ROLL_P 7.0 param set MC_ROLLRATE_P 0.13 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.004 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_P 0.19 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 2.8 + param set MC_YAW_P 4.0 param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 From 3e64ad10e8e53219c9d30f17311dc7f6db57b173 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 17 Jun 2015 06:21:28 -1000 Subject: [PATCH 30/34] Conditional inclusion of the Node Allocation and FW Server - default is OFF --- src/modules/uavcan/uavcan_main.cpp | 25 ++++++++++++++++++------- src/modules/uavcan/uavcan_main.hpp | 21 ++++++++++++--------- 2 files changed, 30 insertions(+), 16 deletions(-) diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index c18d6e5d1c..f4763fce7f 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -53,14 +53,15 @@ #include #include "uavcan_main.hpp" -#include -#include - -#include +#if defined(USE_FW_NODE_SERVER) +# include +# include +# include //todo:The Inclusion of file_server_backend is killing // #include and leaving OK undefined -#define OK 0 +# define OK 0 +#endif /** * @file uavcan_main.cpp @@ -75,20 +76,26 @@ * UavcanNode */ UavcanNode *UavcanNode::_instance; +#if defined(USE_FW_NODE_SERVER) uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance; uavcan_posix::dynamic_node_id_server::FileEventTracer tracer; uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; uavcan_posix::FirmwareVersionChecker fw_version_checker; - +#endif UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), _node_mutex(), +#if !defined(USE_FW_NODE_SERVER) + _esc_controller(_node) +#else _esc_controller(_node), _fileserver_backend(_node), _node_info_retriever(_node), _fw_upgrade_trigger(_node, fw_version_checker), _fw_server(_node, _fileserver_backend) +#endif + { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); @@ -154,7 +161,10 @@ UavcanNode::~UavcanNode() perf_free(_perfcnt_node_spin_elapsed); perf_free(_perfcnt_esc_mixer_output_elapsed); perf_free(_perfcnt_esc_mixer_total_elapsed); + +#if defined(USE_FW_NODE_SERVER) delete(_server_instance); +#endif } @@ -305,7 +315,7 @@ int UavcanNode::init(uavcan::NodeID node_id) br = br->getSibling(); } - +#if defined(USE_FW_NODE_SERVER) /* Initialize the fw version checker. * giving it it's path */ @@ -373,6 +383,7 @@ int UavcanNode::init(uavcan::NodeID node_id) return ret; } +#endif /* Start the Node */ return _node.start(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 30d0a363b7..43d82082b4 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -34,6 +34,7 @@ #pragma once #include + #include #include #include @@ -47,13 +48,13 @@ #include "actuators/esc.hpp" #include "sensors/sensor_bridge.hpp" - -#include -#include -#include -#include -#include - +#if defined(USE_FW_NODE_SERVER) +# include +# include +# include +# include +# include +#endif /** * @file uavcan_main.hpp @@ -147,7 +148,6 @@ private: unsigned _output_count = 0; ///< number of actuators currently available static UavcanNode *_instance; ///< singleton pointer - static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer Node _node; ///< library instance pthread_mutex_t _node_mutex; @@ -155,11 +155,14 @@ private: UavcanEscController _esc_controller; +#if defined(USE_FW_NODE_SERVER) + static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance; ///< server singleton pointer + uavcan_posix::BasicFileSeverBackend _fileserver_backend; uavcan::NodeInfoRetriever _node_info_retriever; uavcan::FirmwareUpdateTrigger _fw_upgrade_trigger; uavcan::BasicFileServer _fw_server; - +#endif List _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; From f6afa23d04f1c708be9664bcfc9e4bb1ed72903d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jun 2015 19:40:57 +0200 Subject: [PATCH 31/34] Fix up SK450 default gains to more reasonable values --- ROMFS/px4fmu_common/init.d/10019_sk450_deadcat | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat index c6861c2d45..b91de228c5 100644 --- a/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat +++ b/ROMFS/px4fmu_common/init.d/10019_sk450_deadcat @@ -10,13 +10,13 @@ sh /etc/init.d/rc.mc_defaults if [ $AUTOCNF == yes ] then param set MC_ROLL_P 6.0 - param set MC_ROLLRATE_P 0.04 - param set MC_ROLLRATE_I 0.1 + param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_I 0.03 param set MC_ROLLRATE_D 0.0015 param set MC_PITCH_P 6.0 - param set MC_PITCHRATE_P 0.08 - param set MC_PITCHRATE_I 0.2 + param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_I 0.03 param set MC_PITCHRATE_D 0.0015 param set MC_YAW_P 2.8 From 959333d6cc8e5f99ee68b2dff9cb65d54d805985 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Jun 2015 22:44:51 +0200 Subject: [PATCH 32/34] Re-balance FMUv2 config in terms of buffer sizes to free some excessively used resources --- nuttx-configs/px4fmu-v2/nsh/defconfig | 33 ++++++++++++++------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 19e0f7c632..ef9c673785 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -367,7 +367,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717 CONFIG_DRAM_START=0x20000000 CONFIG_DRAM_SIZE=262144 CONFIG_ARCH_HAVE_INTERRUPTSTACK=y -CONFIG_ARCH_INTERRUPTSTACK=1500 +# The actual usage is 420 bytes +CONFIG_ARCH_INTERRUPTSTACK=750 # # Boot options @@ -548,8 +549,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y # # USART1 Configuration # -CONFIG_USART1_RXBUFSIZE=512 -CONFIG_USART1_TXBUFSIZE=512 +CONFIG_USART1_RXBUFSIZE=600 +CONFIG_USART1_TXBUFSIZE=600 CONFIG_USART1_BAUD=115200 CONFIG_USART1_BITS=8 CONFIG_USART1_PARITY=0 @@ -561,7 +562,7 @@ CONFIG_USART1_2STOP=0 # USART2 Configuration # CONFIG_USART2_RXBUFSIZE=600 -CONFIG_USART2_TXBUFSIZE=2200 +CONFIG_USART2_TXBUFSIZE=1860 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 @@ -572,8 +573,8 @@ CONFIG_USART2_OFLOWCONTROL=y # # USART3 Configuration # -CONFIG_USART3_RXBUFSIZE=512 -CONFIG_USART3_TXBUFSIZE=512 +CONFIG_USART3_RXBUFSIZE=400 +CONFIG_USART3_TXBUFSIZE=400 CONFIG_USART3_BAUD=57600 CONFIG_USART3_BITS=8 CONFIG_USART3_PARITY=0 @@ -584,8 +585,8 @@ CONFIG_USART3_OFLOWCONTROL=y # # UART4 Configuration # -CONFIG_UART4_RXBUFSIZE=512 -CONFIG_UART4_TXBUFSIZE=512 +CONFIG_UART4_RXBUFSIZE=400 +CONFIG_UART4_TXBUFSIZE=400 CONFIG_UART4_BAUD=57600 CONFIG_UART4_BITS=8 CONFIG_UART4_PARITY=0 @@ -596,8 +597,8 @@ CONFIG_UART4_2STOP=0 # # USART6 Configuration # -CONFIG_USART6_RXBUFSIZE=512 -CONFIG_USART6_TXBUFSIZE=512 +CONFIG_USART6_RXBUFSIZE=400 +CONFIG_USART6_TXBUFSIZE=400 CONFIG_USART6_BAUD=57600 CONFIG_USART6_BITS=8 CONFIG_USART6_PARITY=0 @@ -608,8 +609,8 @@ CONFIG_USART6_2STOP=0 # # UART7 Configuration # -CONFIG_UART7_RXBUFSIZE=512 -CONFIG_UART7_TXBUFSIZE=512 +CONFIG_UART7_RXBUFSIZE=400 +CONFIG_UART7_TXBUFSIZE=400 CONFIG_UART7_BAUD=57600 CONFIG_UART7_BITS=8 CONFIG_UART7_PARITY=0 @@ -620,8 +621,8 @@ CONFIG_UART7_2STOP=0 # # UART8 Configuration # -CONFIG_UART8_RXBUFSIZE=512 -CONFIG_UART8_TXBUFSIZE=512 +CONFIG_UART8_RXBUFSIZE=400 +CONFIG_UART8_TXBUFSIZE=400 CONFIG_UART8_BAUD=57600 CONFIG_UART8_BITS=8 CONFIG_UART8_PARITY=0 @@ -663,8 +664,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=1000 -CONFIG_CDCACM_TXBUFSIZE=8000 +CONFIG_CDCACM_RXBUFSIZE=600 +CONFIG_CDCACM_TXBUFSIZE=5000 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0011 CONFIG_CDCACM_VENDORSTR="3D Robotics" From 3cd211ed72c39a9d9afa392138aff2060a4e2d41 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jun 2015 08:55:34 +0200 Subject: [PATCH 33/34] MC pos control: Do not raise min throttle too far. --- src/modules/mc_pos_control/mc_pos_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 4a58dfdd0c..4865c1c684 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -50,7 +50,7 @@ * @max 1.0 * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.18f); +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f); /** * Maximum thrust From e08dc0df4071ec2b2a6406d16bbc1e502cb2afb4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 18 Jun 2015 11:03:32 +0200 Subject: [PATCH 34/34] Add support for RC_CHANNELS_OVERRIDE in addition to normal message --- src/modules/mavlink/mavlink_receiver.cpp | 46 ++++++++++++++++++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + 2 files changed, 47 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 1a6494ad93..8ebbe47052 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -194,6 +194,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_manual_control(msg); break; + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + handle_message_rc_channels_override(msg); + break; + case MAVLINK_MSG_ID_HEARTBEAT: handle_message_heartbeat(msg); break; @@ -912,6 +916,48 @@ static int decode_switch_pos_n(uint16_t buttons, int sw) { } } +void +MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg) +{ + mavlink_rc_channels_override_t man; + mavlink_msg_rc_channels_override_decode(msg, &man); + + // Check target + if (man.target_system != 0 && man.target_system != _mavlink->get_system_id()) { + return; + } + + struct rc_input_values rc = {}; + rc.timestamp_publication = hrt_absolute_time(); + rc.timestamp_last_signal = rc.timestamp_publication; + + rc.channel_count = 8; + rc.rc_failsafe = false; + rc.rc_lost = false; + rc.rc_lost_frame_count = 0; + rc.rc_total_frame_count = 1; + rc.rc_ppm_frame_length = 0; + rc.input_source = RC_INPUT_SOURCE_MAVLINK; + rc.rssi = RC_INPUT_RSSI_MAX; + + /* channels */ + rc.values[0] = man.chan1_raw; + rc.values[1] = man.chan2_raw; + rc.values[2] = man.chan3_raw; + rc.values[3] = man.chan4_raw; + rc.values[4] = man.chan5_raw; + rc.values[5] = man.chan6_raw; + rc.values[6] = man.chan7_raw; + rc.values[7] = man.chan8_raw; + + if (_rc_pub <= 0) { + _rc_pub = orb_advertise(ORB_ID(input_rc), &rc); + + } else { + orb_publish(ORB_ID(input_rc), _rc_pub, &rc); + } +} + void MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index fe217f3c3b..18a3dc208a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -126,6 +126,7 @@ private: void handle_message_set_attitude_target(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_rc_channels_override(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); void handle_message_ping(mavlink_message_t *msg); void handle_message_request_data_stream(mavlink_message_t *msg);