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

vehicle constraints: remove landing gear

Browse Source 
- landing_gear: refactor state name
- Add the keep state to the landing gear message
- Adapt FlightTaskManual, FlightTaskAutoMapper, mc_pos_control,
to review message definition
This commit is contained in:
Simone Guscetti 2018-11-01 18:00:27 +01:00 committed by Dennis Mannhart
parent 6dbed6636d
commit de185726b3
7 changed files with 24 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
msg/landing_gear.msg
View File

@ -1,6 +1,7 @@
uint64 timestamp # time since system start (microseconds)
uint64 timestamp # time since system start (microseconds)
int8 LANDING_GEAR_UP = 1 # landing gear up
int8 LANDING_GEAR_DOWN = -1 # landing gear down
int8 GEAR_UP = 1 # landing gear up
int8 GEAR_DOWN = -1 # landing gear down
int8 GEAR_KEEP = 0 # keep the current state
int8 landing_gear

5
msg/vehicle_constraints.msg
View File

@ -10,8 +10,3 @@ float32 speed_down # in meters/sec
float32 tilt # in radians [0, PI]
float32 min_distance_to_ground # in meters
float32 max_distance_to_ground # in meters
int8 GEAR_DOWN = -1
int8 GEAR_UP = 1
int8 GEAR_KEEP = 0
int8 landing_gear # Down, UP or KEEP

8
src/lib/FlightTasks/tasks/AutoMapper/FlightTaskAutoMapper.cpp
View File

@ -89,7 +89,7 @@ bool FlightTaskAutoMapper::update()
// during mission and reposition, raise the landing gears but only
// if altitude is high enough
if (_highEnoughForLandingGear()) {
_constraints.landing_gear = vehicle_constraints_s::GEAR_UP;
_gear.landing_gear = landing_gear_s::GEAR_UP;
}
// update previous type
@ -121,7 +121,7 @@ void FlightTaskAutoMapper::_generateLandSetpoints()
// set constraints
_constraints.tilt = MPC_TILTMAX_LND.get();
_constraints.speed_down = MPC_LAND_SPEED.get();
_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper::_generateTakeoffSetpoints()
@ -134,7 +134,7 @@ void FlightTaskAutoMapper::_generateTakeoffSetpoints()
_constraints.speed_up = math::gradual(_alt_above_ground, MPC_LAND_ALT2.get(),
MPC_LAND_ALT1.get(), MPC_TKO_SPEED.get(), _constraints.speed_up);
_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper::_generateVelocitySetpoints()
@ -173,4 +173,4 @@ bool FlightTaskAutoMapper::_highEnoughForLandingGear()
{
// return true if altitude is above two meters
return _alt_above_ground > 2.0f;
}
}

6
src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp
View File

@ -96,7 +96,7 @@ bool FlightTaskAutoMapper2::update()
// during mission and reposition, raise the landing gears but only
// if altitude is high enough
if (_highEnoughForLandingGear()) {
_constraints.landing_gear = vehicle_constraints_s::GEAR_UP;
_gear.landing_gear = landing_gear_s::GEAR_UP;
}
// update previous type
@ -129,7 +129,7 @@ void FlightTaskAutoMapper2::_prepareLandSetpoints()
// set constraints
_constraints.tilt = MPC_TILTMAX_LND.get();
_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
@ -139,7 +139,7 @@ void FlightTaskAutoMapper2::_prepareTakeoffSetpoints()
const float speed_tko = (_alt_above_ground > MPC_LAND_ALT1.get()) ? _constraints.speed_up : MPC_TKO_SPEED.get();
_velocity_setpoint = Vector3f(NAN, NAN, -speed_tko); // Limit the maximum vertical speed
_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
void FlightTaskAutoMapper2::_prepareVelocitySetpoints()

8
src/lib/FlightTasks/tasks/Manual/FlightTaskManual.cpp
View File

@ -91,7 +91,7 @@ bool FlightTaskManual::_evaluateSticks()
// until he toggles the switch to avoid retracting the gear immediately on takeoff.
int8_t gear_switch = _sub_manual_control_setpoint->get().gear_switch;
if (!_constraints.landing_gear) {
if (!_gear.landing_gear) {
if (gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
_applyGearSwitch(gear_switch);
}
@ -112,7 +112,7 @@ bool FlightTaskManual::_evaluateSticks()
/* Timeout: set all sticks to zero */
_sticks.zero();
_sticks_expo.zero();
_constraints.landing_gear = vehicle_constraints_s::GEAR_KEEP;
_gear.landing_gear = landing_gear_s::GEAR_KEEP;
return false;
}
}
@ -120,10 +120,10 @@ bool FlightTaskManual::_evaluateSticks()
void FlightTaskManual::_applyGearSwitch(uint8_t gswitch)
{
if (gswitch == manual_control_setpoint_s::SWITCH_POS_OFF) {
_constraints.landing_gear = vehicle_constraints_s::GEAR_DOWN;
_gear.landing_gear = landing_gear_s::GEAR_DOWN;
}
if (gswitch == manual_control_setpoint_s::SWITCH_POS_ON) {
_constraints.landing_gear = vehicle_constraints_s::GEAR_UP;
_gear.landing_gear = landing_gear_s::GEAR_UP;
}
}

4
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -415,9 +415,9 @@ MulticopterAttitudeControl::get_landing_gear_state()
if (_vehicle_land_detected.landed) {
_gear_state_initialized = false;
}
float landing_gear = landing_gear_s::LANDING_GEAR_DOWN; // default to down
float landing_gear = landing_gear_s::GEAR_DOWN; // default to down
if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) {
landing_gear = landing_gear_s::LANDING_GEAR_UP;
landing_gear = landing_gear_s::GEAR_UP;
} else if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
// Switching the gear off does put it into a safe defined state

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

@ -691,6 +691,7 @@ MulticopterPositionControl::run()
update_avoidance_waypoint_desired(_states, setpoint);
vehicle_constraints_s constraints = _flight_tasks.getConstraints();
landing_gear_s gear = _flight_tasks.getGear();
// check if all local states are valid and map accordingly
set_vehicle_states(setpoint.vz);
@ -732,7 +733,7 @@ MulticopterPositionControl::run()
setpoint.vx = setpoint.vy = setpoint.vz = NAN;
setpoint.yawspeed = NAN;
setpoint.yaw = _states.yaw;
constraints.landing_gear = vehicle_constraints_s::GEAR_KEEP;
gear.landing_gear = landing_gear_s::GEAR_KEEP;
// reactivate the task which will reset the setpoint to current state
_flight_tasks.reActivate();
}
@ -801,11 +802,11 @@ MulticopterPositionControl::run()
// they might conflict with each other such as in offboard attitude control.
publish_attitude();
// if theres any change in landing gear setpoint publish it
if (_old_landing_gear_position != constraints.landing_gear
&& constraints.landing_gear != vehicle_constraints_s::GEAR_KEEP) {
// if there's any change in landing gear setpoint publish it
if (gear.landing_gear != _old_landing_gear_position
&& gear.landing_gear != landing_gear_s::GEAR_KEEP) {
_landing_gear.landing_gear = constraints.landing_gear;
_landing_gear.landing_gear = gear.landing_gear;
_landing_gear.timestamp = hrt_absolute_time();
if (_landing_gear_pub != nullptr) {
@ -816,7 +817,7 @@ MulticopterPositionControl::run()
}
}
_old_landing_gear_position = constraints.landing_gear;
_old_landing_gear_position = gear.landing_gear;
} else {
// no flighttask is active: set attitude setpoint to idle