我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

 

 

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。确实是这样的,还是得真正搞清楚,不然弄不成。真正把他这个工程啃透。我先不说语法上,先逻辑上啃透。

他觉得二次开发简单那是因为他对整个工程有了透彻的了解了。

一个刚来的人听他讲那二次开发还是会乱的。

 

 

acfly的基本逻辑是,你先把传感器注册上,然后它会有函数自动判断你传感器的数据质量如何,并选择用什么传感器。

 

 

 

 

还有问题默认注册的位置传感器的数据单位是多少。

 

 

传感器的处理逻辑都在sensor.c里面,包括像经纬度投影到平面。那几种传感器的数据怎么处理的看这个文件就够了。

 

从contrlsystem.hpp这个文件就可以看书ctrl_attitude.cpp和ctrL_position.cpp这两个文件里面有哪些函数。这也是那个华清老师说的,高手都是先看头文件。不然你直接看那两个C++文件几千行,很乱的。ctrL_position.cpp是没有单独的头文件的。

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

ACfly的用户手册里面也说了单位是cm

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

 

 

找到那个进行传感器数据质量判断,还有根据传感器优先级进行传感器选择的函数。

我们这么来想,所谓传感器的选择实际是位置传感器的选择,姿态传感器就是用IMU,不用选择什么

位置传感器无非就是在气压计,光流,超声波,GPS,摄像头这之间选择。

所以只需要对位置传感器进行判断就可以了

所以你在sensor.h里可以看到,有这么多个位置传感器的偏移量。

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

在sensor.cpp也可以看到

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

/*传感器位置偏移*/

struct SensorPosOffset
{
//飞控位置偏移
float Fc_x[2];
float Fc_y[2];
float Fc_z[2];

//传感器0位置偏移
float S0_x[2];
float S0_y[2];
float S0_z[2];

//传感器1位置偏移
float S1_x[2];
float S1_y[2];
float S1_z[2];

//传感器2位置偏移
float S2_x[2];
float S2_y[2];
float S2_z[2];

//传感器3位置偏移
float S3_x[2];
float S3_y[2];
float S3_z[2];

//传感器4位置偏移
float S4_x[2];
float S4_y[2];
float S4_z[2];

//传感器5位置偏移
float S5_x[2];
float S5_y[2];
float S5_z[2];

//传感器6位置偏移
float S6_x[2];
float S6_y[2];
float S6_z[2];

//传感器7位置偏移
float S7_x[2];
float S7_y[2];
float S7_z[2];

//传感器8位置偏移
float S8_x[2];
float S8_y[2];
float S8_z[2];

//传感器9位置偏移
float S9_x[2];
float S9_y[2];
float S9_z[2];

//传感器10位置偏移
float S10_x[2];
float S10_y[2];
float S10_z[2];

//传感器11位置偏移
float S11_x[2];
float S11_y[2];
float S11_z[2];

//传感器12位置偏移
float S12_x[2];
float S12_y[2];
float S12_z[2];

//传感器13位置偏移
float S13_x[2];
float S13_y[2];
float S13_z[2];

//传感器14位置偏移
float S14_x[2];
float S14_y[2];
float S14_z[2];

//传感器15位置偏移
float S15_x[2];
float S15_y[2];
float S15_z[2];
};

/*传感器位置偏移*/

 

 

这有个传感器位置读取函数。

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

 

所以其实最关键的就是这个位置传感器的判断和选择了。我们自己注册传感器也是注册位置传感器,它已经给我们定义好了三种位置传感器。

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

 

 

我发现对传感器数据是否健康的判断在传感器更新函数里面,不是的,这里应该只是简单判断有没有数据这样子。

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

比如下面这是其中一个位置传感器更新函数 ,这里面有看传感器是否可用。

bool PositionSensorUpdatePosition( uint8_t index, vector3<double> position, bool available, double delay, double xy_trustD, double z_trustD, double TIMEOUT )
{
if( index >= Position_Sensors_Count )
return false;

bool inFlight;
get_is_inFlight(&inFlight);
uint64_t log = 0;
ReadParam( "SDLog_PosSensor", 0, 0, (uint64_t*)&log, 0 );

TickType_t TIMEOUT_Ticks;
if( TIMEOUT >= 0 )
TIMEOUT_Ticks = TIMEOUT*configTICK_RATE_HZ;
else
TIMEOUT_Ticks = portMAX_DELAY;
if( xSemaphoreTake(Position_Sensors_Mutex[index],TIMEOUT_Ticks) )
{ //锁定传感器
if( Position_Sensors[index] == 0 )
{
xSemaphoreGive(Position_Sensors_Mutex[index]);
return false;
}

Position_Sensor* sensor = Position_Sensors[index];
//判断传感器类型、数据是否正确
bool data_effective;
switch( sensor->sensor_DataType )
{
case Position_Sensor_DataType_s_xy:
if( __ARM_isnan( position.x ) || __ARM_isnan( position.y ) || \
__ARM_isinf( position.x ) || __ARM_isinf( position.y ) )
data_effective = false;
else
data_effective = true;
break;

case Position_Sensor_DataType_s_z:
if( __ARM_isnan( position.z ) || __ARM_isinf( position.z ) )
data_effective = false;
else
data_effective = true;
break;

case Position_Sensor_DataType_s_xyz:
if( __ARM_isnan( position.x ) || __ARM_isnan( position.y ) || __ARM_isnan( position.z ) || \
__ARM_isinf( position.x ) || __ARM_isinf( position.y ) || __ARM_isinf( position.z ) )
data_effective = false;
else
data_effective = true;
break;

default:
data_effective = false;
break;
}
if( !data_effective )
{ //数据出错退出
xSemaphoreGive(Position_Sensors_Mutex[index]);
return false;
}

//更新可用状态
if( sensor->available != available )
sensor->available_status_update_time = TIME::now();
sensor->available = available;

//更新数据
sensor->position = position;

//更新采样时间
sensor->sample_time = sensor->last_update_time.get_pass_time_st();

//延时大于0更新延时
if( delay > 0 )
sensor->delay = delay;
//更新信任度
if( xy_trustD >= 0 )
sensor->xy_trustD = xy_trustD;
if( z_trustD >= 0 )
sensor->z_trustD = z_trustD;

//记录位置数据
if(inFlight && log)
SDLog_Msg_PosSensor( index, *sensor );

xSemaphoreGive(Position_Sensors_Mutex[index]);
return true;
} //解锁传感器
return false;
}

 

 

这才是位置控制的代码,当然是在ctrl_position.cpp里面,只是你之前没有细看找到,我觉得位置传感器的选择应该也是在这里面。因为之前这个文件里面大部分函数都是设定一些目标值,但是你用PID或者ADRC肯定是要用当前值减去目标值得到误差输入误差的啊,对不对,这个应该是在位置控制里面完成,那么肯定就有当前值,那我们就可以去找他怎么确定当前值的。

比如下面这个就是目标值减去当前值,可以可以根据这个进一步去找。

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

右键查看position的定义会跳到这里,会发现这里定义了三个向量,位置,速度,加速度。

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

vector3其实就是这样一种数据,包含x y z

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

但是我没有找到position是在哪里得到具体值的。

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

void ctrl_Position()
{
bool Attitude_Control_Enabled; is_Attitude_Control_Enabled(&Attitude_Control_Enabled);
if( Attitude_Control_Enabled == false )
{
Altitude_Control_Enabled = false;
Position_Control_Enabled = false;
return;
}

double e_1_n;
double e_1;
double e_2_n;
double e_2;

bool inFlight; get_is_inFlight(&inFlight);
vector3<double> Position; get_Position_Ctrl(&Position);
vector3<double> VelocityENU; get_VelocityENU_Ctrl(&VelocityENU);
vector3<double> AccelerationENU; get_AccelerationENU_Ctrl(&AccelerationENU);
get_throttle_force(&AccelerationENU.z); AccelerationENU.z-=GravityAcc;

//位置速度滤波
double Ps = cfg.P1[0];
double Pv = cfg.P2[0];
double Pa = cfg.P3[0];

static vector3<double> TAcc;
vector3<double> TargetVelocity;
vector3<double> TargetVelocity_1;
vector3<double> TargetVelocity_2;

//XY或Z其中一个为非3D模式则退出3D模式
if( Is_3DAutoMode(HorizontalPosition_ControlMode) && Is_3DAutoMode(Altitude_ControlMode)==false )
HorizontalPosition_ControlMode = Position_ControlMode_Locking;
else if( Is_3DAutoMode(HorizontalPosition_ControlMode)==false && Is_3DAutoMode(Altitude_ControlMode) )
Altitude_ControlMode = Position_ControlMode_Locking;

if( Position_Control_Enabled )
{ //水平位置控制
if( get_Position_MSStatus() != MS_Ready )
{
Position_Control_Enabled = false;
goto PosCtrl_Finish;
}

switch( HorizontalPosition_ControlMode )
{
case Position_ControlMode_Position:
{
if( inFlight )
{
vector2<double> e1;
e1.x = target_position.x - Position.x;
e1.y = target_position.y - Position.y;
vector2<double> e1_1;
e1_1.x = - VelocityENU.x;
e1_1.y = - VelocityENU.y;
vector2<double> e1_2;
e1_2.x = - TAcc.x;
e1_2.y = - TAcc.y;
double e1_length = safe_sqrt(e1.get_square());
e_1_n = e1.x*e1_1.x + e1.y*e1_1.y;
if( !is_zero(e1_length) )
e_1 = e_1_n / e1_length;
else
e_1 = 0;
e_2_n = ( e1.x*e1_2.x + e1.y*e1_2.y + e1_1.x*e1_1.x + e1_1.y*e1_1.y )*e1_length - e_1*e_1_n;
if( !is_zero(e1_length*e1_length) )
e_2 = e_2_n / (e1_length*e1_length);
else
e_2 = 0;
smooth_kp_d2 d1 = smooth_kp_2( e1_length, e_1, e_2 , Ps, 200 );
vector2<double> T2;
vector2<double> T2_1;
vector2<double> T2_2;
if( !is_zero(e1_length*e1_length*e1_length) )
{
vector2<double> n = e1 * (1.0/e1_length);
vector2<double> n_1 = (e1_1*e1_length - e1*e_1) / (e1_length*e1_length);
vector2<double> n_2 = ( (e1_2*e1_length-e1*e_2)*e1_length - (e1_1*e1_length-e1*e_1)*(2*e_1) ) / (e1_length*e1_length*e1_length);
T2 = n*d1.d0;
T2_1 = n*d1.d1 + n_1*d1.d0;
T2_2 = n*d1.d2 + n_1*(2*d1.d1) + n_2*d1.d0;
}
TargetVelocity.x = T2.x; TargetVelocity.y = T2.y;
TargetVelocity_1.x = T2_1.x; TargetVelocity_1.y = T2_1.y;
TargetVelocity_2.x = T2_2.x; TargetVelocity_2.y = T2_2.y;
}
else
{
//没起飞前在位置控制模式
//重置期望位置
target_position.x = Position.x;
target_position.y = Position.y;
Attitude_Control_set_Target_RollPitch( 0, 0 );
goto PosCtrl_Finish;
}
break;
}
case Position_ControlMode_Velocity:
{
if( !inFlight )
{
//没起飞时重置期望速度
Attitude_Control_set_Target_RollPitch( 0, 0 );
goto PosCtrl_Finish;
}
else
{
TargetVelocity.x = target_velocity.x;
TargetVelocity.y = target_velocity.y;
Pv = cfg.P2_VelXY[0];
}
break;
}

case Position_ControlMode_RouteLine:
{
if( inFlight )
{
//计算垂足
vector2<double> A( target_position.x, target_position.y );
vector2<double> C( Position.x, Position.y );
vector2<double> A_C = C - A;
vector2<double> A_B( route_line_A_B.x, route_line_A_B.y );
double k = (A_C * A_B) * route_line_m;
vector2<double> foot_point = (A_B * k) + A;

//计算偏差
vector2<double> e1r = A - foot_point;
vector2<double> e1d = foot_point - C;
double e1r_length = safe_sqrt(e1r.get_square());
double e1d_length = safe_sqrt(e1d.get_square());

//计算route方向单位向量
vector2<double> route_n;
if( e1r_length > 0.001 )
route_n = e1r * (1.0/e1r_length);

//计算d方向单位向量
vector2<double> d_n;
if( e1d_length > 0.001 )
d_n = e1d * (1.0/e1d_length);

//计算e1导数
vector2<double> e1_1( VelocityENU.x, VelocityENU.y );
double e1r_1 = -(e1_1 * route_n);
double e1d_1 = -(e1_1 * d_n);
//e1二阶导
vector2<double> e1_2( TAcc.x, TAcc.y );
double e1r_2 = -(e1_2 * route_n);
double e1d_2 = -(e1_2 * d_n);

/*route方向*/
smooth_kp_d2 d1r = smooth_kp_2( e1r_length, e1r_1, e1r_2 , Ps, AutoVelXY );
vector2<double> T2r = route_n * d1r.d0;
vector2<double> T2r_1 = route_n * d1r.d1;
vector2<double> T2r_2 = route_n * d1r.d2;
/*route方向*/

/*d方向*/
smooth_kp_d2 d1d = smooth_kp_2( e1d_length, e1d_1, e1d_2 , Ps, AutoVelXY );
vector2<double> T2d = d_n * d1d.d0;
vector2<double> T2d_1 = d_n * d1d.d1;
vector2<double> T2d_2 = d_n * d1d.d2;
/*d方向*/

TargetVelocity.x = T2r.x+T2d.x; TargetVelocity.y = T2r.y+T2d.y;
TargetVelocity_1.x = T2r_1.x+T2d_1.x; TargetVelocity_1.y = T2r_1.y+T2d_1.y;
TargetVelocity_2.x = T2r_2.x+T2d_2.x; TargetVelocity_2.y = T2r_2.y+T2d_2.y;

if( e1r.get_square() + e1d.get_square() < 20*20 )
HorizontalPosition_ControlMode = Position_ControlMode_Position;
}
else
{
//没起飞时重置期望速度
Attitude_Control_set_Target_RollPitch( 0, 0 );
return;
}
break;
}

case Position_ControlMode_RouteLine3D:
{
if( inFlight )
{
//计算垂足
vector3<double> A_C = Position - target_position;
double k = (A_C * route_line_A_B) * route_line_m;
vector3<double> foot_point = (route_line_A_B * k) + target_position;

//计算偏差
vector3<double> e1r = target_position - foot_point;
vector3<double> e1d = foot_point - Position;
double e1r_length = safe_sqrt(e1r.get_square());
double e1d_length = safe_sqrt(e1d.get_square());

//计算route方向单位向量
vector3<double> route_n;
if( e1r_length > 0.001 )
route_n = e1r * (1.0/e1r_length);

//计算e1导数
double e1r_1_length = -(VelocityENU * route_n);
vector3<double> e1r_1 = route_n * e1r_1_length;
vector3<double> e1d_1 = -(VelocityENU + e1r_1);
//e1二阶导
vector3<double> e1_2( TAcc.x, TAcc.y, AccelerationENU.z );
double e1r_2_length = -(e1_2 * route_n);
vector3<double> e1r_2 = route_n * e1r_2_length;
vector3<double> e1d_2 = -(e1_2 + e1r_2);

/*route方向*/
smooth_kp_d2 d1r = smooth_kp_2( e1r_length, e1r_1_length, e1r_2_length , Ps, AutoVelXYZ );
vector3<double> T2r = route_n * d1r.d0;
vector3<double> T2r_1 = route_n * d1r.d1;
vector3<double> T2r_2 = route_n * d1r.d2;
/*route方向*/

/*d方向*/
e_1_n = e1d.x*e1d_1.x + e1d.y*e1d_1.y + e1d.z*e1d_1.z;
if( !is_zero(e1d_length) )
e_1 = e_1_n / e1d_length;
else
e_1 = 0;
e_2_n = ( e1d.x*e1d_2.x + e1d.y*e1d_2.y + e1d.z*e1d_2.z + e1d_1.x*e1d_1.x + e1d_1.y*e1d_1.y + e1d_1.z*e1d_1.z )*e1d_length - e_1*e_1_n;
if( !is_zero(e1d_length*e1d_length) )
e_2 = e_2_n / (e1d_length*e1d_length);
else
e_2 = 0;
smooth_kp_d2 d1d = smooth_kp_2( e1d_length, e_1, e_2 , Ps, AutoVelXYZ );
vector3<double> T2d;
vector3<double> T2d_1;
vector3<double> T2d_2;
if( !is_zero(e1d_length*e1d_length*e1d_length) )
{
vector3<double> n = e1d * (1.0/e1d_length);
vector3<double> n_1 = (e1d_1*e1d_length - e1d*e_1) / (e1d_length*e1d_length);
vector3<double> n_2 = ( (e1d_2*e1d_length-e1d*e_2)*e1d_length - (e1d_1*e1d_length-e1d*e_1)*(2*e_1) ) / (e1d_length*e1d_length*e1d_length);
T2d = n*d1d.d0;
T2d_1 = n*d1d.d1 + n_1*d1d.d0;
T2d_2 = n*d1d.d2 + n_1*(2*d1d.d1) + n_2*d1d.d0;
}
/*d方向*/

TargetVelocity = T2r + T2d;
TargetVelocity_1 = T2r_1 + T2d_1;
TargetVelocity_2 = T2r_2 + T2d_2;

if( e1r.get_square() + e1d.get_square() < 20*20 )
HorizontalPosition_ControlMode = Altitude_ControlMode = Position_ControlMode_Position;
}
else
{
//没起飞时重置期望速度
Attitude_Control_set_Target_RollPitch( 0, 0 );
return;
}
break;
}

case Position_ControlMode_Locking:
default:
{ //刹车锁位置
static uint16_t lock_counter = 0;
if( inFlight )
{
TargetVelocity.x = 0;
TargetVelocity.y = 0;
if( VelocityENU.x*VelocityENU.x + VelocityENU.y*VelocityENU.y < 10*10 )
{
if( ++lock_counter >= CtrlRateHz*0.7 )
{
lock_counter = 0;
target_position.x = Position.x;
target_position.y = Position.y;
HorizontalPosition_ControlMode = Position_ControlMode_Position;
}
}
else
lock_counter = 0;
}
else
{
lock_counter = 0;
target_position.x = Position.x;
target_position.y = Position.y;
HorizontalPosition_ControlMode = Position_ControlMode_Position;
Attitude_Control_set_Target_RollPitch( 0, 0 );
return;
}
break;
}
}

//计算期望加速度
vector2<double> e2;
e2.x = TargetVelocity.x - VelocityENU.x;
e2.y = TargetVelocity.y - VelocityENU.y;
vector2<double> e2_1;
e2_1.x = TargetVelocity_1.x - TAcc.x;
e2_1.y = TargetVelocity_1.y - TAcc.y;
double e2_length = safe_sqrt(e2.get_square());
e_1_n = e2.x*e2_1.x + e2.y*e2_1.y;
if( !is_zero(e2_length) )
e_1 = e_1_n / e2_length;
else
e_1 = 0;
smooth_kp_d1 d2;
if( Is_AutoMode(HorizontalPosition_ControlMode) )
d2 = smooth_kp_1( e2_length, e_1 , Pv, cfg.maxAutoAccXY[0] );
else
d2 = smooth_kp_1( e2_length, e_1 , Pv, cfg.maxAccXY[0] );
vector2<double> T3;
vector2<double> T3_1;
if( !is_zero(e2_length*e2_length) )
{
vector2<double> n = e2 * (1.0/e2_length);
vector2<double> n_1 = (e2_1*e2_length - e2*e_1) / (e2_length*e2_length);
T3 = n*d2.d0;
T3_1 = n*d2.d1 + n_1*d2.d0;
}
T3 += vector2<double>( TargetVelocity_1.x, TargetVelocity_1.y );
T3_1 += vector2<double>( TargetVelocity_2.x, TargetVelocity_2.y );

vector2<double> e3;
e3.x = T3.x - TAcc.x;
e3.y = T3.y - TAcc.y;
double e3_length = safe_sqrt(e3.get_square());
double d3;
if( Is_AutoMode(HorizontalPosition_ControlMode) )
d3 = smooth_kp_0( e3_length , Pa, cfg.maxAutoJerkXY[0] );
else
d3 = smooth_kp_0( e3_length , Pa, cfg.maxJerkXY[0] );
vector2<double> T4;
if( !is_zero(e3_length) )
{
vector2<double> n = e3 * (1.0/e3_length);
T4 = n*d3;
}
if( Is_AutoMode(HorizontalPosition_ControlMode) )
T4.constrain( cfg.maxAutoJerkXY[0] );
else
T4.constrain( cfg.maxJerkXY[0] );
T4 += T3_1;

TAcc.x += T4.x*(1.0/CtrlRateHz);
TAcc.y += T4.y*(1.0/CtrlRateHz);

//去除风力扰动
vector3<double> WindDisturbance;
get_WindDisturbance( &WindDisturbance );
vector2<double> target_acceleration;
// target_acceleration.x = TAcc.x - WindDisturbance.x;
// target_acceleration.y = TAcc.y - WindDisturbance.y;
target_acceleration.x = T3.x - WindDisturbance.x;
target_acceleration.y = T3.y - WindDisturbance.y;

//旋转至Bodyheading
Quaternion attitude;
get_Attitude_quat(&attitude);
double yaw = attitude.getYaw();
double sin_Yaw, cos_Yaw;
fast_sin_cos( yaw, &sin_Yaw, &cos_Yaw );
double target_acceleration_x_bodyheading = ENU2BodyHeading_x( target_acceleration.x , target_acceleration.y , sin_Yaw , cos_Yaw );
double target_acceleration_y_bodyheading = ENU2BodyHeading_y( target_acceleration.x , target_acceleration.y , sin_Yaw , cos_Yaw );
// target_acceleration_x_bodyheading = ThrOut_Filters[0].run(target_acceleration_x_bodyheading);
// target_acceleration_y_bodyheading = ThrOut_Filters[1].run(target_acceleration_y_bodyheading);

//计算风力补偿角度
double WindDisturbance_Bodyheading_x = ENU2BodyHeading_x( WindDisturbance.x , WindDisturbance.y , sin_Yaw , cos_Yaw );
double WindDisturbance_Bodyheading_y = ENU2BodyHeading_y( WindDisturbance.x , WindDisturbance.y , sin_Yaw , cos_Yaw );
//计算角度
double AccUp = GravityAcc + AccelerationENU.z;
double AntiDisturbancePitch = atan2( -WindDisturbance_Bodyheading_x , AccUp );
double AntiDisturbanceRoll = atan2( WindDisturbance_Bodyheading_y , AccUp );

//计算目标角度
double target_Roll = atan2( -target_acceleration_y_bodyheading , AccUp );
double target_Pitch = atan2( target_acceleration_x_bodyheading , AccUp );
if( HorizontalPosition_ControlMode==Position_ControlMode_Velocity )
{ //角度限幅
if( VelCtrlMaxRoll>0 && VelCtrlMaxPitch>0 )
{
target_Roll = constrain( target_Roll , AntiDisturbanceRoll - VelCtrlMaxRoll, AntiDisturbanceRoll + VelCtrlMaxRoll );
target_Pitch = constrain( target_Pitch , AntiDisturbancePitch - VelCtrlMaxPitch, AntiDisturbancePitch + VelCtrlMaxPitch );
}
else if( VelCtrlMaxRoll>0 )
{
vector2<double> Tangle( target_Roll - AntiDisturbanceRoll, target_Pitch - AntiDisturbancePitch );
Tangle.constrain(VelCtrlMaxRoll);
target_Roll = AntiDisturbanceRoll + Tangle.x;
target_Pitch = AntiDisturbancePitch + Tangle.y;
}
}

//设定目标角度
Attitude_Control_set_Target_RollPitch( target_Roll, target_Pitch );

//获取真实目标角度修正TAcc
Attitude_Control_get_Target_RollPitch( &target_Roll, &target_Pitch );
target_acceleration_x_bodyheading = tan(target_Pitch)*GravityAcc;
target_acceleration_y_bodyheading = -tan(target_Roll)*GravityAcc;
target_acceleration.x = BodyHeading2ENU_x( target_acceleration_x_bodyheading, target_acceleration_y_bodyheading , sin_Yaw, cos_Yaw );
target_acceleration.y = BodyHeading2ENU_y( target_acceleration_x_bodyheading, target_acceleration_y_bodyheading , sin_Yaw, cos_Yaw );
TAcc.x = target_acceleration.x + WindDisturbance.x;
TAcc.y = target_acceleration.y + WindDisturbance.y;
}//水平位置控制
else
{
ThrOut_Filters[0].reset(0);
ThrOut_Filters[1].reset(0);
}

PosCtrl_Finish:
if( Altitude_Control_Enabled )
{//高度控制

//设置控制量限幅
Target_tracker[2].r2p = cfg.maxVelUp[0];
Target_tracker[2].r2n = cfg.maxVelDown[0];
Target_tracker[2].r3p = cfg.maxAccUp[0];
Target_tracker[2].r3n = cfg.maxAccDown[0];
Target_tracker[2].r4p = cfg.maxJerkUp[0];
Target_tracker[2].r4n = cfg.maxJerkDown[0];

if( !Is_3DAutoMode(Altitude_ControlMode) )
{
switch( Altitude_ControlMode )
{
case Position_ControlMode_Position:
{ //控制位置
if( inFlight )
{
Target_tracker[2].r2p = 0.3*cfg.maxVelUp[0];
Target_tracker[2].r2n = 0.3*cfg.maxVelDown[0];
Target_tracker[2].track4( target_position.z , 1.0 / CtrlRateHz );
}
else
{
//没起飞前在位置控制模式
//不要起飞
Target_tracker[2].reset();
target_position.z = Target_tracker[2].x1 = Position.z;
Attitude_Control_set_Throttle( get_STThrottle() );
goto AltCtrl_Finish;
}
break;
}
case Position_ControlMode_Velocity:
{ //控制速度
if( inFlight || target_velocity.z > 0 )
{
double TVel;
if( target_velocity.z > cfg.maxVelUp[0] )
TVel = cfg.maxVelUp[0];
else if( target_velocity.z < -cfg.maxVelDown[0] )
TVel = -cfg.maxVelDown[0];
else
TVel = target_velocity.z;
Target_tracker[2].track3( TVel , 1.0 / CtrlRateHz );
}
else
{
//没起飞且期望速度为负
//不要起飞
Target_tracker[2].reset();
Target_tracker[2].x1 = Position.z;
Attitude_Control_set_Throttle( get_STThrottle() );
goto AltCtrl_Finish;
}
break;
}

case Position_ControlMode_Takeoff:
{ //起飞

//设置控制量最大值
Target_tracker[2].r3p = cfg.maxAutoAccUp[0];
Target_tracker[2].r3n = cfg.maxAutoAccDown[0];
Target_tracker[2].r4p = cfg.maxAutoJerkUp[0];
Target_tracker[2].r4n = cfg.maxAutoJerkDown[0];

if( inFlight )
{
//已起飞
//控制达到目标高度
double homeZ;
getHomeLocalZ(&homeZ);
if( Position.z - homeZ < 100 )
Target_tracker[2].r2n = Target_tracker[2].r2p = 50;
else
Target_tracker[2].r2n = Target_tracker[2].r2p = ( AutoVelZ < cfg.maxAutoVelUp[0] ) ? AutoVelZ : cfg.maxAutoVelUp[0];
Target_tracker[2].track4( target_position.z , 1.0 / CtrlRateHz );
if( fabs( target_position.z - Position.z ) < 10 && \
in_symmetry_range( Target_tracker[2].x2 , 0.1 ) && \
in_symmetry_range( Target_tracker[2].x3 , 0.1 ) )
Altitude_ControlMode = Position_ControlMode_Position;
}
else
{
//未起飞
//等待起飞
target_position.z = Position.z + TakeoffHeight;
Target_tracker[2].x1 = Position.z;
Target_tracker[2].track3( 50 , 1.0 / CtrlRateHz );
}
break;
}
case Position_ControlMode_RouteLine:
{ //飞到指定高度

//设置控制量最大值
Target_tracker[2].r3p = cfg.maxAutoAccUp[0];
Target_tracker[2].r3n = cfg.maxAutoAccDown[0];
Target_tracker[2].r4p = cfg.maxAutoJerkUp[0];
Target_tracker[2].r4n = cfg.maxAutoJerkDown[0];

if( inFlight )
{
//已起飞
//控制达到目标高度
Target_tracker[2].r2n = Target_tracker[2].r2p = AutoVelZ;
Target_tracker[2].track4( target_position.z , 1.0f / CtrlRateHz );
if( fabs( target_position.z - Position.z ) < 10 && \
in_symmetry_range( VelocityENU.z , 10.0f ) && \
in_symmetry_range( AccelerationENU.z , 50.0f ) && \
in_symmetry_range( Target_tracker[2].x2 , 0.1f ) && \
in_symmetry_range( Target_tracker[2].x3 , 0.1f ) )
Altitude_ControlMode = Position_ControlMode_Position;
}
else
{
//未起飞
//不要起飞
Target_tracker[2].reset();
Target_tracker[2].x1 = Position.z;
Attitude_Control_set_Throttle( 0 );
goto AltCtrl_Finish;
}
break;
}

case Position_ControlMode_Locking:
default:
{ //锁位置(减速到0然后锁住高度)
if( inFlight )
{
Target_tracker[2].track3( 0 , 1.0 / CtrlRateHz );
if( in_symmetry_range( VelocityENU.z , 10.0 ) && \
in_symmetry_range( Target_tracker[2].x2 , 0.1 ) && \
in_symmetry_range( Target_tracker[2].x3 , 0.1 ) )
{
target_position.z = Target_tracker[2].x1;
Altitude_ControlMode = Position_ControlMode_Position;
}
}
else
{
Altitude_ControlMode = Position_ControlMode_Position;
Attitude_Control_set_Throttle( get_STThrottle() );
goto AltCtrl_Finish;
}
break;
}
}
}

if( inFlight )
{
//计算期望速度
double target_velocity_z;
//期望垂直速度的导数
double Tvz_1, Tvz_2;
if( Is_3DAutoMode(Altitude_ControlMode) )
{
target_velocity_z = TargetVelocity.z;
Tvz_1 = TargetVelocity_1.z;
Tvz_2 = TargetVelocity_2.z;
Target_tracker[2].reset();
Target_tracker[2].x1 = target_position.z;
}
else
{
if( Target_tracker[2].get_tracking_mode() == 4 )
{
double max_fb_vel = ( Target_tracker[2].x1 - Position.z ) > 0 ? cfg.maxAutoVelUp[0] : cfg.maxAutoVelDown[0];
smooth_kp_d2 TvFb = smooth_kp_2(
Target_tracker[2].x1 - Position.z,
Target_tracker[2].x2 - VelocityENU.z,
Target_tracker[2].x3 - AccelerationENU.z,
Ps, max_fb_vel );
target_velocity_z = TvFb.d0 + Target_tracker[2].x2;
Tvz_1 = TvFb.d1 + Target_tracker[2].x3;
Tvz_2 = TvFb.d2 + Target_tracker[2].x4;
}
else
{
target_velocity_z = Target_tracker[2].x2;
Tvz_1 = Target_tracker[2].x3;
Tvz_2 = Target_tracker[2].x4;
}
}

//计算期望加速度
double max_fb_acc = ( target_velocity_z - VelocityENU.z ) > 0 ? cfg.maxAutoAccUp[0] : cfg.maxAutoAccDown[0];
smooth_kp_d1 TaFb = smooth_kp_1(
target_velocity_z - VelocityENU.z,
Tvz_1 - AccelerationENU.z,
Pv, max_fb_acc );
double target_acceleration_z = TaFb.d0 + Tvz_1;
double target_acceleration_z_1 = TaFb.d1 + Tvz_2;
//target_acceleration_z = TargetVelocityFilter[2].run( target_acceleration_z );
//加速度误差
double acceleration_z_error = target_acceleration_z - AccelerationENU.z;

//获取倾角cosin
Quaternion quat;
get_Airframe_quat(&quat);
double lean_cosin = quat.get_lean_angle_cosin();

//获取电机起转油门
double MotorStartThrottle = get_STThrottle();
//获取悬停油门 - 电机起转油门
double hover_throttle;
get_hover_throttle(&hover_throttle);
hover_throttle = hover_throttle - MotorStartThrottle;

//计算输出油门
double force, T, b;
get_throttle_force(&force);
get_ESO_height_T(&T);
get_throttle_b(&b);
if( force < 1 )
force = 1;
double throttle = ( force + T * ( acceleration_z_error * Pa + target_acceleration_z_1 ) )/b;
//倾角补偿
if( lean_cosin > 0.1 )
throttle /= lean_cosin;
else //倾角太大
throttle = (100 - MotorStartThrottle) / 2;

if( inFlight )
{
double logbuf[10];
logbuf[0] = throttle;
logbuf[1] = hover_throttle;
logbuf[2] = force;
logbuf[3] = target_acceleration_z;
logbuf[4] = AccelerationENU.z;
SDLog_Msg_DebugVect( "thr", logbuf, 5 );
}

//油门限幅
throttle += MotorStartThrottle;
if( throttle > 90 )
throttle = 90;
if( inFlight )
{
if( throttle < MotorStartThrottle )
throttle = MotorStartThrottle;
}

//侧翻保护
static uint32_t RollOverProtectCounter = 0;
if( lean_cosin < 0 )
{
if( ++RollOverProtectCounter >= CtrlRateHz*3 )
{
RollOverProtectCounter = CtrlRateHz*3;
throttle = 0;
}
}
else
RollOverProtectCounter = 0;

// throttle = ThrOut_Filters[2].run(throttle);
//输出
Attitude_Control_set_Throttle( throttle );
}
else
{
//没起飞
//均匀增加油门起飞
double throttle;
get_Target_Throttle(&throttle);
ThrOut_Filters[2].reset(throttle);
Attitude_Control_set_Throttle( throttle + 1.0/CtrlRateHz * 15 );
}

}//高度控制
AltCtrl_Finish:
return;
}

 

 

发现一个新文件,在crtl_position.cpp右键查看下面这个函数的定义时发现的,是和position一起定义的,我尝试着想看看,因为没有找到position赋值的地方。

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

这个文件所处的文件夹我之前还真没注意,莫非这部分不开源的?我看到一个lib文件,怪不得我之前找不到啊!!!!不开源的,这样很多东西自己都没法去改啊,他说他弄了好长时间的异常检测,估计这部分不开源。position的值怎么得到的这部分不开源,这样我想尝试一下自己制定只用T265的数据都没法指定了,这样有点不方便。辛亏先把背后基本逻辑挖了下,不然到时候你想弄单纯的SLAM实验可能都弄不了,也不一定,我把其他位置传感器都拔掉只剩T265这样或许应该可以测试。无名只是光流融合部分不开源,这个我无所谓,但是ACfly这个核心关键部分不开源,就给你一些API,我是不太喜欢的。或者找找他以前部分有没有开源,这样在他以前的代码上改改。

他这里自己也说了

https://blog.csdn.net/weixin_40767422/article/details/88081309

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

 姿态解算是哪些呢,他在用户手册里面也有写,正是我找到的这个

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

是的,你关心的那些函数都在这个MeasurementSystem.hpp头文件里,比如传感器数据好坏的判断,真正position数据的获取,但是你想查看这些函数的实现,跳转不了。

#pragma once

#include "vector2.hpp"
#include "vector3.hpp"
#include "Quaternion.hpp"
#include "map_projection.hpp"

//获取三字节WGA识别码
void MS_get_WGA( uint32_t* WGA );
//获取正版验证结果
bool MS_WGA_Correct();

//获取当前使用的陀螺仪
uint8_t get_current_use_IMUGyroscope();
//获取当前使用的加速度计
uint8_t get_current_use_IMUAccelerometer();

enum MS_Status
{
MS_Initializing ,
MS_Ready ,
MS_Err ,
};

/*健康度信息*/
struct PosSensorHealthInf1
{
//传感器序号
uint8_t sensor_ind;
//解算位置
vector3<double> PositionENU;
//传感器位置
double sensor_pos;
//传感器偏移(传感器健康时更新)
//HOffset+PositionENU = 传感器估计值
double HOffset;
//上次健康时间
TIME last_healthy_TIME;
//是否可用(不可用时噪声无效)
bool available;
//传感器噪声上下界(传感器-解算)
double NoiseMin, NoiseMax;
//速度噪声
double VNoise;
};
struct PosSensorHealthInf2
{
//传感器序号
uint8_t sensor_ind;
//是否全球定位传感器
//是才有定位转换信息
bool global_sensor;
//定位坐标转换信息
Map_Projection mp;
//解算位置
vector3<double> PositionENU;
//传感器位置
vector2<double> sensor_pos;
//传感器偏移(传感器健康时更新)
//HOffset+PositionENU = 传感器估计值
vector2<double> HOffset;
//上次健康时间
vector2<TIME> last_healthy_TIME;
//是否可用(不可用时噪声无效)
bool available;
//传感器噪声上下界(传感器-解算)
vector2<double> NoiseMin, NoiseMax;
//速度噪声
vector2<double> VNoise;
};
struct PosSensorHealthInf3
{
//传感器序号
uint8_t sensor_ind;
//是否全球定位传感器
//是才有定位转换信息
bool global_sensor;
//定位坐标转换信息
Map_Projection mp;
//解算位置
vector3<double> PositionENU;
//传感器位置
vector3<double> sensor_pos;
//传感器偏移(传感器健康时更新)
//HOffset+PositionENU = 传感器估计值
vector3<double> HOffset;
//上次健康时间
vector3<TIME> last_healthy_TIME;
//是否可用(不可用时噪声无效)
bool available;
//传感器噪声上下界(传感器-解算)
vector3<double> NoiseMin, NoiseMax;
//速度噪声
vector3<double> VNoise;
};

/*XY传感器健康度*/
//获取当前XY传感器
int8_t get_Current_XYSensor();

//指定序号传感器健康度
bool get_PosSensorHealth_XY( PosSensorHealthInf2* result, uint8_t sensor_ind, double TIMEOUT = -1 );
//当前传感器健康度
bool get_Health_XY( PosSensorHealthInf2* result, double TIMEOUT = -1 );
//最优测距传感器健康度
bool get_OptimalRange_XY( PosSensorHealthInf2* result, double TIMEOUT = -1 );
//最优全球定位传感器健康度
bool get_OptimalGlobal_XY( PosSensorHealthInf2* result, double TIMEOUT = -1 );
/*XY传感器健康度*/

/*Z传感器健康度*/
//获取当前Z传感器
int8_t get_Current_ZSensor();

//指定序号传感器健康度
bool get_PosSensorHealth_Z( PosSensorHealthInf1* result, uint8_t sensor_ind, double TIMEOUT = -1 );
//当前传感器健康度
bool get_Health_Z( PosSensorHealthInf1* result, double TIMEOUT = -1 );
//最优测距传感器健康度
bool get_OptimalRange_Z( PosSensorHealthInf1* result, double TIMEOUT = -1 );
//最优全球定位传感器健康度
bool get_OptimalGlobal_Z( PosSensorHealthInf1* result, double TIMEOUT = -1 );
/*Z传感器健康度*/

/*XYZ传感器健康度*/
//指定序号传感器健康度
bool get_PosSensorHealth_XYZ( PosSensorHealthInf3* result, uint8_t sensor_ind, double TIMEOUT = -1 );
//最优测距传感器健康度
bool get_OptimalRange_XYZ( PosSensorHealthInf3* result, double TIMEOUT = -1 );
//最优全球定位传感器健康度
bool get_OptimalGlobal_XYZ( PosSensorHealthInf3* result, double TIMEOUT = -1 );
/*XYZ传感器健康度*/
/*健康度信息*/

/*姿态信息获取接口*/
//获取解算系统状态
MS_Status get_Attitude_MSStatus();

//获取用于控制的滤波后的角速度
bool get_AngularRate_Ctrl( vector3<double>* result, double TIMEOUT = -1 );
//获取姿态四元数
bool get_Attitude_quat( Quaternion* result, double TIMEOUT = -1 );
//获取机体四元数(偏航不对准)
bool get_Airframe_quat( Quaternion* result, double TIMEOUT = -1 );
//获取机体四元数(偏航对准)
bool get_AirframeY_quat( Quaternion* result, double TIMEOUT = -1 );
//获取历史四元数
bool get_history_AttitudeQuat( Quaternion* result, double t, double TIMEOUT = -1 );
//获取历史机体四元数(偏航不对准)
bool get_history_AirframeQuat( Quaternion* result, double t, double TIMEOUT = -1 );
//获取历史机体四元数(偏航对准)
bool get_history_AirframeQuatY( Quaternion* result, double t, double TIMEOUT = -1 );
/*姿态信息获取接口*/

/*位置信息获取接口*/
//获取解算系统状态
MS_Status get_Altitude_MSStatus();
MS_Status get_Position_MSStatus();

//获取实时位置
bool get_Position( vector3<double>* result, double TIMEOUT = -1 );
//获取实时速度(东北天方向)
bool get_VelocityENU( vector3<double>* result, double TIMEOUT = -1 );
//获取实时地理系加速度(东北天)
bool get_AccelerationENU( vector3<double>* result, double TIMEOUT = -1 );
//获取用于控制的滤波后的地理系加速度
bool get_AccelerationENU_Ctrl( vector3<double>* result, double TIMEOUT = -1 );
bool get_VelocityENU_Ctrl( vector3<double>* result, double TIMEOUT = -1 );
bool get_Position_Ctrl( vector3<double>* result, double TIMEOUT = -1 );
//获取低通滤波后的未补偿(零偏灵敏度温度)的陀螺加速度数据
bool get_AccelerationNC_filted( vector3<double>* vec, double TIMEOUT = -1 );
bool get_AngularRateNC_filted( vector3<double>* vec, double TIMEOUT = -1 );
/*位置信息获取接口*/

/*电池信息接口*/
struct BatteryCfg
{
//标准电压(V)
float STVoltage[2];
//电压测量增益(V)
float VoltMKp[2];
//电流测量增益(A)
float CurrentMKp[2];
//容量(W*h)
float Capacity[2];
//功率电压点0(0%电量时相对标准电压的电压差,此序列必须递增)
float VoltP0[2];
//功率电压点1(10%电量时相对标准电压的电压差,此序列必须递增)
float VoltP1[2];
//功率电压点2(20%电量时相对标准电压的电压差,此序列必须递增)
float VoltP2[2];
//功率电压点3(30%电量时相对标准电压的电压差,此序列必须递增)
float VoltP3[2];
//功率电压点4(40%电量时相对标准电压的电压差,此序列必须递增)
float VoltP4[2];
//功率电压点5(50%电量时相对标准电压的电压差,此序列必须递增)
float VoltP5[2];
//功率电压点6(60%电量时相对标准电压的电压差,此序列必须递增)
float VoltP6[2];
//功率电压点7(70%电量时相对标准电压的电压差,此序列必须递增)
float VoltP7[2];
//功率电压点8(80%电量时相对标准电压的电压差,此序列必须递增)
float VoltP8[2];
//功率电压点9(90%电量时相对标准电压的电压差,此序列必须递增)
float VoltP9[2];
//功率电压点10(100%电量时相对标准电压的电压差,此序列必须递增)
float VoltP10[2];
}__PACKED;

//电压
float get_MainBatteryVoltage();
//滤波电压(V)
float get_MainBatteryVoltage_filted();
//总使用功耗(W*h)
float get_MainBatteryPowerUsage();
//滤波功率(W)
float get_MainBatteryPower_filted();
//电池电流(A)
float get_MainBatteryCurrent();
//CPU温度(℃)
float get_CPUTemerature();
//获取电池信息
void get_MainBatteryInf( float* Volt, float* Current, float* PowerUsage, float* Power_filted, float* RMPercent );
/*电池信息接口*/

 

说实话我想找一个真开源的飞控,来做SLAM实验。我想起有一个是真开源的,匿名是真开源的。

 

 

 

github上的ACfly似乎开源了这部分,可能放的早期的版本。这还是让我自己有一定折腾的可能性。不对,里面也有两个Lib文件,实际也是没有开源。

https://github.com/weihli/ACFly-Prophet/tree/master/MeasurementSystem

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

原创:https://www.panoramacn.com
源码网提供WordPress源码,帝国CMS源码discuz源码,微信小程序,小说源码,杰奇源码,thinkphp源码,ecshop模板源码,微擎模板源码,dede源码,织梦源码等。

专业搭建小说网站,小说程序,杰奇系列,微信小说系列,app系列小说

我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)

免责声明,若由于商用引起版权纠纷,一切责任均由使用者承担。

您必须遵守我们的协议,如果您下载了该资源行为将被视为对《免责声明》全部内容的认可-> 联系客服 投诉资源
www.panoramacn.com资源全部来自互联网收集,仅供用于学习和交流,请勿用于商业用途。如有侵权、不妥之处,请联系站长并出示版权证明以便删除。 敬请谅解! 侵权删帖/违法举报/投稿等事物联系邮箱:2640602276@qq.com
未经允许不得转载:书荒源码源码网每日更新网站源码模板! » 我觉得还是把ACfly的传感器的逻辑弄清楚,这样再去二次开发好一些。(折腾半天发现有很关键一部分没有开源,怪不得找不到,这让我很失望)
关注我们小说电影免费看
关注我们,获取更多的全网素材资源,有趣有料!
120000+人已关注
分享到:
赞(0) 打赏

评论抢沙发

  • 昵称 (必填)
  • 邮箱 (必填)
  • 网址

您的打赏就是我分享的动力!

支付宝扫一扫打赏

微信扫一扫打赏