dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Navigator: Added parameter for loiter radius

This commit is contained in:
Julian Oes 2013-11-26 15:54:19 +01:00
parent a989e79663
commit 8e1af68bfd
2 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/navigator/navigator_main.cpp
View File

@ -173,10 +173,12 @@ private:
struct { struct {
float min_altitude; float min_altitude;
float loiter_radius;
} _parameters; /**< local copies of parameters */ } _parameters; /**< local copies of parameters */
struct { struct {
param_t min_altitude; param_t min_altitude;
param_t loiter_radius;
} _parameter_handles; /**< handles for parameters */ } _parameter_handles; /**< handles for parameters */
@ -282,6 +284,7 @@ Navigator::Navigator() :
_global_pos.valid = false; _global_pos.valid = false;
memset(&_fence, 0, sizeof(_fence)); memset(&_fence, 0, sizeof(_fence));
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
_parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD");
_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count); _mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count);
@ -326,6 +329,7 @@ Navigator::parameters_update()
{ {
param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude));
param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
return OK; return OK;
} }
@ -540,6 +544,8 @@ Navigator::task_main()
/* update parameters from storage */ /* update parameters from storage */
parameters_update(); parameters_update();
/* note that these new parameters won't be in effect until a mission triplet is published again */
} }
/* only update craft capabilities if they have changed */ /* only update craft capabilities if they have changed */
@ -866,7 +872,7 @@ Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission
new_mission_item->lat = (double)_home_pos.lat / 1e7; new_mission_item->lat = (double)_home_pos.lat / 1e7;
new_mission_item->lon = (double)_home_pos.lon / 1e7; new_mission_item->lon = (double)_home_pos.lon / 1e7;
new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude; new_mission_item->altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude;
new_mission_item->loiter_radius = 100.0f; // TODO: get rid of magic number new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
new_mission_item->radius = 50.0f; // TODO: get rid of magic number new_mission_item->radius = 50.0f; // TODO: get rid of magic number
} }
// warnx("added mission item: %d", mission_item_index); // warnx("added mission item: %d", mission_item_index);
@ -1085,7 +1091,7 @@ Navigator::start_loiter(mission_item_s *new_loiter_position)
_mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _mission_item_triplet.current.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
_mission_item_triplet.current.loiter_direction = 1; _mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.loiter_radius = 100.0f; // TODO: get rid of magic number _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
_mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number
_mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current.autocontinue = false;
@ -1113,7 +1119,7 @@ Navigator::start_rtl()
_mission_item_triplet.current.yaw = 0.0f; _mission_item_triplet.current.yaw = 0.0f;
_mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
_mission_item_triplet.current.loiter_direction = 1; _mission_item_triplet.current.loiter_direction = 1;
_mission_item_triplet.current.loiter_radius = 100.0f; // TODO: get rid of magic number _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number
_mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number
_mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current.autocontinue = false;
_mission_item_triplet.current_valid = true; _mission_item_triplet.current_valid = true;

2
src/modules/navigator/navigator_params.c
View File

@ -52,4 +52,4 @@
*/ */
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);
PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f);