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

onboard fix and gimbal ctr

This commit is contained in:
David St-Onge 2017-09-06 18:55:12 -04:00
parent 8b5ba54f71
commit 796bb78dd8
4 changed files with 50 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
buzz_scripts/include/uavstates.bzz
View File

@ -27,27 +27,20 @@ function idle() {
UAVSTATE = "IDLE"
}
firstpassT = 1
function takeoff() {
UAVSTATE = "TAKEOFF"
statef=takeoff
if(firstpassT) {
barrier_create(TAKEOFF_VSTIG)
firstpassT = 0
}
if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
barrier_set(ROBOTS, action, land, -1)
barrier_ready()
} else {
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
log("Altitude: ", TARGET_ALTITUDE)
neighbors.broadcast("cmd", 22)
uav_takeoff(TARGET_ALTITUDE)
}
}
firstpassL = 1
function land() {
UAVSTATE = "LAND"
statef=land
@ -56,26 +49,26 @@ function land() {
uav_land()
if(flight.status != 2 and flight.status != 3){
barrier_set(ROBOTS,turnedoff,land, 21)
barrier_set(ROBOTS,turnedoff,land, 21)
barrier_ready()
}
}
function set_goto(transf) {
UAVSTATE = "GOTO"
UAVSTATE = "GOTOGPS"
statef=function() {
gotoWP(transf);
}
gotoWP(transf);
}
if(rc_goto.id==id){
if(s!=nil){
if(s.in())
s.leave()
}
} else {
neighbors.broadcast("cmd", 16)
neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
}
#if(rc_goto.id==id){
# if(s!=nil){
# if(s.in())
# s.leave()
#}
#} else {
# neighbors.broadcast("cmd", 16)
# neighbors.broadcast("gt", rc_goto.id+rc_goto.longitude+rc_goto.latitude)
#}
}
ptime=0
@ -93,7 +86,7 @@ function picture() {
}
function gotoWP(transf) {
# print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
print(rc_goto.id, " is going alone to lat=", rc_goto.latitude)
rb_from_gps(rc_goto.latitude, rc_goto.longitude)
print(" has to move ", goal.range)
m_navigation = math.vec2.newp(goal.range, goal.bearing)
@ -141,7 +134,7 @@ function uav_rccmd() {
neighbors.broadcast("cmd", 21)
} else if(flight.rc_cmd==16) {
flight.rc_cmd=0
UAVSTATE = "GOTO"
UAVSTATE = "GOTOGPS"
statef = goto
} else if(flight.rc_cmd==400) {
flight.rc_cmd=0
@ -190,7 +183,8 @@ function LimitAngle(angle){
}
function rb_from_gps(lat, lon) {
# print("gps2rb: ",lat,lon,position.latitude,position.longitude)
# print("gps2rb d: ",lat,lon)
# print("gps2rb h: ",position.latitude,position.longitude)
d_lon = lon - position.longitude
d_lat = lat - position.latitude
ned_x = d_lat/180*math.pi * 6371000.0;

2
buzz_scripts/testaloneWP.bzz
View File

@ -23,7 +23,7 @@ function step() {
statef()
log("Current state: ", UAVSTATE)
# log("Current state: ", UAVSTATE)
}
# Executed once when the robot (or the simulator) is reset.

35
src/buzzuav_closures.cpp
View File

@ -35,6 +35,7 @@ namespace buzzuav_closures{
string WPlistname = "";
std::map< int, buzz_utility::RB_struct> targets_map;
std::map< int, buzz_utility::RB_struct> wplist_map;
std::map< int, buzz_utility::Pos_struct> neighbors_map;
std::map< int, buzz_utility::neighbors_status> neighbors_status_map;
@ -158,13 +159,13 @@ namespace buzzuav_closures{
RB_arr.longitude=lon;
RB_arr.altitude=alt;
// Insert elements.
map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(tid);
if(it!=targets_map.end())
targets_map.erase(it);
targets_map.insert(make_pair(tid, RB_arr));
map< int, buzz_utility::RB_struct >::iterator it = wplist_map.find(tid);
if(it!=wplist_map.end())
wplist_map.erase(it);
wplist_map.insert(make_pair(tid, RB_arr));
}
ROS_INFO("----->Saved %i waypoints.", targets_map.size());
ROS_INFO("----->Saved %i waypoints.", wplist_map.size());
// Close the file:
fin.close();
@ -213,23 +214,23 @@ namespace buzzuav_closures{
rb_from_gps(tmp, rb, cur_pos);
//ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
buzzvm_push(vm, targettbl);
/* When we get here, the "targets" table is on top of the stack */
// When we get here, the "targets" table is on top of the stack
//ROS_INFO("Buzz_utility will save user %i.", it->first);
/* Push user id */
// Push user id
buzzvm_pushi(vm, it->first);
/* Create entry table */
// Create entry table
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
/* Insert range */
// Insert range
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, rb[0]);
buzzvm_tput(vm);
/* Insert longitude */
// Insert longitude
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, rb[1]);
buzzvm_tput(vm);
/* Save entry into data table */
// Save entry into data table
buzzvm_push(vm, entry);
buzzvm_tput(vm);
}
@ -341,12 +342,12 @@ namespace buzzuav_closures{
goal[1] = buzzvm_stack_at(vm, 2)->f.value;
goal[2] = buzzvm_stack_at(vm, 1)->f.value;
if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){
if(targets_map.size()<=0)
if(wplist_map.size()<=0)
parse_gpslist();
goal[0] = targets_map.begin()->second.latitude;
goal[1] = targets_map.begin()->second.longitude;
goal[2] = targets_map.begin()->second.altitude;
targets_map.erase(targets_map.begin()->first);
goal[0] = wplist_map.begin()->second.latitude;
goal[1] = wplist_map.begin()->second.longitude;
goal[2] = wplist_map.begin()->second.altitude;
wplist_map.erase(wplist_map.begin()->first);
}
double rb[3];
@ -355,7 +356,7 @@ namespace buzzuav_closures{
if(fabs(rb[0])<150.0)
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
ROS_ERROR("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]);
ROS_WARN("DEBUG ---- %f %f %f (%f %f) %f %f",goal[0],goal[1],goal[2],cur_pos[0],cur_pos[1],rb[0],rb[1]);
return buzzvm_ret0(vm);
}

9
src/roscontroller.cpp
View File

@ -761,6 +761,15 @@ void roscontroller::flight_controller_service_call()
roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
} else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
ROS_INFO("TAKING A PICTURE HERE!! --------------");
cmd_srv.request.param1 = 90;
cmd_srv.request.param2 = 0;
cmd_srv.request.param3 = 0;
cmd_srv.request.param4 = 0;
cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
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");
mavros_msgs::CommandBool capture_command;
capture_srv.call(capture_command);
}