Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-21 21:55:34 +08:00
Code Issues Actions 5 Packages Projects Releases Wiki Activity

Merge pull request #858 from TickTock-/rc_merged

Browse Source 
Rc merged
This commit is contained in:
Lorenz Meier
2014-05-08 23:21:22 -07:00
parent fc54cc2d1f 9173b8bc76
commit e18bdfdf65
17 changed files with 270 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Documentation/rc_mode_switch.odg
BIN
View File
Binary file not shown.
Documentation/rc_mode_switch.pdf
BIN
View File
Binary file not shown.
src/drivers/blinkm/blinkm.cpp
+2 -2
View File
@@ -638,11 +638,11 @@ BlinkM::led()
if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) {
/* indicate main control state */
if (vehicle_status_raw.main_state == MAIN_STATE_EASY)
if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL)
led_color_4 = LED_GREEN;
else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO)
led_color_4 = LED_BLUE;
else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT)
else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL)
led_color_4 = LED_YELLOW;
else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL)
led_color_4 = LED_WHITE;
src/modules/commander/commander.cpp
+27 -26
View File
@@ -435,13 +435,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
/* SEATBELT */
main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) {
/* ALTCTRL */
main_res = main_state_transition(status, MAIN_STATE_ALTCTRL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) {
/* POSCTRL */
main_res = main_state_transition(status, MAIN_STATE_POSCTRL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
@@ -456,8 +456,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
/* POSCTRL */
main_res = main_state_transition(status, MAIN_STATE_POSCTRL);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
@@ -634,8 +634,8 @@ int commander_thread_main(int argc, char *argv[])
char *main_states_str[MAIN_STATE_MAX];
main_states_str[0] = "MANUAL";
main_states_str[1] = "SEATBELT";
main_states_str[2] = "EASY";
main_states_str[1] = "ALTCTRL";
main_states_str[2] = "POSCTRL";
main_states_str[3] = "AUTO";
char *arming_states_str[ARMING_STATE_MAX];
@@ -1159,7 +1159,7 @@ int commander_thread_main(int argc, char *argv[])
/* arm/disarm by RC */
res = TRANSITION_NOT_CHANGED;
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
* do it only for rotary wings */
if (status.is_rotary_wing &&
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
@@ -1242,7 +1242,8 @@ int commander_thread_main(int argc, char *argv[])
status.set_nav_state_timestamp = hrt_absolute_time();
} else {
/* MISSION switch */
/* LOITER switch */
if (sp_man.loiter_switch == SWITCH_POS_ON) {
/* stick is in LOITER position */
status.set_nav_state = NAV_STATE_LOITER;
@@ -1334,8 +1335,8 @@ int commander_thread_main(int argc, char *argv[])
}
// TODO remove this hack
/* flight termination in manual mode if assisted switch is on easy position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
/* flight termination in manual mode if assist switch is on posctrl position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) {
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
tune_positive(armed.armed);
}
@@ -1591,26 +1592,26 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
// TRANSITION_DENIED is not possible here
break;
case SWITCH_POS_MIDDLE: // ASSISTED
if (sp_man->assisted_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_EASY);
case SWITCH_POS_MIDDLE: // ASSIST
if (sp_man->posctrl_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_POSCTRL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
}
// else fallback to SEATBELT
print_reject_mode(status, "EASY");
// else fallback to ALTCTRL
print_reject_mode(status, "POSCTRL");
}
res = main_state_transition(status, MAIN_STATE_SEATBELT);
res = main_state_transition(status, MAIN_STATE_ALTCTRL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this mode
}
if (sp_man->assisted_switch != SWITCH_POS_ON) {
print_reject_mode(status, "SEATBELT");
if (sp_man->posctrl_switch != SWITCH_POS_ON) {
print_reject_mode(status, "ALTCTRL");
}
// else fallback to MANUAL
@@ -1625,9 +1626,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
break; // changed successfully or already in this state
}
// else fallback to SEATBELT (EASY likely will not work too)
// else fallback to ALTCTRL (POSCTRL likely will not work too)
print_reject_mode(status, "AUTO");
res = main_state_transition(status, MAIN_STATE_SEATBELT);
res = main_state_transition(status, MAIN_STATE_ALTCTRL);
if (res != TRANSITION_DENIED) {
break; // changed successfully or already in this state
@@ -1673,7 +1674,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_ALTCTRL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
@@ -1684,7 +1685,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
case MAIN_STATE_EASY:
case MAIN_STATE_POSCTRL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
src/modules/commander/commander_tests/state_machine_helper_test.cpp
+12 -12
View File
@@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to SEATBELT.
// MANUAL to ALTCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = true;
new_main_state = MAIN_STATE_SEATBELT;
ut_assert("tranisition: manual to seatbelt",
new_main_state = MAIN_STATE_ALTCTRL;
ut_assert("tranisition: manual to altctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state);
// MANUAL to SEATBELT, invalid local altitude.
// MANUAL to ALTCTRL, invalid local altitude.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_altitude_valid = false;
new_main_state = MAIN_STATE_SEATBELT;
new_main_state = MAIN_STATE_ALTCTRL;
ut_assert("no transition: invalid local altitude",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
// MANUAL to EASY.
// MANUAL to POSCTRL.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = true;
new_main_state = MAIN_STATE_EASY;
ut_assert("transition: manual to easy",
new_main_state = MAIN_STATE_POSCTRL;
ut_assert("transition: manual to posctrl",
TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state);
// MANUAL to EASY, invalid local position.
// MANUAL to POSCTRL, invalid local position.
current_state.main_state = MAIN_STATE_MANUAL;
current_state.condition_local_position_valid = false;
new_main_state = MAIN_STATE_EASY;
new_main_state = MAIN_STATE_POSCTRL;
ut_assert("no transition: invalid position",
TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
src/modules/commander/px4_custom_mode.h
+2 -2
View File
@@ -12,8 +12,8 @@
enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
PX4_CUSTOM_MAIN_MODE_SEATBELT,
PX4_CUSTOM_MAIN_MODE_EASY,
PX4_CUSTOM_MAIN_MODE_ALTCTRL,
PX4_CUSTOM_MAIN_MODE_POSCTRL,
PX4_CUSTOM_MAIN_MODE_AUTO,
};
src/modules/commander/state_machine_helper.cpp
+2 -2
View File
@@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
ret = TRANSITION_CHANGED;
break;
case MAIN_STATE_SEATBELT:
case MAIN_STATE_ALTCTRL:
/* need at minimum altitude estimate */
if (!status->is_rotary_wing ||
@@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
break;
case MAIN_STATE_EASY:
case MAIN_STATE_POSCTRL:
/* need at minimum local position estimate */
if (status->condition_local_position_valid ||
src/modules/fixedwing_backside/fixedwing.cpp
+2 -2
View File
@@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update()
_actuators.control[CH_RDR] = _manual.yaw;
_actuators.control[CH_THR] = _manual.throttle;
} else if (_status.main_state == MAIN_STATE_SEATBELT ||
_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
} else if (_status.main_state == MAIN_STATE_ALTCTRL ||
_status.main_state == MAIN_STATE_POSCTRL /* TODO, implement pos control */) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
// for the purpose of control we will limit the velocity feedback between
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+21 -21
View File
@@ -153,7 +153,7 @@ private:
bool _setpoint_valid; /**< flag if the position control setpoint is valid */
/** manual control states */
float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */
float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */
double _loiter_hold_lat;
double _loiter_hold_lon;
float _loiter_hold_alt;
@@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_reset_integral = true;
}
} else if (0/* easy mode enabled */) {
} else if (0/* posctrl mode enabled */) {
/** EASY FLIGHT **/
/** POSCTRL FLIGHT **/
if (0/* switched from another mode to easy */) {
_seatbelt_hold_heading = _att.yaw;
if (0/* switched from another mode to posctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* easy on and manual control yaw non-zero */) {
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
if (0/* posctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.yaw;
}
//XXX not used
@@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// climb_out = true;
// }
/* if in seatbelt mode, set airspeed based on manual control */
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float seatbelt_airspeed = _parameters.airspeed_min +
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
seatbelt_airspeed,
altctrl_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
} else if (0/* seatbelt mode enabled */) {
} else if (0/* altctrl mode enabled */) {
/** SEATBELT FLIGHT **/
/** ALTCTRL FLIGHT **/
if (0/* switched from another mode to seatbelt */) {
_seatbelt_hold_heading = _att.yaw;
if (0/* switched from another mode to altctrl */) {
_altctrl_hold_heading = _att.yaw;
}
if (0/* seatbelt on and manual control yaw non-zero */) {
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
if (0/* altctrl on and manual control yaw non-zero */) {
_altctrl_hold_heading = _att.yaw + _manual.yaw;
}
/* if in seatbelt mode, set airspeed based on manual control */
/* if in altctrl mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float seatbelt_airspeed = _parameters.airspeed_min +
float altctrl_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
@@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
climb_out = true;
}
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _manual.roll;
_att_sp.yaw_body = _manual.yaw;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
seatbelt_airspeed,
altctrl_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
climb_out, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
src/modules/mavlink/mavlink_messages.cpp
+4 -4
View File
@@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
} else if (status->main_state == MAIN_STATE_SEATBELT) {
} else if (status->main_state == MAIN_STATE_ALTCTRL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL;
} else if (status->main_state == MAIN_STATE_EASY) {
} else if (status->main_state == MAIN_STATE_POSCTRL) {
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL;
} else if (status->main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
src/modules/mc_pos_control/mc_pos_control_params.c
+5 -5
View File
@@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
/**
* Maximum vertical velocity
*
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY).
* Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
*
* @unit m/s
* @min 0.0
@@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
/**
* Vertical velocity feed forward
*
* Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
* Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
@@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
/**
* Maximum horizontal velocity
*
* Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY).
* Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
*
* @unit m/s
* @min 0.0
@@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
/**
* Horizontal velocity feed forward
*
* Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
* Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
*
* @min 0.0
* @max 1.0
@@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
/**
* Maximum tilt angle in air
*
* Limits maximum tilt in AUTO and EASY modes during flight.
* Limits maximum tilt in AUTO and POSCTRL modes during flight.
*
* @unit deg
* @min 0.0
src/modules/segway/BlockSegwayController.cpp
+2 -2
View File
@@ -35,8 +35,8 @@ void BlockSegwayController::update() {
// handle autopilot modes
if (_status.main_state == MAIN_STATE_AUTO ||
_status.main_state == MAIN_STATE_SEATBELT ||
_status.main_state == MAIN_STATE_EASY) {
_status.main_state == MAIN_STATE_ALTCTRL ||
_status.main_state == MAIN_STATE_POSCTRL) {
_actuators.control[0] = spdCmd;
_actuators.control[1] = spdCmd;
src/modules/sensors/sensor_params.c
+95 -1
View File
@@ -535,6 +535,20 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
*/
PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);
/**
* Failsafe channel mapping.
*
* The RC mapping index indicates which channel is used for failsafe
* If 0, whichever channel is mapped to throttle is used
* otherwise the value indicates the specific rc channel to use
*
* @min 0
* @max 18
*
*
*/
PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function
/**
* Throttle control channel mapping.
*
@@ -591,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
* @max 18
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
/**
* Loiter switch channel mapping.
@@ -655,3 +669,83 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
* @group Radio Calibration
*/
PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
/**
* Threshold for selecting assist mode
*
* min:-1
* max:+1
*
* 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
*
*/
PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f);
/**
* Threshold for selecting auto mode
*
* min:-1
* max:+1
*
* 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
*
*/
PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f);
/**
* Threshold for selecting posctrl mode
*
* min:-1
* max:+1
*
* 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
*
*/
PARAM_DEFINE_FLOAT(RC_POSCTRL_TH, 0.5f);
/**
* Threshold for selecting return to launch mode
*
* min:-1
* max:+1
*
* 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
*
*/
PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f);
/**
* Threshold for selecting loiter mode
*
* min:-1
* max:+1
*
* 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
*
*/
PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f);
src/modules/sensors/sensors.cpp
+91 -29
View File
@@ -175,7 +175,8 @@ private:
/**
* Get switch position for specified function.
*/
switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func);
switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv);
switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv);
/**
* Gather and publish RC input data.
@@ -250,10 +251,11 @@ private:
int rc_map_pitch;
int rc_map_yaw;
int rc_map_throttle;
int rc_map_failsafe;
int rc_map_mode_sw;
int rc_map_return_sw;
int rc_map_assisted_sw;
int rc_map_posctrl_sw;
int rc_map_loiter_sw;
// int rc_map_offboard_ctrl_mode_sw;
@@ -266,7 +268,17 @@ private:
int rc_map_aux4;
int rc_map_aux5;
int32_t rc_fs_thr;
int32_t rc_fails_thr;
float rc_assist_th;
float rc_auto_th;
float rc_posctrl_th;
float rc_return_th;
float rc_loiter_th;
bool rc_assist_inv;
bool rc_auto_inv;
bool rc_posctrl_inv;
bool rc_return_inv;
bool rc_loiter_inv;
float battery_voltage_scaling;
float battery_current_scaling;
@@ -293,10 +305,11 @@ private:
param_t rc_map_pitch;
param_t rc_map_yaw;
param_t rc_map_throttle;
param_t rc_map_failsafe;
param_t rc_map_mode_sw;
param_t rc_map_return_sw;
param_t rc_map_assisted_sw;
param_t rc_map_posctrl_sw;
param_t rc_map_loiter_sw;
// param_t rc_map_offboard_ctrl_mode_sw;
@@ -309,7 +322,12 @@ private:
param_t rc_map_aux4;
param_t rc_map_aux5;
param_t rc_fs_thr;
param_t rc_fails_thr;
param_t rc_assist_th;
param_t rc_auto_th;
param_t rc_posctrl_th;
param_t rc_return_th;
param_t rc_loiter_th;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
@@ -499,6 +517,7 @@ Sensors::Sensors() :
_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
_parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW");
_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
_parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE");
/* mandatory mode switches, mapped to channel 5 and 6 per default */
_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
@@ -507,7 +526,7 @@ Sensors::Sensors() :
_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");
/* optional mode switches, not mapped per default */
_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
_parameter_handles.rc_map_posctrl_sw = param_find("RC_MAP_POSCTL_SW");
_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
@@ -518,8 +537,13 @@ Sensors::Sensors() :
_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");
/* RC failsafe */
_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
/* RC thresholds */
_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
_parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
_parameter_handles.rc_posctrl_th = param_find("RC_POSCTRL_TH");
_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -642,6 +666,10 @@ Sensors::parameters_update()
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
warnx("%s", paramerr);
}
@@ -650,7 +678,7 @@ Sensors::parameters_update()
warnx("%s", paramerr);
}
if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
if (param_get(_parameter_handles.rc_map_posctrl_sw, &(_parameters.rc_map_posctrl_sw)) != OK) {
warnx("%s", paramerr);
}
@@ -671,7 +699,22 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
_parameters.rc_assist_inv = (_parameters.rc_assist_th<0);
_parameters.rc_assist_th = fabs(_parameters.rc_assist_th);
param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
_parameters.rc_auto_inv = (_parameters.rc_auto_th<0);
_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
param_get(_parameter_handles.rc_posctrl_th, &(_parameters.rc_posctrl_th));
_parameters.rc_posctrl_inv = (_parameters.rc_posctrl_th<0);
_parameters.rc_posctrl_th = fabs(_parameters.rc_posctrl_th);
param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th));
_parameters.rc_return_inv = (_parameters.rc_return_th<0);
_parameters.rc_return_th = fabs(_parameters.rc_return_th);
param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th));
_parameters.rc_loiter_inv = (_parameters.rc_loiter_th<0);
_parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th);
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -681,7 +724,7 @@ Sensors::parameters_update()
_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
_rc.function[POSCTRL] = _parameters.rc_map_posctrl_sw - 1;
_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
@@ -1275,18 +1318,35 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
}
switch_pos_t
Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func)
Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
{
if (_rc.function[func] >= 0) {
float value = _rc.chan[_rc.function[func]].scaled;
if (value > STICK_ON_OFF_LIMIT) {
float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return SWITCH_POS_ON;
} else if (value < -STICK_ON_OFF_LIMIT) {
return SWITCH_POS_OFF;
} else if (mid_inv ? value < mid_th : value > mid_th) {
return SWITCH_POS_MIDDLE;
} else {
return SWITCH_POS_MIDDLE;
return SWITCH_POS_OFF;
}
} else {
return SWITCH_POS_NONE;
}
}
switch_pos_t
Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
{
if (_rc.function[func] >= 0) {
float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
if (on_inv ? value < on_th : value > on_th) {
return SWITCH_POS_ON;
} else {
return SWITCH_POS_OFF;
}
} else {
@@ -1318,13 +1378,16 @@ Sensors::rc_poll()
/* signal looks good */
signal_lost = false;
/* check throttle failsafe */
int8_t thr_ch = _rc.function[THROTTLE];
if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) {
/* throttle failsafe configured */
if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) ||
(_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) {
/* throttle failsafe triggered, signal is lost by receiver */
/* check failsafe */
int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle
if (_parameters.rc_map_failsafe>0){ // if not 0, use channel number instead of rc.function mapping
fs_ch = _parameters.rc_map_failsafe - 1;
}
if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) {
/* failsafe configured */
if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) ||
(_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) {
/* failsafe triggered, signal is lost by receiver */
signal_lost = true;
}
}
@@ -1414,10 +1477,10 @@ Sensors::rc_poll()
manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);
/* mode switches */
manual.mode_switch = get_rc_switch_position(MODE);
manual.assisted_switch = get_rc_switch_position(ASSISTED);
manual.loiter_switch = get_rc_switch_position(LOITER);
manual.return_switch = get_rc_switch_position(RETURN);
manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
manual.posctrl_switch = get_rc_sw2pos_position(POSCTRL, _parameters.rc_posctrl_th, _parameters.rc_posctrl_inv);
manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
/* publish manual_control_setpoint topic */
if (_manual_control_pub > 0) {
@@ -1641,4 +1704,3 @@ int sensors_main(int argc, char *argv[])
warnx("unrecognized command");
return 1;
}
src/modules/uORB/topics/manual_control_setpoint.h
+2 -2
View File
@@ -77,8 +77,8 @@ struct manual_control_setpoint_s {
float aux5; /**< default function: payload drop */
switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */
switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */
switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
switch_pos_t return_switch; /**< rturn to launch 2 position switch (mandatory): no effect, return */
switch_pos_t posctrl_switch; /**< posctrl 2 position switch (optional): altctrl, posctrl */
switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */
}; /**< manual control inputs */
src/modules/uORB/topics/rc_channels.h
+1 -1
View File
@@ -64,7 +64,7 @@ enum RC_CHANNELS_FUNCTION {
YAW = 3,
MODE = 4,
RETURN = 5,
ASSISTED = 6,
POSCTRL = 6,
LOITER = 7,
OFFBOARD_MODE = 8,
FLAPS = 9,
src/modules/uORB/topics/vehicle_status.h
+2 -2
View File
@@ -63,8 +63,8 @@
/* main state machine */
typedef enum {
MAIN_STATE_MANUAL = 0,
MAIN_STATE_SEATBELT,
MAIN_STATE_EASY,
MAIN_STATE_ALTCTRL,
MAIN_STATE_POSCTRL,
MAIN_STATE_AUTO,
MAIN_STATE_MAX
} main_state_t;