diff --git a/src/lib/controllib/BlockBoardRotation.hpp b/src/lib/controllib/BlockBoardRotation.hpp new file mode 100644 index 000000000000..adaab4378cef --- /dev/null +++ b/src/lib/controllib/BlockBoardRotation.hpp @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (C) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "block/Block.hpp" + +#include +#include +#include + +namespace control +{ + +class __EXPORT BlockBoardRotation : public SuperBlock, public ModuleParams +{ +public: + + BlockBoardRotation(SuperBlock *parent = nullptr) : + SuperBlock(parent, ""), + ModuleParams(nullptr) + { + _board_rotation.setIdentity(); + + updateParams(); + } + + ~BlockBoardRotation() = default; + + // no copy, assignment, move, move assignment + BlockBoardRotation(const BlockBoardRotation &) = delete; + BlockBoardRotation &operator=(const BlockBoardRotation &) = delete; + BlockBoardRotation(BlockBoardRotation &&) = delete; + BlockBoardRotation &operator=(BlockBoardRotation &&) = delete; + + + const matrix::Dcmf &get() { return _board_rotation; } + + void updateParams() + { + SuperBlock::updateParams(); + + const matrix::Dcmf offset(matrix::Eulerf( + math::radians(_sens_board_offset_x.get()), + math::radians(_sens_board_offset_y.get()), + math::radians(_sens_board_offset_z.get()))); + + _board_rotation = offset * get_rot_matrix((enum Rotation)_sens_board_rot.get()); + } + +private: + + DEFINE_PARAMETERS( + (ParamInt) _sens_board_rot, + + (ParamFloat) _sens_board_offset_x, + (ParamFloat) _sens_board_offset_y, + (ParamFloat) _sens_board_offset_z + ) + + matrix::Dcmf _board_rotation; + +}; + +} // namespace control diff --git a/src/lib/controllib/BlockGyroCorrected.cpp b/src/lib/controllib/BlockGyroCorrected.cpp new file mode 100644 index 000000000000..55220bc93e1e --- /dev/null +++ b/src/lib/controllib/BlockGyroCorrected.cpp @@ -0,0 +1,172 @@ +/**************************************************************************** + * + * Copyright (C) 2017 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "BlockGyroCorrected.hpp" + +#include +#include + +namespace control +{ + +BlockGyroCorrected::BlockGyroCorrected(SuperBlock *parent) : + SuperBlock(parent, ""), + _board_rotation(this) +{ + _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); + _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); + + for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) { + _sensor_gyro_sub[i] = -1; + } + + _gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT); + + if (_gyro_count == 0) { + _gyro_count = 1; + } + + for (unsigned s = 0; s < _gyro_count; s++) { + _sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); + } + + update(); +} + +BlockGyroCorrected::~BlockGyroCorrected() +{ + orb_unsubscribe(_sensor_bias_sub); + orb_unsubscribe(_sensor_correction_sub); + + for (unsigned s = 0; s < _gyro_count; s++) { + orb_unsubscribe(_sensor_gyro_sub[s]); + } +} + +bool BlockGyroCorrected::updateBlocking(int timeout) +{ + // wakeup source: gyro data from sensor selected by the sensor app + px4_pollfd_struct_t poll_fds = {}; + poll_fds.events = POLLIN; + poll_fds.fd = _sensor_gyro_sub[_selected_gyro]; + + int pret = px4_poll(&poll_fds, 1, timeout); + + if ((pret == 1) && (poll_fds.revents & POLLIN)) { + // update gyro data + return update(); + + } else if (pret < 0) { + // this is undesirable but not much we can do - might want to flag unhappy status + PX4_ERR("poll error %d, %d", pret, errno); + + // sleep a bit before next try + usleep(100000); + } + + return false; +} + +void BlockGyroCorrected::update_bias() +{ + bool updated = false; + orb_check(_sensor_bias_sub, &updated); + + if (updated) { + sensor_bias_s bias; + + if (orb_copy(ORB_ID(sensor_bias), _sensor_bias_sub, &bias) == PX4_OK) { + _bias = {bias.gyro_x_bias, bias.gyro_y_bias, bias.gyro_z_bias}; + } + } +} + +void BlockGyroCorrected::update_correction() +{ + bool updated = false; + orb_check(_sensor_correction_sub, &updated); + + if (updated) { + sensor_correction_s corr; + + if (orb_copy(ORB_ID(sensor_correction), _sensor_correction_sub, &corr) == PX4_OK) { + + // update the latest gyro selection + if (corr.selected_gyro_instance < _gyro_count) { + _selected_gyro = corr.selected_gyro_instance; + } + + if (_selected_gyro == 0) { + _offset = corr.gyro_offset_0; + _scale = corr.gyro_scale_0; + + } else if (_selected_gyro == 1) { + _offset = corr.gyro_offset_1; + _scale = corr.gyro_scale_1; + + } else if (_selected_gyro == 2) { + _offset = corr.gyro_offset_2; + _scale = corr.gyro_scale_2; + + } else { + _offset = {0.0f, 0.0f, 0.0f}; + _scale = {1.0f, 1.0f, 1.0f}; + } + } + } +} + +bool BlockGyroCorrected::update() +{ + update_bias(); + update_correction(); + + sensor_gyro_s sensor_gyro; + + if (orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &sensor_gyro) == PX4_OK) { + + const matrix::Vector3f gyro{sensor_gyro.x, sensor_gyro.y, sensor_gyro.z}; + + // rotate corrected measurements from sensor to body frame + // correct for in-run bias errors + _rates = (_board_rotation.get() * (gyro - _offset).emult(_scale)) - _bias; + + _timestamp = sensor_gyro.timestamp; + + return true; + } + + return false; +} + +} // namespacea control diff --git a/src/lib/controllib/BlockGyroCorrected.hpp b/src/lib/controllib/BlockGyroCorrected.hpp new file mode 100644 index 000000000000..ead78189a32d --- /dev/null +++ b/src/lib/controllib/BlockGyroCorrected.hpp @@ -0,0 +1,94 @@ +/**************************************************************************** + * + * Copyright (C) 2018 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include "BlockBoardRotation.hpp" + +#include "block/Block.hpp" + +#include +#include +#include +#include +#include +#include + +namespace control +{ + +static constexpr int MAX_GYRO_COUNT = 3; + +class __EXPORT BlockGyroCorrected : public SuperBlock +{ +public: + + BlockGyroCorrected(SuperBlock *parent = nullptr); + ~BlockGyroCorrected(); + + // no copy, assignment, move, move assignment + BlockGyroCorrected(const BlockGyroCorrected &) = delete; + BlockGyroCorrected &operator=(const BlockGyroCorrected &) = delete; + BlockGyroCorrected(BlockGyroCorrected &&) = delete; + BlockGyroCorrected &operator=(BlockGyroCorrected &&) = delete; + + const matrix::Vector3f &get() { return _rates; } + const uint64_t ×tamp() { return _timestamp; } + + bool updateBlocking(int timeout = 100); + bool update(); + +private: + + void update_bias(); + void update_correction(); + + int _sensor_correction_sub{-1}; /**< sensor thermal correction subscription */ + int _sensor_bias_sub{-1}; /**< sensor in-run bias correction subscription */ + int _sensor_gyro_sub[MAX_GYRO_COUNT] {}; + + BlockBoardRotation _board_rotation; + + uint8_t _selected_gyro{0}; + uint8_t _gyro_count{0}; + + matrix::Vector3f _offset{0.0f, 0.0f, 0.0f}; + matrix::Vector3f _scale{1.0f, 1.0f, 1.0f}; + matrix::Vector3f _bias{0.0f, 0.0f, 0.0f}; + + uint64_t _timestamp{0}; + matrix::Vector3f _rates{0.0f, 0.0f, 0.0f}; + +}; + +} // namespace control diff --git a/src/lib/controllib/CMakeLists.txt b/src/lib/controllib/CMakeLists.txt index 9d33534c2bbb..608f417a7111 100644 --- a/src/lib/controllib/CMakeLists.txt +++ b/src/lib/controllib/CMakeLists.txt @@ -44,6 +44,7 @@ px4_add_module( BlockLimitSym.cpp BlockLowPass.cpp BlockLowPass2.cpp + BlockGyroCorrected.cpp DEPENDS platforms__common ) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index bbf504b45b7e..dc771631eea0 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -149,6 +149,8 @@ #include #include +#include + static const char *sensor_name = "accel"; static int32_t device_id[max_accel_sens]; @@ -568,30 +570,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub */ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num) { - /* get total sensor board rotation matrix */ - param_t board_rotation_h = param_find("SENS_BOARD_ROT"); - param_t board_offset_x = param_find("SENS_BOARD_X_OFF"); - param_t board_offset_y = param_find("SENS_BOARD_Y_OFF"); - param_t board_offset_z = param_find("SENS_BOARD_Z_OFF"); - - float board_offset[3]; - param_get(board_offset_x, &board_offset[0]); - param_get(board_offset_y, &board_offset[1]); - param_get(board_offset_z, &board_offset[2]); - - math::Matrix<3, 3> board_rotation_offset; - board_rotation_offset.from_euler(M_DEG_TO_RAD_F * board_offset[0], - M_DEG_TO_RAD_F * board_offset[1], - M_DEG_TO_RAD_F * board_offset[2]); - - int32_t board_rotation_int; - param_get(board_rotation_h, &(board_rotation_int)); - enum Rotation board_rotation_id = (enum Rotation)board_rotation_int; - math::Matrix<3, 3> board_rotation; - get_rot_matrix(board_rotation_id, &board_rotation); - - /* combine board rotation with offset rotation */ - board_rotation = board_rotation_offset * board_rotation; + control::BlockBoardRotation board_rotation; px4_pollfd_struct_t fds[max_accel_sens]; @@ -668,9 +647,9 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m // rotate sensor measurements from sensor to body frame using board rotation matrix for (unsigned i = 0; i < max_accel_sens; i++) { - math::Vector<3> accel_sum_vec(&accel_sum[i][0]); - accel_sum_vec = board_rotation * accel_sum_vec; - memcpy(&accel_sum[i][0], &accel_sum_vec.data[0], sizeof(accel_sum[i])); + matrix::Vector3f accel_sum_vec(&accel_sum[i][0]); + accel_sum_vec = board_rotation.get() * accel_sum_vec; + memcpy(&accel_sum[i][0], accel_sum_vec.data(), sizeof(accel_sum[i])); } for (unsigned s = 0; s < max_accel_sens; s++) { diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 789695593484..ecb45cf76653 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -118,6 +118,19 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : /* fetch initial parameter values */ parameters_update(); + + _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); + + _control_input.scaler = 1.0f; + _control_input.groundspeed_scaler = 1.0f; } FixedwingAttitudeControl::~FixedwingAttitudeControl() @@ -125,13 +138,23 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl() perf_free(_loop_perf); perf_free(_nonfinite_input_perf); perf_free(_nonfinite_output_perf); + + orb_unsubscribe(_att_sub); + orb_unsubscribe(_att_sp_sub); + orb_unsubscribe(_battery_status_sub); + orb_unsubscribe(_global_pos_sub); + orb_unsubscribe(_manual_sub); + orb_unsubscribe(_params_sub); + orb_unsubscribe(_vcontrol_mode_sub); + orb_unsubscribe(_vehicle_land_detected_sub); + orb_unsubscribe(_vehicle_status_sub); } -int +void FixedwingAttitudeControl::parameters_update() { - int32_t tmp = 0; + param_get(_parameter_handles.p_tc, &(_parameters.p_tc)); param_get(_parameter_handles.p_p, &(_parameters.p_p)); param_get(_parameter_handles.p_i, &(_parameters.p_i)); @@ -241,7 +264,7 @@ FixedwingAttitudeControl::parameters_update() _wheel_ctrl.set_integrator_max(_parameters.w_integrator_max); _wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax)); - return PX4_OK; + _gyro_corrected.updateParams(); } void @@ -253,7 +276,6 @@ FixedwingAttitudeControl::vehicle_control_mode_poll() orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &_vcontrol_mode); } } @@ -336,6 +358,70 @@ FixedwingAttitudeControl::vehicle_manual_poll() } } +bool +FixedwingAttitudeControl::vehicle_attitude_poll() +{ + bool updated = false; + + orb_check(_att_sub, &updated); + + if (updated) { + vehicle_attitude_s att; + + if (orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att) == PX4_OK) { + + Eulerf euler_angles; + + if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) { + /* 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. + * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around + * the pitch axis compared to the neutral position of the vehicle in multicopter mode + * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. + * Additionally, in order to get the correct sign of the pitch, we need to multiply + * the new x axis of the rotation matrix with -1 + * + * original: modified: + * + * Rxx Ryx Rzx -Rzx Ryx Rxx + * Rxy Ryy Rzy -Rzy Ryy Rxy + * Rxz Ryz Rzz -Rzz Ryz Rxz + * + * */ + + const matrix::Dcmf R = matrix::Quatf(att.q); + matrix::Dcmf R_adapted{R}; + + /* move z to x */ + /* change direction of pitch (convert to right handed system) */ + R_adapted(0, 0) = -R(0, 2); + R_adapted(1, 0) = -R(1, 2); + R_adapted(2, 0) = -R(2, 2); + + /* move x to z */ + R_adapted(0, 2) = R(0, 0); + R_adapted(1, 2) = R(1, 0); + R_adapted(2, 2) = R(2, 0); + + euler_angles = R_adapted; + + } else { + euler_angles = matrix::Quatf(att.q); + } + + _control_input.roll = euler_angles.phi(); + _control_input.pitch = euler_angles.theta(); + _control_input.yaw = euler_angles.psi(); + + return true; + } + } + + return false; +} + void FixedwingAttitudeControl::vehicle_setpoint_poll() { @@ -408,35 +494,6 @@ FixedwingAttitudeControl::vehicle_land_detected_poll() void FixedwingAttitudeControl::run() { - /* - * do subscriptions - */ - _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); - - parameters_update(); - - /* get an initial update for all sensor and status data */ - vehicle_setpoint_poll(); - vehicle_control_mode_poll(); - vehicle_manual_poll(); - vehicle_status_poll(); - vehicle_land_detected_poll(); - - /* wakeup source */ - px4_pollfd_struct_t fds[1]; - - /* Setup of loop */ - fds[0].fd = _att_sub; - fds[0].events = POLLIN; - while (!should_exit()) { /* only update parameters if they changed */ @@ -452,24 +509,11 @@ void FixedwingAttitudeControl::run() parameters_update(); } - /* wait for up to 500ms for data */ - int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); - - /* timed out - periodic check for _task_should_exit, etc. */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - PX4_WARN("poll error %d, %d", pret, errno); - continue; - } + /* only run controller if attitude changed */ + if (_gyro_corrected.updateBlocking()) { - perf_begin(_loop_perf); + perf_begin(_loop_perf); - /* only run controller if attitude changed */ - if (fds[0].revents & POLLIN) { static uint64_t last_run = 0; float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; last_run = hrt_absolute_time(); @@ -479,58 +523,8 @@ void FixedwingAttitudeControl::run() deltaT = 0.01f; } - /* load local copies */ - orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att); - - /* get current rotation matrix and euler angles from control state quaternions */ - matrix::Dcmf R = matrix::Quatf(_att.q); - - if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) { - /* 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. - * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around - * the pitch axis compared to the neutral position of the vehicle in multicopter mode - * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix. - * Additionally, in order to get the correct sign of the pitch, we need to multiply - * the new x axis of the rotation matrix with -1 - * - * original: modified: - * - * Rxx Ryx Rzx -Rzx Ryx Rxx - * Rxy Ryy Rzy -Rzy Ryy Rxy - * Rxz Ryz Rzz -Rzz Ryz Rxz - * */ - matrix::Dcmf R_adapted = R; //modified rotation matrix - - /* move z to x */ - R_adapted(0, 0) = R(0, 2); - R_adapted(1, 0) = R(1, 2); - R_adapted(2, 0) = R(2, 2); - - /* move x to z */ - R_adapted(0, 2) = R(0, 0); - R_adapted(1, 2) = R(1, 0); - R_adapted(2, 2) = R(2, 0); - - /* change direction of pitch (convert to right handed system) */ - R_adapted(0, 0) = -R_adapted(0, 0); - R_adapted(1, 0) = -R_adapted(1, 0); - R_adapted(2, 0) = -R_adapted(2, 0); - - /* fill in new attitude data */ - R = R_adapted; - - /* lastly, roll- and yawspeed have to be swaped */ - float helper = _att.rollspeed; - _att.rollspeed = -_att.yawspeed; - _att.yawspeed = helper; - } - - matrix::Eulerf euler_angles(R); - _airspeed_sub.update(); + vehicle_attitude_poll(); vehicle_setpoint_poll(); vehicle_control_mode_poll(); vehicle_manual_poll(); @@ -625,28 +619,28 @@ void FixedwingAttitudeControl::run() _wheel_ctrl.reset_integrator(); } - float roll_sp = _att_sp.roll_body; - float pitch_sp = _att_sp.pitch_body; - float yaw_sp = _att_sp.yaw_body; - - /* Prepare data for attitude controllers */ - struct ECL_ControlData control_input = {}; - control_input.roll = euler_angles.phi(); - control_input.pitch = euler_angles.theta(); - control_input.yaw = euler_angles.psi(); - control_input.body_x_rate = _att.rollspeed; - control_input.body_y_rate = _att.pitchspeed; - control_input.body_z_rate = _att.yawspeed; - control_input.roll_setpoint = roll_sp; - control_input.pitch_setpoint = pitch_sp; - control_input.yaw_setpoint = yaw_sp; - control_input.airspeed_min = _parameters.airspeed_min; - control_input.airspeed_max = _parameters.airspeed_max; - control_input.airspeed = airspeed; - control_input.scaler = airspeed_scaling; - control_input.lock_integrator = lock_integrator; - control_input.groundspeed = groundspeed; - control_input.groundspeed_scaler = groundspeed_scaler; + /* Prepare data for ECL controllers */ + if (_vehicle_status.is_vtol && _parameters.vtol_type == vtol_type::TAILSITTER) { + _control_input.body_x_rate = -_gyro_corrected.get()(2); + _control_input.body_y_rate = _gyro_corrected.get()(1); + _control_input.body_z_rate = _gyro_corrected.get()(0); + + } else { + _control_input.body_x_rate = _gyro_corrected.get()(0); + _control_input.body_y_rate = _gyro_corrected.get()(1); + _control_input.body_z_rate = _gyro_corrected.get()(2); + } + + _control_input.roll_setpoint = _att_sp.roll_body; + _control_input.pitch_setpoint = _att_sp.pitch_body; + _control_input.yaw_setpoint = _att_sp.yaw_body; + _control_input.airspeed_min = _parameters.airspeed_min; + _control_input.airspeed_max = _parameters.airspeed_max; + _control_input.airspeed = airspeed; + _control_input.scaler = airspeed_scaling; + _control_input.lock_integrator = lock_integrator; + _control_input.groundspeed = groundspeed; + _control_input.groundspeed_scaler = groundspeed_scaler; /* reset body angular rate limits on mode change */ if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) { @@ -694,19 +688,19 @@ void FixedwingAttitudeControl::run() /* Run attitude controllers */ if (_vcontrol_mode.flag_control_attitude_enabled) { - if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) { - _roll_ctrl.control_attitude(control_input); - _pitch_ctrl.control_attitude(control_input); - _yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude - _wheel_ctrl.control_attitude(control_input); + if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) { + _roll_ctrl.control_attitude(_control_input); + _pitch_ctrl.control_attitude(_control_input); + _yaw_ctrl.control_attitude(_control_input); //runs last, because is depending on output of roll and pitch attitude + _wheel_ctrl.control_attitude(_control_input); /* Update input data for rate controllers */ - control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); - control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); - control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); + _control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate(); + _control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate(); + _control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate(); /* Run attitude RATE controllers which need the desired attitudes from above, add trim */ - float roll_u = _roll_ctrl.control_euler_rate(control_input); + float roll_u = _roll_ctrl.control_euler_rate(_control_input); _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; if (!PX4_ISFINITE(roll_u)) { @@ -714,7 +708,7 @@ void FixedwingAttitudeControl::run() perf_count(_nonfinite_output_perf); } - float pitch_u = _pitch_ctrl.control_euler_rate(control_input); + float pitch_u = _pitch_ctrl.control_euler_rate(_control_input); _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch; if (!PX4_ISFINITE(pitch_u)) { @@ -725,10 +719,10 @@ void FixedwingAttitudeControl::run() float yaw_u = 0.0f; if (_parameters.w_en && _att_sp.fw_control_yaw) { - yaw_u = _wheel_ctrl.control_bodyrate(control_input); + yaw_u = _wheel_ctrl.control_bodyrate(_control_input); } else { - yaw_u = _yaw_ctrl.control_euler_rate(control_input); + yaw_u = _yaw_ctrl.control_euler_rate(_control_input); } _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; @@ -797,13 +791,13 @@ void FixedwingAttitudeControl::run() _pitch_ctrl.set_bodyrate_setpoint(_rates_sp.pitch); _yaw_ctrl.set_bodyrate_setpoint(_rates_sp.yaw); - float roll_u = _roll_ctrl.control_bodyrate(control_input); + float roll_u = _roll_ctrl.control_bodyrate(_control_input); _actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll; - float pitch_u = _pitch_ctrl.control_bodyrate(control_input); + float pitch_u = _pitch_ctrl.control_bodyrate(_control_input); _actuators.control[actuator_controls_s::INDEX_PITCH] = (PX4_ISFINITE(pitch_u)) ? pitch_u + trim_pitch : trim_pitch; - float yaw_u = _yaw_ctrl.control_bodyrate(control_input); + float yaw_u = _yaw_ctrl.control_bodyrate(_control_input); _actuators.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? yaw_u + trim_yaw : trim_yaw; _actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_rates_sp.thrust) ? _rates_sp.thrust : 0.0f; @@ -811,9 +805,9 @@ void FixedwingAttitudeControl::run() rate_ctrl_status_s rate_ctrl_status; rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.rollspeed = _att.rollspeed; - rate_ctrl_status.pitchspeed = _att.pitchspeed; - rate_ctrl_status.yawspeed = _att.yawspeed; + rate_ctrl_status.rollspeed = _control_input.body_x_rate; + rate_ctrl_status.pitchspeed = _control_input.body_y_rate; + rate_ctrl_status.yawspeed = _control_input.body_z_rate; rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator(); rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator(); rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); @@ -834,16 +828,14 @@ void FixedwingAttitudeControl::run() // FIXME: this should use _vcontrol_mode.landing_gear_pos in the future _actuators.control[7] = _manual.aux3; - /* lazily publish the setpoint only once available */ - _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _att.timestamp; - _actuators_airframe.timestamp = hrt_absolute_time(); - _actuators_airframe.timestamp_sample = _att.timestamp; - /* Only publish if any of the proper modes are enabled */ if (_vcontrol_mode.flag_control_rates_enabled || _vcontrol_mode.flag_control_attitude_enabled || _vcontrol_mode.flag_control_manual_enabled) { + + _actuators.timestamp = hrt_absolute_time(); + _actuators.timestamp_sample = _gyro_corrected.timestamp(); + /* publish the actuator controls */ if (_actuators_0_pub != nullptr) { orb_publish(_actuators_id, _actuators_0_pub, &_actuators); @@ -852,6 +844,9 @@ void FixedwingAttitudeControl::run() _actuators_0_pub = orb_advertise(_actuators_id, &_actuators); } + _actuators_airframe.timestamp = hrt_absolute_time(); + _actuators_airframe.timestamp_sample = _gyro_corrected.timestamp(); + if (_actuators_2_pub != nullptr) { /* publish the actuator controls*/ orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index a66e1ebdaeaf..4eaaa8c2fc4e 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -31,7 +31,7 @@ * ****************************************************************************/ -#include +#include #include #include #include @@ -41,6 +41,7 @@ #include #include #include +#include #include #include #include @@ -61,8 +62,10 @@ #include #include +using matrix::Dcmf; using matrix::Eulerf; using matrix::Quatf; +using matrix::Vector3f; using uORB::Subscription; @@ -115,14 +118,15 @@ class FixedwingAttitudeControl final : public ModuleBase _airspeed_sub; + Subscription _airspeed_sub; + + control::BlockGyroCorrected _gyro_corrected; perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */ @@ -274,18 +278,20 @@ class FixedwingAttitudeControl final : public ModuleBase #include #include #include @@ -47,9 +48,6 @@ #include #include #include -#include -#include -#include #include #include #include @@ -69,7 +67,7 @@ class MulticopterAttitudeControl : public ModuleBase public: MulticopterAttitudeControl(); - virtual ~MulticopterAttitudeControl() = default; + virtual ~MulticopterAttitudeControl(); /** @see ModuleBase */ static int task_spawn(int argc, char *argv[]); @@ -98,8 +96,6 @@ class MulticopterAttitudeControl : public ModuleBase */ void battery_status_poll(); void parameter_update_poll(); - void sensor_bias_poll(); - void sensor_correction_poll(); void vehicle_attitude_poll(); void vehicle_attitude_setpoint_poll(); void vehicle_control_mode_poll(); @@ -123,7 +119,6 @@ class MulticopterAttitudeControl : public ModuleBase */ matrix::Vector3f pid_attenuations(float tpa_breakpoint, float tpa_rate); - int _v_att_sub{-1}; /**< vehicle attitude subscription */ int _v_att_sp_sub{-1}; /**< vehicle attitude setpoint subscription */ int _v_rates_sp_sub{-1}; /**< vehicle rates setpoint subscription */ @@ -133,12 +128,8 @@ class MulticopterAttitudeControl : public ModuleBase int _vehicle_status_sub{-1}; /**< vehicle status subscription */ int _motor_limits_sub{-1}; /**< motor limits subscription */ int _battery_status_sub{-1}; /**< battery status subscription */ - int _sensor_gyro_sub[MAX_GYRO_COUNT]; /**< gyro data subscription */ - int _sensor_correction_sub{-1}; /**< sensor thermal correction subscription */ - int _sensor_bias_sub{-1}; /**< sensor in-run bias correction subscription */ - unsigned _gyro_count{1}; - int _selected_gyro{0}; + control::BlockGyroCorrected _gyro_corrected; orb_advert_t _v_rates_sp_pub{nullptr}; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ @@ -157,9 +148,6 @@ class MulticopterAttitudeControl : public ModuleBase struct actuator_controls_s _actuators {}; /**< actuator controls */ struct vehicle_status_s _vehicle_status {}; /**< vehicle status */ struct battery_status_s _battery_status {}; /**< battery status */ - struct sensor_gyro_s _sensor_gyro {}; /**< gyro data before thermal correctons and ekf bias estimates are applied */ - struct sensor_correction_s _sensor_correction {}; /**< sensor thermal corrections */ - struct sensor_bias_s _sensor_bias {}; /**< sensor in-run bias corrections */ MultirotorMixer::saturation_status _saturation_status{}; @@ -177,8 +165,6 @@ class MulticopterAttitudeControl : public ModuleBase float _thrust_sp; /**< thrust setpoint */ matrix::Vector3f _att_control; /**< attitude control vector */ - matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ - DEFINE_PARAMETERS( (ParamFloat) _roll_p, (ParamFloat) _roll_rate_p, @@ -227,12 +213,6 @@ class MulticopterAttitudeControl : public ModuleBase (ParamBool) _bat_scale_en, - (ParamInt) _board_rotation_param, - - (ParamFloat) _board_offset_x, - (ParamFloat) _board_offset_y, - (ParamFloat) _board_offset_z, - (ParamFloat) _vtol_wv_yaw_rate_scale /**< Scale value [0, 1] for yaw rate setpoint */ ) 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 ce47dfec4670..c25f0ec2fbce 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -45,7 +45,6 @@ #include "mc_att_control.hpp" -#include #include #include #include @@ -107,10 +106,6 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : {initial_update_rate_hz, 50.f}, {initial_update_rate_hz, 50.f}} // will be initialized correctly when params are loaded { - for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) { - _sensor_gyro_sub[i] = -1; - } - _vehicle_status.is_rotary_wing = true; /* initialize quaternions in messages to be valid */ @@ -124,15 +119,30 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _thrust_sp = 0.0f; _att_control.zero(); - /* initialize thermal corrections as we might not immediately get a topic update (only non-zero values) */ - for (unsigned i = 0; i < 3; i++) { - // used scale factors to unity - _sensor_correction.gyro_scale_0[i] = 1.0f; - _sensor_correction.gyro_scale_1[i] = 1.0f; - _sensor_correction.gyro_scale_2[i] = 1.0f; - } - parameters_updated(); + + _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); + _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); + _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); +} + +MulticopterAttitudeControl::~MulticopterAttitudeControl() +{ + orb_unsubscribe(_v_att_sub); + orb_unsubscribe(_v_att_sp_sub); + orb_unsubscribe(_v_rates_sp_sub); + orb_unsubscribe(_v_control_mode_sub); + orb_unsubscribe(_params_sub); + orb_unsubscribe(_manual_control_sp_sub); + orb_unsubscribe(_vehicle_status_sub); + orb_unsubscribe(_motor_limits_sub); + orb_unsubscribe(_battery_status_sub); } void @@ -190,15 +200,7 @@ MulticopterAttitudeControl::parameters_updated() _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); - /* get transformation matrix from sensor/board to body frame */ - _board_rotation = get_rot_matrix((enum Rotation)_board_rotation_param.get()); - - /* fine tune the rotation */ - Dcmf board_rotation_offset(Eulerf( - M_DEG_TO_RAD_F * _board_offset_x.get(), - M_DEG_TO_RAD_F * _board_offset_y.get(), - M_DEG_TO_RAD_F * _board_offset_z.get())); - _board_rotation = board_rotation_offset * _board_rotation; + _gyro_corrected.updateParams(); } void @@ -330,36 +332,6 @@ MulticopterAttitudeControl::vehicle_attitude_poll() } } -void -MulticopterAttitudeControl::sensor_correction_poll() -{ - /* check if there is a new message */ - bool updated; - orb_check(_sensor_correction_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_correction), _sensor_correction_sub, &_sensor_correction); - } - - /* update the latest gyro selection */ - if (_sensor_correction.selected_gyro_instance < _gyro_count) { - _selected_gyro = _sensor_correction.selected_gyro_instance; - } -} - -void -MulticopterAttitudeControl::sensor_bias_poll() -{ - /* check if there is a new message */ - bool updated; - orb_check(_sensor_bias_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensor_bias); - } - -} - /** * Attitude controller. * Input: 'vehicle_attitude_setpoint' topics (depending on mode) @@ -481,43 +453,13 @@ MulticopterAttitudeControl::pid_attenuations(float tpa_breakpoint, float tpa_rat void MulticopterAttitudeControl::control_attitude_rates(float dt) { + const Vector3f& rates = _gyro_corrected.get(); + /* reset integral if disarmed */ if (!_v_control_mode.flag_armed || !_vehicle_status.is_rotary_wing) { _rates_int.zero(); } - // get the raw gyro data and correct for thermal errors - Vector3f rates; - - if (_selected_gyro == 0) { - rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0]; - rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1]; - rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2]; - - } else if (_selected_gyro == 1) { - rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0]; - rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1]; - rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2]; - - } else if (_selected_gyro == 2) { - rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0]; - rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1]; - rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2]; - - } else { - rates(0) = _sensor_gyro.x; - rates(1) = _sensor_gyro.y; - rates(2) = _sensor_gyro.z; - } - - // rotate corrected measurements from sensor to body frame - rates = _board_rotation * rates; - - // correct for in-run bias errors - rates(0) -= _sensor_bias.gyro_x_bias; - rates(1) -= _sensor_bias.gyro_y_bias; - rates(2) -= _sensor_bias.gyro_z_bias; - Vector3f rates_p_scaled = _rate_p.emult(pid_attenuations(_tpa_breakpoint_p.get(), _tpa_rate_p.get())); Vector3f rates_i_scaled = _rate_i.emult(pid_attenuations(_tpa_breakpoint_i.get(), _tpa_rate_i.get())); Vector3f rates_d_scaled = _rate_d.emult(pid_attenuations(_tpa_breakpoint_d.get(), _tpa_rate_d.get())); @@ -579,44 +521,12 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) /* explicitly limit the integrator state */ for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) { _rates_int(i) = math::constrain(_rates_int(i), -_rate_int_lim(i), _rate_int_lim(i)); - } } void MulticopterAttitudeControl::run() { - - /* - * do subscriptions - */ - _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); - _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); - _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits)); - _battery_status_sub = orb_subscribe(ORB_ID(battery_status)); - - _gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT); - - if (_gyro_count == 0) { - _gyro_count = 1; - } - - for (unsigned s = 0; s < _gyro_count; s++) { - _sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s); - } - - _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); - _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); - - /* wakeup source: gyro data from sensor selected by the sensor app */ - px4_pollfd_struct_t poll_fds = {}; - poll_fds.events = POLLIN; - const hrt_abstime task_start = hrt_absolute_time(); hrt_abstime last_run = task_start; float dt_accumulator = 0.f; @@ -624,28 +534,11 @@ MulticopterAttitudeControl::run() while (!should_exit()) { - poll_fds.fd = _sensor_gyro_sub[_selected_gyro]; - - /* wait for up to 100ms for data */ - int pret = px4_poll(&poll_fds, 1, 100); - - /* timed out - periodic check for should_exit() */ - if (pret == 0) { - continue; - } - - /* this is undesirable but not much we can do - might want to flag unhappy status */ - if (pret < 0) { - PX4_ERR("poll error %d, %d", pret, errno); - /* sleep a bit before next try */ - usleep(100000); - continue; - } + /* run controller on gyro changes */ + if (_gyro_corrected.updateBlocking()) { - perf_begin(_loop_perf); + perf_begin(_loop_perf); - /* run controller on gyro changes */ - if (poll_fds.revents & POLLIN) { const hrt_abstime now = hrt_absolute_time(); float dt = (now - last_run) / 1e6f; last_run = now; @@ -658,9 +551,6 @@ MulticopterAttitudeControl::run() dt = 0.02f; } - /* copy gyro data */ - orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro); - /* check for updates in other topics */ parameter_update_poll(); vehicle_control_mode_poll(); @@ -669,8 +559,6 @@ MulticopterAttitudeControl::run() vehicle_motor_limits_poll(); battery_status_poll(); vehicle_attitude_poll(); - sensor_correction_poll(); - sensor_bias_poll(); /* Check if we are in rattitude mode and the pilot is above the threshold on pitch * or roll (yaw can rotate 360 in normal att control). If both are true don't @@ -745,7 +633,7 @@ MulticopterAttitudeControl::run() _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.control[7] = _v_att_sp.landing_gear; _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _sensor_gyro.timestamp; + _actuators.timestamp_sample = _gyro_corrected.timestamp(); /* scale effort by battery status */ if (_bat_scale_en.get() && _battery_status.scale > 0.0f) { @@ -794,7 +682,7 @@ MulticopterAttitudeControl::run() _actuators.control[2] = 0.0f; _actuators.control[3] = 0.0f; _actuators.timestamp = hrt_absolute_time(); - _actuators.timestamp_sample = _sensor_gyro.timestamp; + _actuators.timestamp_sample = _gyro_corrected.timestamp(); if (!_actuators_0_circuit_breaker_enabled) { if (_actuators_0_pub != nullptr) { @@ -829,23 +717,6 @@ MulticopterAttitudeControl::run() perf_end(_loop_perf); } - - orb_unsubscribe(_v_att_sub); - orb_unsubscribe(_v_att_sp_sub); - orb_unsubscribe(_v_rates_sp_sub); - orb_unsubscribe(_v_control_mode_sub); - orb_unsubscribe(_params_sub); - orb_unsubscribe(_manual_control_sp_sub); - orb_unsubscribe(_vehicle_status_sub); - orb_unsubscribe(_motor_limits_sub); - orb_unsubscribe(_battery_status_sub); - - for (unsigned s = 0; s < _gyro_count; s++) { - orb_unsubscribe(_sensor_gyro_sub[s]); - } - - orb_unsubscribe(_sensor_correction_sub); - orb_unsubscribe(_sensor_bias_sub); } int MulticopterAttitudeControl::task_spawn(int argc, char *argv[]) diff --git a/src/modules/sensors/parameters.cpp b/src/modules/sensors/parameters.cpp index cd7261b72a7f..05d787f38aa2 100644 --- a/src/modules/sensors/parameters.cpp +++ b/src/modules/sensors/parameters.cpp @@ -146,14 +146,6 @@ void initialize_parameter_handles(ParameterHandles ¶meter_handles) parameter_handles.battery_a_per_v = param_find("BAT_A_PER_V"); parameter_handles.battery_source = param_find("BAT_SOURCE"); - /* rotations */ - parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); - - /* rotation offsets */ - parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); - parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); - parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); - /* Barometer QNH */ parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); @@ -425,12 +417,6 @@ int update_parameters(const ParameterHandles ¶meter_handles, Parameters &par param_get(parameter_handles.battery_source, &(parameters.battery_source)); - param_get(parameter_handles.board_rotation, &(parameters.board_rotation)); - - param_get(parameter_handles.board_offset[0], &(parameters.board_offset[0])); - param_get(parameter_handles.board_offset[1], &(parameters.board_offset[1])); - param_get(parameter_handles.board_offset[2], &(parameters.board_offset[2])); - param_get(parameter_handles.baro_qnh, &(parameters.baro_qnh)); param_get(parameter_handles.air_cmodel, ¶meters.air_cmodel); diff --git a/src/modules/sensors/parameters.h b/src/modules/sensors/parameters.h index b769c12eddba..2d78f2ec9a73 100644 --- a/src/modules/sensors/parameters.h +++ b/src/modules/sensors/parameters.h @@ -68,10 +68,6 @@ struct Parameters { float diff_pres_analog_scale; #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - int32_t board_rotation; - - float board_offset[3]; - int32_t rc_map_roll; int32_t rc_map_pitch; int32_t rc_map_yaw; @@ -224,10 +220,6 @@ struct ParameterHandles { param_t battery_a_per_v; param_t battery_source; - param_t board_rotation; - - param_t board_offset[3]; - param_t baro_qnh; param_t air_cmodel; diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h index 64a783682579..62c83f5a626c 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/src/modules/sensors/temperature_compensation.h @@ -250,10 +250,8 @@ class TemperatureCompensation */ bool calc_thermal_offsets_3D(const SensorCalData3D &coef, float measured_temp, float offset[]); - Parameters _parameters; - struct PerSensorData { PerSensorData() { diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index ba2fc8d333ad..6e6f4f9133c3 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -127,17 +127,11 @@ void VotedSensorsUpdate::deinit() void VotedSensorsUpdate::parameters_update() { - /* fine tune board offset */ - matrix::Dcmf board_rotation_offset = matrix::Eulerf( - M_DEG_TO_RAD_F * _parameters.board_offset[0], - M_DEG_TO_RAD_F * _parameters.board_offset[1], - M_DEG_TO_RAD_F * _parameters.board_offset[2]); - - _board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)_parameters.board_rotation); + _board_rotation.updateParams(); // initialze all mag rotations with the board rotation in case there is no calibration data available for (int topic_instance = 0; topic_instance < MAG_COUNT_MAX; ++topic_instance) { - _mag_rotation[topic_instance] = _board_rotation; + _mag_rotation[topic_instance] = _board_rotation.get(); } /* Load & apply the sensor calibrations. @@ -509,7 +503,7 @@ void VotedSensorsUpdate::parameters_update() } else { // Set internal magnetometers to use the board rotation - _mag_rotation[topic_instance] = _board_rotation; + _mag_rotation[topic_instance] = _board_rotation.get(); } if (failed) { @@ -603,7 +597,7 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) } // rotate corrected measurements from sensor to body frame - accel_data = _board_rotation * accel_data; + accel_data = _board_rotation.get() * accel_data; _last_sensor_data[uorb_index].accelerometer_m_s2[0] = accel_data(0); _last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1); @@ -708,7 +702,7 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) } // rotate corrected measurements from sensor to body frame - gyro_rate = _board_rotation * gyro_rate; + gyro_rate = _board_rotation.get() * gyro_rate; _last_sensor_data[uorb_index].gyro_rad[0] = gyro_rate(0); _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 590118decaeb..e124d6f9cb7d 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -41,6 +41,7 @@ #include "parameters.h" +#include #include #include #include @@ -251,7 +252,8 @@ class VotedSensorsUpdate uint64_t _last_accel_timestamp[ACCEL_COUNT_MAX]; /**< latest full timestamp */ - matrix::Dcmf _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + control::BlockBoardRotation _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ + matrix::Dcmf _mag_rotation[MAG_COUNT_MAX]; /**< rotation matrix for the orientation that the external mag0 is mounted */ const Parameters &_parameters; @@ -279,6 +281,4 @@ class VotedSensorsUpdate }; - - } /* namespace sensors */