From adbccfaa1cd100609490b61f2081a0619b0a36c8 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Wed, 3 Jun 2015 09:32:02 -0400 Subject: [PATCH 01/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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/88] 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 1ebea1e7590d0d66b18cd39fcc45897bca8b8256 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 11:05:44 +0200 Subject: [PATCH 29/88] ask for climbout mode when doin takeoff help --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 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 01d94a8881..8e516a708f 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 @@ -395,7 +395,7 @@ private: /** * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + bool do_takeoff_help(); /** * Update desired altitude base on user pitch stick input @@ -987,16 +987,20 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -void FixedwingPositionControl::do_takeoff_help() +bool FixedwingPositionControl::do_takeoff_help() { const hrt_abstime delta_takeoff = 10000000; - const float throttle_threshold = 0.3f; - const float delta_alt_takeoff = 30.0f; + const float throttle_threshold = 0.1f; + const float delta_alt_takeoff = 50.0f; + float climbout = false; /* demand 30 m above ground if user switched into this mode during takeoff */ if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { _hold_alt = _ground_alt + delta_alt_takeoff; + climbout = true; + } + return climbout; } bool @@ -1416,7 +1420,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1509,7 +1513,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; From 79944b2c354e15ebbb47901d01d6542060b84c05 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 16 Jun 2015 11:21:36 +0200 Subject: [PATCH 30/88] 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 c91bb76b42d8d85339902c0b29123243156a0f0b Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 11:05:44 +0200 Subject: [PATCH 31/88] ask for climbout mode when doin takeoff help --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 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 01d94a8881..8e516a708f 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 @@ -395,7 +395,7 @@ private: /** * Do takeoff help when in altitude controlled modes */ - void do_takeoff_help(); + bool do_takeoff_help(); /** * Update desired altitude base on user pitch stick input @@ -987,16 +987,20 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -void FixedwingPositionControl::do_takeoff_help() +bool FixedwingPositionControl::do_takeoff_help() { const hrt_abstime delta_takeoff = 10000000; - const float throttle_threshold = 0.3f; - const float delta_alt_takeoff = 30.0f; + const float throttle_threshold = 0.1f; + const float delta_alt_takeoff = 50.0f; + float climbout = false; /* demand 30 m above ground if user switched into this mode during takeoff */ if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { _hold_alt = _ground_alt + delta_alt_takeoff; + climbout = true; + } + return climbout; } bool @@ -1416,7 +1420,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1509,7 +1513,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi 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(); + climbout_requested |= do_takeoff_help(); /* throttle limiting */ throttle_max = _parameters.throttle_max; From 5c59d7a434791222a6fad49e64206432a86c22bf Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 16 Jun 2015 23:05:25 +0200 Subject: [PATCH 32/88] do not run tecs if we are on ground to prevent integrator filling --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 7 ++++++- 1 file changed, 6 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 8e516a708f..05988ef53a 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 @@ -1024,7 +1024,7 @@ 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; - if (!_mTecs.getEnabled()) { + if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) { _tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); } @@ -1757,6 +1757,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ const math::Vector<3> &ground_speed, unsigned mode, bool pitch_max_special) { + /* do not run tecs if we are not in air */ + if (_vehicle_status.condition_landed) { + return; + } + if (_mTecs.getEnabled()) { /* Using mtecs library: prepare arguments for mtecs call */ float flightPathAngle = 0.0f; From 6ce106eea465b9bbaf59f4d44a2954bd107d1035 Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 17 Jun 2015 17:36:26 +0200 Subject: [PATCH 33/88] limit minimum pitch in altitude controller modes if in a takeoff situation --- .../fw_pos_control_l1_main.cpp | 63 +++++++++++++------ 1 file changed, 45 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 05988ef53a..c262e80c5f 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 @@ -389,13 +389,16 @@ private: float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** - * Control position. + * Check if we are in a takeoff situation */ + bool in_takeoff_situation(); /** * Do takeoff help when in altitude controlled modes + * @param hold_altitude altitude setpoint for controller + * @param pitch_limit_min minimum pitch allowed */ - bool do_takeoff_help(); + void do_takeoff_help(float *hold_altitude, float *pitch_limit_min); /** * Update desired altitude base on user pitch stick input @@ -405,6 +408,9 @@ private: */ bool update_desired_altitude(float dt); + /** + * Control position. + */ bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed, const struct position_setpoint_triplet_s &_pos_sp_triplet); @@ -987,20 +993,26 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) return climbout_mode; } -bool FixedwingPositionControl::do_takeoff_help() -{ +bool FixedwingPositionControl::in_takeoff_situation() { const hrt_abstime delta_takeoff = 10000000; const float throttle_threshold = 0.1f; - const float delta_alt_takeoff = 50.0f; - float climbout = false; - - /* demand 30 m above ground if user switched into this mode during takeoff */ - if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) { - _hold_alt = _ground_alt + delta_alt_takeoff; - climbout = true; + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + _parameters.climbout_diff) { + return true; + } + + return false; +} + +void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min) +{ + /* demand "climbout_diff" m above ground if user switched into this mode during takeoff */ + if (in_takeoff_situation()) { + *hold_altitude = _ground_alt + _parameters.climbout_diff; + *pitch_limit_min = math::radians(10.0f); + } else { + *pitch_limit_min = _parameters.pitch_limit_min; } - return climbout; } bool @@ -1419,8 +1431,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - climbout_requested |= do_takeoff_help(); + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + * and set limit to pitch angle to prevent stearing into ground + */ + float pitch_limit_min; + do_takeoff_help(&_hold_alt, &pitch_limit_min); + /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1437,7 +1453,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - math::radians(_parameters.pitch_limit_min), + pitch_limit_min, _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); @@ -1452,6 +1468,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } + /* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration + to make sure the plane does not start rolling + */ + if (in_takeoff_situation()) { + _hdg_hold_enabled = false; + _yaw_lock_engaged = true; + } + if (_yaw_lock_engaged) { /* just switched back from non heading-hold to heading hold */ @@ -1512,8 +1536,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* update desired altitude based on user pitch stick input */ bool climbout_requested = update_desired_altitude(dt); - /* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/ - climbout_requested |= do_takeoff_help(); + /* if we assume that user is taking off then help by demanding altitude setpoint well above ground + * and set limit to pitch angle to prevent stearing into ground + */ + float pitch_limit_min; + do_takeoff_help(&_hold_alt, &pitch_limit_min); /* throttle limiting */ throttle_max = _parameters.throttle_max; @@ -1530,7 +1557,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi throttle_max, _parameters.throttle_cruise, climbout_requested, - math::radians(_parameters.pitch_limit_min), + pitch_limit_min, _global_pos.alt, ground_speed, tecs_status_s::TECS_MODE_NORMAL); From 0446efa9a4b8e9399f64d4c65303bab971342b3e Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 17 Jun 2015 17:46:37 +0200 Subject: [PATCH 34/88] limit roll angle in loiter and position control mode if we are in a takeoff situation --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) 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 c262e80c5f..e4682689af 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 @@ -1134,6 +1134,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + if (in_takeoff_situation()) { + /* 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)); + } + 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, @@ -1505,6 +1511,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + + if (in_takeoff_situation()) { + /* 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 { _hdg_hold_enabled = false; From 3e64ad10e8e53219c9d30f17311dc7f6db57b173 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 17 Jun 2015 06:21:28 -1000 Subject: [PATCH 35/88] 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 36/88] 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 37/88] 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 38/88] 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 39/88] 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); From 2485e037943222b521b6cc98eb6bcf50c82a3fbe Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:54:58 +0200 Subject: [PATCH 40/88] corrected elevon mixer for firefly6 --- ROMFS/px4fmu_common/mixers/firefly6.aux.mix | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix index fda8416403..22dc2a69ce 100644 --- a/ROMFS/px4fmu_common/mixers/firefly6.aux.mix +++ b/ROMFS/px4fmu_common/mixers/firefly6.aux.mix @@ -11,12 +11,12 @@ Elevon mixers ------------- M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 8000 8000 0 -10000 10000 M: 2 O: 10000 10000 0 -10000 10000 -S: 1 0 7500 7500 0 -10000 10000 +S: 1 0 -7500 -7500 0 -10000 10000 S: 1 1 -8000 -8000 0 -10000 10000 Landing gear mixer From 1ccded0305f439b4f8e45f23ec55bf304cc7c1ab Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:55:30 +0200 Subject: [PATCH 41/88] added generic class for vtol types --- src/modules/vtol_att_control/vtol_type.cpp | 133 +++++++++++++++++++++ src/modules/vtol_att_control/vtol_type.h | 115 ++++++++++++++++++ 2 files changed, 248 insertions(+) create mode 100644 src/modules/vtol_att_control/vtol_type.cpp create mode 100644 src/modules/vtol_att_control/vtol_type.h diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp new file mode 100644 index 0000000000..7e8d3217f1 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -0,0 +1,133 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file airframe.cpp + * + * @author Roman Bapst + * + */ + +#include "vtol_type.h" +#include "drivers/drv_pwm_output.h" +#include +#include "vtol_att_control_main.h" + +VtolType::VtolType(VtolAttitudeControl *att_controller) : +_attc(att_controller), +_vtol_mode(ROTARY_WING) +{ + _v_att = _attc->get_att(); + _v_att_sp = _attc->get_att_sp(); + _v_rates_sp = _attc->get_rates_sp(); + _mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp(); + _fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp(); + _manual_control_sp = _attc->get_manual_control_sp(); + _v_control_mode = _attc->get_control_mode(); + _vtol_vehicle_status = _attc->get_vehicle_status(); + _actuators_out_0 = _attc->get_actuators_out0(); + _actuators_out_1 = _attc->get_actuators_out1(); + _actuators_mc_in = _attc->get_actuators_mc_in(); + _actuators_fw_in = _attc->get_actuators_fw_in(); + _armed = _attc->get_armed(); + _local_pos = _attc->get_local_pos(); + _airspeed = _attc->get_airspeed(); + _batt_status = _attc->get_batt_status(); + _params = _attc->get_params(); + + flag_idle_mc = true; +} + +VtolType::~VtolType() +{ + +} + +/** +* Adjust idle speed for mc mode. +*/ +void VtolType::set_idle_mc() +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + unsigned pwm_value = _params->idle_pwm_mc; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting min values");} + + close(fd); + + flag_idle_mc = true; +} + +/** +* Adjust idle speed for fw mode. +*/ +void VtolType::set_idle_fw() +{ + int ret; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + unsigned pwm_value = PWM_LOWEST_MIN; + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + + pwm_values.values[i] = pwm_value; + pwm_values.channel_count++; + } + + ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting min values");} + + close(fd); +} \ No newline at end of file diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h new file mode 100644 index 0000000000..57448a7587 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_type.h @@ -0,0 +1,115 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file airframe.h + * + * @author Roman Bapst + * + */ + +#ifndef VTOL_YYPE_H +#define VTOL_YYPE_H + +struct Params { + int idle_pwm_mc; // pwm value for idle in mc mode + int vtol_motor_count; // number of motors + int vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode + float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) + float mc_airspeed_trim; // trim airspeed in multicopter mode + float mc_airspeed_max; // max airpseed in multicopter mode + float fw_pitch_trim; // trim for neutral elevon position in fw mode + float power_max; // maximum power of one engine + float prop_eff; // factor to calculate prop efficiency + float arsp_lp_gain; // total airspeed estimate low pass gain + int vtol_type; +}; + +enum mode { + ROTARY_WING = 0, + FIXED_WING, + TRANSITION, + EXTERNAL +}; + +class VtolAttitudeControl; + +class VtolType +{ +public: + + VtolType(VtolAttitudeControl *att_controller); + + virtual ~VtolType(); + + virtual void update_vtol_state() = 0; + virtual void update_mc_state() = 0; + virtual void process_mc_data() = 0; + virtual void update_fw_state() = 0; + virtual void process_fw_data() = 0; + virtual void update_transition_state() = 0; + virtual void update_external_state() = 0; + + void set_idle_mc(); + void set_idle_fw(); + + mode get_mode () {return _vtol_mode;}; + +protected: + VtolAttitudeControl *_attc; + mode _vtol_mode; + + struct vehicle_attitude_s *_v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode + struct vtol_vehicle_status_s *_vtol_vehicle_status; + struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s *_armed; //actuator arming status + struct vehicle_local_position_s *_local_pos; + struct airspeed_s *_airspeed; // airspeed + struct battery_status_s *_batt_status; // battery status + + struct Params *_params; + + bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" + +}; + +#endif From a212e457448ed41c8a33c39696a8963ce631f63d Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:56:11 +0200 Subject: [PATCH 42/88] added tiltrotor attitude control class --- src/modules/vtol_att_control/tiltrotor.cpp | 357 ++++++++++++++++++ src/modules/vtol_att_control/tiltrotor.h | 110 ++++++ .../vtol_att_control/tiltrotor_params.c | 118 ++++++ 3 files changed, 585 insertions(+) create mode 100644 src/modules/vtol_att_control/tiltrotor.cpp create mode 100644 src/modules/vtol_att_control/tiltrotor.h create mode 100644 src/modules/vtol_att_control/tiltrotor_params.c diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp new file mode 100644 index 0000000000..2cf074fe42 --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -0,0 +1,357 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tiltrotor.cpp + * + * @author Roman Bapst + * +*/ + +#include "tiltrotor.h" +#include "vtol_att_control_main.h" + +#define ARSP_BLEND_START 8.0f // airspeed at which we start blending mc/fw controls + +Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) : +VtolType(attc), +flag_max_mc(true), +_tilt_control(0.0f), +_roll_weight_mc(1.0f) +{ + _vtol_schedule.flight_mode = MC_MODE; + _vtol_schedule.transition_start = 0; + + _params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR"); + _params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR"); + _params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC"); + _params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS"); + _params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW"); + _params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS"); + _params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); + } + +Tiltrotor::~Tiltrotor() +{ + +} + +int +Tiltrotor::parameters_update() +{ + float v; + int l; + + /* vtol duration of a front transition */ + param_get(_params_handles_tiltrotor.front_trans_dur, &v); + _params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f); + + /* vtol duration of a back transition */ + param_get(_params_handles_tiltrotor.back_trans_dur, &v); + _params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f); + + /* vtol tilt mechanism position in mc mode */ + param_get(_params_handles_tiltrotor.tilt_mc, &v); + _params_tiltrotor.tilt_mc = v; + + /* vtol tilt mechanism position in transition mode */ + param_get(_params_handles_tiltrotor.tilt_transition, &v); + _params_tiltrotor.tilt_transition = v; + + /* vtol tilt mechanism position in fw mode */ + param_get(_params_handles_tiltrotor.tilt_fw, &v); + _params_tiltrotor.tilt_fw = v; + + /* vtol airspeed at which it is ok to switch to fw mode */ + param_get(_params_handles_tiltrotor.airspeed_trans, &v); + _params_tiltrotor.airspeed_trans = v; + + /* vtol lock elevons in multicopter */ + param_get(_params_handles_tiltrotor.elevons_mc_lock, &l); + _params_tiltrotor.elevons_mc_lock = l; + + return OK; +} + +void Tiltrotor::update_vtol_state() +{ + parameters_update(); + + /* simple logic using a two way switch to perform transitions. + * after flipping the switch the vehicle will start tilting rotors, picking up + * forward speed. After the vehicle has picked up enough speed the rotors are tilted + * forward completely. For the backtransition the motors simply rotate back. + */ + + if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == MC_MODE) { + // mc mode + _vtol_schedule.flight_mode = MC_MODE; + _tilt_control = _params_tiltrotor.tilt_mc; + _roll_weight_mc = 1.0f; + } else if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == FW_MODE) { + _vtol_schedule.flight_mode = TRANSITION_BACK; + flag_max_mc = true; + _vtol_schedule.transition_start = hrt_absolute_time(); + } else if (_manual_control_sp->aux1 >= 0.0f && _vtol_schedule.flight_mode == MC_MODE) { + // instant of doeing a front-transition + _vtol_schedule.flight_mode = TRANSITION_FRONT_P1; + _vtol_schedule.transition_start = hrt_absolute_time(); + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 > 0.0f) { + // check if we have reached airspeed to switch to fw mode + if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) { + _vtol_schedule.flight_mode = TRANSITION_FRONT_P2; + flag_max_mc = true; + _vtol_schedule.transition_start = hrt_absolute_time(); + } + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 > 0.0f) { + if (_tilt_control >= _params_tiltrotor.tilt_fw) { + _vtol_schedule.flight_mode = FW_MODE; + _tilt_control = _params_tiltrotor.tilt_fw; + } + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 < 0.0f) { + // failsave into mc mode + _vtol_schedule.flight_mode = MC_MODE; + _tilt_control = _params_tiltrotor.tilt_mc; + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 < 0.0f) { + // failsave into mc mode + _vtol_schedule.flight_mode = MC_MODE; + _tilt_control = _params_tiltrotor.tilt_mc; + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 < 0.0f) { + if (_tilt_control <= _params_tiltrotor.tilt_mc) { + _vtol_schedule.flight_mode = MC_MODE; + _tilt_control = _params_tiltrotor.tilt_mc; + flag_max_mc = false; + } + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 > 0.0f) { + // failsave into fw mode + _vtol_schedule.flight_mode = FW_MODE; + _tilt_control = _params_tiltrotor.tilt_fw; + } + + // tilt rotors if necessary + update_transition_state(); + + // map tiltrotor specific control phases to simple control modes + switch(_vtol_schedule.flight_mode) { + case MC_MODE: + _vtol_mode = ROTARY_WING; + break; + case FW_MODE: + _vtol_mode = FIXED_WING; + break; + case TRANSITION_FRONT_P1: + case TRANSITION_FRONT_P2: + case TRANSITION_BACK: + _vtol_mode = TRANSITION; + break; + } +} + +void Tiltrotor::update_mc_state() +{ + // adjust max pwm for rear motors to spin up + if (!flag_max_mc) { + set_max_mc(); + flag_max_mc = true; + } + + // set idle speed for rotary wing mode + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } +} + +void Tiltrotor::process_mc_data() +{ + fill_mc_att_control_output(); +} + + void Tiltrotor::update_fw_state() +{ + /* in fw mode we need the rear motors to stop spinning, in backtransition + * mode we let them spin in idle + */ + if (flag_max_mc) { + if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + set_max_fw(1200); + set_idle_mc(); + } else { + set_max_fw(950); + set_idle_fw(); + } + flag_max_mc = false; + } + + // adjust idle for fixed wing flight + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } + } + +void Tiltrotor::process_fw_data() +{ + fill_fw_att_control_output(); +} + +void Tiltrotor::update_transition_state() +{ + if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) { + // tilt rotors forward up to certain angle + if (_tilt_control <= _params_tiltrotor.tilt_transition) { + _tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f); + } + + // do blending of mc and fw controls + if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) { + _roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START); + } else { + // at low speeds give full weight to mc + _roll_weight_mc = 1.0f; + } + + _roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f); + + } else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) { + _tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f); + _roll_weight_mc = 0.0f; + } else if (_vtol_schedule.flight_mode == TRANSITION_BACK) { + // tilt rotors forward up to certain angle + float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f); + if (_tilt_control > _params_tiltrotor.tilt_mc) { + _tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress; + } + + _roll_weight_mc = progress; + } +} + +void Tiltrotor::update_external_state() +{ + +} + + /** +* Prepare message to acutators with data from mc attitude controller. +*/ +void Tiltrotor::fill_mc_att_control_output() +{ + _actuators_out_0->control[0] = _actuators_mc_in->control[0]; + _actuators_out_0->control[1] = _actuators_mc_in->control[1]; + _actuators_out_0->control[2] = _actuators_mc_in->control[2]; + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; + + _actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc); //roll elevon + _actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon + + _actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control +} + +/** +* Prepare message to acutators with data from fw attitude controller. +*/ +void Tiltrotor::fill_fw_att_control_output() +{ + /*For the first test in fw mode, only use engines for thrust!!!*/ + _actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc; + _actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc; + _actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc; + _actuators_out_0->control[3] = _actuators_fw_in->control[3]; + /*controls for the elevons */ + _actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon + _actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon + // unused now but still logged + _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle + _actuators_out_1->control[4] = _tilt_control; +} + +/** +* Kill rear motors for the FireFLY6 when in fw mode. +*/ +void +Tiltrotor::set_max_fw(unsigned pwm_value) +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + if (i == 2 || i == 3) { + pwm_values.values[i] = pwm_value; + } else { + pwm_values.values[i] = 2000; + } + pwm_values.channel_count = _params->vtol_motor_count; + } + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting max values");} + + close(fd); +} + +void +Tiltrotor::set_max_mc() +{ + int ret; + unsigned servo_count; + char *dev = PWM_OUTPUT0_DEVICE_PATH; + int fd = open(dev, 0); + + if (fd < 0) {err(1, "can't open %s", dev);} + + ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); + struct pwm_output_values pwm_values; + memset(&pwm_values, 0, sizeof(pwm_values)); + + for (int i = 0; i < _params->vtol_motor_count; i++) { + pwm_values.values[i] = 2000; + pwm_values.channel_count = _params->vtol_motor_count; + } + + ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values); + + if (ret != OK) {errx(ret, "failed setting max values");} + + close(fd); +} diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h new file mode 100644 index 0000000000..3ef1362d00 --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -0,0 +1,110 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file tiltrotor.h + * + * @author Roman Bapst + * + */ + +#ifndef TILTROTOR_H +#define TILTROTOR_H +#include "vtol_type.h" +#include +#include + +class Tiltrotor : public VtolType +{ + +public: + + Tiltrotor(VtolAttitudeControl * _att_controller); + ~Tiltrotor(); + + void update_vtol_state(); + void update_mc_state(); + void process_mc_data(); + void update_fw_state(); + void process_fw_data(); + void update_transition_state(); + void update_external_state(); + +private: + + struct { + float front_trans_dur; + float back_trans_dur; + float tilt_mc; + float tilt_transition; + float tilt_fw; + float airspeed_trans; + int elevons_mc_lock; // lock elevons in multicopter mode + } _params_tiltrotor; + + struct { + param_t front_trans_dur; + param_t back_trans_dur; + param_t tilt_mc; + param_t tilt_transition; + param_t tilt_fw; + param_t airspeed_trans; + param_t elevons_mc_lock; + } _params_handles_tiltrotor; + + enum vtol_mode { + MC_MODE = 0, + TRANSITION_FRONT_P1, + TRANSITION_FRONT_P2, + TRANSITION_BACK, + FW_MODE + }; + + struct { + vtol_mode flight_mode; // indicates in which mode the vehicle is in + hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition) + }_vtol_schedule; + + bool flag_max_mc; + float _tilt_control; + float _roll_weight_mc; + + void fill_mc_att_control_output(); + void fill_fw_att_control_output(); + void set_max_mc(); + void set_max_fw(unsigned pwm_value); + + int parameters_update(); + +}; +#endif diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c new file mode 100644 index 0000000000..76f3ee6c3c --- /dev/null +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -0,0 +1,118 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file tiltrotor_params.c + * Parameters for vtol attitude controller. + * + * @author Roman Bapst + */ + +#include + +/** + * Duration of a front transition + * + * Time in seconds used for a transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f); + +/** + * Duration of a back transition + * + * Time in seconds used for a back transition + * + * @min 0.0 + * @max 5 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f); + +/** + * Position of tilt servo in mc mode + * + * Position of tilt servo in mc mode + * + * @min 0.0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f); + +/** + * Position of tilt servo in transition mode + * + * Position of tilt servo in transition mode + * + * @min 0.0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f); + +/** + * Position of tilt servo in fw mode + * + * Position of tilt servo in fw mode + * + * @min 0.0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f); + +/** + * Transition airspeed + * + * Airspeed at which we can switch to fw mode + * + * @min 0.0 + * @max 20 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f); + +/** + * Lock elevons in multicopter mode + * + * If set to 1 the elevons are locked in multicopter mode + * + * @min 0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0); From 77077cb92ab11b78b4636072c7e1ae9c01c5e0e8 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:57:10 +0200 Subject: [PATCH 43/88] added tailsitter attitude control class --- src/modules/vtol_att_control/tailsitter.cpp | 184 ++++++++++++++++++++ src/modules/vtol_att_control/tailsitter.h | 74 ++++++++ 2 files changed, 258 insertions(+) create mode 100644 src/modules/vtol_att_control/tailsitter.cpp create mode 100644 src/modules/vtol_att_control/tailsitter.h diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp new file mode 100644 index 0000000000..4479783b92 --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -0,0 +1,184 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file tailsitter.cpp + * + * @author Roman Bapst + * + */ + + #include "tailsitter.h" + #include "vtol_att_control_main.h" + +Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) : +VtolType(att_controller), +_airspeed_tot(0), +_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")), +_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input")) +{ + +} + +Tailsitter::~Tailsitter() +{ + +} + +void Tailsitter::update_vtol_state() +{ + // simply switch between the two modes + if (_manual_control_sp->aux1 < 0.0f) { + _vtol_mode = ROTARY_WING; + } else if (_manual_control_sp->aux1 > 0.0f) { + _vtol_mode = FIXED_WING; + } +} + +void Tailsitter::update_mc_state() +{ + if (!flag_idle_mc) { + set_idle_mc(); + flag_idle_mc = true; + } +} + +void Tailsitter::process_mc_data() +{ + // scale pitch control with total airspeed + //scale_mc_output(); + fill_mc_att_control_output(); +} + +void Tailsitter::update_fw_state() +{ + if (flag_idle_mc) { + set_idle_fw(); + flag_idle_mc = false; + } +} + +void Tailsitter::process_fw_data() +{ + fill_fw_att_control_output(); +} + +void Tailsitter::update_transition_state() +{ + +} + +void Tailsitter::update_external_state() +{ + +} + + void Tailsitter::calc_tot_airspeed() + { + float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama + // calculate momentary power of one engine + float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count; + P = math::constrain(P,1.0f,_params->power_max); + // calculate prop efficiency + float power_factor = 1.0f - P*_params->prop_eff/_params->power_max; + float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; + eta = math::constrain(eta,0.001f,1.0f); // live on the safe side + // calculate induced airspeed by propeller + float v_ind = (airspeed/eta - airspeed)*2.0f; + // calculate total airspeed + float airspeed_raw = airspeed + v_ind; + // apply low-pass filter + _airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; +} + +void +Tailsitter::scale_mc_output() +{ + // scale around tuning airspeed + float airspeed; + calc_tot_airspeed(); // estimate air velocity seen by elevons + // if airspeed is not updating, we assume the normal average speed + if (bool nonfinite = !isfinite(_airspeed->true_airspeed_m_s) || + hrt_elapsed_time(&_airspeed->timestamp) > 1e6) { + airspeed = _params->mc_airspeed_trim; + if (nonfinite) { + perf_count(_nonfinite_input_perf); + } + } else { + airspeed = _airspeed_tot; + airspeed = math::constrain(airspeed,_params->mc_airspeed_min, _params->mc_airspeed_max); + } + + _vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging + /* + * For scaling our actuators using anything less than the min (close to stall) + * speed doesn't make any sense - its the strongest reasonable deflection we + * want to do in flight and its the baseline a human pilot would choose. + * + * Forcing the scaling to this value allows reasonable handheld tests. + */ + float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : airspeed); + _actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); +} + +/** +* Prepare message to acutators with data from fw attitude controller. +*/ +void Tailsitter::fill_fw_att_control_output() +{ + /*For the first test in fw mode, only use engines for thrust!!!*/ + _actuators_out_0->control[0] = 0; + _actuators_out_0->control[1] = 0; + _actuators_out_0->control[2] = 0; + _actuators_out_0->control[3] = _actuators_fw_in->control[3]; + /*controls for the elevons */ + _actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon + _actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon + // unused now but still logged + _actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw + _actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle +} + +/** +* Prepare message to acutators with data from mc attitude controller. +*/ +void Tailsitter::fill_mc_att_control_output() +{ + _actuators_out_0->control[0] = _actuators_mc_in->control[0]; + _actuators_out_0->control[1] = _actuators_mc_in->control[1]; + _actuators_out_0->control[2] = _actuators_mc_in->control[2]; + _actuators_out_0->control[3] = _actuators_mc_in->control[3]; + //set neutral position for elevons + _actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon + _actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon +} diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h new file mode 100644 index 0000000000..e681a9bf8b --- /dev/null +++ b/src/modules/vtol_att_control/tailsitter.h @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + /** + * @file tiltrotor.h + * + * @author Roman Bapst + * + */ + +#ifndef TAILSITTER_H +#define TAILSITTER_H + +#include "vtol_type.h" +#include + +class Tailsitter : public VtolType +{ + +public: + Tailsitter(VtolAttitudeControl * _att_controller); + ~Tailsitter(); + + void update_vtol_state(); + void update_mc_state(); + void process_mc_data(); + void update_fw_state(); + void process_fw_data(); + void update_transition_state(); + void update_external_state(); + +private: + void fill_mc_att_control_output(); + void fill_fw_att_control_output(); + void calc_tot_airspeed(); + void scale_mc_output(); + + float _airspeed_tot; + + perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ + +}; +#endif From 526698854cabd3af875259bf966a9004c65d71df Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:57:54 +0200 Subject: [PATCH 44/88] adapt vtol attitude control class to new vtol type classes --- .../vtol_att_control_main.cpp | 437 +++--------------- .../vtol_att_control/vtol_att_control_main.h | 212 +++++++++ .../vtol_att_control_params.c | 8 + 3 files changed, 293 insertions(+), 364 deletions(-) create mode 100644 src/modules/vtol_att_control/vtol_att_control_main.h diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index fdb4de4343..a565c618e3 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -43,166 +43,7 @@ * @author Thomas Gubler * */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "drivers/drv_pwm_output.h" -#include - -#include - - -extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); - -class VtolAttitudeControl -{ -public: - - VtolAttitudeControl(); - ~VtolAttitudeControl(); - - int start(); /* start the task and return OK on success */ - - -private: -//******************flags & handlers****************************************************** - bool _task_should_exit; - int _control_task; //task handle for VTOL attitude controller - - /* handlers for subscriptions */ - int _v_att_sub; //vehicle attitude subscription - int _v_att_sp_sub; //vehicle attitude setpoint subscription - int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription - int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription - int _v_control_mode_sub; //vehicle control mode subscription - int _params_sub; //parameter updates subscription - int _manual_control_sp_sub; //manual control setpoint subscription - int _armed_sub; //arming status subscription - int _local_pos_sub; // sensor subscription - int _airspeed_sub; // airspeed subscription - int _battery_status_sub; // battery status subscription - - int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs - int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs - - //handlers for publishers - orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) - orb_advert_t _actuators_1_pub; - orb_advert_t _vtol_vehicle_status_pub; - orb_advert_t _v_rates_sp_pub; -//*******************data containers*********************************************************** - struct vehicle_attitude_s _v_att; //vehicle attitude - struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint - struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint - struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint - struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint - struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint - struct vehicle_control_mode_s _v_control_mode; //vehicle control mode - struct vtol_vehicle_status_s _vtol_vehicle_status; - struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer - struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) - struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control - struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control - struct actuator_armed_s _armed; //actuator arming status - struct vehicle_local_position_s _local_pos; - struct airspeed_s _airspeed; // airspeed - struct battery_status_s _batt_status; // battery status - - struct { - param_t idle_pwm_mc; //pwm value for idle in mc mode - param_t vtol_motor_count; - param_t vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode - float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash) - float mc_airspeed_trim; // trim airspeed in multicopter mode - float mc_airspeed_max; // max airpseed in multicopter mode - float fw_pitch_trim; // trim for neutral elevon position in fw mode - float power_max; // maximum power of one engine - float prop_eff; // factor to calculate prop efficiency - float arsp_lp_gain; // total airspeed estimate low pass gain - } _params; - - struct { - param_t idle_pwm_mc; - param_t vtol_motor_count; - param_t vtol_fw_permanent_stab; - param_t mc_airspeed_min; - param_t mc_airspeed_trim; - param_t mc_airspeed_max; - param_t fw_pitch_trim; - param_t power_max; - param_t prop_eff; - param_t arsp_lp_gain; - } _params_handles; - - perf_counter_t _loop_perf; /**< loop performance counter */ - perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ - - /* for multicopters it is usual to have a non-zero idle speed of the engines - * for fixed wings we want to have an idle speed of zero since we do not want - * to waste energy when gliding. */ - bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode" - unsigned _motor_count; // number of motors - float _airspeed_tot; - float _tilt_control; -//*****************Member functions*********************************************************************** - - void task_main(); //main task - static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. - - void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. - void vehicle_manual_poll(); //Check for changes in manual inputs. - void arming_status_poll(); //Check for arming status updates. - void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output - void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output - void vehicle_rates_sp_mc_poll(); - void vehicle_rates_sp_fw_poll(); - void vehicle_local_pos_poll(); // Check for changes in sensor values - void vehicle_airspeed_poll(); // Check for changes in airspeed - void vehicle_battery_poll(); // Check for battery updates - void parameters_update_poll(); //Check if parameters have changed - int parameters_update(); //Update local paraemter cache - void fill_mc_att_control_output(); //write mc_att_control results to actuator message - void fill_fw_att_control_output(); //write fw_att_control results to actuator message - void fill_mc_att_rates_sp(); - void fill_fw_att_rates_sp(); - void set_idle_fw(); - void set_idle_mc(); - void scale_mc_output(); - void calc_tot_airspeed(); // estimated airspeed seen by elevons -}; +#include "vtol_att_control_main.h" namespace VTOL_att_control { @@ -230,19 +71,12 @@ VtolAttitudeControl::VtolAttitudeControl() : _battery_status_sub(-1), //init publication handlers - _actuators_0_pub(-1), - _actuators_1_pub(-1), - _vtol_vehicle_status_pub(-1), - _v_rates_sp_pub(-1), + _actuators_0_pub(0), + _actuators_1_pub(0), + _vtol_vehicle_status_pub(0), + _v_rates_sp_pub(0) - _loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input")) { - - flag_idle_mc = true; - _airspeed_tot = 0.0f; - _tilt_control = 0.0f; - memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status)); _vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/ memset(&_v_att, 0, sizeof(_v_att)); @@ -276,9 +110,20 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.power_max = param_find("VT_POWER_MAX"); _params_handles.prop_eff = param_find("VT_PROP_EFF"); _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); + _params_handles.vtol_type = param_find("VT_TYPE"); /* fetch initial parameter values */ parameters_update(); + + if (_params.vtol_type == 0) { + _tailsitter = new Tailsitter(this); + _vtol_type = _tailsitter; + } else if (_params.vtol_type == 1) { + _tiltrotor = new Tiltrotor(this); + _vtol_type = _tiltrotor; + } else { + _task_should_exit = true; + } } /** @@ -470,6 +315,7 @@ int VtolAttitudeControl::parameters_update() { float v; + int l; /* idle pwm for mc mode */ param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc); @@ -507,42 +353,12 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.arsp_lp_gain, &v); _params.arsp_lp_gain = v; + param_get(_params_handles.vtol_type, &l); + _params.vtol_type = l; + return OK; } -/** -* Prepare message to acutators with data from mc attitude controller. -*/ -void VtolAttitudeControl::fill_mc_att_control_output() -{ - _actuators_out_0.control[0] = _actuators_mc_in.control[0]; - _actuators_out_0.control[1] = _actuators_mc_in.control[1]; - _actuators_out_0.control[2] = _actuators_mc_in.control[2]; - _actuators_out_0.control[3] = _actuators_mc_in.control[3]; - //set neutral position for elevons - _actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon - _actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon - _actuators_out_1.control[4] = _tilt_control; // for tilt-rotor control -} - -/** -* Prepare message to acutators with data from fw attitude controller. -*/ -void VtolAttitudeControl::fill_fw_att_control_output() -{ - /*For the first test in fw mode, only use engines for thrust!!!*/ - _actuators_out_0.control[0] = 0; - _actuators_out_0.control[1] = 0; - _actuators_out_0.control[2] = 0; - _actuators_out_0.control[3] = _actuators_fw_in.control[3]; - /*controls for the elevons */ - _actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon - _actuators_out_1.control[1] = _actuators_fw_in.control[1] + _params.fw_pitch_trim; // pitch elevon - // unused now but still logged - _actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw - _actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle -} - /** * Prepare message for mc attitude rates setpoint topic */ @@ -565,109 +381,6 @@ void VtolAttitudeControl::fill_fw_att_rates_sp() _v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust; } -/** -* Adjust idle speed for fw mode. -*/ -void VtolAttitudeControl::set_idle_fw() -{ - int ret; - char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - unsigned pwm_value = PWM_LOWEST_MIN; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < _params.vtol_motor_count; i++) { - - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -/** -* Adjust idle speed for mc mode. -*/ -void VtolAttitudeControl::set_idle_mc() -{ - int ret; - unsigned servo_count; - char *dev = PWM_OUTPUT0_DEVICE_PATH; - int fd = open(dev, 0); - - if (fd < 0) {err(1, "can't open %s", dev);} - - ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count); - unsigned pwm_value = _params.idle_pwm_mc; - struct pwm_output_values pwm_values; - memset(&pwm_values, 0, sizeof(pwm_values)); - - for (unsigned i = 0; i < _params.vtol_motor_count; i++) { - pwm_values.values[i] = pwm_value; - pwm_values.channel_count++; - } - - ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values); - - if (ret != OK) {errx(ret, "failed setting min values");} - - close(fd); -} - -void -VtolAttitudeControl::scale_mc_output() { - // scale around tuning airspeed - float airspeed; - calc_tot_airspeed(); // estimate air velocity seen by elevons - // if airspeed is not updating, we assume the normal average speed - if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) || - hrt_elapsed_time(&_airspeed.timestamp) > 1e6) { - airspeed = _params.mc_airspeed_trim; - if (nonfinite) { - perf_count(_nonfinite_input_perf); - } - } else { - airspeed = _airspeed_tot; - airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max); - } - - _vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging - /* - * For scaling our actuators using anything less than the min (close to stall) - * speed doesn't make any sense - its the strongest reasonable deflection we - * want to do in flight and its the baseline a human pilot would choose. - * - * Forcing the scaling to this value allows reasonable handheld tests. - */ - float airspeed_scaling = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed); - _actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f); -} - -void VtolAttitudeControl::calc_tot_airspeed() { - float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama - // calculate momentary power of one engine - float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count; - P = math::constrain(P,1.0f,_params.power_max); - // calculate prop efficiency - float power_factor = 1.0f - P*_params.prop_eff/_params.power_max; - float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f; - eta = math::constrain(eta,0.001f,1.0f); // live on the safe side - // calculate induced airspeed by propeller - float v_ind = (airspeed/eta - airspeed)*2.0f; - // calculate total airspeed - float airspeed_raw = airspeed + v_ind; - // apply low-pass filter - _airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw; -} - void VtolAttitudeControl::task_main_trampoline(int argc, char *argv[]) { @@ -701,8 +414,7 @@ void VtolAttitudeControl::task_main() _vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false; // make sure we start with idle in mc mode - set_idle_mc(); - flag_idle_mc = true; + _vtol_type->set_idle_mc(); /* wakeup source*/ struct pollfd fds[3]; /*input_mc, input_fw, parameters*/ @@ -764,83 +476,80 @@ void VtolAttitudeControl::task_main() vehicle_airspeed_poll(); vehicle_battery_poll(); + // update the vtol state machine which decides which mode we are in + _vtol_type->update_vtol_state(); - if (_manual_control_sp.aux1 < 0.0f) { /* vehicle is in mc mode */ + // check in which mode we are in and call mode specific functions + if (_vtol_type->get_mode() == ROTARY_WING) { + // vehicle is in rotary wing mode _vtol_vehicle_status.vtol_in_rw_mode = true; - if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */ - set_idle_mc(); - flag_idle_mc = true; - } + _vtol_type->update_mc_state(); - /* got data from mc_att_controller */ + // got data from mc attitude controller if (fds[0].revents & POLLIN) { - vehicle_manual_poll(); /* update remote input */ orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); - // scale pitch control with total airspeed - scale_mc_output(); + _vtol_type->process_mc_data(); - fill_mc_att_control_output(); fill_mc_att_rates_sp(); - - /* Only publish if the proper mode(s) are enabled */ - if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled) - { - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); - - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); - - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } - } } - } - - if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */ + } else if (_vtol_type->get_mode() == FIXED_WING) { + // vehicle is in fw mode _vtol_vehicle_status.vtol_in_rw_mode = false; - if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */ - set_idle_fw(); - flag_idle_mc = false; + _vtol_type->update_fw_state(); + + // got data from fw attitude controller + if (fds[1].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); + vehicle_manual_poll(); + + _vtol_type->process_fw_data(); + + fill_fw_att_rates_sp(); + } + } else if (_vtol_type->get_mode() == TRANSITION) { + // vehicle is doing a transition + bool got_new_data = false; + if (fds[0].revents & POLLIN) { + orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in); + got_new_data = true; } - if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */ + if (fds[1].revents & POLLIN) { orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in); - vehicle_manual_poll(); //update remote input + got_new_data = true; + } - fill_fw_att_control_output(); - fill_fw_att_rates_sp(); + // update transition state if got any new data + if (got_new_data) { + _vtol_type->update_transition_state(); + } - /* Only publish if the proper mode(s) are enabled */ - if(_v_control_mode.flag_control_attitude_enabled || - _v_control_mode.flag_control_rates_enabled || - _v_control_mode.flag_control_manual_enabled) - { - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + } else if (_vtol_type->get_mode() == EXTERNAL) { + // we are using external module to generate attitude/thrust setpoint + _vtol_type->update_external_state(); + } - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); - } - if (_actuators_1_pub > 0) { - orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + /* Only publish if the proper mode(s) are enabled */ + if(_v_control_mode.flag_control_attitude_enabled || + _v_control_mode.flag_control_rates_enabled || + _v_control_mode.flag_control_manual_enabled) + { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0); + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0); + } - } else { - _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); - } + if (_actuators_1_pub > 0) { + orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1); + } else { + _actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1); } } - } // publish the attitude rates setpoint if(_v_rates_sp_pub > 0) { diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h new file mode 100644 index 0000000000..2772f9bcb1 --- /dev/null +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -0,0 +1,212 @@ +/**************************************************************************** + * + * 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 VTOL_att_control_main.cpp + * Implementation of an attitude controller for VTOL airframes. This module receives data + * from both the fixed wing- and the multicopter attitude controllers and processes it. + * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward- + * flight or transition). It also publishes the resulting controls on the actuator controls topics. + * + * @author Roman Bapst + * @author Lorenz Meier + * @author Thomas Gubler + * + */ +#ifndef VTOL_ATT_CONTROL_MAIN_H +#define VTOL_ATT_CONTROL_MAIN_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "tiltrotor.h" +#include "tailsitter.h" + + +extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]); + + +class VtolAttitudeControl +{ +public: + + VtolAttitudeControl(); + ~VtolAttitudeControl(); + + int start(); /* start the task and return OK on success */ + + struct vehicle_attitude_s* get_att () {return &_v_att;} + struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;} + struct vehicle_rates_setpoint_s* get_rates_sp () {return &_v_rates_sp;} + struct vehicle_rates_setpoint_s* get_mc_virtual_rates_sp () {return &_mc_virtual_v_rates_sp;} + struct vehicle_rates_setpoint_s* get_fw_virtual_rates_sp () {return &_fw_virtual_v_rates_sp;} + struct manual_control_setpoint_s* get_manual_control_sp () {return &_manual_control_sp;} + struct vehicle_control_mode_s* get_control_mode () {return &_v_control_mode;} + struct vtol_vehicle_status_s* get_vehicle_status () {return &_vtol_vehicle_status;} + struct actuator_controls_s* get_actuators_out0 () {return &_actuators_out_0;} + struct actuator_controls_s* get_actuators_out1 () {return &_actuators_out_1;} + struct actuator_controls_s* get_actuators_mc_in () {return &_actuators_mc_in;} + struct actuator_controls_s* get_actuators_fw_in () {return &_actuators_fw_in;} + struct actuator_armed_s* get_armed () {return &_armed;} + struct vehicle_local_position_s* get_local_pos () {return &_local_pos;} + struct airspeed_s* get_airspeed () {return &_airspeed;} + struct battery_status_s* get_batt_status () {return &_batt_status;} + + struct Params* get_params () {return &_params;} + + +private: +//******************flags & handlers****************************************************** + bool _task_should_exit; + int _control_task; //task handle for VTOL attitude controller + + /* handlers for subscriptions */ + int _v_att_sub; //vehicle attitude subscription + int _v_att_sp_sub; //vehicle attitude setpoint subscription + int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription + int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription + int _v_control_mode_sub; //vehicle control mode subscription + int _params_sub; //parameter updates subscription + int _manual_control_sp_sub; //manual control setpoint subscription + int _armed_sub; //arming status subscription + int _local_pos_sub; // sensor subscription + int _airspeed_sub; // airspeed subscription + int _battery_status_sub; // battery status subscription + + int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs + int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs + + //handlers for publishers + orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust) + orb_advert_t _actuators_1_pub; + orb_advert_t _vtol_vehicle_status_pub; + orb_advert_t _v_rates_sp_pub; +//*******************data containers*********************************************************** + struct vehicle_attitude_s _v_att; //vehicle attitude + struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint + struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint + struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint + struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint + struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint + struct vehicle_control_mode_s _v_control_mode; //vehicle control mode + struct vtol_vehicle_status_s _vtol_vehicle_status; + struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer + struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons) + struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control + struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control + struct actuator_armed_s _armed; //actuator arming status + struct vehicle_local_position_s _local_pos; + struct airspeed_s _airspeed; // airspeed + struct battery_status_s _batt_status; // battery status + + Params _params; // struct holding the parameters + + struct { + param_t idle_pwm_mc; + param_t vtol_motor_count; + param_t vtol_fw_permanent_stab; + param_t mc_airspeed_min; + param_t mc_airspeed_trim; + param_t mc_airspeed_max; + param_t fw_pitch_trim; + param_t power_max; + param_t prop_eff; + param_t arsp_lp_gain; + param_t vtol_type; + } _params_handles; + + /* for multicopters it is usual to have a non-zero idle speed of the engines + * for fixed wings we want to have an idle speed of zero since we do not want + * to waste energy when gliding. */ + unsigned _motor_count; // number of motors + float _airspeed_tot; + + VtolType * _vtol_type; // base class for different vtol types + Tiltrotor * _tiltrotor; // tailsitter vtol type + Tailsitter * _tailsitter; // tiltrotor vtol type + +//*****************Member functions*********************************************************************** + + void task_main(); //main task + static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create. + + void vehicle_control_mode_poll(); //Check for changes in vehicle control mode. + void vehicle_manual_poll(); //Check for changes in manual inputs. + void arming_status_poll(); //Check for arming status updates. + void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output + void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output + void vehicle_rates_sp_mc_poll(); + void vehicle_rates_sp_fw_poll(); + void vehicle_local_pos_poll(); // Check for changes in sensor values + void vehicle_airspeed_poll(); // Check for changes in airspeed + void vehicle_battery_poll(); // Check for battery updates + void parameters_update_poll(); //Check if parameters have changed + int parameters_update(); //Update local paraemter cache + void fill_mc_att_rates_sp(); + void fill_fw_att_rates_sp(); +}; + +#endif diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 6da28b1304..429d44c46c 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -142,3 +142,11 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f); */ PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); +/** + * VTOL Type (Tailsitter=0, Tiltrotor=1) + * + * @min 0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_TYPE, 0); From 12feef85bfd9ae92eaa50c1efcc2d27d2c7bff72 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 18 Jun 2015 23:58:47 +0200 Subject: [PATCH 45/88] lower lowest allowed max pwm value to be able to cut rear motors for firefly6 in fw mode --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 6271ad2086..2fb9469c8a 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -93,7 +93,7 @@ __BEGIN_DECLS /** * Lowest PWM allowed as the maximum PWM */ -#define PWM_LOWEST_MAX 1400 +#define PWM_LOWEST_MAX 950 /** * Do not output a channel with this value From b3c3d6634c31322df1e17666dc97e44bcbe47302 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 00:00:23 +0200 Subject: [PATCH 46/88] added vtol types --- src/modules/vtol_att_control/module.mk | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index 0cf3072c8f..ad6efd2b27 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -38,7 +38,10 @@ MODULE_COMMAND = vtol_att_control SRCS = vtol_att_control_main.cpp \ - vtol_att_control_params.c + vtol_att_control_params.c \ + tiltrotor_params.c \ + tiltrotor.cpp \ + vtol_type.cpp \ + tailsitter.cpp EXTRACXXFLAGS = -Wno-write-strings - From d320dc8ada0ca85722a1b57d5550fd5333d53980 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:45:41 +0200 Subject: [PATCH 47/88] added VTOL type param to VTOL configuration files --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 3 ++- ROMFS/px4fmu_common/init.d/13002_firefly6 | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index 5c00041492..912202f962 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -1,7 +1,7 @@ # # Generic configuration file for caipirinha VTOL version # -# Roman Bapst +# Roman Bapst # sh /etc/init.d/rc.vtol_defaults @@ -13,3 +13,4 @@ set PWM_MAX 2000 set PWM_RATE 400 param set VT_MOT_COUNT 2 param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 0 diff --git a/ROMFS/px4fmu_common/init.d/13002_firefly6 b/ROMFS/px4fmu_common/init.d/13002_firefly6 index ed90dabf41..963341d138 100644 --- a/ROMFS/px4fmu_common/init.d/13002_firefly6 +++ b/ROMFS/px4fmu_common/init.d/13002_firefly6 @@ -2,7 +2,7 @@ # # Generic configuration file for BirdsEyeView Aerobotics FireFly6 # -# Roman Bapst +# Roman Bapst # sh /etc/init.d/rc.vtol_defaults @@ -41,3 +41,4 @@ set MAV_TYPE 21 param set VT_MOT_COUNT 6 param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 1 From 4e9fd5b2a4be4ef4054781f1ed5e6f6896e2fca8 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:46:20 +0200 Subject: [PATCH 48/88] rotate attitude for fw mode only if VTOL is a tailsitter --- .../fw_att_control/fw_att_control_main.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 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 fe27de14f5..ccf12a0791 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -193,12 +193,14 @@ private: float trim_roll; float trim_pitch; float trim_yaw; - float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ - float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ - float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ - float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ - float man_roll_max; /**< Max Roll in rad */ - float man_pitch_max; /**< Max Pitch in rad */ + float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */ + float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */ + float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */ + float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */ + float man_roll_max; /**< Max Roll in rad */ + float man_pitch_max; /**< Max Pitch in rad */ + + int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */ } _parameters; /**< local copies of interesting parameters */ @@ -241,6 +243,8 @@ private: param_t man_roll_max; param_t man_pitch_max; + param_t vtol_type; + } _parameter_handles; /**< handles for interesting parameters */ @@ -404,6 +408,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX"); _parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX"); + _parameter_handles.vtol_type = param_find("VT_TYPE"); + /* fetch initial parameter values */ parameters_update(); } @@ -481,6 +487,8 @@ FixedwingAttitudeControl::parameters_update() _parameters.man_roll_max = math::radians(_parameters.man_roll_max); _parameters.man_pitch_max = math::radians(_parameters.man_pitch_max); + param_get(_parameter_handles.vtol_type, &_parameters.vtol_type); + /* pitch control parameters */ _pitch_ctrl.set_time_constant(_parameters.tconst); _pitch_ctrl.set_k_p(_parameters.p_p); @@ -703,10 +711,8 @@ FixedwingAttitudeControl::task_main() /* load local copies */ orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - if (_vehicle_status.is_vtol) { - /* vehicle type is VTOL, need to modify attitude! - * The following modification to the attitude is vehicle specific and in this case applies - * to tail-sitter models !!! + if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) { + /* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode * * Since the VTOL airframe is initialized as a multicopter we need to * modify the estimated attitude for the fixed wing operation. From 51b1968bddc4d976b047f5edfe165dbf4259de4f Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:51:02 +0200 Subject: [PATCH 49/88] added configuration file for tailsitter with motors in quad x configuration --- .../px4fmu_common/init.d/13003_quad_tailsitter | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/13003_quad_tailsitter diff --git a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter new file mode 100644 index 0000000000..daad3d9351 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter @@ -0,0 +1,17 @@ +# +# Generic configuration file for a tailsitter with motors in X configuration. +# +# Roman Bapst +# + +sh /etc/init.d/rc.vtol_defaults + +set MIXER quad_x_vtol + +set PWM_OUT 1234 +set PWM_MAX 2000 +set PWM_RATE 400 +set MAV_TYPE 20 +param set VT_MOT_COUNT 4 +param set VT_IDLE_PWM_MC 1080 +param set VT_TYPE 0 From fb70b0a2b59bfbf517ddb6bd0a5f989bfed16666 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 08:51:48 +0200 Subject: [PATCH 50/88] added mixer file for tailsitter with motors in quad x configuration and 2 elevons --- .../px4fmu_common/mixers/quad_x_vtol.main.mix | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix diff --git a/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix new file mode 100644 index 0000000000..4fd323353a --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix @@ -0,0 +1,18 @@ +Mixer for Tailsitter with x motor configuration and elevons +=========================================================== + +This file defines a single mixer for tailsitter with motors in X configuration. All controls +are mixed 100%. + +R: 4x 10000 10000 10000 0 + +#mixer for the elevons +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 1 10000 10000 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 1 0 10000 10000 0 -10000 10000 +S: 1 1 -10000 -10000 0 -10000 10000 From aade901ef02f604c468359d8c9e937c717035f9c Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 09:02:55 +0200 Subject: [PATCH 51/88] added new tailsitter type to autostart list --- ROMFS/px4fmu_common/init.d/rc.autostart | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index de81795b44..a45ceeb276 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -268,6 +268,14 @@ then sh /etc/init.d/13002_firefly6 fi +# +# Tailsitter with 4 motors in x config +# +if param compare SYS_AUTOSTART 13003 +then + sh /etc/init.d/13003_quad_tailsitter +fi + # # TriCopter Y Yaw+ # From 60857c7940d13af7d0168e7950a9799d30c3bb68 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 09:46:40 +0200 Subject: [PATCH 52/88] add option to lock elevons for tailsitters in mc mode --- src/modules/vtol_att_control/tailsitter.cpp | 11 ++++++++--- src/modules/vtol_att_control/tiltrotor_params.c | 11 ----------- .../vtol_att_control/vtol_att_control_main.cpp | 5 +++++ src/modules/vtol_att_control/vtol_att_control_main.h | 1 + .../vtol_att_control/vtol_att_control_params.c | 11 +++++++++++ src/modules/vtol_att_control/vtol_type.h | 1 + 6 files changed, 26 insertions(+), 14 deletions(-) diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 4479783b92..358f636561 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -178,7 +178,12 @@ void Tailsitter::fill_mc_att_control_output() _actuators_out_0->control[1] = _actuators_mc_in->control[1]; _actuators_out_0->control[2] = _actuators_mc_in->control[2]; _actuators_out_0->control[3] = _actuators_mc_in->control[3]; - //set neutral position for elevons - _actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon - _actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon + + if (_params->elevons_mc_lock == 1) { + _actuators_out_1->control[0] = 0; + _actuators_out_1->control[1] = 0; + } else { + _actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon + _actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon + } } diff --git a/src/modules/vtol_att_control/tiltrotor_params.c b/src/modules/vtol_att_control/tiltrotor_params.c index 76f3ee6c3c..7d233f6f50 100644 --- a/src/modules/vtol_att_control/tiltrotor_params.c +++ b/src/modules/vtol_att_control/tiltrotor_params.c @@ -105,14 +105,3 @@ PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f); - -/** - * Lock elevons in multicopter mode - * - * If set to 1 the elevons are locked in multicopter mode - * - * @min 0 - * @max 1 - * @group VTOL Attitude Control - */ -PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index a565c618e3..b70cd19dd8 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -111,6 +111,7 @@ VtolAttitudeControl::VtolAttitudeControl() : _params_handles.prop_eff = param_find("VT_PROP_EFF"); _params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN"); _params_handles.vtol_type = param_find("VT_TYPE"); + _params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK"); /* fetch initial parameter values */ parameters_update(); @@ -356,6 +357,10 @@ VtolAttitudeControl::parameters_update() param_get(_params_handles.vtol_type, &l); _params.vtol_type = l; + /* vtol lock elevons in multicopter */ + param_get(_params_handles.elevons_mc_lock, &l); + _params.elevons_mc_lock = l; + return OK; } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 2772f9bcb1..43e8969929 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -176,6 +176,7 @@ private: param_t prop_eff; param_t arsp_lp_gain; param_t vtol_type; + param_t elevons_mc_lock; } _params_handles; /* for multicopters it is usual to have a non-zero idle speed of the engines diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index 429d44c46c..f302314a23 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -150,3 +150,14 @@ PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f); * @group VTOL Attitude Control */ PARAM_DEFINE_INT32(VT_TYPE, 0); + +/** + * Lock elevons in multicopter mode + * + * If set to 1 the elevons are locked in multicopter mode + * + * @min 0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0); diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 57448a7587..bbe6a8642e 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -53,6 +53,7 @@ struct Params { float prop_eff; // factor to calculate prop efficiency float arsp_lp_gain; // total airspeed estimate low pass gain int vtol_type; + int elevons_mc_lock; // lock elevons in multicopter mode }; enum mode { From fc7dc297fc168465456b868cfd5cef26174f76d6 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 09:48:07 +0200 Subject: [PATCH 53/88] lock elevons in mc mode for tailsitter with motors in x config --- ROMFS/px4fmu_common/init.d/13003_quad_tailsitter | 1 + 1 file changed, 1 insertion(+) diff --git a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter index daad3d9351..17560526a3 100644 --- a/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter +++ b/ROMFS/px4fmu_common/init.d/13003_quad_tailsitter @@ -15,3 +15,4 @@ set MAV_TYPE 20 param set VT_MOT_COUNT 4 param set VT_IDLE_PWM_MC 1080 param set VT_TYPE 0 +param set VT_ELEV_MC_LOCK 1 From 66cdfeca6235f6ee7254e0d194e443ed257150d9 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 21 Jun 2015 00:31:23 +0300 Subject: [PATCH 54/88] ROMFS: Enable FTP on companion link Not sure that ftp usable in 57600. Tested on 921600. --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 8a93737ef8..0fb6e117cc 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -479,11 +479,11 @@ then # but this works for now if param compare SYS_COMPANION 921600 then - mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 + mavlink start -d /dev/ttyS2 -b 921600 -m onboard -r 20000 -x fi if param compare SYS_COMPANION 57600 then - mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 + mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000 -x fi if param compare SYS_COMPANION 157600 then From 8fa161b7c4947ccc988a737d2fd86610bca8cab2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 18:58:06 +0200 Subject: [PATCH 55/88] Multicopter configs: Remove duplicate defaults, each line checked to match new param-level defaults --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 32 ----------------------- 1 file changed, 32 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 1f34282aec..fa3653e0d5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -4,38 +4,6 @@ set VEHICLE_TYPE mc if [ $AUTOCNF == yes ] then - param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 - param set MC_ROLLRATE_I 0.0 - param set MC_ROLLRATE_D 0.003 - param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 - param set MC_PITCHRATE_I 0.0 - param set MC_PITCHRATE_D 0.003 - param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.1 - param set MC_YAWRATE_D 0.0 - param set MC_YAW_FF 0.5 - - param set MPC_THR_MAX 1.0 - param set MPC_THR_MIN 0.1 - param set MPC_XY_P 1.0 - param set MPC_XY_VEL_P 0.1 - param set MPC_XY_VEL_I 0.02 - param set MPC_XY_VEL_D 0.01 - param set MPC_XY_VEL_MAX 5 - param set MPC_XY_FF 0.5 - param set MPC_Z_P 1.0 - param set MPC_Z_VEL_P 0.1 - param set MPC_Z_VEL_I 0.02 - param set MPC_Z_VEL_D 0.0 - param set MPC_Z_VEL_MAX 3 - param set MPC_Z_FF 0.5 - param set MPC_TILTMAX_AIR 45.0 - param set MPC_TILTMAX_LND 15.0 - param set MPC_LAND_SPEED 1.0 - param set PE_VELNE_NOISE 0.5 param set PE_VELD_NOISE 0.7 param set PE_POSNE_NOISE 0.5 From 9365c5a4383d8702e66b5f0e3f95a1acb3fd5cac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 18:59:28 +0200 Subject: [PATCH 56/88] systemlib: Remove file present 2x from Makefile --- ROMFS/px4fmu_common/init.d/rc.usb | 6 +++--- src/modules/systemlib/module.mk | 3 +-- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.usb b/ROMFS/px4fmu_common/init.d/rc.usb index 0442637941..027d2aca5d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.usb +++ b/ROMFS/px4fmu_common/init.d/rc.usb @@ -3,21 +3,21 @@ # USB MAVLink start # -mavlink start -r 80000 -d /dev/ttyACM0 -x +mavlink start -r 800000 -d /dev/ttyACM0 -x # Enable a number of interesting streams we want via USB mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300 mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50 mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10 mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10 mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20 -mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20 +mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100 mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30 mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5 mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20 mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10 mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30 mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5 -mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 20 +mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100 mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20 # Exit shell to make it available to MAVLink diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index f80d8009ae..f2499bbb13 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,8 +55,7 @@ SRCS = err.c \ pwm_limit/pwm_limit.c \ circuit_breaker.cpp \ circuit_breaker_params.c \ - mcu_version.c \ - circuit_breaker_params.c + mcu_version.c MAXOPTIMIZATION = -Os From 62b102d0b49f5cf47f5623761aed4db2a390ceaf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 19:00:06 +0200 Subject: [PATCH 57/88] MC attitude controller: Set better defaults --- .../mc_att_control/mc_att_control_params.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index c9bf3753b4..c0f110123a 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -50,7 +50,7 @@ * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); /** * Roll rate P gain @@ -70,7 +70,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.05f); /** * Roll rate D gain @@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f); /** * Roll rate feedforward @@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); +PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); /** * Pitch rate P gain @@ -121,7 +121,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.05f); /** * Pitch rate D gain @@ -131,7 +131,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f); /** * Pitch rate feedforward @@ -152,7 +152,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); +PARAM_DEFINE_FLOAT(MC_YAW_P, 2.8f); /** * Yaw rate P gain @@ -162,7 +162,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f); /** * Yaw rate I gain @@ -172,7 +172,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f); /** * Yaw rate D gain From 2c2a6b710c13f835d408a66104a47e73fb3685ac Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 19:00:23 +0200 Subject: [PATCH 58/88] MC position controller: Set better defaults --- src/modules/mc_pos_control/mc_pos_control_params.c | 3 ++- 1 file changed, 2 insertions(+), 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 4865c1c684..a09ed4a3e6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -107,9 +107,10 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); * * @unit m/s * @min 0.0 + * @max 8 m/s * @group Multicopter Position Control */ -PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); +PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f); /** * Vertical velocity feed forward From 1c82f73822c1523be1bcaefd3b7986ddb8c59bdb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 22:15:45 +0200 Subject: [PATCH 59/88] Dataman: Reduce excessive stack allocation --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index b442b74303..6cfbb4d830 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -794,7 +794,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1500, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From d673bf8457cdfe7fede5097e3b932645d07f1c62 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 22:16:03 +0200 Subject: [PATCH 60/88] Navigator: Reduce excessive stack allocation --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 1460972cc2..dc97600434 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -521,7 +521,7 @@ Navigator::start() _navigator_task = task_spawn_cmd("navigator", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT + 20, - 1700, + 1500, (main_t)&Navigator::task_main_trampoline, nullptr); From ff3977366607d1c54beb605029764b88261207b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:11:22 +0200 Subject: [PATCH 61/88] MC: Better attitude control defaults --- src/modules/mc_att_control/mc_att_control_params.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index c0f110123a..42c7bc3d04 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.12f); /** * Roll rate I gain @@ -111,7 +111,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); +PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.12f); /** * Pitch rate I gain From ae9f1ec955906038652c459e6343b0d30f816722 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:11:34 +0200 Subject: [PATCH 62/88] CAN config: Better attitude control defaults --- ROMFS/px4fmu_common/init.d/4012_quad_x_can | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4012_quad_x_can b/ROMFS/px4fmu_common/init.d/4012_quad_x_can index 05b4355138..c6341a4f74 100644 --- a/ROMFS/px4fmu_common/init.d/4012_quad_x_can +++ b/ROMFS/px4fmu_common/init.d/4012_quad_x_can @@ -9,18 +9,17 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then - # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.16 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.16 param set MC_PITCHRATE_I 0.05 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.3 + param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi From 4c975a11e5aa0402d3179e772d6580b71c4ae2de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:33:07 +0200 Subject: [PATCH 63/88] param command: Complete help text --- src/systemcmds/param/param.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 45fb2fd830..210d93bde8 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -189,7 +189,7 @@ param_main(int argc, char *argv[]) } } - errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'"); + errx(1, "expected a command, try 'load', 'import', 'show', 'set', 'compare',\n'index', 'index_used', 'select' or 'save'"); } static void From c192398a6517648d3a46729c2ddf7268beed89a1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:36:19 +0200 Subject: [PATCH 64/88] mavlink app: Be more verbose on param load fails --- src/modules/mavlink/mavlink_parameters.cpp | 23 +++++++++++++++++----- src/modules/mavlink/mavlink_parameters.h | 5 +++-- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/modules/mavlink/mavlink_parameters.cpp b/src/modules/mavlink/mavlink_parameters.cpp index 524effb205..73d7580d21 100644 --- a/src/modules/mavlink/mavlink_parameters.cpp +++ b/src/modules/mavlink/mavlink_parameters.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,7 @@ * Mavlink parameters manager implementation. * * @author Anton Babushkin + * @author Lorenz Meier */ #include @@ -130,7 +131,17 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg) } else { /* when index is >= 0, send this parameter again */ - send_param(param_for_used_index(req_read.param_index)); + int ret = send_param(param_for_used_index(req_read.param_index)); + + if (ret == 1) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } else if (ret == 2) { + char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; + sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index); + _mavlink->send_statustext_info(buf); + } } } break; @@ -207,11 +218,11 @@ MavlinkParametersManager::send(const hrt_abstime t) } } -void +int MavlinkParametersManager::send_param(param_t param) { if (param == PARAM_INVALID) { - return; + return 1; } mavlink_param_value_t msg; @@ -221,7 +232,7 @@ MavlinkParametersManager::send_param(param_t param) * space during transmission, copy param onto float val_buf */ if (param_get(param, &msg.param_value) != OK) { - return; + return 2; } msg.param_count = param_count_used(); @@ -248,4 +259,6 @@ MavlinkParametersManager::send_param(param_t param) } _mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg); + + return 0; } diff --git a/src/modules/mavlink/mavlink_parameters.h b/src/modules/mavlink/mavlink_parameters.h index b6736f2128..3dfed084b3 100644 --- a/src/modules/mavlink/mavlink_parameters.h +++ b/src/modules/mavlink/mavlink_parameters.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +36,7 @@ * Mavlink parameters manager definition. * * @author Anton Babushkin + * @author Lorenz Meier */ #pragma once @@ -113,7 +114,7 @@ protected: void send(const hrt_abstime t); - void send_param(param_t param); + int send_param(param_t param); orb_advert_t _rc_param_map_pub; struct rc_parameter_map_s _rc_param_map; From f1582e67de83a4c5b30e22f5322b5686613c0580 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:36:42 +0200 Subject: [PATCH 65/88] F330 config: Better default gains --- ROMFS/px4fmu_common/init.d/4010_dji_f330 | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4010_dji_f330 b/ROMFS/px4fmu_common/init.d/4010_dji_f330 index d07e926a79..512ad132be 100644 --- a/ROMFS/px4fmu_common/init.d/4010_dji_f330 +++ b/ROMFS/px4fmu_common/init.d/4010_dji_f330 @@ -10,11 +10,11 @@ sh /etc/init.d/4001_quad_x if [ $AUTOCNF == yes ] then param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.13 param set MC_ROLLRATE_I 0.05 param set MC_ROLLRATE_D 0.003 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.05 param set MC_PITCHRATE_D 0.003 param set MC_YAW_P 2.8 From 7b588c5bd0a0b3c0cba09a16a3657ba44022b3fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 23 Jun 2015 09:07:49 +0200 Subject: [PATCH 66/88] Fix F450 default gains --- ROMFS/px4fmu_common/init.d/4011_dji_f450 | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4011_dji_f450 b/ROMFS/px4fmu_common/init.d/4011_dji_f450 index 9b3954be6f..2a77f13866 100644 --- a/ROMFS/px4fmu_common/init.d/4011_dji_f450 +++ b/ROMFS/px4fmu_common/init.d/4011_dji_f450 @@ -11,15 +11,15 @@ if [ $AUTOCNF == yes ] then # TODO REVIEW param set MC_ROLL_P 7.0 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLLRATE_P 0.16 param set MC_ROLLRATE_I 0.05 - param set MC_ROLLRATE_D 0.003 + param set MC_ROLLRATE_D 0.01 param set MC_PITCH_P 7.0 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCHRATE_P 0.16 param set MC_PITCHRATE_I 0.05 - param set MC_PITCHRATE_D 0.003 + param set MC_PITCHRATE_D 0.01 param set MC_YAW_P 2.8 - param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_P 0.3 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 fi From 26c47f25cb9613f1ceb9fffe04eef3d0cc70c293 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 10:37:56 +0200 Subject: [PATCH 67/88] PWM outputs: Allow the new p:PWM_OUT etc params for setting PWM limits via params at boot-time. --- src/modules/sensors/sensor_params.c | 83 +++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 6 +++ src/systemcmds/pwm/pwm.c | 32 +++++++++-- 3 files changed, 118 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7b3932638b..83568c0069 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1401,3 +1401,86 @@ PARAM_DEFINE_INT32(RC_RSSI_PWM_MAX, 1000); * */ PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000); + +/** + * Enable Lidar-Lite (LL40LS) pwm driver + * + * @min 0 + * @max 1 + * @group Sensor Enable + */ +PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0); + +/** + * Set the minimum PWM for the MAIN outputs + * + * Set to 1000 for default or 900 to increase servo travel + * + * @min 800 + * @max 1400 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MIN, 1000); + +/** + * Set the maximum PWM for the MAIN outputs + * + * Set to 2000 for default or 2100 to increase servo travel + * + * @min 1600 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_MAX, 2000); + +/** + * Set the disarmed PWM for MAIN outputs + * + * This is the PWM pulse the autopilot is outputting if not armed. + * The main use of this parameter is to silence ESCs when they are disarmed. + * + * @min 0 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_DISARMED, 0); + +/** + * Set the minimum PWM for the MAIN outputs + * + * Set to 1000 for default or 900 to increase servo travel + * + * @min 800 + * @max 1400 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000); + +/** + * Set the maximum PWM for the MAIN outputs + * + * Set to 2000 for default or 2100 to increase servo travel + * + * @min 1600 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000); + +/** + * Set the disarmed PWM for AUX outputs + * + * This is the PWM pulse the autopilot is outputting if not armed. + * The main use of this parameter is to silence ESCs when they are disarmed. + * + * @min 0 + * @max 2200 + * @unit microseconds + * @group PWM Outputs + */ +PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1000); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 1e831becd9..0c7b0467f7 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -634,6 +634,12 @@ Sensors::Sensors() : (void)param_find("CAL_MAG2_ROT"); (void)param_find("SYS_PARAM_VER"); (void)param_find("SYS_AUTOSTART"); + (void)param_find("PWM_MIN"); + (void)param_find("PWM_MAX"); + (void)param_find("PWM_DISARMED"); + (void)param_find("PWM_AUX_MIN"); + (void)param_find("PWM_AUX_MAX"); + (void)param_find("PWM_AUX_DISARMED"); /* fetch initial parameter values */ parameters_update(); diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 6bb9f235cb..168a1d8603 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -59,6 +59,7 @@ #include "systemlib/systemlib.h" #include "systemlib/err.h" +#include "systemlib/param/param.h" #include "drivers/drv_pwm_output.h" static void usage(const char *reason); @@ -187,10 +188,35 @@ pwm_main(int argc, char *argv[]) break; case 'p': - pwm_value = strtoul(optarg, &ep, 0); + { + /* check if this is a param name */ + if (strncmp("p:", optarg, 2) == 0) { - if (*ep != '\0') { - usage("BAD PWM VAL"); + char buf[32]; + strncpy(buf, optarg + 2, 16); + /* user wants to use a param name */ + param_t parm = param_find(buf); + + if (parm != PARAM_INVALID) { + int32_t pwm_parm; + int gret = param_get(parm, &pwm_parm); + + if (gret == 0) { + pwm_value = pwm_parm; + } else { + usage("PARAM LOAD FAIL"); + } + } else { + usage("PARAM NAME NOT FOUND"); + } + } else { + + pwm_value = strtoul(optarg, &ep, 0); + } + + if (*ep != '\0') { + usage("BAD PWM VAL"); + } } break; From 20d735701f6c4806d246e4d8fd75758f698b40bb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jun 2015 10:40:12 +0200 Subject: [PATCH 68/88] sensor params: Add hint to reboot system after changing PWM params --- src/modules/sensors/sensor_params.c | 24 ++++++++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 83568c0069..72d139b113 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1414,6 +1414,10 @@ PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0); /** * Set the minimum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 1000 for default or 900 to increase servo travel * * @min 800 @@ -1426,6 +1430,10 @@ PARAM_DEFINE_INT32(PWM_MIN, 1000); /** * Set the maximum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 2000 for default or 2100 to increase servo travel * * @min 1600 @@ -1438,6 +1446,10 @@ PARAM_DEFINE_INT32(PWM_MAX, 2000); /** * Set the disarmed PWM for MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * This is the PWM pulse the autopilot is outputting if not armed. * The main use of this parameter is to silence ESCs when they are disarmed. * @@ -1451,6 +1463,10 @@ PARAM_DEFINE_INT32(PWM_DISARMED, 0); /** * Set the minimum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 1000 for default or 900 to increase servo travel * * @min 800 @@ -1463,6 +1479,10 @@ PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000); /** * Set the maximum PWM for the MAIN outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * Set to 2000 for default or 2100 to increase servo travel * * @min 1600 @@ -1475,6 +1495,10 @@ PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000); /** * Set the disarmed PWM for AUX outputs * + * IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM + * REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE + * THE SYSTEM TO PUT CHANGES INTO EFFECT. + * * This is the PWM pulse the autopilot is outputting if not armed. * The main use of this parameter is to silence ESCs when they are disarmed. * From 2284aa0c96d5abbe132931c7bcb91710c505fd24 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 13 Jun 2015 00:47:20 +0200 Subject: [PATCH 69/88] Caipi config: Move to param based config --- ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha index 1fd96d6d3d..8a661f25e2 100644 --- a/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha +++ b/ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha @@ -34,7 +34,9 @@ then param set PWM_MAIN_REV1 1 fi +set PWM_DISARMED p:PWM_DISARMED +set PWM_MIN p:PWM_MIN +set PWM_MAX p:PWM_MAX + set MIXER caipi -# Provide ESC a constant 1000 us pulse -set PWM_OUT 4 -set PWM_DISARMED 1000 +set PWM_OUT 1234 From 52b0f17ff31213e1c073cf53c069e8883a3ca0e9 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 23 Jun 2015 12:29:25 +0200 Subject: [PATCH 70/88] increase highest pwm to 2150 --- src/drivers/drv_pwm_output.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 2fb9469c8a..18a75d063f 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -83,7 +83,7 @@ __BEGIN_DECLS /** * Highest maximum PWM in us */ -#define PWM_HIGHEST_MAX 2100 +#define PWM_HIGHEST_MAX 2150 /** * Default maximum PWM in us From 5d92927991f9375dccc125ef229a1dec33bf6f55 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 19 Jun 2015 10:25:40 +0200 Subject: [PATCH 71/88] make motors spin in POSCTRL and ATTCTRL when landed and throttle applied by user --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 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 e4682689af..392b31cf42 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 @@ -98,7 +98,8 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode #define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane #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 HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading +#define TAKEOFF_IDLE 0.1f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) 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 @@ -1606,8 +1607,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.thrust = 0.0f; } else { /* Copy thrust and pitch values from tecs */ - _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : - _tecs.get_throttle_demand(), throttle_max); + if (_vehicle_status.condition_landed && + (_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE)) + { + // when we are landed in these modes we want the motor to spin + _att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max); + } else { + _att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() : + _tecs.get_throttle_demand(), throttle_max); + } + + } /* During a takeoff waypoint while waiting for launch the pitch sp is set From 6a00fce009528a99512cd6c2d0b105a2a97bef3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 19:55:04 +0200 Subject: [PATCH 72/88] EKF: Publish global position also if GPS is not yet valid so that controllers can get a valid altitude --- .../ekf_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 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 84da033adb..4208ed0deb 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 @@ -221,6 +221,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() : _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); _parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT"); + /* indicate consumers that the current position data is not valid */ + _gps.eph = 10000.0f; + _gps.epv = 10000.0f; + /* fetch initial parameter values */ parameters_update(); @@ -686,21 +690,21 @@ void AttitudePositionEstimatorEKF::task_main() continue; } - //Run EKF data fusion steps + // Run EKF data fusion steps updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData); - //Publish attitude estimations + // Publish attitude estimations publishAttitude(); - //Publish Local Position estimations + // Publish Local Position estimations publishLocalPosition(); - //Publish Global Position, but only if it's any good - if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) { + // Publish Global Position, but only if it's any good + if (_gpsIsGood || _global_pos.dead_reckoning) { publishGlobalPosition(); } - //Publish wind estimates + // Publish wind estimates if (hrt_elapsed_time(&_wind.timestamp) > 99000) { publishWindEstimate(); } @@ -891,6 +895,10 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_utc_usec = _gps.time_utc_usec; + } else { + _global_pos.lat = 0.0; + _global_pos.lon = 0.0; + _global_pos.time_utc_usec = 0; } if (_local_pos.v_xy_valid) { @@ -907,6 +915,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition() if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; + } else { + _global_pos.vel_d = 0.0f; } /* terrain altitude */ From c46b4a29b8c0be96e40959305e52e6e753e555ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 20:00:38 +0200 Subject: [PATCH 73/88] EKF: Publish initial altitude estimate in any case --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 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 4208ed0deb..877bff6585 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 @@ -699,10 +699,9 @@ void AttitudePositionEstimatorEKF::task_main() // Publish Local Position estimations publishLocalPosition(); - // Publish Global Position, but only if it's any good - if (_gpsIsGood || _global_pos.dead_reckoning) { - publishGlobalPosition(); - } + // Publish Global Position, it will have a large uncertainty + // set if only altitude is known + publishGlobalPosition(); // Publish wind estimates if (hrt_elapsed_time(&_wind.timestamp) > 99000) { From f4845b2b8f8b65d756dc37d51514a4c58c9cdd05 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:22:06 +0200 Subject: [PATCH 74/88] FW pos control: Guard against altitude estimate change --- .../fw_pos_control_l1_main.cpp | 19 ++++++++++++++++++- 1 file changed, 18 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 392b31cf42..79f25f3945 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 @@ -103,6 +103,7 @@ static int _control_task = -1; /**< task handle for sensor task */ 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 +static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f; /** * L1 control app start / stop handling function @@ -174,7 +175,7 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for altitude mode */ - float _ground_alt; /**< ground altitude at which plane was launched */ + float _ground_alt; /**< ground altitude at which plane was launched */ float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ @@ -969,9 +970,24 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) { const float deadBand = (60.0f/1000.0f); const float factor = 1.0f - deadBand; + // XXX this should go into a manual stick mapper + // class + static float _althold_epv = 0.0f; static bool was_in_deadband = false; bool climbout_mode = false; + /* + * Reset the hold altitude to the current altitude if the uncertainty + * changes significantly. + * This is to guard against uncommanded altitude changes + * when the altitude certainty increases or decreases. + */ + + if (fabsf(_althold_epv - _global_pos.epv) > ALTHOLD_EPV_RESET_THRESH) { + _hold_alt = _global_pos.alt; + _althold_epv = _global_pos.epv; + } + // XXX the sign magic in this function needs to be fixed if (_manual.x > deadBand) { @@ -988,6 +1004,7 @@ bool FixedwingPositionControl::update_desired_altitude(float dt) * The aircraft should immediately try to fly at this altitude * as this is what the pilot expects when he moves the stick to the center */ _hold_alt = _global_pos.alt; + _althold_epv = _global_pos.epv; was_in_deadband = true; } From f680bbed545eea3a23b174bac4ccda4bf96b027d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 22 Jun 2015 09:23:17 +0200 Subject: [PATCH 75/88] FW pos control: Rename _ground_alt to _takeoff_ground_alt to make it less ambigious with the actual terrain altitude --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 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 79f25f3945..90e4da3479 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 @@ -175,10 +175,10 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ float _hold_alt; /**< hold altitude for altitude mode */ - float _ground_alt; /**< ground altitude at which plane was launched */ + float _takeoff_ground_alt; /**< ground altitude at which plane was launched */ float _hdg_hold_yaw; /**< hold heading for velocity mode */ bool _hdg_hold_enabled; /**< heading hold enabled */ - bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ + bool _yaw_lock_engaged; /**< yaw is locked for heading hold */ struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */ struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */ hrt_abstime _control_position_last_called; /** throttle_threshold && _global_pos.alt <= _ground_alt + _parameters.climbout_diff) { + if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) { return true; } @@ -1026,7 +1026,7 @@ void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitc { /* demand "climbout_diff" m above ground if user switched into this mode during takeoff */ if (in_takeoff_situation()) { - *hold_altitude = _ground_alt + _parameters.climbout_diff; + *hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff; *pitch_limit_min = math::radians(10.0f); } else { *pitch_limit_min = _parameters.pitch_limit_min; @@ -1068,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (!_was_in_air && !_vehicle_status.condition_landed) { _was_in_air = true; _time_went_in_air = hrt_absolute_time(); - _ground_alt = _global_pos.alt; + _takeoff_ground_alt = _global_pos.alt; } /* reset flag when airplane landed */ if (_vehicle_status.condition_landed) { From 5cf20c8dcfeba450bcc926f4a73b81c382a9ad43 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 23 Jun 2015 12:57:31 +0200 Subject: [PATCH 76/88] increase fw idle for ATTCTL and POSCTL to 0.2 --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 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 90e4da3479..95c8545e73 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 @@ -99,7 +99,7 @@ static int _control_task = -1; /**< task handle for sensor task */ #define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane #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 TAKEOFF_IDLE 0.1f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) +#define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0) 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 From 0f21733cfc38edda892237bbb57deefd0de8b713 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 17:42:48 +0200 Subject: [PATCH 77/88] Wing-wing: Remove unused params. Camflyer: Copy wing-wing defaults --- ROMFS/px4fmu_common/init.d/3030_io_camflyer | 24 +++++++++++++++++++++ ROMFS/px4fmu_common/init.d/3033_wingwing | 10 --------- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3030_io_camflyer b/ROMFS/px4fmu_common/init.d/3030_io_camflyer index 1886783249..040b78dc77 100644 --- a/ROMFS/px4fmu_common/init.d/3030_io_camflyer +++ b/ROMFS/px4fmu_common/init.d/3030_io_camflyer @@ -2,6 +2,30 @@ sh /etc/init.d/rc.fw_defaults +if [ $AUTOCNF == yes ] +then + param set FW_AIRSPD_MAX 15 + param set FW_AIRSPD_MIN 10 + param set FW_AIRSPD_TRIM 13 + param set FW_ATT_TC 0.3 + param set FW_L1_DAMPING 0.74 + param set FW_L1_PERIOD 16 + param set FW_LND_ANG 15 + param set FW_LND_FLALT 5 + param set FW_LND_HHDIST 15 + param set FW_LND_HVIRT 13 + param set FW_LND_TLALT 5 + param set FW_THR_LND_MAX 0 + param set FW_PR_FF 0.35 + param set FW_PR_I 0.005 + param set FW_PR_IMAX 0.4 + param set FW_PR_P 0.08 + param set FW_RR_FF 0.6 + param set FW_RR_I 0.005 + param set FW_RR_IMAX 0.2 + param set FW_RR_P 0.04 +fi + set MIXER Q # Provide ESC a constant 1000 us pulse while disarmed set PWM_OUT 4 diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index add905b115..708c34491b 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -30,16 +30,6 @@ then param set FW_RR_I 0.005 param set FW_RR_IMAX 0.2 param set FW_RR_P 0.04 - param set MT_TKF_PIT_MAX 30.0 - param set MT_ACC_D 0.2 - param set MT_ACC_P 0.6 - param set MT_A_LP 0.5 - param set MT_PIT_OFF 0.1 - param set MT_PIT_I 0.1 - param set MT_THR_OFF 0.65 - param set MT_THR_I 0.35 - param set MT_THR_P 0.2 - param set MT_THR_FF 1.5 fi set MIXER wingwing From 289ad91bcc2e49c33ae338152d7af19a13c4b64d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 17:44:11 +0200 Subject: [PATCH 78/88] Fixed wing land detector: Filter GPS speeds more since they are unreliable, leave airspeed filter where it was --- src/modules/land_detector/FixedwingLandDetector.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/land_detector/FixedwingLandDetector.cpp b/src/modules/land_detector/FixedwingLandDetector.cpp index 5f7ded9cb2..741bc02ad4 100644 --- a/src/modules/land_detector/FixedwingLandDetector.cpp +++ b/src/modules/land_detector/FixedwingLandDetector.cpp @@ -85,12 +85,12 @@ bool FixedwingLandDetector::update() bool landDetected = false; if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) { - float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx * + float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx * _vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy); if (isfinite(val)) { _velocity_xy_filtered = val; } - val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz); + val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz); if (isfinite(val)) { _velocity_z_filtered = val; From 640024357f3b3a261031b750cf7a7b5a82e53a78 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 17:44:44 +0200 Subject: [PATCH 79/88] Land detector: increase ground speed threshold --- src/modules/land_detector/land_detector_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/land_detector/land_detector_params.c b/src/modules/land_detector/land_detector_params.c index b670dcc035..f182495ac3 100644 --- a/src/modules/land_detector/land_detector_params.c +++ b/src/modules/land_detector/land_detector_params.c @@ -96,7 +96,7 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f); * * @group Land Detector */ -PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 4.0f); +PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f); /** * Fixedwing max climb rate From 4fadb65ac65852bdf78534f7c50d289a1338eba1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 24 Jun 2015 12:21:40 +0200 Subject: [PATCH 80/88] commander: Reject mag samples which are on top of each other --- src/modules/commander/mag_calibration.cpp | 109 ++++++++++++++++++---- 1 file changed, 92 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 04e66a5cb6..260316666c 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,6 +65,8 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; static const unsigned max_mags = 3; +static constexpr float mag_sphere_radius = 0.2f; +static const unsigned int calibration_sides = 3; calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); @@ -76,7 +78,7 @@ typedef struct { unsigned int calibration_points_perside; unsigned int calibration_interval_perside_seconds; uint64_t calibration_interval_perside_useconds; - unsigned int calibration_counter_total; + unsigned int calibration_counter_total[max_mags]; bool side_data_collected[detect_orientation_side_count]; float* x[max_mags]; float* y[max_mags]; @@ -184,6 +186,25 @@ int do_mag_calibration(int mavlink_fd) return result; } +static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count) +{ + float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; + //float min_sample_dist = (2.0f * M_PI_F * mag_sphere_radius / max_count) / 2.0f; + + for (size_t i = 0; i < count; i++) { + float dx = sx - x[i]; + float dy = sy - y[i]; + float dz = sz - z[i]; + float dist = sqrtf(dx * dx + dy * dy + dz * dz); + + if (dist < min_sample_dist) { + return true; + } + } + + return false; +} + static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data) { calibrate_return result = calibrate_return_ok; @@ -286,27 +307,47 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta int poll_ret = poll(fds, fd_count, 1000); if (poll_ret > 0) { + + int prev_count[max_mags]; + bool rejected = false; + for (size_t cur_mag=0; cur_magcalibration_counter_total[cur_mag]; + if (worker_data->sub_mag[cur_mag] >= 0) { struct mag_report mag; orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); + + // Check if this measurement is good to go in + rejected = rejected || reject_sample(mag.x, mag.y, mag.z, + worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], + worker_data->calibration_counter_total[cur_mag], + calibration_sides * worker_data->calibration_points_perside); - worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x; - worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y; - worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z; - + worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x; + worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y; + worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z; + worker_data->calibration_counter_total[cur_mag]++; } } - - worker_data->calibration_counter_total++; - calibration_counter_side++; - - // Progress indicator for side - mavlink_and_console_log_info(worker_data->mavlink_fd, - "[cal] %s side calibration: progress <%u>", - detect_orientation_str(orientation), - (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + + // Keep calibration of all mags in lockstep + if (rejected) { + // Reset counts, since one of the mags rejected the measurement + for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag]; + } + } else { + calibration_counter_side++; + + // Progress indicator for side + mavlink_and_console_log_info(worker_data->mavlink_fd, + "[cal] %s side calibration: progress <%u>", + detect_orientation_str(orientation), + (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + } } else { poll_errcount++; } @@ -336,7 +377,6 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; - worker_data.calibration_counter_total = 0; worker_data.calibration_points_perside = 80; worker_data.calibration_interval_perside_seconds = 20; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; @@ -357,9 +397,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.x[cur_mag] = NULL; worker_data.y[cur_mag] = NULL; worker_data.z[cur_mag] = NULL; + worker_data.calibration_counter_total[cur_mag] = 0; } - const unsigned int calibration_sides = 3; const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; char str[30]; @@ -438,7 +478,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag // Mag in this slot is available and we should have values for it to calibrate sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], - worker_data.calibration_counter_total, + worker_data.calibration_counter_total[cur_mag], 100, 0.0f, &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag], &sphere_radius[cur_mag]); @@ -450,6 +490,41 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag } } } + + // Print uncalibrated data points + if (result == calibrate_return_ok) { + + printf("RAW DATA:\n--------------------\n"); + for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + + printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]); + + for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { + float x = worker_data.x[cur_mag][i]; + float y = worker_data.y[cur_mag][i]; + float z = worker_data.z[cur_mag][i]; + printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); + } + + printf(">>>>>>>\n"); + } + + printf("CALIBRATED DATA:\n--------------------\n"); + for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) { + + printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]); + + for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { + float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag]; + float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag]; + float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag]; + printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z); + } + + printf("SPHERE RADIUS: %8.4f", (double)sphere_radius[cur_mag]); + printf(">>>>>>>\n"); + } + } // Data points are no longer needed for (size_t cur_mag=0; cur_mag Date: Wed, 24 Jun 2015 12:22:01 +0200 Subject: [PATCH 81/88] Tools: Add Matlab script to plot mag data --- Tools/Matlab/plot_mag.m | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 Tools/Matlab/plot_mag.m diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m new file mode 100644 index 0000000000..5fa4db4c23 --- /dev/null +++ b/Tools/Matlab/plot_mag.m @@ -0,0 +1,41 @@ +% +% Tool for plotting mag data +% +close all; +clear all; + +plot_scale = 0.8; + +xmax = plot_scale; +xmin = -xmax; +ymax = plot_scale; +ymin = -ymax; +zmax = plot_scale; +zmin = -zmax; + +mag0_raw = load('../../mag0_raw.csv'); +mag1_raw = load('../../mag1_raw.csv'); + +mag0_cal = load('../../mag0_cal.csv'); +mag1_cal = load('../../mag1_cal.csv'); + +fm0r = figure(); + +mag0_x_scale = 1.07; +mag0_y_scale = 0.95; +mag0_z_scale = 1.00; + +plot3(mag0_raw(:,1) .* mag0_x_scale, mag0_raw(:,2) .* mag0_y_scale, mag0_raw(:,3) .* mag0_z_scale, '*r'); +axis([xmin xmax ymin ymax zmin zmax]) + +fm1r = figure(); +plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); +axis([xmin xmax ymin ymax zmin zmax]) + +fm0c = figure(); +plot3(mag0_cal(:,1), mag0_cal(:,2), mag0_cal(:,3), '*b'); +axis([xmin xmax ymin ymax zmin zmax]) + +fm1c = figure(); +plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b'); +axis([xmin xmax ymin ymax zmin zmax]) From a4a6e69521658bae87597819274bd417110a44cd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 08:42:59 +0200 Subject: [PATCH 82/88] Matlab tools: Add ellipsoid fit --- Tools/Matlab/ellipsoid_fit.m | 174 +++++++++++++++++++++++++++++++++++ Tools/Matlab/plot_mag.m | 31 +++++-- 2 files changed, 199 insertions(+), 6 deletions(-) create mode 100644 Tools/Matlab/ellipsoid_fit.m diff --git a/Tools/Matlab/ellipsoid_fit.m b/Tools/Matlab/ellipsoid_fit.m new file mode 100644 index 0000000000..d288aa3821 --- /dev/null +++ b/Tools/Matlab/ellipsoid_fit.m @@ -0,0 +1,174 @@ +% Copyright (c) 2009, Yury Petrov +% All rights reserved. +% +% Redistribution and use in source and binary forms, with or without +% modification, are permitted provided that the following conditions are +% met: +% +% * Redistributions of source code must retain the above copyright +% notice, this list of conditions and the following disclaimer. +% * 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 +% +% 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. +% + +function [ center, radii, evecs, v ] = ellipsoid_fit( X, flag, equals ) +% +% Fit an ellispoid/sphere to a set of xyz data points: +% +% [center, radii, evecs, pars ] = ellipsoid_fit( X ) +% [center, radii, evecs, pars ] = ellipsoid_fit( [x y z] ); +% [center, radii, evecs, pars ] = ellipsoid_fit( X, 1 ); +% [center, radii, evecs, pars ] = ellipsoid_fit( X, 2, 'xz' ); +% [center, radii, evecs, pars ] = ellipsoid_fit( X, 3 ); +% +% Parameters: +% * X, [x y z] - Cartesian data, n x 3 matrix or three n x 1 vectors +% * flag - 0 fits an arbitrary ellipsoid (default), +% - 1 fits an ellipsoid with its axes along [x y z] axes +% - 2 followed by, say, 'xy' fits as 1 but also x_rad = y_rad +% - 3 fits a sphere +% +% Output: +% * center - ellispoid center coordinates [xc; yc; zc] +% * ax - ellipsoid radii [a; b; c] +% * evecs - ellipsoid radii directions as columns of the 3x3 matrix +% * v - the 9 parameters describing the ellipsoid algebraically: +% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1 +% +% Author: +% Yury Petrov, Northeastern University, Boston, MA +% + +error( nargchk( 1, 3, nargin ) ); % check input arguments +if nargin == 1 + flag = 0; % default to a free ellipsoid +end +if flag == 2 && nargin == 2 + equals = 'xy'; +end + +if size( X, 2 ) ~= 3 + error( 'Input data must have three columns!' ); +else + x = X( :, 1 ); + y = X( :, 2 ); + z = X( :, 3 ); +end + +% need nine or more data points +if length( x ) < 9 && flag == 0 + error( 'Must have at least 9 points to fit a unique ellipsoid' ); +end +if length( x ) < 6 && flag == 1 + error( 'Must have at least 6 points to fit a unique oriented ellipsoid' ); +end +if length( x ) < 5 && flag == 2 + error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two axes equal' ); +end +if length( x ) < 3 && flag == 3 + error( 'Must have at least 4 points to fit a unique sphere' ); +end + +if flag == 0 + % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1 + D = [ x .* x, ... + y .* y, ... + z .* z, ... + 2 * x .* y, ... + 2 * x .* z, ... + 2 * y .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; % ndatapoints x 9 ellipsoid parameters +elseif flag == 1 + % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1 + D = [ x .* x, ... + y .* y, ... + z .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; % ndatapoints x 6 ellipsoid parameters +elseif flag == 2 + % fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1, + % where A = B or B = C or A = C + if strcmp( equals, 'yz' ) || strcmp( equals, 'zy' ) + D = [ y .* y + z .* z, ... + x .* x, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; + elseif strcmp( equals, 'xz' ) || strcmp( equals, 'zx' ) + D = [ x .* x + z .* z, ... + y .* y, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; + else + D = [ x .* x + y .* y, ... + z .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; + end +else + % fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1 + D = [ x .* x + y .* y + z .* z, ... + 2 * x, ... + 2 * y, ... + 2 * z ]; % ndatapoints x 4 sphere parameters +end + +% solve the normal system of equations +v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) ); + +% find the ellipsoid parameters +if flag == 0 + % form the algebraic form of the ellipsoid + A = [ v(1) v(4) v(5) v(7); ... + v(4) v(2) v(6) v(8); ... + v(5) v(6) v(3) v(9); ... + v(7) v(8) v(9) -1 ]; + % find the center of the ellipsoid + center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ]; + % form the corresponding translation matrix + T = eye( 4 ); + T( 4, 1:3 ) = center'; + % translate to the center + R = T * A * T'; + % solve the eigenproblem + [ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) ); + radii = sqrt( 1 ./ diag( evals ) ); +else + if flag == 1 + v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ]; + elseif flag == 2 + if strcmp( equals, 'xz' ) || strcmp( equals, 'zx' ) + v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ]; + elseif strcmp( equals, 'yz' ) || strcmp( equals, 'zy' ) + v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ]; + else % xy + v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ]; + end + else + v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ]; + end + center = ( -v( 7:9 ) ./ v( 1:3 ) )'; + gam = 1 + ( v(7)^2 / v(1) + v(8)^2 / v(2) + v(9)^2 / v(3) ); + radii = ( sqrt( gam ./ v( 1:3 ) ) )'; + evecs = eye( 3 ); +end + + diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m index 5fa4db4c23..f5dbfc5edd 100644 --- a/Tools/Matlab/plot_mag.m +++ b/Tools/Matlab/plot_mag.m @@ -1,6 +1,19 @@ % % Tool for plotting mag data % +% Reference values: +% telem> [cal] mag #0 off: x:0.15 y:0.07 z:0.14 Ga +% MATLAB: x:0.1581 y: 0.0701 z: 0.1439 Ga +% telem> [cal] mag #0 scale: x:1.10 y:0.97 z:1.02 +% MATLAB: 0.5499, 0.5190, 0.4907 +% +% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga +% MATLAB: x:-0.1827 y:0.1147 z:-0.0848 Ga +% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00 +% MATLAB: 0.5122, 0.5065, 0.4915 +% +% + close all; clear all; @@ -13,11 +26,11 @@ ymin = -ymax; zmax = plot_scale; zmin = -zmax; -mag0_raw = load('../../mag0_raw.csv'); -mag1_raw = load('../../mag1_raw.csv'); +mag0_raw = load('../../mag0_raw2.csv'); +mag1_raw = load('../../mag1_raw2.csv'); -mag0_cal = load('../../mag0_cal.csv'); -mag1_cal = load('../../mag1_cal.csv'); +mag0_cal = load('../../mag0_cal2.csv'); +mag1_cal = load('../../mag1_cal2.csv'); fm0r = figure(); @@ -25,15 +38,21 @@ mag0_x_scale = 1.07; mag0_y_scale = 0.95; mag0_z_scale = 1.00; -plot3(mag0_raw(:,1) .* mag0_x_scale, mag0_raw(:,2) .* mag0_y_scale, mag0_raw(:,3) .* mag0_z_scale, '*r'); +plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); +[center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); +center +radii axis([xmin xmax ymin ymax zmin zmax]) fm1r = figure(); plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); +[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); +center +radii axis([xmin xmax ymin ymax zmin zmax]) fm0c = figure(); -plot3(mag0_cal(:,1), mag0_cal(:,2), mag0_cal(:,3), '*b'); +plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); axis([xmin xmax ymin ymax zmin zmax]) fm1c = figure(); From cae604ac1f8177775048dacdc899d4372efaf0ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 08:43:34 +0200 Subject: [PATCH 83/88] HMC5883: Increase the number of calibration cycles to ensure a stable result --- src/drivers/hmc5883/hmc5883.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5bf88da3df..22c8c6359c 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1124,8 +1124,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) } } - /* read the sensor up to 50x, stopping when we have 10 good values */ - for (uint8_t i = 0; i < 50 && good_count < 10; i++) { + /* read the sensor up to 100x, stopping when we have 30 good values */ + for (uint8_t i = 0; i < 100 && good_count < 30; i++) { struct pollfd fds; /* wait for data to be ready */ From ef6092afd9c65d3937fe60e402f234eec5ffad38 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 10:44:19 +0200 Subject: [PATCH 84/88] HMC5883: Calculate correct scaling to apply using multiplication --- src/drivers/hmc5883/hmc5883.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 22c8c6359c..a9672dd7de 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1172,9 +1172,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable) scaling[2] = sum_excited[2] / good_count; /* set scaling in device */ - mscale_previous.x_scale = scaling[0]; - mscale_previous.y_scale = scaling[1]; - mscale_previous.z_scale = scaling[2]; + mscale_previous.x_scale = 1.0f / scaling[0]; + mscale_previous.y_scale = 1.0f / scaling[1]; + mscale_previous.z_scale = 1.0f / scaling[2]; ret = OK; From e28e4cb84cf387c5749fce8548954e82dcfc4761 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 10:44:48 +0200 Subject: [PATCH 85/88] Matlab mag: Update to real scaling, resulting fits confirm results --- Tools/Matlab/plot_mag.m | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m index f5dbfc5edd..b344e41bc0 100644 --- a/Tools/Matlab/plot_mag.m +++ b/Tools/Matlab/plot_mag.m @@ -34,9 +34,9 @@ mag1_cal = load('../../mag1_cal2.csv'); fm0r = figure(); -mag0_x_scale = 1.07; -mag0_y_scale = 0.95; -mag0_z_scale = 1.00; +mag0_x_scale = 0.88; +mag0_y_scale = 0.99; +mag0_z_scale = 0.95; plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); [center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); @@ -53,6 +53,9 @@ axis([xmin xmax ymin ymax zmin zmax]) fm0c = figure(); plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); +[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); +center +radii axis([xmin xmax ymin ymax zmin zmax]) fm1c = figure(); From 1fdc6a922115c235e1164aef81c2487750f9cf5b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 10:51:59 +0200 Subject: [PATCH 86/88] commander: Remove unused min sample dist --- src/modules/commander/mag_calibration.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 260316666c..7af023fd82 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -189,7 +189,6 @@ int do_mag_calibration(int mavlink_fd) static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count) { float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; - //float min_sample_dist = (2.0f * M_PI_F * mag_sphere_radius / max_count) / 2.0f; for (size_t i = 0; i < count; i++) { float dx = sx - x[i]; From 338404b4b395c21d75c0c5599610d4cebbfb0ef1 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 24 Jun 2015 13:19:46 -0700 Subject: [PATCH 87/88] Change mag cal to 6 orientations --- src/modules/commander/calibration_messages.h | 2 +- src/modules/commander/mag_calibration.cpp | 12 ++++++------ 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 53775ffe4f..1dbc5b6dbf 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -49,7 +49,7 @@ // instead of visual calibration until such a time as QGC is update to the new version. // The number in the cal started message is used to indicate the version stamp for the current calibration code. -#define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s" +#define CAL_QGC_STARTED_MSG "[cal] calibration started: 2 %s" #define CAL_QGC_DONE_MSG "[cal] calibration done: %s" #define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s" #define CAL_QGC_WARNING_MSG "[cal] calibration warning: %s" diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7af023fd82..36cc0cdd5d 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -66,7 +66,7 @@ static const int ERROR = -1; static const char *sensor_name = "mag"; static const unsigned max_mags = 3; static constexpr float mag_sphere_radius = 0.2f; -static const unsigned int calibration_sides = 3; +static const unsigned int calibration_sides = 6; calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]); @@ -376,7 +376,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.mavlink_fd = mavlink_fd; worker_data.done_count = 0; - worker_data.calibration_points_perside = 80; + worker_data.calibration_points_perside = 40; worker_data.calibration_interval_perside_seconds = 20; worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000; @@ -384,9 +384,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false; worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false; worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false; - worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; - worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; - worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true; + worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false; + worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false; for (size_t cur_mag=0; cur_mag>>>>>>\n"); } } From c402d0c2f7f856e8fd6b94be3ef2824a423f7843 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 25 Jun 2015 13:20:43 +0200 Subject: [PATCH 88/88] Commander: updated mag calibration routine, matlab script updates --- Tools/Matlab/plot_mag.m | 34 +++++++++++++------ .../commander/calibration_routines.cpp | 6 ++-- 2 files changed, 27 insertions(+), 13 deletions(-) diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m index b344e41bc0..c9f0c29925 100644 --- a/Tools/Matlab/plot_mag.m +++ b/Tools/Matlab/plot_mag.m @@ -13,6 +13,12 @@ % MATLAB: 0.5122, 0.5065, 0.4915 % % +% User-guided values: +% +% telem> [cal] mag #0 off: x:0.12 y:0.09 z:0.14 Ga +% telem> [cal] mag #0 scale: x:0.88 y:0.99 z:0.95 +% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga +% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00 close all; clear all; @@ -26,11 +32,11 @@ ymin = -ymax; zmax = plot_scale; zmin = -zmax; -mag0_raw = load('../../mag0_raw2.csv'); -mag1_raw = load('../../mag1_raw2.csv'); +mag0_raw = load('../../mag0_raw3.csv'); +mag1_raw = load('../../mag1_raw3.csv'); -mag0_cal = load('../../mag0_cal2.csv'); -mag1_cal = load('../../mag1_cal2.csv'); +mag0_cal = load('../../mag0_cal3.csv'); +mag1_cal = load('../../mag1_cal3.csv'); fm0r = figure(); @@ -39,10 +45,11 @@ mag0_y_scale = 0.99; mag0_z_scale = 0.95; plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); -[center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); -center -radii +[mag0_raw_center, mag0_raw_radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); +mag0_raw_center +mag0_raw_radii axis([xmin xmax ymin ymax zmin zmax]) +viscircles([mag0_raw_center(1), mag0_raw_center(2)], [mag0_raw_radii(1)]); fm1r = figure(); plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); @@ -53,11 +60,18 @@ axis([xmin xmax ymin ymax zmin zmax]) fm0c = figure(); plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); -[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); -center -radii +[mag0_cal_center, mag0_cal_radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) .* mag0_x_scale mag1_raw(:,2) .* mag0_y_scale mag1_raw(:,3) .* mag0_z_scale] ); +mag0_cal_center +mag0_cal_radii axis([xmin xmax ymin ymax zmin zmax]) +viscircles([0, 0], [mag0_cal_radii(3)]); fm1c = figure(); plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b'); axis([xmin xmax ymin ymax zmin zmax]) +[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); +viscircles([0, 0], [radii(3)]); + +mag0_x_scale_matlab = 1 / (mag0_cal_radii(1) / mag0_raw_radii(1)) +mag0_y_scale_matlab = 1 / (mag0_cal_radii(2) / mag0_raw_radii(2)) +mag0_z_scale_matlab = 1 / (mag0_cal_radii(3) / mag0_raw_radii(3)) diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index e9f83775d7..045dbb032d 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -243,7 +243,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub const float normal_still_thr = 0.25; // normal still threshold float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2); float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2 - hrt_abstime still_time = lenient_still_position ? 1000000 : 1500000; // still time required in us + hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us struct pollfd fds[1]; fds[0].fd = accel_sub; @@ -324,7 +324,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub /* not still, reset still start time */ if (t_still != 0) { mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); - usleep(500000); + usleep(200000); t_still = 0; } } @@ -488,7 +488,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd, // Note that this side is complete side_data_collected[orient] = true; tune_neutral(true); - usleep(500000); + usleep(200000); } if (sub_accel >= 0) {