change in filght controller cmd handling

This commit is contained in:
vivek-shankar 2016-12-24 20:59:00 -05:00
parent be5bd79e16
commit b9fe7a6e2c
5 changed files with 33 additions and 12 deletions

View File

@ -7,6 +7,7 @@
#include "mavros_msgs/CommandCode.h" #include "mavros_msgs/CommandCode.h"
#include "ros/ros.h" #include "ros/ros.h"
namespace buzzuav_closures{ namespace buzzuav_closures{
/* /*
* prextern int() function in Buzz * prextern int() function in Buzz
@ -71,5 +72,7 @@ int buzzuav_update_flight_status(buzzvm_t vm);
*/ */
int buzzuav_update_prox(buzzvm_t vm); int buzzuav_update_prox(buzzvm_t vm);
int bzz_cmd();
//#endif //#endif
} }

View File

@ -11,7 +11,6 @@
#include "sensor_msgs/NavSatStatus.h" #include "sensor_msgs/NavSatStatus.h"
#include <sstream> #include <sstream>
#include <buzz/buzzasm.h> #include <buzz/buzzasm.h>
#include "buzzuav_closures.h"
#include "buzz_utility.h" #include "buzz_utility.h"
#include "uav_utility.h" #include "uav_utility.h"
#include <stdlib.h> #include <stdlib.h>
@ -20,6 +19,7 @@
#include <signal.h> #include <signal.h>
#include <ostream> #include <ostream>
#include <map> #include <map>
#include "buzzuav_closures.h"
using namespace std; using namespace std;
@ -40,9 +40,8 @@ private:
std::map< int, buzz_utility::Pos_struct> neighbours_pos_map; std::map< int, buzz_utility::Pos_struct> neighbours_pos_map;
int timer_step=0; int timer_step=0;
int robot_id=0; int robot_id=0;
int oldcmdID=0; //int oldcmdID=0;
int rc_cmd; int rc_cmd;
int bzz_old_cmd=0;
std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient; std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload; //, rcclient;
bool rcclient; bool rcclient;
ros::ServiceClient mav_client; ros::ServiceClient mav_client;

View File

@ -15,6 +15,7 @@ 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 buzz_cmd=0;
/****************************************/ /****************************************/
/****************************************/ /****************************************/
@ -74,6 +75,7 @@ int buzzuav_goto(buzzvm_t vm) {
goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value; goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value;
cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT; cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
printf(" Buzz requested Go To, to Latitude: %15f , Longitude: %15f, Altitude: %15f \n",goto_pos[0],goto_pos[1],goto_pos[2]); printf(" Buzz requested Go To, to Latitude: %15f , Longitude: %15f, Altitude: %15f \n",goto_pos[0],goto_pos[1],goto_pos[2]);
buzz_cmd=2;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -94,6 +96,12 @@ goto_pos[2]=pos[2];
} }
int bzz_cmd(){
int cmd = buzz_cmd;
buzz_cmd=0;
return cmd;
}
void rc_set_goto(double pos[]){ void rc_set_goto(double pos[]){
rc_goto_pos[0]=pos[0]; rc_goto_pos[0]=pos[0];
rc_goto_pos[1]=pos[1]; rc_goto_pos[1]=pos[1];
@ -110,18 +118,21 @@ rc_cmd=rc_cmd_in;
int buzzuav_takeoff(buzzvm_t vm) { int buzzuav_takeoff(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF; cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
printf(" Buzz requested Take off !!! \n"); printf(" Buzz requested Take off !!! \n");
buzz_cmd=1;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_land(buzzvm_t vm) { int buzzuav_land(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_LAND; cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
printf(" Buzz requested Land !!! \n"); printf(" Buzz requested Land !!! \n");
buzz_cmd=1;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
int buzzuav_gohome(buzzvm_t vm) { int buzzuav_gohome(buzzvm_t vm) {
cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH; cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n"); printf(" Buzz requested gohome !!! \n");
buzz_cmd=1;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
@ -252,4 +263,7 @@ int buzzuav_update_prox(buzzvm_t vm) {
/****************************************/ /****************************************/
/****************************************/ /****************************************/
} }

View File

@ -124,8 +124,15 @@ namespace rosbzz_node{
void roscontroller::prepare_msg_and_publish(){ void roscontroller::prepare_msg_and_publish(){
/* flight controller client call*/ /* flight controller client call if requested from Buzz*/
if (bzz_old_cmd != buzzuav_closures::getcmd()) { /*FC call for takeoff,land and gohome*/
if (buzzuav_closures::bzz_cmd()==1){
cmd_srv.request.command = buzzuav_closures::getcmd();
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");
}
/*FC call for goto*/
else if (buzzuav_closures::bzz_cmd() == 2) {
/*prepare the goto publish message */ /*prepare the goto publish message */
double* goto_pos = buzzuav_closures::getgoto(); double* goto_pos = buzzuav_closures::getgoto();
cmd_srv.request.param1 = goto_pos[0]; cmd_srv.request.param1 = goto_pos[0];
@ -134,7 +141,6 @@ namespace rosbzz_node{
cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
if (mav_client.call(cmd_srv)){ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } 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 ROS_ERROR("Failed to call service from flight controller");
bzz_old_cmd = cmd_srv.request.command;
} }
/*obtain Pay load to be sent*/ /*obtain Pay load to be sent*/
uint64_t* payload_out_ptr= buzz_utility::out_msg_process(); uint64_t* payload_out_ptr= buzz_utility::out_msg_process();

View File

@ -72,7 +72,7 @@ function takeoff() {
barrier_set(ROBOTS,transition_to_land) barrier_set(ROBOTS,transition_to_land)
barrier_ready() barrier_ready()
} }
else else if(flight.status!=3)
uav_takeoff() uav_takeoff()
} }
function land() { function land() {
@ -80,18 +80,17 @@ function land() {
barrier_set(ROBOTS,idle) barrier_set(ROBOTS,idle)
barrier_ready() barrier_ready()
} }
else else if(flight.status!=0 and flight.status!=4)
uav_land() uav_land()
} }
function transition_to_land() { function transition_to_land() {
statef=transition_to_land statef=transition_to_land
if(tim>=100){ if(battery.capacity<50){
print("Low battery! Landing the fleet")
statef=land statef=land
tim=0 neighbors.broadcast("cmd", 21)
}else{
tim=tim+1
} }
} }