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

Add switch for landing gear, pass it to actuators

This commit is contained in:
Julian Oes 2016-10-20 18:12:09 +02:00 committed by Lorenz Meier
parent 10c4ec2e1a
commit 2fff2ab9ac
8 changed files with 97 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/manual_control_setpoint.msg
View File

@ -52,4 +52,5 @@ uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO
uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD
uint8 kill_switch # throttle kill: _NORMAL_, KILL
uint8 transition_switch # VTOL transition switch: _HOVER, FORWARD_FLIGHT
uint8 gear_switch # landing gear switch: _DOWN_, UP
int8 mode_slot # the slot a specific model selector is in

3
msg/rc_channels.msg
View File

@ -21,11 +21,12 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18
uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19
uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20
uint8 RC_CHANNELS_FUNCTION_TRANSITION=21
uint8 RC_CHANNELS_FUNCTION_GEAR=22
uint64 timestamp_last_valid # Timestamp of last valid RC signal
float32[18] channels # Scaled to -1..1 (throttle: 0..1)
uint8 channel_count # Number of valid channels
int8[22] function # Functions mapping
int8[23] function # Functions mapping
uint8 rssi # Receive signal strength indicator (0-100)
bool signal_lost # Control signal lost, should be checked together with topic timeout
uint32 frame_drop_count # Number of dropped frames

2
msg/vehicle_attitude_setpoint.msg
View File

@ -30,5 +30,7 @@ bool disable_mc_yaw_control # control yaw for mc (used for vtol weather-vane m
bool apply_flaps
float32 landing_gear
# WAS vehicle_attitude_setpoint mc_virtual_attitude_setpoint fw_virtual_attitude_setpoint
# TOPICS vehicle_attitude_setpoint

1
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@ -1156,6 +1156,7 @@ FixedwingAttitudeControl::task_main()
_actuators.control[actuator_controls_s::INDEX_FLAPS] = _flaps_applied;
_actuators.control[5] = _manual.aux1;
_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = _flaperons_applied;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuators.control[7] = _manual.aux3;
/* lazily publish the setpoint only once available */

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

@ -976,6 +976,7 @@ MulticopterAttitudeControl::task_main()
_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.timestamp = hrt_absolute_time();
_actuators.timestamp_sample = _ctrl_state.timestamp;

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

@ -66,7 +66,6 @@
#include <arch/board/board.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/control_state.h>
#include <uORB/topics/mc_virtual_attitude_setpoint.h>
@ -1195,6 +1194,22 @@ void MulticopterPositionControl::control_auto(float dt)
_reset_alt_sp = true;
}
// During a mission or in loiter it's safe to retract the landing gear.
if ((_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION ||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LOITER) &&
!_vehicle_land_detected.landed) {
_att_sp.landing_gear = 1.0f;
// During takeoff and landing, we better put it down again.
} else if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF ||
_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
_att_sp.landing_gear = -1.0f;
} else {
// For the rest of the setpoint types, just leave it as is.
}
} else {
/* no waypoint, do nothing, setpoint was already reset */
}
@ -1240,6 +1255,9 @@ MulticopterPositionControl::task_main()
math::Vector<3> thrust_int;
thrust_int.zero();
// Let's be safe and have the landing gear down by default
_att_sp.landing_gear = -1.0f;
matrix::Dcmf R;
R.identity();
@ -2064,6 +2082,15 @@ MulticopterPositionControl::task_main()
_att_sp.q_d_valid = true;
}
if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON &&
!_vehicle_land_detected.landed) {
_att_sp.landing_gear = 1.0f;
} else if (_manual.gear_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
_att_sp.landing_gear = -1.0f;
}
_att_sp.timestamp = hrt_absolute_time();
} else {

45
src/modules/sensors/sensor_params.c
View File

@ -2544,6 +2544,33 @@ PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
*/
PARAM_DEFINE_INT32(RC_MAP_TRANS_SW, 0);
/**
* Landing gear switch channel
*
* @min 0
* @max 18
* @group Radio Switches
* @value 0 Unassigned
* @value 1 Channel 1
* @value 2 Channel 2
* @value 3 Channel 3
* @value 4 Channel 4
* @value 5 Channel 5
* @value 6 Channel 6
* @value 7 Channel 7
* @value 8 Channel 8
* @value 9 Channel 9
* @value 10 Channel 10
* @value 11 Channel 11
* @value 12 Channel 12
* @value 13 Channel 13
* @value 14 Channel 14
* @value 15 Channel 15
* @value 16 Channel 16
* @value 17 Channel 17
* @value 18 Channel 18
*/
PARAM_DEFINE_INT32(RC_MAP_GEAR_SW, 0);
/**
* AUX1 Passthrough RC Channel
*
@ -2976,6 +3003,24 @@ PARAM_DEFINE_FLOAT(RC_KILLSWITCH_TH, 0.25f);
*/
PARAM_DEFINE_FLOAT(RC_TRANS_TH, 0.25f);
/**
* Threshold for the landing gear switch
*
* 0-1 indicate where in the full channel range the threshold sits
* 0 : min
* 1 : max
* sign indicates polarity of comparison
* positive : true when channel>th
* negative : true when channel<th
*
* @min -1
* @max 1
* @group Radio Switches
*
*
*/
PARAM_DEFINE_FLOAT(RC_GEAR_TH, 0.25f);
/**
* PWM input channel that provides RSSI.
*

17
src/modules/sensors/sensors.cpp
View File

@ -306,6 +306,7 @@ private:
int rc_map_offboard_sw;
int rc_map_kill_sw;
int rc_map_trans_sw;
int rc_map_gear_sw;
int rc_map_flaps;
@ -330,6 +331,7 @@ private:
float rc_offboard_th;
float rc_killswitch_th;
float rc_trans_th;
float rc_gear_th;
bool rc_assist_inv;
bool rc_auto_inv;
bool rc_rattitude_inv;
@ -340,6 +342,7 @@ private:
bool rc_offboard_inv;
bool rc_killswitch_inv;
bool rc_trans_inv;
bool rc_gear_inv;
float battery_voltage_scaling;
float battery_current_scaling;
@ -379,6 +382,7 @@ private:
param_t rc_map_offboard_sw;
param_t rc_map_kill_sw;
param_t rc_map_trans_sw;
param_t rc_map_gear_sw;
param_t rc_map_flaps;
@ -407,6 +411,7 @@ private:
param_t rc_offboard_th;
param_t rc_killswitch_th;
param_t rc_trans_th;
param_t rc_gear_th;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
@ -656,6 +661,7 @@ Sensors::Sensors() :
_parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
_parameter_handles.rc_map_kill_sw = param_find("RC_MAP_KILL_SW");
_parameter_handles.rc_map_trans_sw = param_find("RC_MAP_TRANS_SW");
_parameter_handles.rc_map_gear_sw = param_find("RC_MAP_GEAR_SW");
_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@ -685,6 +691,7 @@ Sensors::Sensors() :
_parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
_parameter_handles.rc_killswitch_th = param_find("RC_KILLSWITCH_TH");
_parameter_handles.rc_trans_th = param_find("RC_TRANS_TH");
_parameter_handles.rc_gear_th = param_find("RC_GEAR_TH");
/* Differential pressure offset */
@ -862,6 +869,10 @@ Sensors::parameters_update()
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_gear_sw, &(_parameters.rc_map_gear_sw)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
PX4_WARN("%s", paramerr);
}
@ -909,6 +920,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_trans_th, &(_parameters.rc_trans_th));
_parameters.rc_trans_inv = (_parameters.rc_trans_th < 0);
_parameters.rc_trans_th = fabs(_parameters.rc_trans_th);
param_get(_parameter_handles.rc_gear_th, &(_parameters.rc_gear_th));
_parameters.rc_gear_inv = (_parameters.rc_gear_th < 0);
_parameters.rc_gear_th = fabs(_parameters.rc_gear_th);
/* update RC function mappings */
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_THROTTLE] = _parameters.rc_map_throttle - 1;
@ -925,6 +939,7 @@ Sensors::parameters_update()
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_KILLSWITCH] = _parameters.rc_map_kill_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION] = _parameters.rc_map_trans_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_GEAR] = _parameters.rc_map_gear_sw - 1;
_rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_FLAPS] = _parameters.rc_map_flaps - 1;
@ -2131,6 +2146,8 @@ Sensors::rc_poll()
_parameters.rc_killswitch_th, _parameters.rc_killswitch_inv);
manual.transition_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_TRANSITION,
_parameters.rc_trans_th, _parameters.rc_trans_inv);
manual.gear_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_GEAR,
_parameters.rc_gear_th, _parameters.rc_gear_inv);
/* publish manual_control_setpoint topic */
if (_manual_control_pub != nullptr) {