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

50
buzz_scripts/include/uavstates.bzz
View File

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

2
buzz_scripts/testaloneWP.bzz
View File

@ -23,7 +23,7 @@ function step() {
statef() statef()
log("Current state: ", UAVSTATE) # log("Current state: ", UAVSTATE)
} }
# Executed once when the robot (or the simulator) is reset. # 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 = ""; string WPlistname = "";
std::map< int, buzz_utility::RB_struct> targets_map; 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::Pos_struct> neighbors_map;
std::map< int, buzz_utility::neighbors_status> neighbors_status_map; std::map< int, buzz_utility::neighbors_status> neighbors_status_map;
@ -158,13 +159,13 @@ namespace buzzuav_closures{
RB_arr.longitude=lon; RB_arr.longitude=lon;
RB_arr.altitude=alt; RB_arr.altitude=alt;
// Insert elements. // Insert elements.
map< int, buzz_utility::RB_struct >::iterator it = targets_map.find(tid); map< int, buzz_utility::RB_struct >::iterator it = wplist_map.find(tid);
if(it!=targets_map.end()) if(it!=wplist_map.end())
targets_map.erase(it); wplist_map.erase(it);
targets_map.insert(make_pair(tid, RB_arr)); 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: // Close the file:
fin.close(); fin.close();
@ -213,23 +214,23 @@ namespace buzzuav_closures{
rb_from_gps(tmp, rb, cur_pos); rb_from_gps(tmp, rb, cur_pos);
//ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]); //ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
buzzvm_push(vm, targettbl); 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); //ROS_INFO("Buzz_utility will save user %i.", it->first);
/* Push user id */ // Push user id
buzzvm_pushi(vm, it->first); buzzvm_pushi(vm, it->first);
/* Create entry table */ // Create entry table
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE); buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
/* Insert range */ // Insert range
buzzvm_push(vm, entry); buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, rb[0]); buzzvm_pushf(vm, rb[0]);
buzzvm_tput(vm); buzzvm_tput(vm);
/* Insert longitude */ // Insert longitude
buzzvm_push(vm, entry); buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, rb[1]); buzzvm_pushf(vm, rb[1]);
buzzvm_tput(vm); buzzvm_tput(vm);
/* Save entry into data table */ // Save entry into data table
buzzvm_push(vm, entry); buzzvm_push(vm, entry);
buzzvm_tput(vm); buzzvm_tput(vm);
} }
@ -341,12 +342,12 @@ namespace buzzuav_closures{
goal[1] = buzzvm_stack_at(vm, 2)->f.value; goal[1] = buzzvm_stack_at(vm, 2)->f.value;
goal[2] = buzzvm_stack_at(vm, 1)->f.value; goal[2] = buzzvm_stack_at(vm, 1)->f.value;
if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){ if(goal[0]==-1 && goal[1]==-1 && goal[2]==-1){
if(targets_map.size()<=0) if(wplist_map.size()<=0)
parse_gpslist(); parse_gpslist();
goal[0] = targets_map.begin()->second.latitude; goal[0] = wplist_map.begin()->second.latitude;
goal[1] = targets_map.begin()->second.longitude; goal[1] = wplist_map.begin()->second.longitude;
goal[2] = targets_map.begin()->second.altitude; goal[2] = wplist_map.begin()->second.altitude;
targets_map.erase(targets_map.begin()->first); wplist_map.erase(wplist_map.begin()->first);
} }
double rb[3]; double rb[3];
@ -355,7 +356,7 @@ namespace buzzuav_closures{
if(fabs(rb[0])<150.0) if(fabs(rb[0])<150.0)
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]); 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); 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); 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*/ } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
ROS_INFO("TAKING A PICTURE HERE!! --------------"); 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; mavros_msgs::CommandBool capture_command;
capture_srv.call(capture_command); capture_srv.call(capture_command);
} }