diff --git a/include/buzzuav_closures.h b/include/buzzuav_closures.h index c205c47..d1f7619 100644 --- a/include/buzzuav_closures.h +++ b/include/buzzuav_closures.h @@ -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 */ diff --git a/include/roscontroller.h b/include/roscontroller.h index 31e7537..ae0b7d0 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -39,6 +39,7 @@ #define XBEE_STOP_TRANSMISSION 4355356352 #define TIMEOUT 60 #define BUZZRATE 10 +#define CMD_REQUEST_UPDATE 666 using namespace std; diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp index 019fe35..da8d9b4 100644 --- a/src/buzzuav_closures.cpp +++ b/src/buzzuav_closures.cpp @@ -16,20 +16,22 @@ 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; - static int rssi = 0; - static int message_number = 0; - static float raw_packet_loss = 0.0; - static int filtered_packet_loss = 0; - static float api_rssi = 0.0; + static bool deque_full = false; + static int rssi = 0; + static int message_number = 0; + static float raw_packet_loss = 0.0; + static int filtered_packet_loss = 0; + static float api_rssi = 0.0; std::map< int, buzz_utility::RB_struct> targets_map; std::map< int, buzz_utility::Pos_struct> neighbors_map; @@ -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; } diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index ce77de7..3a19b05 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -658,37 +658,37 @@ void roscontroller::flight_controller_service_call() // SOLO SPECIFIC: SITL DOES NOT USE 21 TO LAND -- workaround: set to LAND // mode switch (buzzuav_closures::getcmd()) { - case mavros_msgs::CommandCode::NAV_LAND: - if (current_mode != "LAND") { - SetMode("LAND", 0); + case mavros_msgs::CommandCode::NAV_LAND: + if (current_mode != "LAND") { + 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; - 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; - break; - case mavros_msgs::CommandCode::NAV_TAKEOFF: - if (!armstate) { + break; + case mavros_msgs::CommandCode::NAV_TAKEOFF: + if (!armstate) { - SetMode("LOITER", 0); - armstate = 1; - Arm(); - ros::Duration(0.5).sleep(); - // Registering HOME POINT. - home = cur_pos; - } - if (current_mode != "GUIDED") - SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it - // should always be in loiter after arm/disarm) - 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"); - break; + SetMode("LOITER", 0); + armstate = 1; + Arm(); + ros::Duration(0.5).sleep(); + // Registering HOME POINT. + home = cur_pos; + } + if (current_mode != "GUIDED") + SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it + // should always be in loiter after arm/disarm) + 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"); + break; } } 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; 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);