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

本文主要是介绍我觉得还是把ACfly的传感器的逻辑弄清楚，这样再去二次开发好一些。（折腾半天发现有很关键一部分没有开源，怪不得找不到，这让我很失望），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

我觉得还是把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;elseTIMEOUT_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;elsedata_effective = true;break;case Position_Sensor_DataType_s_z:if( __ARM_isnan( position.z ) || __ARM_isinf( position.z ) )data_effective = false;elsedata_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;elsedata_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;elsee_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);elsee_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;elsee_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);elsee_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;}}elselock_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;elsee_1 = 0;smooth_kp_d1 d2;if( Is_AutoMode(HorizontalPosition_ControlMode) )d2 = smooth_kp_1( e2_length, e_1 , Pv, cfg.maxAutoAccXY[0] );elsed2 = 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] );elsed3 = 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] );elseT4.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;//旋转至BodyheadingQuaternion 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 );//获取真实目标角度修正TAccAttitude_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];elseTVel = 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;elseTarget_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;//获取倾角cosinQuaternion 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;}}elseRollOverProtectCounter = 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，我是不太喜欢的。或者找找他以前部分有没有开源，这样在他以前的代码上改改。仔细一想，ACfly的光流融合部分应该也是没有开源的，这么来看还不如无名。

他这里自己也说了

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传感器健康度*///获取当前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