Spiri/ROSBuzz_MISTLab
13
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

arm/disarm

Browse Source
This commit is contained in:
David St-Onge 2017-02-23 11:49:10 -05:00
parent def06f4c7e
commit 9461d4a55c
7 changed files with 44 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
include/buzzuav_closures.h
View File

@ -46,6 +46,7 @@ void set_obstacle_dist(float dist[]);
* Commands the UAV to takeoff
*/
int buzzuav_takeoff(buzzvm_t vm);
int buzzuav_arm(buzzvm_t vm);
/* Commands the UAV to land
*/

1
include/roscontroller.h
View File

@ -53,6 +53,7 @@ private:
int robot_id=0;
//int oldcmdID=0;
int rc_cmd;
int armstate;
int barrier;
int message_number=0;
std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;

2
launch/launch_config/m100.yaml
View File

@ -3,7 +3,7 @@ topics:
battery : /power_status
status : /flight_status
fcclient : /dji_mavcmd
armclient: /dji_mavarming
armclient: /dji_mavarm
modeclient: /dji_mavmode
type:
gps : sensor_msgs/NavSatFix

6
src/buzz_utility.cpp
View File

@ -239,6 +239,9 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_goto));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
buzzvm_gstore(VM);
@ -268,6 +271,9 @@ namespace buzz_utility{
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_goto", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
buzzvm_gstore(VM);

7
src/buzzuav_closures.cpp
View File

@ -170,6 +170,13 @@ int buzzuav_goto(buzzvm_t vm) {
buzz_cmd=2;
return buzzvm_ret0(vm);
}
int buzzuav_arm(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm/Disarm \n");
buzz_cmd=3;
return buzzvm_ret0(vm);
}
/******************************/
double* getgoto() {

47
src/roscontroller.cpp
View File

@ -13,21 +13,6 @@ namespace rosbzz_node{
/*Compile the .bzz file to .basm, .bo and .bdbg*/
Compile_bzz();
set_bzz_file(bzzfile_name.c_str());
/* double temp_gps[3]={45.564898,-73.563072,10.1};
double temp2_gps[3]={45.565246,-73.561951,10.1};
double temp_car[3], temp2_car[3];
double out[3], dif[3], out2[3];
cvt_cartesian_coordinates(temp_gps,temp_car);
printf("TEST FIRST: %.7f,%.7f,%.7f\n",temp_car[0],temp_car[1],temp_car[2]);
cvt_cartesian_coordinates(temp2_gps,temp2_car);
printf("TEST THIRD: %.7f,%.7f,%.7f\n",temp2_car[0],temp2_car[1],temp2_car[2]);
for(int i=0;i<3;i++)
dif[i]=temp2_car[i]-temp_car[i];
cvt_spherical_coordinates(dif,out);
cvt_rangebearingGB_coordinates(temp2_gps,out2,temp_gps);
// cvt_rangebearing2D_coordinates(temp2_gps,out2,temp_gps);
printf("TEST RESULT: %.7f,%.7f,%.7f\n",out[0],out[1],out[2]);
printf("TEST RESULT2: %.7f,%.7f,%.7f\n",out2[0],out2[1],out2[2]);*/
}
/***Destructor***/
@ -39,7 +24,7 @@ namespace rosbzz_node{
/* Stop the robot */
uav_done();
}
/*rosbuzz_node run*/
void roscontroller::RosControllerRun(){
/* Set the Buzz bytecode */
@ -228,11 +213,14 @@ namespace rosbzz_node{
void roscontroller::Arm(){
mavros_msgs::CommandBool arming_message;
arming_message.request.value = true;
arming_message.request.value = armstate;
if(arm_client.call(arming_message)) {
ROS_INFO("Service called!");
if(arming_message.response.success==1)
ROS_INFO("FC Arm Service called!");
else
ROS_INFO("FC Arm Service call failed!");
} else {
ROS_INFO("Service call failed!");
ROS_INFO("FC Arm Service call failed!");
}
}
@ -241,9 +229,9 @@ namespace rosbzz_node{
set_mode_message.request.base_mode = 0;
set_mode_message.request.custom_mode = "GUIDED"; // shit!
if(mode_client.call(set_mode_message)) {
ROS_INFO("Service called!");
ROS_INFO("Set Mode Service call successful!");
} else {
ROS_INFO("Service call failed!");
ROS_INFO("Set Mode Service call failed!");
}
}
@ -279,6 +267,8 @@ namespace rosbzz_node{
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else ROS_ERROR("Failed to call service from flight controller");
} else if (tmp == 3) { /*FC call for arm*/
Arm();
}
/*obtain Pay load to be sent*/
//fprintf(stdout, "before getting msg from utility\n");
@ -696,7 +686,10 @@ namespace rosbzz_node{
rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
/* arming */
SetMode();
Arm();
if(!armstate) {
armstate =1;
Arm();
}
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
@ -706,6 +699,16 @@ namespace rosbzz_node{
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM:
rc_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
armstate = req.param1;
if(armstate)
ROS_INFO("RC_Call: ARM!!!!");
else
ROS_INFO("RC_Call: DISARM!!!!");
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
ROS_INFO("RC_Call: GO HOME!!!!");
rc_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;

3
src/testflockfev.bzz
View File

@ -156,6 +156,9 @@ function step() {
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
uav_goto()
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
uav_arm()
}
statef()
log("Current state: ", CURSTATE)