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

integrate PY comments and start waypoints control

This commit is contained in:
dave 2017-08-18 17:13:57 -04:00
parent 97e0a2c4b2
commit 5804aace94
4 changed files with 65 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/buzzuav_closures.h
View File

@ -42,7 +42,9 @@ int getcmd();
/*Sets goto position */
void set_goto(double pos[]);
/*Sets goto position from rc client*/
void rc_set_goto(double pos[]);
void rc_set_goto(int id, double latitude, double longitude, double altitude);
/*Sets gimbal orientation from rc client*/
void rc_set_gimbal(int id, float yaw, float pitch);
/*sets rc requested command */
void rc_call(int rc_cmd);
/* sets the battery state */

1
include/roscontroller.h
View File

@ -39,6 +39,7 @@
#define XBEE_STOP_TRANSMISSION 4355356352
#define TIMEOUT 60
#define BUZZRATE 10
#define CMD_REQUEST_UPDATE 666
using namespace std;

20
src/buzzuav_closures.cpp
View File

@ -16,12 +16,14 @@ namespace buzzuav_closures{
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
static double goto_pos[3];
static double rc_goto_pos[3];
static float rc_gimbal[2];
static float batt[3];
static float obst[5]={0,0,0,0,0};
static double cur_pos[3];
static uint8_t status;
static int cur_cmd = 0;
static int rc_cmd=0;
static int rc_id=-1;
static int buzz_cmd=0;
static float height=0;
static bool deque_full = false;
@ -344,12 +346,22 @@ namespace buzzuav_closures{
return cmd;
}
void rc_set_goto(double pos[]) {
rc_goto_pos[0] = pos[0];
rc_goto_pos[1] = pos[1];
rc_goto_pos[2] = pos[2];
void rc_set_goto(int id, double longitude, double latitude, double altitude) {
rc_id = id;
rc_goto_pos[0] = latitude;
rc_goto_pos[1] = longitude;
rc_goto_pos[2] = altitude;
}
void rc_set_gimbal(int id, float yaw, float pitch) {
rc_id = id;
rc_gimbal[0] = yaw;
rc_gimbal[1] = pitch;
}
void rc_call(int rc_cmd_in) {
rc_cmd = rc_cmd_in;
}

18
src/roscontroller.cpp
View File

@ -1121,18 +1121,20 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
res.success = true;
break;
case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! --- Doing this! ");
double rc_goto[3];
// testing PositionTarget
rc_goto[0] = req.param5;
rc_goto[1] = req.param6;
rc_goto[2] = req.param7;
buzzuav_closures::rc_set_goto(rc_goto);
ROS_INFO("RC_Call: GO TO!!!! ");
buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7);
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case 666:
case mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL:
ROS_INFO("RC_Call: Gimbal!!!! ");
buzzuav_closures::rc_set_gimbal(req.param1,req.param2,req.param3);
rc_cmd = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
buzzuav_closures::rc_call(rc_cmd);
res.success = true;
break;
case CMD_REQUEST_UPDATE:
ROS_INFO("RC_Call: Update Fleet Status!!!!");
rc_cmd = 666;
buzzuav_closures::rc_call(rc_cmd);