我覺得還是把ACfly的感測器的邏輯弄清楚，這樣再去二次開發好一些。（折騰半天發現有很關鍵一部分沒有開源，怪不得找不到，這讓我很失望）
2020-10-03 12:01:25
我覺得還是把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);
/*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賦值的地方。
這個檔案所處的資料夾我之前還真沒注意，莫非這部分不開源的？我看到一個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感測器健康度*/
//獲取當前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
