From f062822b679f0f010d6c71db49f4372b7a6161cb Mon Sep 17 00:00:00 2001 From: astik <41.astiksrivastava@gmail.com> Date: Tue, 7 May 2024 15:12:15 +0530 Subject: [PATCH] adding INDI custom controller --- .../AC_CustomControl/AC_CustomControl.cpp | 8 + libraries/AC_CustomControl/AC_CustomControl.h | 3 +- .../AC_CustomControl_INDI.cpp | 394 ++++++++++++++++++ .../AC_CustomControl/AC_CustomControl_INDI.h | 89 ++++ libraries/AP_DDS/dds_xrce_profile.xml | 8 +- 5 files changed, 497 insertions(+), 5 deletions(-) create mode 100644 libraries/AC_CustomControl/AC_CustomControl_INDI.cpp create mode 100644 libraries/AC_CustomControl/AC_CustomControl_INDI.h diff --git a/libraries/AC_CustomControl/AC_CustomControl.cpp b/libraries/AC_CustomControl/AC_CustomControl.cpp index 903d854c127a92..b24ee4ab50de16 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.cpp +++ b/libraries/AC_CustomControl/AC_CustomControl.cpp @@ -7,6 +7,7 @@ #include "AC_CustomControl_Backend.h" // #include "AC_CustomControl_Empty.h" #include "AC_CustomControl_PID.h" +#include "AC_CustomControl_INDI.h" #include #include @@ -33,6 +34,9 @@ const AP_Param::GroupInfo AC_CustomControl::var_info[] = { // parameters for PID controller AP_SUBGROUPVARPTR(_backend, "2_", 7, AC_CustomControl, _backend_var_info[1]), + // parameters for INDI controller + AP_SUBGROUPVARPTR(_backend, "3_", 8, AC_CustomControl, _backend_var_info[2]), + AP_GROUPEND }; @@ -62,6 +66,10 @@ void AC_CustomControl::init(void) _backend = new AC_CustomControl_PID(*this, _ahrs, _att_control, _motors, _dt); _backend_var_info[get_type()] = AC_CustomControl_PID::var_info; break; + case CustomControlType::CONT_INDI: + _backend = new AC_CustomControl_INDI(*this, _ahrs, _att_control, _motors, _dt); + _backend_var_info[get_type()] = AC_CustomControl_INDI::var_info; + break; default: return; } diff --git a/libraries/AC_CustomControl/AC_CustomControl.h b/libraries/AC_CustomControl/AC_CustomControl.h index 9a12a347067da2..c40424f73e9c76 100644 --- a/libraries/AC_CustomControl/AC_CustomControl.h +++ b/libraries/AC_CustomControl/AC_CustomControl.h @@ -14,7 +14,7 @@ #include #ifndef CUSTOMCONTROL_MAX_TYPES -#define CUSTOMCONTROL_MAX_TYPES 2 +#define CUSTOMCONTROL_MAX_TYPES 3 #endif class AC_CustomControl_Backend; @@ -49,6 +49,7 @@ class AC_CustomControl { CONT_NONE = 0, CONT_EMPTY = 1, CONT_PID = 2, + CONT_INDI = 3, }; // controller that should be used enum class CustomControlOption { diff --git a/libraries/AC_CustomControl/AC_CustomControl_INDI.cpp b/libraries/AC_CustomControl/AC_CustomControl_INDI.cpp new file mode 100644 index 00000000000000..75a71b6877157d --- /dev/null +++ b/libraries/AC_CustomControl/AC_CustomControl_INDI.cpp @@ -0,0 +1,394 @@ +#include "AC_CustomControl_INDI.h" + +#if CUSTOMCONTROL_INDI_ENABLED + +#include +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +// table of user settable parameters +const AP_Param::GroupInfo AC_CustomControl_INDI::var_info[] = { + // @Param: ANG_RLL_P + // @DisplayName: Roll axis angle controller P gain + // @Description: Roll axis angle controller P gain. Converts the roll angle error into the angular acceleration command which is used in torque increment + // @Range: 3.000 12.000 + // @User: Standard + AP_SUBGROUPINFO(_p_angle_x, "ANG_RLL_", 1, AC_CustomControl_INDI, AC_P), + + // @DisplayName: Pitch axis angle controller P gain + // @Description: Pitch axis angle controller P gain. Converts the pitch angle error into the angular acceleration command which is used in torque increment + // @Range: 3.000 12.000 + // @User: Standard + AP_SUBGROUPINFO(_p_angle_y, "ANG_PIT_", 2, AC_CustomControl_INDI, AC_P), + + // @DisplayName: Yaw axis angle controller P gain + // @Description: Yaw axis angle controller P gain. Converts the yaw angle error into the angular acceleration command which is used in torque increment + // @Range: 3.000 12.000 + // @User: Standard + AP_SUBGROUPINFO(_p_angle_z, "ANG_YAW_", 3, AC_CustomControl_INDI, AC_P), + + // @Param: RAT_RLL_P + // @DisplayName: Roll axis rate controller P gain + // @Description: Roll axis rate controller P gain. Converts the body x axis angular velocity error into the angular acceleration command which is used in torque increment + // @Range: 0.01 0.50 + // @Increment: 0.005 + // @User: Standard + AP_SUBGROUPINFO(_p_ang_rate_x, "RAT_RLL_", 4, AC_CustomControl_INDI, AC_P), + + // @Param: RAT_PIT_P + // @DisplayName: Pitch axis rate controller P gain + // @Description: Pitch axis rate controller P gain. Converts the body y axis angular velocity error into the angular acceleration command which is used in torque increment + // @Range: 0.01 0.50 + // @Increment: 0.005 + // @User: Standard + AP_SUBGROUPINFO(_p_ang_rate_y, "RAT_PIT_", 5, AC_CustomControl_INDI, AC_P), + + // @Param: RAT_YAW_P + // @DisplayName: Yaw axis rate controller P gain + // @Description: Yaw axis rate controller P gain. Converts the body z axis angular velocity error into the angular acceleration command which is used in torque increment + // @Range: 0.01 0.50 + // @Increment: 0.005 + // @User: Standard + AP_SUBGROUPINFO(_p_ang_rate_z, "RAT_YAW_", 6, AC_CustomControl_INDI, AC_P), + + // @Param: _MOI_XY + // @DisplayName: Moment of inertia of vehicle in x-y axis in kg.m² + // @Description: Moment of inertia of vehicle in body frame x-y axis in kg.m². Vehicle assumed to be symmetrical about its z axis. + // @Units: kg.m² + // @Range: 0.001 1 + // @User: Standard + AP_GROUPINFO("MOI_XY", 7, AC_CustomControl_INDI, _moment_inertia_xy_kgm2, 3.544e-3f), + + // @Param: _MOI_Z + // @DisplayName: Moment of inertia of vehicle in z axis in kg.m² + // @Description: Moment of inertia of vehicle in body frame z axis in kg.m². + // @Units: kg.m² + // @Range: 0.001 1 + // @User: Standard + AP_GROUPINFO("MOI_Z", 8, AC_CustomControl_INDI, _moment_inertia_z_kgm2, 6.935e-3f), + + // @Param: _ARM_LEN + // @DisplayName: Distance to motor in m + // @Description: Distance between center of mass and motor or arm length + // @Units: m + // @Range: 0.01 1 + // @User: Standard + AP_GROUPINFO("ARM_LEN", 9, AC_CustomControl_INDI, _arm_length_m, 0.125f), + + // @Param: _THR_COEF + // @DisplayName: Thrust coefficent in N/(rad/s)² + // @Description: Thrust coefficent defines relation between square of motor speed and force generated by motor. This value can be obtained by static thrust test. + // @Units: N/(rad/s)² + // @Range: 0.000001 0.0001 + // @User: Standard + AP_GROUPINFO("THR_COTHR_COEFEF", 10, AC_CustomControl_INDI, _thrust_coefficient, 1.489e-6f), + + // @Param: _TRQ_COEF + // @DisplayName: Torque coefficent in Nm/(rad/s)² + // @Description: Torque coefficent defines relation between square of motor speed and torque generated by motor.This value can be obtained by static thrust test. + // @Units: N/(rad/s)² + // @Range: 0.000001 0.0001 + // @User: Standard + AP_GROUPINFO("TRQ_COEF", 11, AC_CustomControl_INDI, _torque_coefficient, 1.523e-8f), + + // @Param: _THR2RTRSPD + // @DisplayName: Throttle command to motor speed coefficient + // @Description: Defines relation from scaled throttle command(between 0-1) to motor speed in rad/s. Relation between throttle command and motor speed assumed to be linear. + // @Range: 100 4000 + // @User: Standard + AP_GROUPINFO("THR2RTRSPD", 12, AC_CustomControl_INDI, _throttle2motor_speed, 3251.0f), + + // @Param: _TRQEST_FILT + // @DisplayName: Torque estimate cutoff frequency in Hz + // @Description: First order low pass filter applied to the torque estimation + // @Units: Hz + // @Range: 20 100 + // @User: Standard + AP_GROUPINFO("TRQEST_FILT", 13, AC_CustomControl_INDI, _torque_est_filter_cutoff, 30.0f), + + // @Param: _YAW_FILT + // @DisplayName: Toruqe command z axis filter + // @Description: First order low pass filter applied to the z axis of toruqe command. This is compansate for the mechanical D-term of yaw axis. + // @Units: Hz + // @Range: 2 20 + // @User: Standard + AP_GROUPINFO("YAW_FILT", 14, AC_CustomControl_INDI, _yaw_rate_filter_cutoff, 3.0f), + + AP_GROUPEND +}; + +// initialize in the constructor +AC_CustomControl_INDI::AC_CustomControl_INDI(AC_CustomControl &frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt) : + AC_CustomControl_Backend(frontend, ahrs, att_control, motors, dt), + _p_angle_x(7.5), + _p_ang_rate_x(30), + _p_angle_y(7.5), + _p_ang_rate_y(30), + _p_angle_z(6), + _p_ang_rate_z(24) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +// update controller +// return roll, pitch, yaw controller output +Vector3f AC_CustomControl_INDI::update(void) +{ + // reset controller based on spool state + switch (_motors->get_spool_state()) { + case AP_Motors::SpoolState::SHUT_DOWN: + case AP_Motors::SpoolState::GROUND_IDLE: + // We are still at the ground. Reset custom controller to avoid + // build up, ex: integrator + reset(); + break; + + case AP_Motors::SpoolState::THROTTLE_UNLIMITED: + case AP_Motors::SpoolState::SPOOLING_UP: + case AP_Motors::SpoolState::SPOOLING_DOWN: + // we are off the ground + break; + } + + // run custom controller after here + Quaternion attitude_body, attitude_target; + _ahrs->get_quat_body_to_ned(attitude_body); + attitude_target = _att_control->get_attitude_target_quat(); + Vector3f gyro_latest = _ahrs->get_gyro_latest(); + calculate_torque_thrust_est(); + + // recalculate ang vel feedforward from attitude target model + // rotation from the target frame to the body frame + Quaternion rotation_target_to_body = attitude_body.inverse() * attitude_target; + // target angle velocity vector in the body frame + Vector3f ang_vel_body_feedforward = rotation_target_to_body * _att_control->get_attitude_target_ang_vel(); + + Vector3f ang_vel_target = run_attitude_controller(attitude_target, attitude_body); + run_angvel_controller(ang_vel_target + ang_vel_body_feedforward, gyro_latest, Vector3f(0.0f, 0.0f, 0.0f)); + + // return what arducopter main controller outputted + return Vector3f(_torque_cmd_scaled.x, _torque_cmd_scaled.y, _torque_cmd_scaled.z); +} + +// reset controller to avoid build up on the ground +// or to provide bumpless transfer from arducopter main controller +void AC_CustomControl_INDI::reset(void) +{ + _ang_acc_filter.reset(); + _torque_est_filter.reset(); + _yaw_rate_filter.reset(); +} + +/* + Calculate attitude error using two ordered rotation. First calculate error between + z axis of current and target rotation matrix. Second correct for the yaw error. + Same as thrust_heading_rotation_angles function in AC_AttitudeControl.cpp +*/ +Vector3f AC_CustomControl_INDI::calculate_att_error(Quaternion target, Quaternion measurment) +{ + Quaternion att_cur_quat; + + _att_target_quat = target; + att_cur_quat = measurment; + + Vector3f e_cur_z, e_des_z; + + Matrix3f att_cur_matrix; + att_cur_quat.rotation_matrix(att_cur_matrix); + + e_cur_z = att_cur_matrix.colz(); + + Matrix3f att_target_rot_matrix; + _att_target_quat.rotation_matrix(att_target_rot_matrix); + e_des_z = att_target_rot_matrix.colz(); + + // the cross product of the desired and target thrust vector defines the rotation vector + Vector3f thrust_correction_vec_cross = e_cur_z % e_des_z; + + // the dot product is used to calculate the angle between the target and desired thrust vectors + float thrust_correction_vec_dot = acosf(constrain_float(e_cur_z * e_des_z, -1.0f, 1.0f)); + + // Normalize the thrust rotation vector + float thrust_correction_vec_length = thrust_correction_vec_cross.length(); + if (is_zero(thrust_correction_vec_length) || is_zero(thrust_correction_vec_dot)) { + thrust_correction_vec_cross = Vector3f(0, 0, 1); + thrust_correction_vec_dot = 0.0f; + } else { + thrust_correction_vec_cross /= thrust_correction_vec_length; + } + + Quaternion thrust_vec_correction_quat; + thrust_vec_correction_quat.from_axis_angle(thrust_correction_vec_cross, thrust_correction_vec_dot); + + // Rotate thrust_vec_correction_quat to the body frame + thrust_vec_correction_quat = att_cur_quat.inverse() * thrust_vec_correction_quat * att_cur_quat; + + // calculate the remaining rotation required after thrust vector is rotated transformed to the body frame + Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse() * att_cur_quat.inverse() * _att_target_quat; + + // calculate the angle error in x and y. + Vector3f rotation, error_att; + thrust_vec_correction_quat.to_axis_angle(rotation); + error_att.x = rotation.x; + error_att.y = rotation.y; + + // calculate the angle error in z (x and y should be zero here). + yaw_vec_correction_quat.to_axis_angle(rotation); + error_att.z = rotation.z; + + return error_att; +} + +// run attitude controller +Vector3f AC_CustomControl_INDI::run_attitude_controller(Quaternion target, Quaternion measurment) +{ + Vector3f error_att; + error_att = calculate_att_error(target, measurment); + + return Vector3f(_p_angle_x.get_p(error_att.x), _p_angle_y.get_p(error_att.y), _p_angle_z.get_p(error_att.z)); +} + +// run angular velocity controller +void AC_CustomControl_INDI::run_angvel_controller(Vector3f target, Vector3f measurment, Vector3f ang_acc_desired) +{ + Vector3f error_ang_vel; + + _ang_vel_target_radps = target; + error_ang_vel = _ang_vel_target_radps - measurment; + + _ang_acc_target_radpss.x = _p_ang_rate_x.get_p(error_ang_vel.x); + _ang_acc_target_radpss.y = _p_ang_rate_y.get_p(error_ang_vel.y); + _ang_acc_target_radpss.z = _p_ang_rate_z.get_p(error_ang_vel.z); + + _ang_acc_target_radpss += ang_acc_desired; + + indi_angular_accel(); + scale_torque_cmd(); +} + +// torque increment based on angular acceleration difference +// conceptually this function behaves like an integrator +void AC_CustomControl_INDI::indi_angular_accel(void) +{ + // calculate angular acceleration + Vector3f gyro_latest = _ahrs->get_gyro_latest(); + Vector3f ang_acc_flt = _ang_acc_filter.apply((gyro_latest - _gyro_prev)/AP::scheduler().get_loop_period_s()); + _gyro_prev = gyro_latest; + + Matrix3f moment_of_inertia_xyz ( + _moment_inertia_xy_kgm2, 0.0f, 0.0f, + 0.0f, _moment_inertia_xy_kgm2, 0.0f, + 0.0f, 0.0f, _moment_inertia_z_kgm2 + ); + + _torque_cmd_body_Nm = _torque_est_body_Nm + moment_of_inertia_xyz * (_ang_acc_target_radpss - ang_acc_flt); + + // mechanical yaw is not considered in arducopter current control allocation + // filter body z axis torque command to compansate for the mechanicaly yaw + // this render rotor inertia information(_motor_moment_inertia_kgm2) unneccessary + _yaw_rate_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), _yaw_rate_filter_cutoff); + _yaw_rate_filter.apply(_torque_cmd_body_Nm.z); + _torque_cmd_body_Nm.z = _yaw_rate_filter.get(); +} + +// TODO: apply scaling to the torque command to correct scaling due to the arducopter control allocation +void AC_CustomControl_INDI::scale_torque_cmd(void) +{ + float xy_scale = 2.0f / (4.0f * sq(_throttle2motor_speed) * _thrust_coefficient * _arm_length_m * HALF_SQRT_2); + float z_scale = 2.0f / (4.0f * sq(_throttle2motor_speed) * _torque_coefficient ); + + z_scale *= 0.1f; + + _torque_cmd_scaled.x = _torque_cmd_body_Nm.x * xy_scale; + _torque_cmd_scaled.y = _torque_cmd_body_Nm.y * xy_scale; + _torque_cmd_scaled.z = _torque_cmd_body_Nm.z * z_scale; +} + +// get current rotation speed of each motor in rad/s +// TODO: SITL rpm per motor is not implemented +void AC_CustomControl_INDI::get_motor_speed(void) +{ + float motor_speed_hz[4] = {0.0f, 0.0f, 0.0f, 0.0f}; // somehow firs element of this variable assigned nan value at start + +#ifdef HAL_WITH_ESC_TELEM + // get motor rotation speed telemetry + float esc_freq_hz[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + AP::esc_telem().get_motor_frequencies_hz(4, esc_freq_hz); + + // map esc servo order to quad x frame order + float temp[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + for (uint8_t i=0; i < 4; i++) { + uint8_t chan; + SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(i); + SRV_Channels::find_channel(function, chan); + temp[i] = esc_freq_hz[chan]; + } + + // change quad x frame order so that motor order start from top right and increases counter clockwise + // TODO: add support for different frame type and configuration + motor_speed_hz[0] = temp[0]; + motor_speed_hz[1] = temp[3]; + motor_speed_hz[2] = temp[1]; + motor_speed_hz[3] = temp[2]; +#endif +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + float motor_speed_rpm[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + // AP::rpm()->get_rpms(0, motor_speed_rpm); + for (int i=0; i<4; i++) { + motor_speed_hz[i] = motor_speed_rpm[i] / 60.0f; + } +#endif + for (uint8_t i=0; i < 4; i++) { + _motor_speed_meas_radps[i] = motor_speed_hz[i] * M_2PI; + } +} + +/* + Calculate estimate of torque and thrust produced by motor using + the measured motor speed. + Motor speed derivative is not taken into account since it is not considered + in control allocation. + TODO: might require motor reordering in real hardware +*/ +void AC_CustomControl_INDI::calculate_torque_thrust_est(void) +{ + // read last rotation speed of motors + get_motor_speed(); + + // pre calculate some common constant + float l = _arm_length_m * HALF_SQRT_2; + float k = _torque_coefficient / _thrust_coefficient; + + // control allocation matrix for quad-x frame type + // motor order start from top right and increase counter clockwise + float control_alloc_G1[4][4] = { + { -l, -l, l, l}, + { l, -l, -l, l}, + { k, -k, k, -k}, + {-1.0f, -1.0f, -1.0f, -1.0f}}; + + // calculate square of measured motor speed + float motor_speed_meas_sq[4]; + for (uint8_t i=0; i < 4; i++) { + motor_speed_meas_sq[i] = sq(_motor_speed_meas_radps[i]); + } + + float cmd[4] = {0.0f, 0.0f, 0.0f, 0.0f}; + // perform multiplication + // can whole multiplication be performed in single step? + for (uint8_t i=0; i < 4; i++) { + for (uint8_t j=0; j < 4; j++) { + cmd[i] += control_alloc_G1[i][j] * motor_speed_meas_sq[j]; + } + cmd[i] *= _thrust_coefficient; + } + + // filter torque estimate + _torque_est_filter.set_cutoff_frequency(AP::scheduler().get_loop_rate_hz(), _torque_est_filter_cutoff); + _torque_est_body_Nm = _torque_est_filter.apply(Vector3f(cmd[0], cmd[1], cmd[2])); +} + +#endif diff --git a/libraries/AC_CustomControl/AC_CustomControl_INDI.h b/libraries/AC_CustomControl/AC_CustomControl_INDI.h new file mode 100644 index 00000000000000..2d8e46207cf17a --- /dev/null +++ b/libraries/AC_CustomControl/AC_CustomControl_INDI.h @@ -0,0 +1,89 @@ +#pragma once + +#include +#include +#include // P library +#include +#include +#include + +#include "AC_CustomControl_Backend.h" + +#ifndef CUSTOMCONTROL_INDI_ENABLED + #define CUSTOMCONTROL_INDI_ENABLED AP_CUSTOMCONTROL_ENABLED +#endif + +#if CUSTOMCONTROL_INDI_ENABLED + +class AC_CustomControl_INDI : public AC_CustomControl_Backend { +public: + AC_CustomControl_INDI(AC_CustomControl &frontend, AP_AHRS_View*& ahrs, AC_AttitudeControl*& att_control, AP_MotorsMulticopter*& motors, float dt); + + + Vector3f update(void) override; + void reset(void) override; + + // run attitude controller + Vector3f run_attitude_controller(Quaternion target, Quaternion meas); + + // run angular velocity controller + void run_angvel_controller(Vector3f target, Vector3f meas, Vector3f ang_acc_desired); + + // calculate estimated torque and thrust values using current motor speed + void calculate_torque_thrust_est(void); + + // calculate attiude error + // same as thrust_heading_rotation_angles function in AC_AttitudeControl.cpp + Vector3f calculate_att_error(Quaternion target, Quaternion meas); + + // add delta angular accleration to current torque to obtain torque command + void indi_angular_accel(void); + + // set mixer input from the torque and thrust command + void scale_torque_cmd(void); + + // assign measured motor speed to _motor_speed_meas_radps + void get_motor_speed(void); + + // user settable parameters + static const struct AP_Param::GroupInfo var_info[]; + +protected: + // declare parameters here + // attitude and angular velocity P controller + AC_P _p_angle_x; + AC_P _p_angle_y; + AC_P _p_angle_z; + AC_P _p_ang_rate_x; + AC_P _p_ang_rate_y; + AC_P _p_ang_rate_z; + + AP_Float _moment_inertia_xy_kgm2; // moment of inertia of xy axis in kg.m² + AP_Float _moment_inertia_z_kgm2; // moment of inertia of z axis in kg.m² + AP_Float _arm_length_m; // distance to motors in m + AP_Float _thrust_coefficient; // thrust coefficient in N/(rad/s)² + AP_Float _torque_coefficient; // torque coefficient in Nm/(rad/s)² + AP_Float _throttle2motor_speed; // coefficient between scaled throttle(between 0-1) command and motor speed in rad/s + AP_Float _ang_acc_filter_cutoff; // angular acceleration filter cutoff frequency in Hz + AP_Float _torque_est_filter_cutoff; // rpm filter cutoff frequency for torque estimation in Hz + AP_Float _yaw_rate_filter_cutoff; // torque command filter cutoff frequency in Hz + + Quaternion _att_target_quat; // target attitude defined using desired yaw and specific thrust command + float _att_target_euler_angle_yaw_rad;// target attitude used only for target heading in rad + Vector3f _ang_vel_target_radps; // angular velocity target in body frame in rad/s + Vector3f _ang_acc_target_radpss; // angular acceleration target in body frame in rad/s² + Vector3f _ang_acc_desired_radpss; // feedforwarded angular acceleration in NED frame rad/s² + Vector3f _torque_cmd_body_Nm; // torque command in body frame in Nm + Vector3f _torque_cmd_scaled ; // scaled torque command between -1 ~ +1 + Vector3f _torque_est_body_Nm; // estimated torque from motor speed in body frame in Nm + + float _motor_speed_meas_radps[4]; // current motor speed in rad/s + + + LowPassFilterVector3f _ang_acc_filter; + LowPassFilterVector3f _torque_est_filter; + LowPassFilterFloat _yaw_rate_filter; + Vector3f _gyro_prev; +}; + +#endif diff --git a/libraries/AP_DDS/dds_xrce_profile.xml b/libraries/AP_DDS/dds_xrce_profile.xml index b978658efcf749..24afe03baecb51 100644 --- a/libraries/AP_DDS/dds_xrce_profile.xml +++ b/libraries/AP_DDS/dds_xrce_profile.xml @@ -149,10 +149,10 @@ PREALLOCATED_WITH_REALLOC - BEST_EFFORT + RELIABLE - VOLATILE + TRANSIENT_LOCAL @@ -177,10 +177,10 @@ PREALLOCATED_WITH_REALLOC - BEST_EFFORT + RELIABLE - VOLATILE + TRANSIENT_LOCAL