我觉得还是把ACfly的传感器的逻辑弄清楚，这样再去二次开发好一些。（折腾半天发现有很关键一部分没有开源，怪不得找不到，这让我很失望）
本文共 29148 字，大约阅读时间需要 97 分钟。
我觉得还是把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, vector3position, 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 Position; get_Position_Ctrl(&Position); vector3 VelocityENU; get_VelocityENU_Ctrl(&VelocityENU); vector3 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 TAcc; vector3 TargetVelocity; vector3 TargetVelocity_1; vector3 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 e1; e1.x = target_position.x - Position.x; e1.y = target_position.y - Position.y; vector2 e1_1; e1_1.x = - VelocityENU.x; e1_1.y = - VelocityENU.y; vector2 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 T2; vector2 T2_1; vector2 T2_2; if( !is_zero(e1_length*e1_length*e1_length) ) { vector2 n = e1 * (1.0/e1_length); vector2 n_1 = (e1_1*e1_length - e1*e_1) / (e1_length*e1_length); vector2 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 A( target_position.x, target_position.y ); vector2 C( Position.x, Position.y ); vector2 A_C = C - A; vector2 A_B( route_line_A_B.x, route_line_A_B.y ); double k = (A_C * A_B) * route_line_m; vector2 foot_point = (A_B * k) + A; //计算偏差 vector2 e1r = A - foot_point; vector2 e1d = foot_point - C; double e1r_length = safe_sqrt(e1r.get_square()); double e1d_length = safe_sqrt(e1d.get_square()); //计算route方向单位向量 vector2 route_n; if( e1r_length > 0.001 ) route_n = e1r * (1.0/e1r_length); //计算d方向单位向量 vector2 d_n; if( e1d_length > 0.001 ) d_n = e1d * (1.0/e1d_length); //计算e1导数 vector2 e1_1( VelocityENU.x, VelocityENU.y ); double e1r_1 = -(e1_1 * route_n); double e1d_1 = -(e1_1 * d_n); //e1二阶导 vector2 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 T2r = route_n * d1r.d0; vector2 T2r_1 = route_n * d1r.d1; vector2 T2r_2 = route_n * d1r.d2; /*route方向*/ /*d方向*/ smooth_kp_d2 d1d = smooth_kp_2( e1d_length, e1d_1, e1d_2 , Ps, AutoVelXY ); vector2 T2d = d_n * d1d.d0; vector2 T2d_1 = d_n * d1d.d1; vector2 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 A_C = Position - target_position; double k = (A_C * route_line_A_B) * route_line_m; vector3 foot_point = (route_line_A_B * k) + target_position; //计算偏差 vector3 e1r = target_position - foot_point; vector3 e1d = foot_point - Position; double e1r_length = safe_sqrt(e1r.get_square()); double e1d_length = safe_sqrt(e1d.get_square()); //计算route方向单位向量 vector3 route_n; if( e1r_length > 0.001 ) route_n = e1r * (1.0/e1r_length); //计算e1导数 double e1r_1_length = -(VelocityENU * route_n); vector3 e1r_1 = route_n * e1r_1_length; vector3 e1d_1 = -(VelocityENU + e1r_1); //e1二阶导 vector3 e1_2( TAcc.x, TAcc.y, AccelerationENU.z ); double e1r_2_length = -(e1_2 * route_n); vector3 e1r_2 = route_n * e1r_2_length; vector3 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 T2r = route_n * d1r.d0; vector3 T2r_1 = route_n * d1r.d1; vector3 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 T2d; vector3 T2d_1; vector3 T2d_2; if( !is_zero(e1d_length*e1d_length*e1d_length) ) { vector3 n = e1d * (1.0/e1d_length); vector3 n_1 = (e1d_1*e1d_length - e1d*e_1) / (e1d_length*e1d_length); vector3 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 e2; e2.x = TargetVelocity.x - VelocityENU.x; e2.y = TargetVelocity.y - VelocityENU.y; vector2 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 T3; vector2 T3_1; if( !is_zero(e2_length*e2_length) ) { vector2 n = e2 * (1.0/e2_length); vector2 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 ( TargetVelocity_1.x, TargetVelocity_1.y ); T3_1 += vector2 ( TargetVelocity_2.x, TargetVelocity_2.y ); vector2 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 T4; if( !is_zero(e3_length) ) { vector2 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 WindDisturbance; get_WindDisturbance( &WindDisturbance ); vector2 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 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，我是不太喜欢的。或者找找他以前部分有没有开源，这样在他以前的代码上改改。仔细一想，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 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 PositionENU; //传感器位置 vector2 sensor_pos; //传感器偏移（传感器健康时更新） //HOffset+PositionENU = 传感器估计值 vector2 HOffset; //上次健康时间 vector2
说实话我想找一个真开源的飞控，来做SLAM实验。我想起有一个是真开源的，匿名是真开源的。
github上的ACfly似乎开源了这部分，可能放的早期的版本。这还是让我自己有一定折腾的可能性。不对，里面也有两个Lib文件，实际也是没有开源。
转载地址：http://falni.baihongyu.com/
你可能感兴趣的文章
Encoding Schemes
查看>>
带WiringPi库的交叉笔译如何处理二之软链接概念
查看>>
Java8 HashMap集合解析
查看>>
自定义 select 下拉框 多选插件
查看>>
Linux常用统计命令之wc
查看>>
fastcgi_param 详解
查看>>
搞定Java面试中的数据结构问题
查看>>
React Native（一）：搭建开发环境、出Hello World
查看>>
mysql 跨机器查询，使用dblink
查看>>
Winform多线程
查看>>
MongoDB数据库插入、更新和删除操作详解
查看>>
MongoDB文档（Document）全局唯一ID的设计思路
查看>>
Redis持久化存储(AOF与RDB两种模式)
查看>>
memcached工作原理与优化建议
查看>>
Redis与Memcached的区别
查看>>
程序员最核心的竞争力是什么？
查看>>
分布式存储系统设计（1）—— 系统架构
查看>>
MySQL数据库的高可用方案总结
查看>>
Spring AOP + Redis + 注解实现redis 分布式锁
查看>>
【设计模式】学习笔记14：状态模式（State）
查看>>