fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 12:39:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Handle offboard

Browse Source 
Add fw indi lib

F

WIP Unit tests

Tune rate controllers

Publish vehicle attitude setpoints

Separate rate loop

F

F

Publish attitude setpoints

Minor cleanup
This commit is contained in:
JaeyoungLim 2025-07-08 13:08:31 +09:00
parent 3e63aec18d
commit 344babfe5a
9 changed files with 330 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/lib/CMakeLists.txt
View File

@ -62,6 +62,7 @@ add_subdirectory(motion_planning EXCLUDE_FROM_ALL)
add_subdirectory(npfg EXCLUDE_FROM_ALL)
add_subdirectory(perf EXCLUDE_FROM_ALL)
add_subdirectory(fw_performance_model EXCLUDE_FROM_ALL)
add_subdirectory(fw_indi_control EXCLUDE_FROM_ALL)
add_subdirectory(pid EXCLUDE_FROM_ALL)
add_subdirectory(pid_design EXCLUDE_FROM_ALL)
add_subdirectory(pure_pursuit EXCLUDE_FROM_ALL)

42
src/lib/fw_indi_control/CMakeLists.txt Normal file
View File

@ -0,0 +1,42 @@
############################################################################
#
# Copyright (c) 2019 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.
#
############################################################################
px4_add_library(FixedWingIndiControl
fw_indi_control.cpp
fw_indi_control.hpp
)
target_compile_options(FixedWingIndiControl PRIVATE ${MAX_CUSTOM_OPT_LEVEL})
target_include_directories(FixedWingIndiControl PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(FixedWingIndiControl PRIVATE mathlib)
px4_add_unit_gtest(SRC fw_indi_control_test.cpp LINKLIBS FixedWingIndiControl)

89
src/lib/fw_indi_control/fw_indi_control.cpp Normal file
View File

@ -0,0 +1,89 @@
/****************************************************************************
*
* Copyright (c) 2019-2023 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 rate_control.cpp
*/
#include "fw_indi_control.hpp"
#include <px4_platform_common/defines.h>
using namespace matrix;
Quatf
FixedWingIndiControl::get_flat_attitude(Vector3f vel, Vector3f f)
{
const Vector3f vel_air = vel - wind_estimate_;
// compute force component projected onto lift axis
const Vector3f vel_normalized = vel_air.normalized();
Vector3f f_drag = (f * vel_normalized) * vel_normalized;
Vector3f f_lift = f - f_drag;
const Vector3f lift_normalized = f_lift.normalized();
const Vector3f wing_normalized = -vel_normalized.cross(lift_normalized);
// compute rotation matrix between ENU and FRD frame
Dcmf R_bi;
R_bi(0, 0) = vel_normalized(0);
R_bi(0, 1) = vel_normalized(1);
R_bi(0, 2) = vel_normalized(2);
R_bi(1, 0) = wing_normalized(0);
R_bi(1, 1) = wing_normalized(1);
R_bi(1, 2) = wing_normalized(2);
R_bi(2, 0) = lift_normalized(0);
R_bi(2, 1) = lift_normalized(1);
R_bi(2, 2) = lift_normalized(2);
R_bi.renormalize();
float rho_corrected;
if (_cal_airspeed >= _stall_speed) {
rho_corrected = _rho * powf(_cal_airspeed / _true_airspeed, 2);
} else {
rho_corrected = _rho;
}
// compute required AoA
Vector3f f_phi = R_bi * f_lift;
float AoA = ((2.f * f_phi(2)) / (rho_corrected * _area * (vel_air * vel_air) + 0.001f) - _C_L0) / _C_L1 - _aoa_offset;
// compute final rotation matrix
Eulerf e(0.f, AoA, 0.f);
Dcmf R_pitch(e);
Dcmf Rotation(R_pitch * R_bi);
Quatf q(Rotation);
return q;
}

76
src/lib/fw_indi_control/fw_indi_control.hpp Normal file
View File

@ -0,0 +1,76 @@
/****************************************************************************
*
* Copyright (c) 2019-2023 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 rate_control.hpp
*
* PID 3 axis angular rate / angular velocity control.
*/
#pragma once
#include <matrix/matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <uORB/topics/rate_ctrl_status.h>
class FixedWingIndiControl
{
public:
FixedWingIndiControl() = default;
~FixedWingIndiControl() = default;
void set_coefficients(float area, float aoa_offset, float C_L0, float C_L1)
{
_area = area;
_aoa_offset = aoa_offset;
_C_L0 = C_L0;
_C_L1 = C_L1;
}
matrix::Quatf get_flat_attitude(matrix::Vector3f vel, matrix::Vector3f f);
private:
// Airdata
matrix::Vector3f wind_estimate_;
float _cal_airspeed;
float _stall_speed;
float _true_airspeed;
float _rho{1.0f};
// Model parameters
float _area{0.4f};
float _C_L0{0.3f};
float _C_L1{2.354f};
float _aoa_offset{0.07f};
};

47
src/lib/fw_indi_control/fw_indi_control_test.cpp Normal file
View File

@ -0,0 +1,47 @@
/****************************************************************************
*
* Copyright (C) 2019 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 <gtest/gtest.h>
#include <lib/fw_indi_control/fw_indi_control.hpp>
using namespace matrix;
TEST(FixedWingIndiControlTest, AllZeroCase)
{
FixedWingIndiControl indi_control;
Vector3f forward_vel{10.0, 0.0, 0.0};
Vector3f f_command{0.0, 0.0, 10.0};
indi_control.set_coefficients(1.0, 0.0, 0.0, 1.0);
Quatf attitude = indi_control.get_flat_attitude(forward_vel, f_command);
EXPECT_EQ(attitude, Quatf());
}

2
src/modules/fw_indi_pos_control/CMakeLists.txt
View File

@ -39,6 +39,6 @@ px4_add_module(
FixedwingIndiPosControl.hpp
DEPENDS
px4_work_queue
RateControl
FixedWingIndiControl
SlewRate
)

84
src/modules/fw_indi_pos_control/FixedwingIndiPosControl.cpp
View File

@ -153,8 +153,18 @@ FixedwingIndiPosControl::airspeed_poll()
_airspeed_valid = airspeed_valid;
}
void
FixedwingIndiPosControl::vehicle_status_poll()
{
vehicle_status_s vehicle_status;
if (_vehicle_status_sub.update(&vehicle_status)) {
_vehicle_status = vehicle_status;
}
}
Quatf
FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f)
FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f, float rho_corrected)
{
const Vector3f vel_air = vel - wind_estimate_;
@ -180,15 +190,6 @@ FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f)
R_bi(2, 2) = lift_normalized(2);
R_bi.renormalize();
float rho_corrected;
if (_cal_airspeed >= _stall_speed) {
rho_corrected = _rho * powf(_cal_airspeed / _true_airspeed, 2);
} else {
rho_corrected = _rho;
}
// compute required AoA
Vector3f f_phi = R_bi * f_lift;
float AoA = ((2.f * f_phi(2)) / (rho_corrected * _area * (vel_air * vel_air) + 0.001f) - _C_L0) / _C_L1 - _aoa_offset;
@ -201,19 +202,18 @@ FixedwingIndiPosControl::get_flat_attitude(Vector3f vel, Vector3f f)
return q;
}
Vector3f
Quatf
FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3f acc_ref)
{
Dcmf R_ib(vehicle_attitude_);
Dcmf R_bi(R_ib.transpose());
// apply LP filter to acceleration & velocity
Vector3f acc_filtered;
// Poll acceleration
PX4_INFO("[PX4PosiNDI] - _acc: %f, %f, %f", double(_acc(0)), double(_acc(1)), double(_acc(2)));
acc_filtered(0) = _lp_filter_accel[0].apply(_acc(0));
acc_filtered(1) = _lp_filter_accel[1].apply(_acc(1));
acc_filtered(2) = _lp_filter_accel[2].apply(_acc(2));
acc_filtered_(0) = _lp_filter_accel[0].apply(_acc(0));
acc_filtered_(1) = _lp_filter_accel[1].apply(_acc(1));
acc_filtered_(2) = _lp_filter_accel[2].apply(_acc(2));
// =========================================
// apply PD control law on the body position
@ -226,8 +226,7 @@ FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3
// ==================================
// compute expected aerodynamic force
// ==================================
Vector3f _wind_estimate{0.0, 0.0, 0.0}; ///TODO: Plug in wind estimate
Vector3f airvel_body = R_bi * (vehicle_velocity_ - _wind_estimate);
Vector3f airvel_body = R_bi * (vehicle_velocity_ - wind_estimate_);
float angle_of_attack = atan2f(airvel_body(2), airvel_body(0)) + _aoa_offset;
@ -258,7 +257,7 @@ FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3
// ================================
// get force command in world frame
// ================================
Vector3f f_command = _mass * (acc_command - acc_filtered) + f_current_filtered;
Vector3f f_command = _mass * (acc_command - acc_filtered_) + f_current_filtered;
// ============================================================================================================
// apply some filtering to the force command. This introduces some time delay,
@ -300,10 +299,19 @@ FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3
// ==========================================================================
// get required attitude (assuming we can fly the target velocity), and error
// ==========================================================================
Dcmf R_ref(get_flat_attitude(vel_ref, f_command));
Quatf ref_attitude(R_ref);
Quatf ref_attitude = get_flat_attitude(vel_ref, f_command, rho_corrected);
return ref_attitude;
}
Vector3f FixedwingIndiPosControl::controlAttitude(Quatf ref_attitude)
{
PX4_INFO("[PX4PosiNDI] - R_ref: %f, %f, %f, %f", double(ref_attitude(0)), double(ref_attitude(1)),
double(ref_attitude(2)), double(ref_attitude(3)));
Dcmf R_ref(ref_attitude);
Dcmf R_ib(vehicle_attitude_);
Dcmf R_bi(R_ib.transpose());
// get attitude error
Dcmf R_ref_true(R_ref.transpose()*R_ib);
// get required rotation vector (in body frame)
@ -340,9 +348,9 @@ FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3
// ==============================================================
// overwrite rudder rot_acc_command with turn coordination values
// ==============================================================
Vector3f vel_air = vehicle_velocity_ - _wind_estimate;
Vector3f vel_air = vehicle_velocity_ - wind_estimate_;
Vector3f vel_normalized = vel_air.normalized();
Vector3f acc_normalized = acc_filtered.normalized();
Vector3f acc_normalized = acc_filtered_.normalized();
PX4_INFO("[PX4PosiNDI] - acc_normalized: %f, %f, %f", double(acc_normalized(0)), double(acc_normalized(1)),
double(acc_normalized(2)));
@ -350,7 +358,7 @@ FixedwingIndiPosControl::computeIndi(Vector3f pos_ref, Vector3f vel_ref, Vector3
Vector3f omega_turn_ref_normalized = vel_normalized.cross(acc_normalized);
Vector3f omega_turn_ref;
// constuct acc perpendicular to flight path
Vector3f acc_perp = acc_filtered - (acc_filtered * vel_normalized) * vel_normalized;
Vector3f acc_perp = acc_filtered_ - (acc_filtered_ * vel_normalized) * vel_normalized;
if (_airspeed_valid && _cal_airspeed > _stall_speed) {
omega_turn_ref = sqrtf(acc_perp * acc_perp) / (_true_airspeed) * R_bi * omega_turn_ref_normalized.normalized();
@ -397,6 +405,7 @@ void FixedwingIndiPosControl::Run()
parameters_update();
}
vehicle_status_poll();
// Poll local position
vehicle_attitude_poll();
@ -466,18 +475,29 @@ void FixedwingIndiPosControl::Run()
vel_ref_ = 15.0f * progress_vector;
// Compute bodyrate commands
Vector3f rate_command = computeIndi(pos_ref_, vel_ref_, acc_ref_);
Quatf ref_attitude = computeIndi(pos_ref_, vel_ref_, acc_ref_);
// TODO: Place holder
// ref_attitude = Quatf(1.0, 0.0, 0.0, 0.0);
Vector3f rate_command = controlAttitude(ref_attitude);
PX4_INFO("[PX4PosiNDI] angular_rate_command: %f, %f, %f", double(rate_command(0)), double(rate_command(1)),
double(rate_command(2)));
vehicle_rates_setpoint_s rates_sp;
rates_sp.thrust_body[0] = 0.5; ///TODO: Add some throttle
rates_sp.thrust_body[1] = 0.0;
rates_sp.thrust_body[2] = 0.0;
rates_sp.roll = rate_command(0);
rates_sp.pitch = rate_command(1);
rates_sp.yaw = rate_command(2);
_rate_sp_pub.publish(rates_sp);
if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD) {
vehicle_attitude_setpoint_s attitude_sp;
attitude_sp.timestamp = hrt_absolute_time();
ref_attitude.copyTo(attitude_sp.q_d);
_attitude_sp_pub.publish(attitude_sp);
vehicle_rates_setpoint_s rates_sp;
rates_sp.thrust_body[0] = 0.7; ///TODO: Add some throttle
rates_sp.thrust_body[1] = 0.0;
rates_sp.thrust_body[2] = 0.0;
rates_sp.roll = rate_command(0);
rates_sp.pitch = rate_command(1);
rates_sp.yaw = rate_command(2);
rates_sp.timestamp = hrt_absolute_time();
_rate_sp_pub.publish(rates_sp);
}
// =================================
// publish offboard control commands

37
src/modules/fw_indi_pos_control/FixedwingIndiPosControl.hpp
View File

@ -64,6 +64,7 @@
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@ -101,11 +102,23 @@ public:
private:
void Run() override;
/**
* Update our local parameter cache.
*/
int parameters_update();
matrix::Quatf get_flat_attitude(matrix::Vector3f vel, matrix::Vector3f f);
void airspeed_poll();
void vehicle_acceleration_poll();
void vehicle_local_position_poll();
void vehicle_attitude_poll();
void vehicle_status_poll();
matrix::Vector3f computeIndi(matrix::Vector3f pos_ref, matrix::Vector3f vel_ref, matrix::Vector3f acc_ref);
matrix::Quatf get_flat_attitude(matrix::Vector3f vel, matrix::Vector3f f, float rho_corrected);
matrix::Quatf computeIndi(matrix::Vector3f pos_ref, matrix::Vector3f vel_ref, matrix::Vector3f acc_ref);
matrix::Vector3f controlAttitude(matrix::Quatf ref_attitude);
uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
@ -126,6 +139,7 @@ private:
uORB::Publication<vehicle_rates_setpoint_s> _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_sp_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Publication<offboard_control_mode_s> _offboard_control_mode_pub{ORB_ID(offboard_control_mode)};
manual_control_setpoint_s _manual_control_setpoint{};
@ -158,11 +172,11 @@ private:
matrix::Vector3f vehicle_position_;
matrix::Vector3f vehicle_velocity_;
matrix::Vector3f _acc;
matrix::Vector3f wind_estimate_;
matrix::Vector3f wind_estimate_{0.0, 0.0, 0.0};
matrix::Quatf vehicle_attitude_;
float _cal_airspeed{0.0f};
float _rho{1.0};
float _rho{1.225f};
float _true_airspeed{1.0};
@ -172,6 +186,7 @@ private:
matrix::Vector3f _f_command {};
matrix::Vector3f _m_command {};
matrix::Vector3f acc_filtered_{};
matrix::Matrix3f _K_x;
matrix::Matrix3f _K_v;
@ -187,7 +202,7 @@ private:
float _C_D2{1.984};
float _area{0.4};
float _stall_speed{1.0};
float _mass{1.0};
float _mass{1.5};
bool _landed{true};
bool _switch_saturation{true};
@ -204,16 +219,4 @@ private:
(ParamFloat<px4::params::FW_INDI_K_ROLL>) _param_rot_k_roll,
(ParamFloat<px4::params::FW_INDI_K_PITCH>) _param_rot_k_pitch
)
RateControl _rate_control; ///< class for rate control calculations
/**
* Update our local parameter cache.
*/
int parameters_update();
void airspeed_poll();
void vehicle_acceleration_poll();
void vehicle_local_position_poll();
void vehicle_attitude_poll();
};

4
src/modules/fw_indi_pos_control/fw_indi_pos_control_params.c
View File

@ -139,7 +139,7 @@ PARAM_DEFINE_FLOAT(FW_INDI_KV_Z, 0.1f);
* @increment 0.1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(FW_INDI_K_ROLL, 30.0f);
PARAM_DEFINE_FLOAT(FW_INDI_K_ROLL, 7.0f);
/**
* control gain of attitude PD-controller (body roll-direction)
@ -151,4 +151,4 @@ PARAM_DEFINE_FLOAT(FW_INDI_K_ROLL, 30.0f);
* @increment 0.1
* @group FW DYN SOAR Control
*/
PARAM_DEFINE_FLOAT(FW_INDI_K_PITCH, 10.0f);
PARAM_DEFINE_FLOAT(FW_INDI_K_PITCH, 2.0f);