From 163d201c2852c59a31ddbfa0abea88711e1bead9 Mon Sep 17 00:00:00 2001 From: Simone Guscetti Date: Tue, 30 Oct 2018 18:23:56 +0100 Subject: [PATCH] remove the landing gear from the attitude setpoint - An new message is created just for the landing gear - The old logic is repalaced by publishing this new topic --- msg/CMakeLists.txt | 1 + msg/landing_gear.msg | 6 +++ msg/vehicle_attitude_setpoint.msg | 4 -- src/modules/mc_att_control/mc_att_control.hpp | 5 +++ .../mc_att_control/mc_att_control_main.cpp | 27 ++++++++++--- .../mc_pos_control/mc_pos_control_main.cpp | 38 ++++++++++++++----- src/modules/navigator/gpsfailure.cpp | 1 + src/modules/navigator/navigator.h | 1 - 8 files changed, 64 insertions(+), 19 deletions(-) create mode 100644 msg/landing_gear.msg diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index c66a748587..698cf00f57 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -66,6 +66,7 @@ set(msg_files input_rc.msg iridiumsbd_status.msg irlock_report.msg + landing_gear.msg landing_target_innovations.msg landing_target_pose.msg led_control.msg diff --git a/msg/landing_gear.msg b/msg/landing_gear.msg new file mode 100644 index 0000000000..754de8988f --- /dev/null +++ b/msg/landing_gear.msg @@ -0,0 +1,6 @@ +uint64 timestamp # time since system start (microseconds) + +int8 LANDING_GEAR_UP = 1 # landing gear up +int8 LANDING_GEAR_DOWN = -1 # landing gear down + +float32 landing_gear diff --git a/msg/vehicle_attitude_setpoint.msg b/msg/vehicle_attitude_setpoint.msg index 9b5c5db015..2540f0ee15 100644 --- a/msg/vehicle_attitude_setpoint.msg +++ b/msg/vehicle_attitude_setpoint.msg @@ -1,6 +1,4 @@ uint64 timestamp # time since system start (microseconds) -int8 LANDING_GEAR_UP = 1 # landing gear up -int8 LANDING_GEAR_DOWN = -1 # landing gear down float32 roll_body # body angle in NED frame (can be NaN for FW) float32 pitch_body # body angle in NED frame (can be NaN for FW) @@ -27,6 +25,4 @@ uint8 FLAPS_OFF = 0 # no flaps uint8 FLAPS_LAND = 1 # landing config flaps uint8 FLAPS_TAKEOFF = 2 # take-off config flaps -float32 landing_gear - # TOPICS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint diff --git a/src/modules/mc_att_control/mc_att_control.hpp b/src/modules/mc_att_control/mc_att_control.hpp index c3dad3c7ab..d19bb03458 100644 --- a/src/modules/mc_att_control/mc_att_control.hpp +++ b/src/modules/mc_att_control/mc_att_control.hpp @@ -56,6 +56,7 @@ #include #include #include +#include /** * Multicopter attitude control app start / stop handling function @@ -109,6 +110,7 @@ private: void vehicle_motor_limits_poll(); bool vehicle_rates_setpoint_poll(); void vehicle_status_poll(); + void landing_gear_state_poll(); void publish_actuator_controls(); void publish_rates_setpoint(); @@ -157,6 +159,7 @@ private: int _sensor_correction_sub{-1}; /**< sensor thermal correction subscription */ int _sensor_bias_sub{-1}; /**< sensor in-run bias correction subscription */ int _vehicle_land_detected_sub{-1}; /**< vehicle land detected subscription */ + int _landing_gear_sub{-1}; unsigned _gyro_count{1}; int _selected_gyro{0}; @@ -165,6 +168,7 @@ private: orb_advert_t _actuators_0_pub{nullptr}; /**< attitude actuator controls publication */ orb_advert_t _controller_status_pub{nullptr}; /**< controller status publication */ orb_advert_t _vehicle_attitude_setpoint_pub{nullptr}; + orb_advert_t _landing_gear_pub{nullptr}; orb_id_t _actuators_id{nullptr}; /**< pointer to correct actuator controls0 uORB metadata structure */ @@ -182,6 +186,7 @@ private: struct sensor_correction_s _sensor_correction {}; /**< sensor thermal corrections */ struct sensor_bias_s _sensor_bias {}; /**< sensor in-run bias corrections */ struct vehicle_land_detected_s _vehicle_land_detected {}; + struct landing_gear_s _landing_gear_state {}; MultirotorMixer::saturation_status _saturation_status{}; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 020096ac63..7264b0edfa 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -376,6 +376,17 @@ MulticopterAttitudeControl::vehicle_land_detected_poll() } +void +MulticopterAttitudeControl::landing_gear_state_poll() +{ + bool updated; + orb_check(_landing_gear_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(landing_gear), _landing_gear_sub, &_landing_gear_state); + } +} + float MulticopterAttitudeControl::throttle_curve(float throttle_stick_input) { @@ -404,9 +415,9 @@ MulticopterAttitudeControl::get_landing_gear_state() if (_vehicle_land_detected.landed) { _gear_state_initialized = false; } - float landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; // default to down + float landing_gear = landing_gear_s::LANDING_GEAR_DOWN; // default to down if (_manual_control_sp.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON && _gear_state_initialized) { - landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP; + landing_gear = landing_gear_s::LANDING_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 @@ -420,6 +431,7 @@ void MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_sp) { vehicle_attitude_setpoint_s attitude_setpoint{}; + landing_gear_s landing_gear{}; const float yaw = Eulerf(Quatf(_v_att.q)).psi(); /* reset yaw setpoint to current position if needed */ @@ -508,9 +520,11 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_ attitude_setpoint.thrust_body[2] = -throttle_curve(_manual_control_sp.z); - attitude_setpoint.landing_gear = get_landing_gear_state(); - attitude_setpoint.timestamp = hrt_absolute_time(); + _landing_gear_state.landing_gear = get_landing_gear_state(); + + attitude_setpoint.timestamp = landing_gear.timestamp = hrt_absolute_time(); orb_publish_auto(ORB_ID(vehicle_attitude_setpoint), &_vehicle_attitude_setpoint_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); + orb_publish_auto(ORB_ID(landing_gear), &_landing_gear_pub, &attitude_setpoint, nullptr, ORB_PRIO_DEFAULT); } /** @@ -759,7 +773,7 @@ MulticopterAttitudeControl::publish_actuator_controls() _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f; _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f; _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f; - _actuators.control[7] = _v_att_sp.landing_gear; + _actuators.control[7] = _landing_gear_state.landing_gear; _actuators.timestamp = hrt_absolute_time(); _actuators.timestamp_sample = _sensor_gyro.timestamp; @@ -805,6 +819,7 @@ MulticopterAttitudeControl::run() _sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction)); _sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias)); _vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); + _landing_gear_sub = orb_subscribe(ORB_ID(landing_gear)); /* wakeup source: gyro data from sensor selected by the sensor app */ px4_pollfd_struct_t poll_fds = {}; @@ -873,6 +888,7 @@ MulticopterAttitudeControl::run() sensor_correction_poll(); sensor_bias_poll(); vehicle_land_detected_poll(); + landing_gear_state_poll(); const bool manual_control_updated = vehicle_manual_poll(); const bool attitude_updated = vehicle_attitude_poll(); attitude_dt += dt; @@ -982,6 +998,7 @@ MulticopterAttitudeControl::run() orb_unsubscribe(_sensor_correction_sub); orb_unsubscribe(_sensor_bias_sub); orb_unsubscribe(_vehicle_land_detected_sub); + orb_unsubscribe(_landing_gear_sub); } int MulticopterAttitudeControl::task_spawn(int argc, char *argv[]) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 0dcd57e2a1..ecdab84e7a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -55,6 +55,7 @@ #include #include #include +#include #include #include @@ -110,6 +111,7 @@ private: orb_advert_t _traj_wp_avoidance_desired_pub{nullptr}; /**< trajectory waypoint desired publication */ orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command publication */ orb_id_t _attitude_setpoint_id{nullptr}; + orb_advert_t _landing_gear_pub{nullptr}; int _vehicle_status_sub{-1}; /**< vehicle status subscription */ int _vehicle_land_detected_sub{-1}; /**< vehicle land detected subscription */ @@ -135,6 +137,7 @@ private: home_position_s _home_pos{}; /**< home position */ vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */ vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */ + landing_gear_s _landing_gear_state{}; DEFINE_PARAMETERS( (ParamFloat) _takeoff_ramp_time, /**< time constant for smooth takeoff ramp */ @@ -593,7 +596,15 @@ MulticopterPositionControl::run() poll_subscriptions(); // Let's be safe and have the landing gear down by default - _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; + _landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_DOWN; + _landing_gear_state.timestamp = hrt_absolute_time(); + + if (_landing_gear_pub != nullptr) { + orb_publish(ORB_ID(landing_gear), _landing_gear_pub, &_landing_gear_state); + + } else { + _landing_gear_pub = orb_advertise(ORB_ID(landing_gear), &_landing_gear_state); + } // setup file descriptor to poll the local position as loop wakeup source px4_pollfd_struct_t poll_fd = {.fd = _local_pos_sub}; @@ -792,22 +803,31 @@ MulticopterPositionControl::run() _att_sp.fw_control_yaw = false; _att_sp.apply_flaps = false; + // publish attitude setpoint + // Note: this requires review. The reason for not sending + // an attitude setpoint is because for non-flighttask modes + // the attitude septoint should come from another source, otherwise + // they might conflict with each other such as in offboard attitude control. + publish_attitude(); + if (!constraints.landing_gear) { if (constraints.landing_gear == vehicle_constraints_s::GEAR_UP) { - _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_UP; + _landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_UP; } if (constraints.landing_gear == vehicle_constraints_s::GEAR_DOWN) { - _att_sp.landing_gear = vehicle_attitude_setpoint_s::LANDING_GEAR_DOWN; + _landing_gear_state.landing_gear = landing_gear_s::LANDING_GEAR_DOWN; } } - // publish attitude setpoint - // Note: this requires review. The reason for not sending - // an attitude setpoint is because for non-flighttask modes - // the attitude septoint should come from another source, otherwise - // they might conflict with each other such as in offboard attitude control. - publish_attitude(); + _landing_gear_state.timestamp = hrt_absolute_time(); + + if (_landing_gear_pub != nullptr) { + orb_publish(ORB_ID(landing_gear), _landing_gear_pub, &_landing_gear_state); + + } else { + _landing_gear_pub = orb_advertise(ORB_ID(landing_gear), &_landing_gear_state); + } } else { // no flighttask is active: set attitude setpoint to idle diff --git a/src/modules/navigator/gpsfailure.cpp b/src/modules/navigator/gpsfailure.cpp index 69b37c793f..94bdf97a38 100644 --- a/src/modules/navigator/gpsfailure.cpp +++ b/src/modules/navigator/gpsfailure.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include using matrix::Eulerf; diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 54d35dff82..8dc30815d7 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -67,7 +67,6 @@ #include #include #include -#include #include #include #include