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

uavlink test

Browse Source
protobuf
Xuan0319 3 years ago
parent 8afdfec98d
commit fd0f017749
23 changed files with 102 additions and 94 deletions
Show Stats Download Patch File Download Diff File

6
class_model/include/class_model/follower.h
Unescape Escape View File

@ -37,7 +37,7 @@ public:
DroneStatus samePlane[5];
void follower(global_location* member_data, size_t index, int refID);
int find_ref_drone(global_location data[], size_t index, int leaderID);
int find_ref_drone(global_location* data[], size_t index, int leaderID);
float lon[3],lat[3];
@ -62,8 +62,8 @@ private:
float ignore_small = 0.50;
void calculate_position(float k,float theta,int plane);
void plane(global_location target, global_location leader, global_location follower);
int direct(global_location target, global_location leader, DroneStatus self, DroneStatus member);
void plane(global_location* target, global_location* leader, global_location* follower);
int direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member);
void choose_leader();

14
class_model/src/bio_main.cpp
Unescape Escape View File

@ -48,23 +48,23 @@ int main(int argc, char **argv) {
checkLeader = check_leader(*pos,index).is_leader;
if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(true){
// self = request_object.get_self_GPS();
// M1 = request_object.get_M1_GPS();
// M2 = request_object.get_M2_GPS();
// M3 = request_object.get_M3_GPS();
// M4 = request_object.get_M4_GPS();
self = request_object.get_self_GPS();
M1 = request_object.get_M1_GPS();
M2 = request_object.get_M2_GPS();
M3 = request_object.get_M3_GPS();
M4 = request_object.get_M4_GPS();
// pos[] = {target,self,M1,M2,M3,M4};
// member[] = {M1,M2,M3,M4};
if(checkLeader == 1){
// leader_object.leader(target);
}else{
ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID);
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(*member,m_index,ref_ID); //follow reference drone position
}
}

4
class_model/src/bio_main2.cpp
Unescape Escape View File

@ -48,7 +48,7 @@ int main(int argc, char **argv) {
checkLeader = check_leader(*pos,index).is_leader;
if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(true){
@ -64,7 +64,7 @@ int main(int argc, char **argv) {
if(checkLeader == 1){
// leader_object.leader(target);
}else{
ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID);
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(*member,m_index,ref_ID); //follow reference drone position
}
}

4
class_model/src/bio_main3.cpp
Unescape Escape View File

@ -48,7 +48,7 @@ int main(int argc, char **argv) {
checkLeader = check_leader(*pos,index).is_leader;
if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(true){
@ -64,7 +64,7 @@ int main(int argc, char **argv) {
if(checkLeader == 1){
// leader_object.leader(target);
}else{
ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID);
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(*member,m_index,ref_ID); //follow reference drone position
}
}

4
class_model/src/bio_main4.cpp
Unescape Escape View File

@ -48,7 +48,7 @@ int main(int argc, char **argv) {
checkLeader = check_leader(*pos,index).is_leader;
if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(true){
@ -64,7 +64,7 @@ int main(int argc, char **argv) {
if(checkLeader == 1){
// leader_object.leader(target);
}else{
ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID);
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(*member,m_index,ref_ID); //follow reference drone position
}
}

4
class_model/src/bio_main5.cpp
Unescape Escape View File

@ -48,7 +48,7 @@ int main(int argc, char **argv) {
checkLeader = check_leader(*pos,index).is_leader;
if(checkLeader != 1){
ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID); //find_ref_drone(pos,index,leader_ID);
}
while(true){
@ -64,7 +64,7 @@ int main(int argc, char **argv) {
if(checkLeader == 1){
// leader_object.leader(target);
}else{
ref_ID = follower_object.find_ref_drone(*pos,index,check_leader(*pos,index).leader_ID);
ref_ID = follower_object.find_ref_drone(pos,index,check_leader(*pos,index).leader_ID);
follower_object.follower(*member,m_index,ref_ID); //follow reference drone position
}
}

134
class_model/src/follower.cpp
Unescape Escape View File

@ -43,80 +43,80 @@ void FollowerClass::follower(global_location* member_data, size_t index, int ref
//}
}
void FollowerClass::plane(global_location target, global_location leader, global_location follower){
void FollowerClass::plane(global_location* target, global_location* leader, global_location* follower){
//caculate drone is locate on R/L plane
float slope,c,y;
slope = (target.lat-leader.lat)/(target.lon-leader.lon);
c = leader.lat - slope*leader.lon;
y = slope * follower.lon + c;
slope = (target->lat-leader->lat)/(target->lon-leader->lon);
c = leader->lat - slope*leader->lon;
y = slope * follower->lon + c;
// std::cout<< follower.ID << ","<< follower.lat <<","<< y <<std::endl;
if(follower.lat <= y){
if(follower->lat <= y){
// std::cout<< follower.lat <<","<< y <<std::endl;
switch (follower.ID){
switch (follower->ID){
case 1:
drone1.init_location.lat = follower.lat;
drone1.init_location.lon = follower.lon;
drone1.ID = follower.ID;
drone1.init_location.lat = follower->lat;
drone1.init_location.lon = follower->lon;
drone1.ID = follower->ID;
drone1.plane = 1; //right plane
break;
case 2:
drone2.init_location.lat = follower.lat;
drone2.init_location.lon = follower.lon;
drone2.ID = follower.ID;
drone2.init_location.lat = follower->lat;
drone2.init_location.lon = follower->lon;
drone2.ID = follower->ID;
drone2.plane = 1;
break;
case 3:
drone3.init_location.lat = follower.lat;
drone3.init_location.lon = follower.lon;
drone3.ID = follower.ID;
drone3.init_location.lat = follower->lat;
drone3.init_location.lon = follower->lon;
drone3.ID = follower->ID;
drone3.plane = 1;
break;
case 4:
drone4.init_location.lat = follower.lat;
drone4.init_location.lon = follower.lon;
drone4.ID = follower.ID;
drone4.init_location.lat = follower->lat;
drone4.init_location.lon = follower->lon;
drone4.ID = follower->ID;
drone4.plane = 1;
break;
case 5:
drone5.init_location.lat = follower.lat;
drone5.init_location.lon = follower.lon;
drone5.ID = follower.ID;
drone5.init_location.lat = follower->lat;
drone5.init_location.lon = follower->lon;
drone5.ID = follower->ID;
drone5.plane = 1;
break;
}
}else if(follower.lat > y){
}else if(follower->lat > y){
// std::cout<< follower.lat <<","<< y <<std::endl;
switch (follower.ID){
switch (follower->ID){
case 1:
drone1.init_location.lat = follower.lat;
drone1.init_location.lon = follower.lon;
drone1.ID = follower.ID;
drone1.init_location.lat = follower->lat;
drone1.init_location.lon = follower->lon;
drone1.ID = follower->ID;
drone1.plane = -1; //left plane
break;
case 2:
drone2.init_location.lat = follower.lat;
drone2.init_location.lon = follower.lon;
drone2.ID = follower.ID;
drone2.init_location.lat = follower->lat;
drone2.init_location.lon = follower->lon;
drone2.ID = follower->ID;
drone2.plane = -1;
break;
case 3:
drone3.init_location.lat = follower.lat;
drone3.init_location.lon = follower.lon;
drone3.ID = follower.ID;
drone3.init_location.lat = follower->lat;
drone3.init_location.lon = follower->lon;
drone3.ID = follower->ID;
drone3.plane = -1;
break;
case 4:
drone4.init_location.lat = follower.lat;
drone4.init_location.lon = follower.lon;
drone4.ID = follower.ID;
drone4.init_location.lat = follower->lat;
drone4.init_location.lon = follower->lon;
drone4.ID = follower->ID;
drone4.plane = -1;
break;
case 5:
drone5.init_location.lat = follower.lat;
drone5.init_location.lon = follower.lon;
drone5.ID = follower.ID;
drone5.init_location.lat = follower->lat;
drone5.init_location.lon = follower->lon;
drone5.ID = follower->ID;
drone5.plane = -1;
break;
}
@ -125,12 +125,12 @@ void FollowerClass::plane(global_location target, global_location leader, global
}
int FollowerClass::direct(global_location target, global_location leader, DroneStatus self, DroneStatus member){
int FollowerClass::direct(global_location* target, global_location* leader, DroneStatus self, DroneStatus member){
global_location vector_LT,vector_SM;
float m,n,k,cosTheta;
vector_LT.lon = target.lon - leader.lon; //vector_LT (leader->target)
vector_LT.lat = target.lat - leader.lat;
vector_LT.lon = target->lon - leader->lon; //vector_LT (leader->target)
vector_LT.lat = target->lat - leader->lat;
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
@ -146,40 +146,40 @@ int FollowerClass::direct(global_location target, global_location leader, DroneS
}
int FollowerClass::find_ref_drone(global_location data[], size_t index, int leaderID){
int FollowerClass::find_ref_drone(global_location* data[], size_t index, int leaderID){
for(int i=1 ; i<index ; i++){ //check leader drone
if(leaderID == data[i].ID){
leader_drone = data[i];
if(leaderID == data[i]->ID){
leader_drone = *data[i];
switch(leaderID){
case 1:
drone1.init_location.lat = data[i].lat;
drone1.init_location.lon = data[i].lon;
drone1.ID = data[i].ID;
drone1.init_location.lat = data[i]->lat;
drone1.init_location.lon = data[i]->lon;
drone1.ID = data[i]->ID;
drone1.plane = 0; //center
break;
case 2:
drone2.init_location.lat = data[i].lat;
drone2.init_location.lon = data[i].lon;
drone2.ID = data[i].ID;
drone2.init_location.lat = data[i]->lat;
drone2.init_location.lon = data[i]->lon;
drone2.ID = data[i]->ID;
drone2.plane = 0;
break;
case 3:
drone3.init_location.lat = data[i].lat;
drone3.init_location.lon = data[i].lon;
drone3.ID = data[i].ID;
drone3.init_location.lat = data[i]->lat;
drone3.init_location.lon = data[i]->lon;
drone3.ID = data[i]->ID;
drone3.plane = 0;
break;
case 4:
drone4.init_location.lat = data[i].lat;
drone4.init_location.lon = data[i].lon;
drone4.ID = data[i].ID;
drone4.init_location.lat = data[i]->lat;
drone4.init_location.lon = data[i]->lon;
drone4.ID = data[i]->ID;
drone4.plane = 0;
break;
case 5:
drone5.init_location.lat = data[i].lat;
drone5.init_location.lon = data[i].lon;
drone5.ID = data[i].ID;
drone5.init_location.lat = data[i]->lat;
drone5.init_location.lon = data[i]->lon;
drone5.ID = data[i]->ID;
drone5.plane = 0;
break;
@ -189,41 +189,41 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead
}
for(int i=1 ; i<index ; i++){
if(data[i].ID != leader_drone.ID){ //caculate the plane of each drone
plane(data[0],leader_drone,data[i]);
if(data[i]->ID != leader_drone.ID){ //caculate the plane of each drone
plane(data[0],&leader_drone,data[i]);
}
}
int j=0,s_index=0;
if(drone1.ID == data[1].ID){ //seperate self and other
if(drone1.ID == data[1]->ID){ //seperate self and other
self = drone1; //problem
}else{
memberDrone[j] = drone1;
j+=1;
// std::cout << "drone1" <<std::endl;
}
if(drone2.ID == data[1].ID){
if(drone2.ID == data[1]->ID){
self = drone2;
}else{
memberDrone[j] = drone2;
j+=1;
// std::cout << "drone2" <<std::endl;
}
if(drone3.ID == data[1].ID){
if(drone3.ID == data[1]->ID){
self = drone3;
}else{
memberDrone[j] = drone3;
j+=1;
// std::cout << "drone3" <<std::endl;
}
if(drone4.ID == data[1].ID){
if(drone4.ID == data[1]->ID){
self = drone4;
}else{
memberDrone[j] = drone4;
j+=1;
// std::cout << "drone4" <<std::endl;
}
if(drone5.ID == data[1].ID){
if(drone5.ID == data[1]->ID){
self = drone5;
}else{
memberDrone[j] = drone5;
@ -249,7 +249,7 @@ int FollowerClass::find_ref_drone(global_location data[], size_t index, int lead
// std::cout <<"s_index " <<s_index <<std::endl;
for(int i=0 ; i<s_index ; i++){ //caculate each same plane mamber's distance
dir = direct(data[0],leader_drone,self,samePlane[i]);
dir = direct(data[0],&leader_drone,self,samePlane[i]);
d[i] = dir * sqrt( pow((self.init_location.lat - samePlane[i].init_location.lat ),2) + pow((self.init_location.lon - samePlane[i].init_location.lon),2) );
// std::cout << i<<"," <<d[i] <<std::endl;

4
class_model/src/mqttPubMain5.py
Unescape Escape View File

@ -61,7 +61,6 @@ if __name__ == '__main__':
FilePath = os.path.join(os.path.dirname(__file__),"utils","mqttConfig_PUB5.yml")
cfg = utils.Read_PUB_Config(FilePath)
client = utils.MQTTClient(cfg.MQTTClientNamePub)
# set log
log_format = "%(asctime)s - %(levelname)s - %(message)s"
formatter = logging.Formatter(log_format)
@ -78,7 +77,8 @@ if __name__ == '__main__':
logger.setLevel(logging.INFO)
logger.addHandler(file_handler)
logger.addHandler(stream_handler)
logger.info(cfg)
# logger.info(cfg)
print(cfg.keepalive)
# Mqtt
client.on_connect = on_connect

3
class_model/src/test_lib.cpp
Unescape Escape View File

@ -259,9 +259,6 @@ int main(int argc,char** argv){
global_location data[]={target,self,M1,M2};
size_t index = sizeof(data)/sizeof(data[0]);
int x = 0;
int y = 0;
int* arr[] = {&x,&y};
find_ref_drone(data,index,check_leader(data,index).leader_ID);

2
class_model/src/uavlinkPubMain.py
Unescape Escape View File

@ -24,7 +24,7 @@ from geometry_msgs.msg import Vector3
def init_dataFormat(cfg:Read_PUB_Config):
ros_namespace="/drone1"
f1data = b'\xf1drone550..............\r\n'
sel = serial.Serial(cfg.port, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE)
sel = serial.Serial(cfg.ttyport, cfg.baudrate, serial.EIGHTBITS, serial.PARITY_NONE, serial.STOPBITS_ONE)
sel.write(f1data)
Proto_msg_from_ros.sel = sel
Proto_msg_from_ros.flight_information_msg = flight_information_pb2.flight_information_message()

1
class_model/src/utils/mqttConfig_CMD.yml
Unescape Escape View File

@ -13,3 +13,4 @@ ROS:
ROStopicName_Cmd_Broadcast_Receiver: cmd_receiver
LOG:
logFileName: cmd.log
UAVLINK: "None"

1
class_model/src/utils/mqttConfig_PUB.yml
Unescape Escape View File

@ -20,3 +20,4 @@ ROS:
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
UAVLINK: "None"

1
class_model/src/utils/mqttConfig_PUB2.yml
Unescape Escape View File

@ -20,3 +20,4 @@ ROS:
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
UAVLINK: "None"

1
class_model/src/utils/mqttConfig_PUB3.yml
Unescape Escape View File

@ -20,3 +20,4 @@ ROS:
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
UAVLINK: "None"

1
class_model/src/utils/mqttConfig_PUB4.yml
Unescape Escape View File

@ -20,3 +20,4 @@ ROS:
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
UAVLINK: "None"

1
class_model/src/utils/mqttConfig_PUB5.yml
Unescape Escape View File

@ -20,3 +20,4 @@ ROS:
ROStopicName_Fly_Formation: Fly_Formation_reciver
LOG:
logFileName: pub.log
UAVLINK: "None"

1
class_model/src/utils/mqttConfig_SUB.yml
Unescape Escape View File

@ -35,3 +35,4 @@ ROS:
Dron555_ROStopicName_Flight_Information: Dron555_Flight_Information_reciver
LOG:
logFileName: sub.log
UAVLINK: "None"

1
class_model/src/utils/mqttConfig_SUB2.yml
Unescape Escape View File

@ -35,3 +35,4 @@ ROS:
Dron555_ROStopicName_Flight_Information: Dron555_Flight_Information_reciver
LOG:
logFileName: sub.log
UAVLINK: "None"

1
class_model/src/utils/mqttConfig_SUB3.yml
Unescape Escape View File

@ -34,3 +34,4 @@ ROS:
Dron555_ROStopicName_Flight_Information: Dron555_Flight_Information_reciver
LOG:
logFileName: sub.log
UAVLINK: "None"

1
class_model/src/utils/mqttConfig_SUB4.yml
Unescape Escape View File

@ -36,3 +36,4 @@ ROS:
Dron555_ROStopicName_Flight_Information: Dron555_Flight_Information_reciver
LOG:
logFileName: sub.log
UAVLINK: "None"

1
class_model/src/utils/mqttConfig_SUB5.yml
Unescape Escape View File

@ -36,3 +36,4 @@ ROS:
Dron555_ROStopicName_Flight_Information: Dron555_Flight_Information_reciver
LOG:
logFileName: sub.log
UAVLINK: "None"

2
class_model/src/utils/readConfig.py
Unescape Escape View File

@ -65,7 +65,7 @@ class Read_PUB_Config(Config):
"msg_format": (str,False),
"uav_id": (str,False),
"baudrate": (int,False),
"port": (str,False)}}
"ttyport": (str,False)}}
self.setAttribute()
def __str__(self):

2
class_model/src/utils/uavlinkConfig_PUB.yml
Unescape Escape View File

@ -2,7 +2,7 @@ UAVLINK:
msg_format: Proto
uav_id: \x01\x01
baudrate: 250000
port: /dev/ttyUSB0
ttyport: /dev/ttyUSB0
MQTT: "None"
#ROS
ROS:

Write Preview
Loading…
Cancel
Save