diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index c66a74858756ac6fa301326299a170bc3bd611d2..698cf00f5788dae8bb111cd2a3759f629f1849f9 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 0000000000000000000000000000000000000000..754de8988f8dd93ac3e2a907492cf9d3d48e0752
--- /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 9b5c5db015946a82c9d5abe1cf8859e2d1439d43..2540f0ee15ec204a3765646d5dd3d6359fe91309 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 c3dad3c7ab562c3f542b08d46b13ed6dcfb0905d..d19bb03458c48fbed922550766b05b21d81b778d 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 <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/landing_gear.h>
/**
* 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 020096ac63c1e3321f355cf638e9f93f8c00945f..7264b0edfaee201a9f554b42e528d12b0c7c0d9e 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 0dcd57e2a188a7361c7c2ce8461726efe90203aa..ecdab84e7ad5661bec03e61488c4f146317ca19d 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 <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/landing_gear.h>
#include <float.h>
#include <mathlib/mathlib.h>
@@ -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<px4::params::MPC_TKO_RAMP_T>) _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 69b37c793fbfde811cbe08cf6e896cc7cd4c6c4c..94bdf97a38363091d8cc0868a91995319e6586bd 100644
--- a/src/modules/navigator/gpsfailure.cpp
+++ b/src/modules/navigator/gpsfailure.cpp
@@ -46,6 +46,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/home_position.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <mathlib/mathlib.h>
using matrix::Eulerf;
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index 54d35dff82f0d9e9b85370e4f17f81877bf8fcc8..8dc30815d7a138b8966a6b7a72c13e3807ecf171 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -67,7 +67,6 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_gps_position.h>