fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 11:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ACRO mode implemented

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-31 22:44:05 +01:00
parent d933d523eb
commit 7f4f9a5f5f
9 changed files with 144 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+31 -4
View File
@@ -619,9 +619,10 @@ 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[3] = "AUTO";
main_states_str[1] = "ACRO";
main_states_str[2] = "SEATBELT";
main_states_str[3] = "EASY";
main_states_str[4] = "AUTO";
char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[0] = "INIT";
@@ -1510,6 +1511,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
} else {
status->mission_switch = MISSION_SWITCH_MISSION;
}
/* acro switch */
if (!isfinite(sp_man->acro_switch)) {
status->acro_switch = ACRO_SWITCH_NONE;
} else if (sp_man->acro_switch > STICK_ON_OFF_LIMIT) {
status->acro_switch = ACRO_SWITCH_ACRO;
} else {
status->acro_switch = ACRO_SWITCH_NORMAL;
}
}
transition_result_t
@@ -1520,7 +1532,11 @@ set_main_state_rc(struct vehicle_status_s *status)
switch (status->mode_switch) {
case MODE_SWITCH_MANUAL:
res = main_state_transition(status, MAIN_STATE_MANUAL);
if (status->acro_switch == ACRO_SWITCH_ACRO) {
res = main_state_transition(status, MAIN_STATE_ACRO);
} else {
res = main_state_transition(status, MAIN_STATE_MANUAL);
}
// TRANSITION_DENIED is not possible here
break;
@@ -1600,6 +1616,17 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
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;
case MAIN_STATE_SEATBELT:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
src/modules/commander/state_machine_helper.cpp
+4
View File
@@ -230,6 +230,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_SEATBELT:
/* need at minimum altitude estimate */
src/modules/mc_att_control/mc_att_control_main.cpp
+76 -26
View File
@@ -161,7 +161,12 @@ private:
param_t yaw_rate_i;
param_t yaw_rate_d;
param_t yaw_ff;
param_t roll_scale_acro;
param_t pitch_scale_acro;
param_t yaw_scale_acro;
param_t rc_scale_roll;
param_t rc_scale_pitch;
param_t rc_scale_yaw;
} _params_handles; /**< handles for interesting parameters */
@@ -171,8 +176,9 @@ private:
math::Vector<3> rate_i; /**< I gain for angular rate error */
math::Vector<3> rate_d; /**< D gain for angular rate error */
float yaw_ff; /**< yaw control feed-forward */
math::Vector<3> scale_acro; /**< scale for ACRO mode */
float rc_scale_yaw;
math::Vector<3> rc_scale; /**< scale for MANUAL mode */
} _params;
/**
@@ -275,6 +281,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
_params.rate_p.zero();
_params.rate_i.zero();
_params.rate_d.zero();
_params.scale_acro.zero();
_params.rc_scale.zero();
_R_sp.identity();
_R.identity();
@@ -286,21 +294,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
I.identity();
_params_handles.roll_p = param_find("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
_params_handles.roll_rate_d = param_find("MC_ROLLRATE_D");
_params_handles.pitch_p = param_find("MC_PITCH_P");
_params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
_params_handles.yaw_p = param_find("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.roll_p = param_find("MC_ROLL_P");
_params_handles.roll_rate_p = param_find("MC_ROLLRATE_P");
_params_handles.roll_rate_i = param_find("MC_ROLLRATE_I");
_params_handles.roll_rate_d = param_find("MC_ROLLRATE_D");
_params_handles.pitch_p = param_find("MC_PITCH_P");
_params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P");
_params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I");
_params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D");
_params_handles.yaw_p = param_find("MC_YAW_P");
_params_handles.yaw_rate_p = param_find("MC_YAWRATE_P");
_params_handles.yaw_rate_i = param_find("MC_YAWRATE_I");
_params_handles.yaw_rate_d = param_find("MC_YAWRATE_D");
_params_handles.yaw_ff = param_find("MC_YAW_FF");
_params_handles.roll_scale_acro = param_find("MC_ROLL_S_ACRO");
_params_handles.pitch_scale_acro = param_find("MC_PITCH_S_ACRO");
_params_handles.yaw_scale_acro = param_find("MC_YAW_S_ACRO");
_params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
_params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL");
_params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH");
_params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW");
/* fetch initial parameter values */
parameters_update();
@@ -344,6 +357,10 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_i(0) = v;
param_get(_params_handles.roll_rate_d, &v);
_params.rate_d(0) = v;
param_get(_params_handles.roll_scale_acro, &v);
_params.scale_acro(0) = v;
param_get(_params_handles.rc_scale_roll, &v);
_params.rc_scale(0) = v;
/* pitch */
param_get(_params_handles.pitch_p, &v);
@@ -354,6 +371,10 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_i(1) = v;
param_get(_params_handles.pitch_rate_d, &v);
_params.rate_d(1) = v;
param_get(_params_handles.pitch_scale_acro, &v);
_params.scale_acro(1) = v;
param_get(_params_handles.rc_scale_pitch, &v);
_params.rc_scale(1) = v;
/* yaw */
param_get(_params_handles.yaw_p, &v);
@@ -364,10 +385,11 @@ MulticopterAttitudeControl::parameters_update()
_params.rate_i(2) = v;
param_get(_params_handles.yaw_rate_d, &v);
_params.rate_d(2) = v;
param_get(_params_handles.yaw_ff, &_params.yaw_ff);
param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
param_get(_params_handles.yaw_scale_acro, &v);
_params.scale_acro(2) = v;
param_get(_params_handles.rc_scale_yaw, &v);
_params.rc_scale(2) = v;
return OK;
}
@@ -463,6 +485,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
if (_v_control_mode.flag_control_manual_enabled) {
/* manual input, set or modify attitude setpoint */
vehicle_manual_poll();
if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
@@ -488,16 +511,16 @@ MulticopterAttitudeControl::control_attitude(float dt)
// reset_yaw_sp = true;
//}
} else {
float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale(2);
if (_params.rc_scale(2) > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale(2);
if (_manual_control_sp.yaw > 0.0f) {
yaw_sp_move_rate -= YAW_DEADZONE;
} else {
yaw_sp_move_rate += YAW_DEADZONE;
}
yaw_sp_move_rate *= _params.rc_scale_yaw;
yaw_sp_move_rate *= _params.rc_scale(2);
_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
_v_att_sp.R_valid = false;
publish_att_sp = true;
@@ -743,7 +766,6 @@ MulticopterAttitudeControl::task_main()
parameter_update_poll();
vehicle_control_mode_poll();
arming_status_poll();
vehicle_manual_poll();
if (_v_control_mode.flag_control_attitude_enabled) {
control_attitude(dt);
@@ -764,9 +786,37 @@ MulticopterAttitudeControl::task_main()
} else {
/* attitude controller disabled */
// TODO poll 'attitude_rates_setpoint' topic
_rates_sp.zero();
_thrust_sp = 0.0f;
if (_v_control_mode.flag_control_manual_enabled) {
/* manual rates control, ACRO mode */
vehicle_manual_poll();
_rates_sp(0) = _manual_control_sp.roll;
_rates_sp(1) = _manual_control_sp.pitch;
_rates_sp(2) = _manual_control_sp.yaw;
/* rescale controls for ACRO mode */
_rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro);
_thrust_sp = _manual_control_sp.throttle;
/* 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 {
// TODO poll 'attitude_rates_setpoint' topic
_rates_sp.zero();
_thrust_sp = 0.0f;
}
}
if (_v_control_mode.flag_control_rates_enabled) {
src/modules/mc_att_control/mc_att_control_params.c
+3
View File
@@ -54,3 +54,6 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
PARAM_DEFINE_FLOAT(MC_ROLL_S_ACRO, 5.0f);
PARAM_DEFINE_FLOAT(MC_PITCH_S_ACRO, 5.0f);
PARAM_DEFINE_FLOAT(MC_YAW_S_ACRO, 3.0f);
src/modules/sensors/sensor_params.c
+1
View File
@@ -449,6 +449,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
src/modules/sensors/sensors.cpp
+14
View File
@@ -246,6 +246,7 @@ private:
int rc_map_return_sw;
int rc_map_assisted_sw;
int rc_map_mission_sw;
int rc_map_acro_sw;
// int rc_map_offboard_ctrl_mode_sw;
@@ -296,6 +297,7 @@ private:
param_t rc_map_return_sw;
param_t rc_map_assisted_sw;
param_t rc_map_mission_sw;
param_t rc_map_acro_sw;
// param_t rc_map_offboard_ctrl_mode_sw;
@@ -515,6 +517,7 @@ Sensors::Sensors() :
/* optional mode switches, not mapped per default */
_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
@@ -669,6 +672,10 @@ Sensors::parameters_update()
warnx("Failed getting mission sw chan index");
}
if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) {
warnx("Failed getting acro sw chan index");
}
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
warnx("Failed getting flaps chan index");
}
@@ -700,6 +707,7 @@ Sensors::parameters_update()
_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
@@ -1290,6 +1298,7 @@ Sensors::rc_poll()
manual_control.return_switch = NAN;
manual_control.assisted_switch = NAN;
manual_control.mission_switch = NAN;
manual_control.acro_switch = NAN;
// manual_control.auto_offboard_input_switch = NAN;
manual_control.flaps = NAN;
@@ -1428,6 +1437,11 @@ Sensors::rc_poll()
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
}
/* acro switch input */
if (_rc.function[ACRO] >= 0) {
manual_control.acro_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ACRO]].scaled);
}
/* return switch input */
if (_rc.function[RETURN] >= 0) {
manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
src/modules/uORB/topics/manual_control_setpoint.h
+1
View File
@@ -60,6 +60,7 @@ struct manual_control_setpoint_s {
float return_switch; /**< land 2 position switch (mandatory): land, no effect */
float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */
float mission_switch; /**< mission 2 position switch (optional): mission, loiter */
float acro_switch; /**< acro 2 position switch (optional): normal, acro */
/**
* Any of the channels below may not be available and be set to NaN
src/modules/uORB/topics/rc_channels.h
+6 -5
View File
@@ -68,11 +68,12 @@ enum RC_CHANNELS_FUNCTION
ASSISTED = 6,
MISSION = 7,
OFFBOARD_MODE = 8,
FLAPS = 9,
AUX_1 = 10,
AUX_2 = 11,
AUX_3 = 12,
AUX_4 = 13,
ACRO = 9,
FLAPS = 10,
AUX_1 = 11,
AUX_2 = 12,
AUX_3 = 13,
AUX_4 = 14,
AUX_5 = 14,
RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
};
src/modules/uORB/topics/vehicle_status.h
+8
View File
@@ -63,6 +63,7 @@
/* main state machine */
typedef enum {
MAIN_STATE_MANUAL = 0,
MAIN_STATE_ACRO,
MAIN_STATE_SEATBELT,
MAIN_STATE_EASY,
MAIN_STATE_AUTO,
@@ -116,6 +117,12 @@ typedef enum {
MISSION_SWITCH_MISSION
} mission_switch_pos_t;
typedef enum {
ACRO_SWITCH_NONE = 0,
ACRO_SWITCH_NORMAL,
ACRO_SWITCH_ACRO
} acro_switch_pos_t;
enum VEHICLE_MODE_FLAG {
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -192,6 +199,7 @@ struct vehicle_status_s
return_switch_pos_t return_switch;
assisted_switch_pos_t assisted_switch;
mission_switch_pos_t mission_switch;
acro_switch_pos_t acro_switch;
bool condition_battery_voltage_valid;
bool condition_system_in_air_restore; /**< true if we can restore in mid air */