我觉得还是把ACfly的传感器的逻辑弄清楚，这样再去二次开发好一些。确实是这样的，还是得真正搞清楚，不然弄不成。真正把他这个工程啃透。我先不说语法上，先逻辑上啃透。
他觉得二次开发简单那是因为他对整个工程有了透彻的了解了。
一个刚来的人听他讲那二次开发还是会乱的。
acfly的基本逻辑是，你先把传感器注册上，然后它会有函数自动判断你传感器的数据质量如何，并选择用什么传感器。
还有问题默认注册的位置传感器的数据单位是多少。
传感器的处理逻辑都在sensor.c里面，包括像经纬度投影到平面。那几种传感器的数据怎么处理的看这个文件就够了。
从contrlsystem.hpp这个文件就可以看书ctrl_attitude.cpp和ctrL_position.cpp这两个文件里面有哪些函数。这也是那个华清老师说的，高手都是先看头文件。不然你直接看那两个C++文件几千行，很乱的。ctrL_position.cpp是没有单独的头文件的。
ACfly的用户手册里面也说了单位是cm
找到那个进行传感器数据质量判断，还有根据传感器优先级进行传感器选择的函数。
我们这么来想，所谓传感器的选择实际是位置传感器的选择，姿态传感器就是用IMU，不用选择什么
位置传感器无非就是在气压计，光流，超声波，GPS，摄像头这之间选择。
所以只需要对位置传感器进行判断就可以了
所以你在sensor.h里可以看到，有这么多个位置传感器的偏移量。
在sensor.cpp也可以看到
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];
};
这有个传感器位置读取函数。
所以其实最关键的就是这个位置传感器的判断和选择了。我们自己注册传感器也是注册位置传感器，它已经给我们定义好了三种位置传感器。
我发现对传感器数据是否健康的判断在传感器更新函数里面，不是的，这里应该只是简单判断有没有数据这样子。
比如下面这是其中一个位置传感器更新函数 ，这里面有看传感器是否可用。
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肯定是要用当前值减去目标值得到误差输入误差的啊，对不对，这个应该是在位置控制里面完成，那么肯定就有当前值，那我们就可以去找他怎么确定当前值的。
比如下面这个就是目标值减去当前值，可以可以根据这个进一步去找。
右键查看position的定义会跳到这里，会发现这里定义了三个向量，位置，速度，加速度。
vector3其实就是这样一种数据，包含x y z
但是我没有找到position是在哪里得到具体值的。
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);
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;
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;
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);
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;
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;
}
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赋值的地方。
这个文件所处的文件夹我之前还真没注意，莫非这部分不开源的？我看到一个lib文件，怪不得我之前找不到啊！！！！不开源的，这样很多东西自己都没法去改啊，他说他弄了好长时间的异常检测，估计这部分不开源。position的值怎么得到的这部分不开源，这样我想尝试一下自己制定只用T265的数据都没法指定了，这样有点不方便。辛亏先把背后基本逻辑挖了下，不然到时候你想弄单纯的SLAM实验可能都弄不了，也不一定，我把其他位置传感器都拔掉只剩T265这样或许应该可以测试。无名只是光流融合部分不开源，这个我无所谓，但是ACfly这个核心关键部分不开源，就给你一些API，我是不太喜欢的。或者找找他以前部分有没有开源，这样在他以前的代码上改改。
他这里自己也说了
https://blog.csdn.net/weixin_40767422/article/details/88081309
姿态解算是哪些呢，他在用户手册里面也有写，正是我找到的这个
是的，你关心的那些函数都在这个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传感器
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 );
//获取当前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 );
//指定序号传感器健康度
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 );
//获取解算系统状态
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
