Xuan0319/DroneControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

PI control problem

Browse Source
This commit is contained in:
Xuan0319
2023-03-27 22:35:29 +08:00
parent 414c01a502
commit 3597e3d901
2 changed files with 4 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
class_model/include/class_model/follower.h
+1
View File
@@ -40,6 +40,7 @@ public:
int find_ref_drone(global_location** data, size_t index, int leaderID);
float lon[3],lat[3];
global_location vector_LT,vector_SM;
private:
class_model/src/follower.cpp
+3 -1
View File
@@ -127,11 +127,13 @@ void FollowerClass::plane(global_location* target, global_location* leader, glob
int FollowerClass::direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member){
global_location vector_LT,vector_SM;
float m,n,k,cosTheta;
if(flag == 0){
vector_LT.lon = target->lon - leader->lon; //vector_LT (leader->target)
vector_LT.lat = target->lat - leader->lat;
flag = 1;
}
vector_SM.lon = member.init_location.lon - self.init_location.lon; //vector_SM (self->member)
vector_SM.lat = member.init_location.lat - self.init_location.lat;
k = (vector_LT.lon*vector_SM.lon)+(vector_LT.lat*vector_SM.lat); //k = vector_LT dot vector_SM