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

Changes on speed limit, navigation and local pos control.

Browse Source
This commit is contained in:
Gwaihir 2018-07-31 09:25:02 +00:00
parent 861305bfcd
commit 25968cfe6d
3 changed files with 10 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
buzz_scripts/include/act/states.bzz
View File

@ -10,7 +10,7 @@ include "utils/conversions.bzz"
TARGET_ALTITUDE = 15.0 # m. TARGET_ALTITUDE = 15.0 # m.
BVMSTATE = "TURNEDOFF" BVMSTATE = "TURNEDOFF"
PICTURE_WAIT = 20 # steps PICTURE_WAIT = 20 # steps
GOTO_MAXVEL = 0.5 # m/steps GOTO_MAXVEL = 5.0 # m/steps
GOTO_MAXDIST = 150 # m. GOTO_MAXDIST = 150 # m.
GOTODIST_TOL = 2.0 # m. GOTODIST_TOL = 2.0 # m.
GOTOANG_TOL = 0.2 # rad. GOTOANG_TOL = 0.2 # rad.
@ -89,6 +89,7 @@ function take_picture() {
} }
function goto_gps(transf) { function goto_gps(transf) {
log(" has to move to lat : ", rc_goto.latitude, " long: ", rc_goto.longitude, "Current lat : ", pose.position.latitude," lon ",pose.position.longitude)
m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0) m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation)) print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
if(math.vec2.length(m_navigation)>GOTO_MAXDIST) if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
@ -96,8 +97,9 @@ function goto_gps(transf) {
else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
transf() transf()
else { else {
#LimitSpeed(m_navigation, 1.0) log("Waypoint before scale : x ",m_navigation.x, " Y ",m_navigation.y )
log("NAVIGATION !!!! distance to target: ", math.vec2.length(m_navigation)," angle", math.vec2.angle(m_navigation) ) m_navigation = LimitSpeed(m_navigation, 1.0)
log("Waypoint after scale : x ",m_navigation.x, " Y ",m_navigation.y )
goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0) goto_abs(m_navigation.x, m_navigation.y, rc_goto.altitude - pose.position.altitude, 0.0)
} }
} }

2
buzz_scripts/include/taskallocate/graphformGPS.bzz
View File

@ -537,7 +537,7 @@ function DoJoining(){
if(m_gotjoinedparent!=1) if(m_gotjoinedparent!=1)
set_rc_goto() set_rc_goto()
else else
goto_gps(TransitionToJoined) goto_gps(TransitionToJoined)
#pack the communication package #pack the communication package
m_selfMessage.State=s2i(BVMSTATE) m_selfMessage.State=s2i(BVMSTATE)

8
buzz_scripts/include/utils/conversions.bzz
View File

@ -44,9 +44,9 @@ function vec_from_gps(lat, lon, home_ref) {
} }
ned_x = d_lat/180*math.pi * 6371000.0; ned_x = d_lat/180*math.pi * 6371000.0;
ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi); ned_y = d_lon/180*math.pi * 6371000.0 * math.cos(lat/180*math.pi);
#Lgoal.range = math.sqrt(ned_x*ned_x+ned_y*ned_y); m_Lgoal_range = math.sqrt(ned_x*ned_x+ned_y*ned_y);
#Lgoal.bearing = LimitAngle(math.atan(ned_y,ned_x)); m_Lgoal_bearing = LimitAngle(math.atan(ned_y,ned_x));
return math.vec2.new(ned_x,ned_y) return math.vec2.newp(m_Lgoal_range,m_Lgoal_bearing)
} }
function gps_from_vec(vec) { function gps_from_vec(vec) {
@ -66,4 +66,4 @@ function gps_from_vec(vec) {
#goal.longitude = d_lon + pose.position.longitude; #goal.longitude = d_lon + pose.position.longitude;
return Lgoal return Lgoal
} }