diff options
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/commander.cpp | 23 | ||||
-rw-r--r-- | src/modules/commander/px4_custom_mode.h | 1 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 4 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 5 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 48 | ||||
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_params.c | 29 | ||||
-rw-r--r-- | src/modules/sensors/sensor_params.c | 25 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 16 | ||||
-rw-r--r-- | src/modules/uORB/topics/manual_control_setpoint.h | 1 | ||||
-rw-r--r-- | src/modules/uORB/topics/rc_channels.h | 17 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_status.h | 1 |
11 files changed, 156 insertions, 14 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 504696ff9..8336bcf32 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -453,6 +453,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { + /* ACRO */ + main_res = main_state_transition(status, MAIN_STATE_ACRO); } } else { @@ -652,6 +656,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[1] = "ALTCTL"; main_states_str[2] = "POSCTL"; main_states_str[3] = "AUTO"; + main_states_str[4] = "ACRO"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -1600,7 +1605,12 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_OFF: // MANUAL - res = main_state_transition(status, MAIN_STATE_MANUAL); + if (sp_man->acro_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_ACRO); + + } else { + res = main_state_transition(status, MAIN_STATE_MANUAL); + } // TRANSITION_DENIED is not possible here break; @@ -1712,6 +1722,17 @@ set_control_mode() navigator_enabled = true; break; + case MAIN_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + default: break; } diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index a83c81850..e0f8dc95d 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -15,6 +15,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_ALTCTL, PX4_CUSTOM_MAIN_MODE_POSCTL, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_MAIN_MODE_ACRO, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index abcf72717..f1a353d5b 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -222,6 +222,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; + case MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + case MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 79dd88657..2837c16b8 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -136,6 +136,11 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + + } else if (status->main_state == MAIN_STATE_ACRO) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; + custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; } } else { 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 b23166a5e..77c7c61e9 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -162,6 +162,9 @@ private: param_t man_roll_max; param_t man_pitch_max; param_t man_yaw_max; + param_t acro_roll_max; + param_t acro_pitch_max; + param_t acro_yaw_max; } _params_handles; /**< handles for interesting parameters */ struct { @@ -175,6 +178,7 @@ private: float man_roll_max; float man_pitch_max; float man_yaw_max; + math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ } _params; /** @@ -284,6 +288,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.man_roll_max = 0.0f; _params.man_pitch_max = 0.0f; _params.man_yaw_max = 0.0f; + _params.acro_rate_max.zero(); _rates_prev.zero(); _rates_sp.zero(); @@ -310,6 +315,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); + _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); + _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); + _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); /* fetch initial parameter values */ parameters_update(); @@ -386,6 +394,14 @@ MulticopterAttitudeControl::parameters_update() _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); + /* acro control scale */ + param_get(_params_handles.acro_roll_max, &v); + _params.acro_rate_max(0) = math::radians(v); + param_get(_params_handles.acro_pitch_max, &v); + _params.acro_rate_max(1) = math::radians(v); + param_get(_params_handles.acro_yaw_max, &v); + _params.acro_rate_max(2) = math::radians(v); + return OK; } @@ -782,11 +798,33 @@ MulticopterAttitudeControl::task_main() } else { /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; - _thrust_sp = _v_rates_sp.thrust; + if (_v_control_mode.flag_control_manual_enabled) { + /* manual rates control - ACRO mode */ + _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); + _thrust_sp = _manual_control_sp.z; + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + /* attitude controller disabled, poll rates setpoint topic */ + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; + } } if (_v_control_mode.flag_control_rates_enabled) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index ad38a0a03..819847b40 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -215,3 +215,32 @@ PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f); * @group Multicopter Attitude Control */ PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f); + +/** + * Max acro roll rate + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); + +/** + * Max acro pitch rate + * + * @unit deg/s + * @min 0.0 + * @max 360.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); + +/** + * Max acro yaw rate + * + * @unit deg/s + * @min 0.0 + * @group Multicopter Attitude Control + */ +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c021e1fde..999cf8bb3 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -626,6 +626,15 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0); /** + * Acro switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Calibration + */ +PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); + +/** * Flaps channel mapping. * * @min 0 @@ -756,3 +765,19 @@ PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f); * */ PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f); + +/** + * Threshold for selecting acro 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_ACRO_TH, 0.5f); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index c5954d02a..b268b1b36 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -263,6 +263,7 @@ private: int rc_map_return_sw; int rc_map_posctl_sw; int rc_map_loiter_sw; + int rc_map_acro_sw; int rc_map_flaps; @@ -278,11 +279,13 @@ private: float rc_posctl_th; float rc_return_th; float rc_loiter_th; + float rc_acro_th; bool rc_assist_inv; bool rc_auto_inv; bool rc_posctl_inv; bool rc_return_inv; bool rc_loiter_inv; + bool rc_acro_inv; float battery_voltage_scaling; float battery_current_scaling; @@ -315,6 +318,7 @@ private: param_t rc_map_return_sw; param_t rc_map_posctl_sw; param_t rc_map_loiter_sw; + param_t rc_map_acro_sw; param_t rc_map_flaps; @@ -330,6 +334,7 @@ private: param_t rc_posctl_th; param_t rc_return_th; param_t rc_loiter_th; + param_t rc_acro_th; param_t battery_voltage_scaling; param_t battery_current_scaling; @@ -530,6 +535,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW"); _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); + _parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -544,6 +550,7 @@ Sensors::Sensors() : _parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH"); _parameter_handles.rc_return_th = param_find("RC_RETURN_TH"); _parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH"); + _parameter_handles.rc_acro_th = param_find("RC_ACRO_TH"); /* gyro offsets */ _parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF"); @@ -687,6 +694,10 @@ Sensors::parameters_update() warnx("%s", paramerr); } + if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { + warnx("%s", paramerr); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("%s", paramerr); } @@ -712,6 +723,9 @@ Sensors::parameters_update() 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); + param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th)); + _parameters.rc_acro_inv = (_parameters.rc_acro_th < 0); + _parameters.rc_acro_th = fabs(_parameters.rc_acro_th); /* update RC function mappings */ _rc.function[THROTTLE] = _parameters.rc_map_throttle - 1; @@ -723,6 +737,7 @@ Sensors::parameters_update() _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1; _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; + _rc.function[ACRO] = _parameters.rc_map_acro_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1508,6 +1523,7 @@ Sensors::rc_poll() manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_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); + manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 19a29635b..910b8a623 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -97,6 +97,7 @@ struct manual_control_setpoint_s { switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */ switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */ switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */ + switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index b60402452..370c54442 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -45,12 +45,12 @@ /** * The number of RC channel inputs supported. * Current (Q4/2013) radios support up to 18 channels, - * leaving at a sane value of 15. + * leaving at a sane value of 16. * This number can be greater then number of RC channels, * because single RC channel can be mapped to multiple * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAPPED_MAX 15 +#define RC_CHANNELS_MAPPED_MAX 16 /** * This defines the mapping of the RC functions. @@ -67,12 +67,13 @@ enum RC_CHANNELS_FUNCTION { POSCTL = 6, LOITER = 7, OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, - AUX_5 = 14, + ACRO = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, + AUX_5 = 15, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index f56355246..ea20a317a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -66,6 +66,7 @@ typedef enum { MAIN_STATE_ALTCTL, MAIN_STATE_POSCTL, MAIN_STATE_AUTO, + MAIN_STATE_ACRO, MAIN_STATE_MAX } main_state_t; |