I don't quite understand this position speed loop, because this document can't really see anything. It's not as rich as the angle and angular speed rings in theory. At least I haven't seen it yet. Maybe it's not enough for me, but it doesn't prevent us from moving forward. Try to look down. What if there's something to gain?
- First enter the function in the file name''Ano_Scheduler.c', as follows
static void Loop_100Hz(void) //10ms executed once
{
test_rT[0]= GetSysTime_us ();
//////////////////////////////////////////////////////////////////////
/*Data Processing of Remote Controller*/
RC_duty_task(10);
/*Flight mode setting task*/
Flight_Mode_Set(10);
/*Obtaining attitude angle (ROLL PITCH YAW)*/
calculate_RPY();
/*Attitude Angle Loop Control*/
Att_2level_Ctrl(10e-3f,CH_N);
/*Position Velocity Loop Control (Not yet available)*/
Loc_1level_Ctrl(10,CH_N);
/*High Data Fusion Task*/
WCZ_Fus_Task(10);
/*High Speed Loop Control*/
Alt_1level_Ctrl(10e-3f);
/*Altitude loop control*/
Alt_2level_Ctrl(10e-3f);
/*Analysis of Optical Flow Data*/
AnoOF_DataAnl_Task(10);
/*Lighting control*/
LED_Task(10);
//////////////////////////////////////////////////////////////////////
test_rT[1]= GetSysTime_us ();
test_rT[2] = (u32)(test_rT[1] - test_rT[0]) ;
}
Obviously we can see that there is no such two words, but I feel this position speed loop and optical flow in the process of analyzing the source code, and it has something to do with the height loop. What's the relationship? Then look down.
- The source code with the file name "Ano_LocCtrl.c" has been added with some comments, and I don't know if it's right or not. Just take a look.
#include "Ano_LocCtrl.h"// position control, which has always been regarded as logical control==
#include "Ano_Imu.h"
#include "Ano_FlightCtrl.h"
#include "Ano_OF.h"
#include "Ano_Parameter.h"
//What is this position velocity loop for? Seems to be related to optical flow?
//Position control and height control seem to be closely related?
//Control parameters of position velocity loop
_PID_arg_st loc_arg_1[2] ;
//Control Data of Position and Velocity Loop
_PID_val_st loc_val_1[2] ;
//Why does this need to be corrected?
//Modified Control Parameters of Position Velocity Loop
_PID_arg_st loc_arg_1_fix[2] ;
//Modified Control Data of Position and Velocity Loop
_PID_val_st loc_val_1_fix[2] ;
/*Initialization of PID parameters in position-velocity loop*/
void Loc_1level_PID_Init()
{
loc_arg_1[X].kp = Ano_Parame.set.pid_loc_1level[KP];//0.22f ;
loc_arg_1[X].ki = 0.0f ;
loc_arg_1[X].kd_ex = 0.00f ;
loc_arg_1[X].kd_fb = Ano_Parame.set.pid_loc_1level[KD];
loc_arg_1[X].k_ff = 0.02f;
loc_arg_1[Y] = loc_arg_1[X];
//fix
loc_arg_1_fix[X].kp = 0.0f ;
loc_arg_1_fix[X].ki = Ano_Parame.set.pid_loc_1level[KI] ;
loc_arg_1_fix[X].kd_ex = 0.00f;
loc_arg_1_fix[X].kd_fb = 0.00f;
loc_arg_1_fix[X].k_ff = 0.0f;
loc_arg_1_fix[Y] = loc_arg_1_fix[X];
}
//These two structures are very important. On the one hand, they provide expected and feedback values for the following PID functions.
//On the other hand, there is a clear relationship between the height loop, because its expected and feedback values are applied to the cascade PID of the height loop.
//Specifically introduced in the Highly Ring Document
_loc_ctrl_st loc_ctrl_1;
_loc_ctrl_st loc_ctrl_2;
static float fb_speed_fix[2];
//All of the following uses loc_ctrl_1 instead of loc_ctrl_2. I don't know why.
//However, both of these structures are applied to the high loop, and note that the feed forward in PID is no longer zero.
/*Positional Velocity Loop*/
void Loc_1level_Ctrl(u16 dT_ms,s16 *CH_N)
{
//If the optical flow is turned on
if(switchs.of_flow_on)
{
//Bring all the streams of light.
loc_ctrl_1.exp[X] = fs.speed_set_h[X]; //X = 0 / Y = 1
loc_ctrl_1.exp[Y] = fs.speed_set_h[Y];
loc_ctrl_1.fb[X] = OF_DX2;
loc_ctrl_1.fb[Y] = OF_DY2;
fb_speed_fix[0] = OF_DX2FIX;
fb_speed_fix[1] = OF_DY2FIX;
for(u8 i =0;i<2;i++)
{
//Main PID
//The input parameters of the two structures are conventional, as before, but the expectations and feedback call the values in the optical flow.
//Anonymous optical flow is a bit special, but I don't know about optical flow, so I won't go into it deeply. I'm interested in it.
//As above, X = 0 / Y = 1 corresponds to the i value in this loop, but why are two PID s?
PID_calculate( dT_ms*1e-3f, //Period (in seconds)
loc_ctrl_1.exp[i] , //Feedforward value
loc_ctrl_1.exp[i] , //Expectations (set values)
loc_ctrl_1.fb[i] , //Feedback value ()
&loc_arg_1[i], //PID parameter structure
&loc_val_1[i], //PID Data Structure
50, //Integral error limit
10 *flag.taking_off //integration limit
) ;
//Modified PID
//== As mentioned above, why should we amend it? And why does this need to be corrected?
PID_calculate( dT_ms*1e-3f, //Period (in seconds)
loc_ctrl_1.exp[i] , //Feedforward value
loc_ctrl_1.exp[i] , //Expectations (set values)
fb_speed_fix[i] , //Feedback value ()
&loc_arg_1_fix[i], //PID parameter structure
&loc_val_1_fix[i], //PID Data Structure
50, //Integral error limit
10 *flag.taking_off //integration limit
) ;
//Get the final output
//Position Control Output = Output of Position Value + Output of Position Correction Value
loc_ctrl_1.out[i] = loc_val_1[i].out + loc_val_1_fix[i].out;
}
}
else
{
//If you don't turn on the optical flow
loc_ctrl_1.out[X] = (float)MAX_ANGLE/MAX_SPEED *fs.speed_set_h[X] ;
loc_ctrl_1.out[Y] = (float)MAX_ANGLE/MAX_SPEED *fs.speed_set_h[Y] ;
}
}
This file is very short, maybe it's really useless as I said at the beginning, and I don't understand where this position speed loop is used? Because that angle loop and angular speed loop are used for PID control of three remote control communication channels, and the height loop and the high speed loop are used for PID control of throttle channel. If this position speed loop is used in more complex control, I think it can be used. Put it in one place, first realize the basic functions, then you can study it when you have time, or you can find the answer from the height ring?