forked from Archive/PX4-Autopilot
Move land detector changes to vtol
This commit is contained in:
parent
49d2e8a3ff
commit
6ee24a0c80
|
@ -123,7 +123,6 @@ void MulticopterLandDetector::_initialize_topics()
|
|||
_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
|
||||
_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
|
||||
_battery_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
}
|
||||
|
||||
void MulticopterLandDetector::_update_topics()
|
||||
|
@ -137,7 +136,6 @@ void MulticopterLandDetector::_update_topics()
|
|||
_orb_update(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
|
||||
_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
|
||||
_orb_update(ORB_ID(battery_status), _battery_sub, &_battery);
|
||||
_orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
|
||||
}
|
||||
|
||||
void MulticopterLandDetector::_update_params()
|
||||
|
@ -194,11 +192,6 @@ bool MulticopterLandDetector::_get_ground_contact_state()
|
|||
_arming_time = now;
|
||||
}
|
||||
|
||||
// Only trigger in RW mode
|
||||
if (!_vehicle_status.is_rotary_wing) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// If in manual flight mode never report landed if the user has more than idle throttle
|
||||
// Check if user commands throttle and if so, report no ground contact based on
|
||||
// the user intent to take off (even if the system might physically still have
|
||||
|
@ -249,11 +242,6 @@ bool MulticopterLandDetector::_get_maybe_landed_state()
|
|||
return true;
|
||||
}
|
||||
|
||||
// Only trigger in RW mode
|
||||
if (!_vehicle_status.is_rotary_wing) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// If we control manually and are still landed, we want to stay idle until the pilot rises the throttle for takeoff
|
||||
if (_state == LandDetectionState::LANDED && _has_manual_control_present()) {
|
||||
if (_manual.z < _get_takeoff_throttle()) {
|
||||
|
|
|
@ -126,7 +126,6 @@ private:
|
|||
int _ctrl_state_sub;
|
||||
int _vehicle_control_mode_sub;
|
||||
int _battery_sub;
|
||||
int _vehicle_status_sub;
|
||||
|
||||
struct vehicle_local_position_s _vehicleLocalPosition;
|
||||
struct vehicle_local_position_setpoint_s _vehicleLocalPositionSetpoint;
|
||||
|
@ -137,7 +136,6 @@ private:
|
|||
struct control_state_s _ctrl_state;
|
||||
struct vehicle_control_mode_s _control_mode;
|
||||
struct battery_status_s _battery;
|
||||
struct vehicle_status_s _vehicle_status;
|
||||
|
||||
uint64_t _min_trust_start; ///< timestamp when minimum trust was applied first
|
||||
uint64_t _arming_time;
|
||||
|
|
|
@ -50,6 +50,7 @@ VtolLandDetector::VtolLandDetector() :
|
|||
_paramHandle(),
|
||||
_params(),
|
||||
_airspeedSub(-1),
|
||||
_vehicle_status_sub(-1),
|
||||
_airspeed{},
|
||||
_was_in_air(false),
|
||||
_airspeed_filtered(0)
|
||||
|
@ -62,6 +63,7 @@ void VtolLandDetector::_initialize_topics()
|
|||
MulticopterLandDetector::_initialize_topics();
|
||||
|
||||
_airspeedSub = orb_subscribe(ORB_ID(airspeed));
|
||||
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
|
||||
}
|
||||
|
||||
void VtolLandDetector::_update_topics()
|
||||
|
@ -69,10 +71,27 @@ void VtolLandDetector::_update_topics()
|
|||
MulticopterLandDetector::_update_topics();
|
||||
|
||||
_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
|
||||
_orb_update(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
|
||||
}
|
||||
|
||||
bool VtolLandDetector::_get_maybe_landed_state()
|
||||
{
|
||||
|
||||
// Only trigger in RW mode
|
||||
if (!_vehicle_status.is_rotary_wing) {
|
||||
return false;
|
||||
}
|
||||
|
||||
return MulticopterLandDetector::_get_maybe_landed_state();
|
||||
}
|
||||
|
||||
bool VtolLandDetector::_get_landed_state()
|
||||
{
|
||||
// Only trigger in RW mode
|
||||
if (!_vehicle_status.is_rotary_wing) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// this is returned from the mutlicopter land detector
|
||||
bool landed = MulticopterLandDetector::_get_landed_state();
|
||||
|
||||
|
|
|
@ -58,6 +58,7 @@ protected:
|
|||
void _update_params() override;
|
||||
void _update_topics() override;
|
||||
bool _get_landed_state() override;
|
||||
bool _get_maybe_landed_state() override;
|
||||
|
||||
|
||||
private:
|
||||
|
@ -70,8 +71,10 @@ private:
|
|||
} _params;
|
||||
|
||||
int _airspeedSub;
|
||||
int _vehicle_status_sub;
|
||||
|
||||
struct airspeed_s _airspeed;
|
||||
struct airspeed_s _airspeed;
|
||||
struct vehicle_status_s _vehicle_status;
|
||||
|
||||
bool _was_in_air; /**< indicates whether the vehicle was in the air in the previous iteration */
|
||||
float _airspeed_filtered; /**< low pass filtered airspeed */
|
||||
|
|
Loading…
Reference in New Issue