From 31089a290ba089b2b5cbcc76ed677e3f401ffa36 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Mon, 28 Apr 2014 21:47:45 -0700 Subject: Replaces poshold/althold with posctrl/altctrl --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++++----------- .../commander_tests/state_machine_helper_test.cpp | 24 +++++------ src/modules/commander/px4_custom_mode.h | 4 +- src/modules/commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 42 ++++++++++---------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- src/modules/mc_pos_control/mc_pos_control_params.c | 10 ++--- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 ++++++------ src/modules/uORB/topics/manual_control_setpoint.h | 2 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 15 files changed, 95 insertions(+), 95 deletions(-) (limited to 'src') diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index aa9c64502..974e20ca2 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -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_POSHOLD) + 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_ALTHOLD) + 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; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a99456370..be3e6d269 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,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_ALTHOLD) { - /* ALTHOLD */ - main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); + } 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_POSHOLD) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + } 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 */ @@ -455,8 +455,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) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,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] = "ALTHOLD"; - main_states_str[2] = "POSHOLD"; + main_states_str[1] = "ALTCTRL"; + main_states_str[2] = "POSCTRL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on poshold position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted 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); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->poshold_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSHOLD); + 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 ALTHOLD - print_reject_mode(status, "POSHOLD"); + // else fallback to ALTCTRL + print_reject_mode(status, "POSCTRL"); } - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->poshold_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTHOLD"); + if (sp_man->posctrl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTRL"); } // else fallback to MANUAL @@ -1590,9 +1590,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 ALTHOLD (POSHOLD 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_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTHOLD: + 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; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSHOLD: + 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; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index b440e561b..18586053b 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to ALTHOLD. + // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTHOLD; - ut_assert("tranisition: manual to althold", + new_main_state = MAIN_STATE_ALTCTRL; + ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); - // MANUAL to ALTHOLD, 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_ALTHOLD; + new_main_state = MAIN_STATE_ALTCTRL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to POSHOLD. + // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSHOLD; - ut_assert("transition: manual to poshold", + new_main_state = MAIN_STATE_POSCTRL; + ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); - // MANUAL to POSHOLD, 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_POSHOLD; + new_main_state = MAIN_STATE_POSCTRL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 962d2804c..e6274fb89 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTHOLD, - PX4_CUSTOM_MAIN_MODE_POSHOLD, + PX4_CUSTOM_MAIN_MODE_ALTCTRL, + PX4_CUSTOM_MAIN_MODE_POSCTRL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21e36a87d..3b6d95135 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -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_ALTHOLD: + 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_POSHOLD: + case MAIN_STATE_POSCTRL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index fafab9bfe..dc82ee475 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -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_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d5a68e21f..024dd98ec 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _althold_hold_heading; /**< heading the system should hold in althold 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> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* poshold mode enabled */) { + } else if (0/* posctrl mode enabled */) { - /** POSHOLD FLIGHT **/ + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to poshold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* poshold on and manual control yaw non-zero */) { - _althold_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> ¤t_positi // climb_out = true; // } - /* if in althold 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 althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_althold_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, - althold_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/* althold mode enabled */) { + } else if (0/* altctrl mode enabled */) { - /** ALTHOLD FLIGHT **/ + /** ALTCTRL FLIGHT **/ - if (0/* switched from another mode to althold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to altctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* althold on and manual control yaw non-zero */) { - _althold_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 althold 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 althold_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> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_althold_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, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 108ef8ad4..38a5433ff 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -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_ALTHOLD) { + } 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_ALTHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; - } else if (status->main_state == MAIN_STATE_POSHOLD) { + } 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_POSHOLD; + 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; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 015d8ad16..dacdd46f0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -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 (ALTHOLD, POSHOLD). + * 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 (ALTHOLD, POSHOLD). 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 (POSHOLD). + * 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 (POSHOLD). 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 POSHOLD modes during flight. + * Limits maximum tilt in AUTO and POSCTRL modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 2cb4fc900..6d46db9bd 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD) { + _status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 59bd99db7..02890b452 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting poshold mode + * Threshold for selecting posctrl mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel