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

mc_pos_control: use correct altitude as limit 

fadfdasf
This commit is contained in:
Dennis Mannhart 2017-06-26 10:22:29 +02:00
parent 115e7246b0
commit 8a0a8e20e1
15 changed files with 58 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/land_detector/land_detector_params.c
View File

@ -206,7 +206,7 @@ PARAM_DEFINE_INT32(LND_FLIGHT_T_LO, 0);
* hard altitude limit. This setting will
* be consolidated with the GF_MAX_VER_DIST
* parameter.
* A negative value indicates that there is no limit
* A negative value indicates no altitude limitation.
*
* @unit m
* @min -1

38
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -919,28 +919,30 @@ MulticopterPositionControl::reset_alt_sp()
void
MulticopterPositionControl::limit_altitude()
{
if (_vehicle_land_detected.alt_max > 0.0f) {
/* in altitude control, limit setpoint */
if (_run_alt_control && _pos_sp(2) <= -_vehicle_land_detected.alt_max) {
_pos_sp(2) = -_vehicle_land_detected.alt_max;
return;
}
if (_vehicle_land_detected.alt_max < 0.0f) {
// there is no altitude limitation present
return;
}
/* in velocity control mode and want to fly upwards */
if (!_run_alt_control && _vel_sp(2) <= 0.0f) {
float altitude_above_home = -(_pos(2) - _home_pos.z);
/* time to travel to reach zero velocity */
float delta_t = -_vel(2) / _acceleration_z_max_down.get();
if (_run_alt_control && (altitude_above_home > _vehicle_land_detected.alt_max)) {
// we are above maximum altitude
_pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z;
/* predicted position */
float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f *
_acceleration_z_max_down.get() * delta_t *delta_t;
} else if (!_run_alt_control && _vel_sp(2) <= 0.0f) {
// we want to fly upwards: check if vehicle does not exceed altitude
if (pos_z_next <= -_vehicle_land_detected.alt_max) {
_pos_sp(2) = -_vehicle_land_detected.alt_max;
_run_alt_control = true;
return;
}
// time to reach zero velocity
float delta_t = -_vel(2) / _acceleration_z_max_down.get();
// predict next position based on current position, velocity, max acceleration downwards and time to reach zero velocity
float pos_z_next = _pos(2) + _vel(2) * delta_t + 0.5f * _acceleration_z_max_down.get() * delta_t *delta_t;
if (-(pos_z_next - _home_pos.z) > _vehicle_land_detected.alt_max) {
// prevent the vehicle from exceeding maximum altitude by switching back to altitude control with maximum altitude as setpoint
_pos_sp(2) = -_vehicle_land_detected.alt_max + _home_pos.z;
_run_alt_control = true;
}
}
}

3
src/modules/navigator/enginefailure.cpp
View File

@ -126,9 +126,8 @@ EngineFailure::set_ef_item()
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_position_setpoint_triplet_updated();

2
src/modules/navigator/follow_target.cpp
View File

@ -361,7 +361,7 @@ void FollowTarget::update_position_sp(bool use_velocity, bool use_position, floa
pos_sp_triplet->previous.valid = use_position;
pos_sp_triplet->previous = pos_sp_triplet->current;
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->current.type = position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET;
pos_sp_triplet->current.position_valid = use_position;

5
src/modules/navigator/land.cpp
View File

@ -81,12 +81,13 @@ Land::on_activation()
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->previous.valid = false;
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
_navigator->set_can_loiter_at_sp(false);
_navigator->set_position_setpoint_triplet_updated();
}

2
src/modules/navigator/loiter.cpp
View File

@ -129,7 +129,7 @@ Loiter::set_loiter_position()
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
pos_sp_triplet->current.velocity_valid = false;
pos_sp_triplet->previous.valid = false;
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;

10
src/modules/navigator/mission.cpp
View File

@ -508,7 +508,7 @@ Mission::set_mission_items()
/* update position setpoint triplet */
pos_sp_triplet->previous.valid = false;
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;
@ -765,7 +765,7 @@ Mission::set_mission_items()
set_align_mission_item(&_mission_item, &mission_item_next_position);
/* set position setpoint to target during the transition */
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->current);
}
@ -808,7 +808,7 @@ Mission::set_mission_items()
/*********************************** set setpoints and check next *********************************************/
/* set current position setpoint from mission item (is protected against non-position items) */
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
/* issue command if ready (will do nothing for position mission items) */
@ -835,7 +835,7 @@ Mission::set_mission_items()
/* try to process next mission item */
if (has_next_position_item) {
/* got next mission item, update setpoint triplet */
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(mission_item_next_position, &pos_sp_triplet->next);
} else {
@ -1187,7 +1187,7 @@ Mission::do_abort_landing()
_mission_item.origin = ORIGIN_ONBOARD;
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();

24
src/modules/navigator/mission_block.cpp
View File

@ -751,22 +751,30 @@ MissionBlock::set_idle_item(struct mission_item_s *item)
}
void
MissionBlock::mission_apply_limitation(struct mission_item_s *item)
MissionBlock::mission_apply_limitation(mission_item_s &item)
{
/*
* Limit altitude
*/
/* do nothing if altitude max is negative */
if (_navigator->get_land_detected()->alt_max > 0.0f) {
/* get absolut altitude */
float altitude_abs = item->altitude_is_relative
? item->altitude + _navigator->get_home_position()->alt
: item->altitude;
/* absolute altitude */
float altitude_abs = item.altitude_is_relative
? item.altitude + _navigator->get_home_position()->alt
: item.altitude;
/* limit altitude to maximum allowed altitude */
if ((_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt) < altitude_abs) {
item->altitude = item->altitude_is_relative ?
_navigator->get_land_detected()->alt_max :
_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt;
item.altitude = item.altitude_is_relative ?
_navigator->get_land_detected()->alt_max :
_navigator->get_land_detected()->alt_max + _navigator->get_home_position()->alt;
}
}
/*
* Add other limitations here
*/
}

7
src/modules/navigator/mission_block.h
View File

@ -121,10 +121,9 @@ protected:
void set_follow_target_item(struct mission_item_s *item, float min_clearance, follow_target_s &target, float yaw);
/**
* This function limits the setpoint dependent
* on vehicle model
*/
void mission_apply_limitation(struct mission_item_s *item);
* General function used to adjust the mission item based on vehicle specific limitations
*/
void mission_apply_limitation(mission_item_s &item);
void issue_command(const mission_item_s &item);

1
src/modules/navigator/navigator.h
View File

@ -36,7 +36,6 @@
* @author Julian Oes <julian@oes.ch>
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Dennis Mannhart <dennis@px4.io>
*/
#ifndef NAVIGATOR_H

2
src/modules/navigator/rcloss.cpp
View File

@ -140,7 +140,7 @@ RCLoss::set_rcl_item()
reset_mission_item_reached();
/* convert mission item to current position setpoint and make it valid */
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;

4
src/modules/navigator/rtl.cpp
View File

@ -84,7 +84,7 @@ RTL::on_activation()
{
set_current_position_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->next.valid = false;
@ -292,7 +292,7 @@ RTL::set_rtl_item()
}
/* convert mission item to current position setpoint and make it valid */
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->next.valid = false;

1
src/modules/navigator/rtl.h
View File

@ -79,6 +79,7 @@ private:
*/
float get_rtl_altitude();
enum RTLState {
RTL_STATE_NONE = 0,
RTL_STATE_CLIMB,

4
src/modules/navigator/takeoff.cpp
View File

@ -92,7 +92,7 @@ Takeoff::on_active()
// set loiter item so position controllers stop doing takeoff logic
set_loiter_item(&_mission_item);
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();
}
@ -149,7 +149,7 @@ Takeoff::set_takeoff_position()
// convert mission item to current setpoint
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
mission_apply_limitation(&_mission_item);
mission_apply_limitation(_mission_item);
mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.yaw_valid = true;

2
src/modules/systemlib/battery.cpp
View File

@ -225,7 +225,7 @@ void
Battery::determineWarning(bool connected)
{
if (connected) {
// propagate warning state only if the state is higher, otherwise remain in current waringin state
// propagate warning state only if the state is higher, otherwise remain in current warning state
if (_remaining < _param_emergency_thr.get() || (_warning == battery_status_s::BATTERY_WARNING_EMERGENCY)) {
_warning = battery_status_s::BATTERY_WARNING_EMERGENCY;