fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Navigator: Use parameter for minium altitude when starting loiter

Browse Source
This commit is contained in:
Julian Oes 2013-11-26 15:08:43 +01:00
parent 92f7fb2732
commit 9a79ad4cdb
2 changed files with 29 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -172,13 +172,13 @@ private:
unsigned _current_mission_index;
struct {
float throttle_cruise;
} _parameters; /**< local copies of interesting parameters */
float min_altitude;
} _parameters; /**< local copies of parameters */
struct {
param_t throttle_cruise;
param_t min_altitude;
} _parameter_handles; /**< handles for interesting parameters */
} _parameter_handles; /**< handles for parameters */
/**
@ -281,7 +281,7 @@ Navigator::Navigator() :
{
_global_pos.valid = false;
memset(&_fence, 0, sizeof(_fence));
_parameter_handles.throttle_cruise = param_find("NAV_DUMMY");
_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
_mission_item = (mission_item_s*)malloc(sizeof(mission_item_s) * _max_mission_item_count);
@ -325,7 +325,7 @@ int
Navigator::parameters_update()
{
//param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude));
return OK;
}
@ -491,7 +491,7 @@ Navigator::task_main()
case NAVIGATION_STATE_AUTO_READY:
case NAVIGATION_STATE_AUTO_TAKEOFF:
/* TODO add this */
/* TODO probably not needed since takeoff WPs will just be passed on */
//set_mode(NAVIGATION_MODE_TAKEOFF);
break;
@ -762,18 +762,29 @@ Navigator::set_mode(navigation_mode_t new_nav_mode)
case NAVIGATION_MODE_NONE:
case NAVIGATION_MODE_WAYPOINT:
case NAVIGATION_MODE_RTL:
case NAVIGATION_MODE_RTL: {
/* use current position and loiter around it */
mission_item_s global_position_mission_item;
global_position_mission_item.lat = (double)_global_pos.lat / 1e7;
global_position_mission_item.lon = (double)_global_pos.lon / 1e7;
global_position_mission_item.altitude = _global_pos.alt;
/* XXX get rid of ugly conversion for home position altitude */
float global_min_alt = _parameters.min_altitude + (float)_home_pos.alt/1e3f;
/* Use current altitude if above min altitude set by parameter */
if (_global_pos.alt < global_min_alt) {
global_position_mission_item.altitude = global_min_alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", global_min_alt - _global_pos.alt);
} else {
global_position_mission_item.altitude = _global_pos.alt;
mavlink_log_info(_mavlink_fd, "[navigator] loiter here");
}
start_loiter(&global_position_mission_item);
_mode = new_nav_mode;
mavlink_log_info(_mavlink_fd, "[navigator] loiter here");
break;
}
case NAVIGATION_MODE_LOITER_WAYPOINT:
case NAVIGATION_MODE_LOITER_RTL:
/* already loitering, just continue */
@ -836,7 +847,7 @@ Navigator::set_mode(navigation_mode_t new_nav_mode)
mission_item_s home_position_mission_item;
home_position_mission_item.lat = (double)_home_pos.lat / 1e7;
home_position_mission_item.lon = (double)_home_pos.lon / 1e7;
home_position_mission_item.altitude = _home_pos.alt / 1e3f + 50.0f;
home_position_mission_item.altitude = _home_pos.alt / 1e3f + _parameters.min_altitude;
start_loiter(&home_position_mission_item);
mavlink_log_info(_mavlink_fd, "[navigator] loiter after RTL");
_mode = new_nav_mode;
@ -854,7 +865,7 @@ Navigator::set_waypoint_mission_item(unsigned mission_item_index, struct mission
/* if it is a RTL waypoint, append the home position */
new_mission_item->lat = (double)_home_pos.lat / 1e7;
new_mission_item->lon = (double)_home_pos.lon / 1e7;
new_mission_item->altitude = (float)_home_pos.alt / 1e3f + 50.0f; // TODO: add parameter for RTL 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->radius = 50.0f; // TODO: get rid of magic number
}
@ -1093,7 +1104,7 @@ Navigator::start_rtl()
_mission_item_triplet.current.lat = (double)_home_pos.lat / 1e7;
_mission_item_triplet.current.lon = (double)_home_pos.lon / 1e7;
_mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + 50.0f; // TODO: add parameter for RTL altitude
_mission_item_triplet.current.altitude = (float)_home_pos.alt / 1e3f + _parameters.min_altitude;
_mission_item_triplet.current.yaw = 0.0f;
_mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
_mission_item_triplet.current.loiter_direction = 1;

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

@ -1,7 +1,8 @@
/****************************************************************************
*
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
* Author: Lorenz Meier <lm@inf.ethz.ch>
* Author: @autho Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -38,6 +39,7 @@
* Parameters defined by the navigator task.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
*/
#include <nuttx/config.h>
@ -49,5 +51,5 @@
*
*/
PARAM_DEFINE_FLOAT(NAV_DUMMY, 0.0f);
PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f);