Spiri
/
ROSBuzz_MISTLab
12
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 */ /*Sets goto position */
void set_goto(double pos[]); void set_goto(double pos[]);
/*Sets goto position from rc client*/ /*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 */ /*sets rc requested command */
void rc_call(int rc_cmd); void rc_call(int rc_cmd);
/* sets the battery state */ /* sets the battery state */

1
include/roscontroller.h
View File

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

32
src/buzzuav_closures.cpp
View File

@ -16,20 +16,22 @@ namespace buzzuav_closures{
//void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin); //void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
static double goto_pos[3]; static double goto_pos[3];
static double rc_goto_pos[3]; static double rc_goto_pos[3];
static float rc_gimbal[2];
static float batt[3]; static float batt[3];
static float obst[5]={0,0,0,0,0}; static float obst[5]={0,0,0,0,0};
static double cur_pos[3]; static double cur_pos[3];
static uint8_t status; static uint8_t status;
static int cur_cmd = 0; static int cur_cmd = 0;
static int rc_cmd=0; static int rc_cmd=0;
static int rc_id=-1;
static int buzz_cmd=0; static int buzz_cmd=0;
static float height=0; static float height=0;
static bool deque_full = false; static bool deque_full = false;
static int rssi = 0; static int rssi = 0;
static int message_number = 0; static int message_number = 0;
static float raw_packet_loss = 0.0; static float raw_packet_loss = 0.0;
static int filtered_packet_loss = 0; static int filtered_packet_loss = 0;
static float api_rssi = 0.0; static float api_rssi = 0.0;
std::map< int, buzz_utility::RB_struct> targets_map; std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::Pos_struct> neighbors_map; std::map< int, buzz_utility::Pos_struct> neighbors_map;
@ -344,12 +346,22 @@ namespace buzzuav_closures{
return cmd; return cmd;
} }
void rc_set_goto(double pos[]) { void rc_set_goto(int id, double longitude, double latitude, double altitude) {
rc_goto_pos[0] = pos[0]; rc_id = id;
rc_goto_pos[1] = pos[1]; rc_goto_pos[0] = latitude;
rc_goto_pos[2] = pos[2]; 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) { void rc_call(int rc_cmd_in) {
rc_cmd = rc_cmd_in; rc_cmd = rc_cmd_in;
} }

76
src/roscontroller.cpp
View File

@ -658,37 +658,37 @@ void roscontroller::flight_controller_service_call()
// SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND // SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND
// mode // mode
switch (buzzuav_closures::getcmd()) { switch (buzzuav_closures::getcmd()) {
case mavros_msgs::CommandCode::NAV_LAND: case mavros_msgs::CommandCode::NAV_LAND:
if (current_mode != "LAND") { if (current_mode != "LAND") {
SetMode("LAND", 0); SetMode("LAND", 0);
armstate = 0;
Arm();
}
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");
}
armstate = 0; armstate = 0;
Arm(); break;
} case mavros_msgs::CommandCode::NAV_TAKEOFF:
if (mav_client.call(cmd_srv)) { if (!armstate) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else {
ROS_ERROR("Failed to call service from flight controller");
}
armstate = 0;
break;
case mavros_msgs::CommandCode::NAV_TAKEOFF:
if (!armstate) {
SetMode("LOITER", 0); SetMode("LOITER", 0);
armstate = 1; armstate = 1;
Arm(); Arm();
ros::Duration(0.5).sleep(); ros::Duration(0.5).sleep();
// Registering HOME POINT. // Registering HOME POINT.
home = cur_pos; home = cur_pos;
} }
if (current_mode != "GUIDED") if (current_mode != "GUIDED")
SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it
// should always be in loiter after arm/disarm) // should always be in loiter after arm/disarm)
if (mav_client.call(cmd_srv)) { if (mav_client.call(cmd_srv)) {
ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
} else } else
ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
break; break;
} }
} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/ } else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/
@ -1121,18 +1121,20 @@ bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request &req,
res.success = true; res.success = true;
break; break;
case mavros_msgs::CommandCode::NAV_WAYPOINT: case mavros_msgs::CommandCode::NAV_WAYPOINT:
ROS_INFO("RC_Call: GO TO!!!! --- Doing this! "); ROS_INFO("RC_Call: GO TO!!!! ");
double rc_goto[3]; buzzuav_closures::rc_set_goto(req.param1,req.param5,req.param6,req.param7);
// testing PositionTarget
rc_goto[0] = req.param5;
rc_goto[1] = req.param6;
rc_goto[2] = req.param7;
buzzuav_closures::rc_set_goto(rc_goto);
rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT; rc_cmd = mavros_msgs::CommandCode::NAV_WAYPOINT;
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);
res.success = true; res.success = true;
break; 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!!!!"); ROS_INFO("RC_Call: Update Fleet Status!!!!");
rc_cmd = 666; rc_cmd = 666;
buzzuav_closures::rc_call(rc_cmd); buzzuav_closures::rc_call(rc_cmd);