diff --git a/msg/commander_state.msg b/msg/commander_state.msg index 7d5c85e11d..1493035b4b 100644 --- a/msg/commander_state.msg +++ b/msg/commander_state.msg @@ -16,7 +16,8 @@ uint8 MAIN_STATE_AUTO_LAND = 11 uint8 MAIN_STATE_AUTO_FOLLOW_TARGET = 12 uint8 MAIN_STATE_AUTO_PRECLAND = 13 uint8 MAIN_STATE_ORBIT = 14 -uint8 MAIN_STATE_MAX = 15 +uint8 MAIN_STATE_AIRSPEED = 15 +uint8 MAIN_STATE_MAX = 16 uint8 main_state # main state machine diff --git a/msg/vehicle_control_mode.msg b/msg/vehicle_control_mode.msg index 85ee40229d..cf1df8485c 100644 --- a/msg/vehicle_control_mode.msg +++ b/msg/vehicle_control_mode.msg @@ -15,3 +15,4 @@ bool flag_control_position_enabled # true if position is controlled bool flag_control_altitude_enabled # true if altitude is controlled bool flag_control_climb_rate_enabled # true if climb rate is controlled bool flag_control_termination_enabled # true if flighttermination is enabled +bool flag_control_airspeed_enabled # true if airspeed is controlled (only fixed-wing) diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 843b84beff..e605db0bf7 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -32,7 +32,7 @@ uint8 NAVIGATION_STATE_AUTO_RTL = 5 # Auto return to launch mode uint8 NAVIGATION_STATE_AUTO_LANDENGFAIL = 8 # Auto land on engine failure uint8 NAVIGATION_STATE_AUTO_LANDGPSFAIL = 9 # Auto land on gps failure (e.g. open loop loiter down) uint8 NAVIGATION_STATE_ACRO = 10 # Acro mode -uint8 NAVIGATION_STATE_UNUSED = 11 # Free slot +uint8 NAVIGATION_STATE_AIRSPEED = 11 # Airspeed control mode (only fixed-wing) uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 diff --git a/msg/vehicle_status_flags.msg b/msg/vehicle_status_flags.msg index ae2da300f2..300fc96280 100644 --- a/msg/vehicle_status_flags.msg +++ b/msg/vehicle_status_flags.msg @@ -14,6 +14,7 @@ bool condition_local_position_valid # set to true by the commander app if the q bool condition_local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation bool condition_global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation bool condition_home_position_valid # indicates a valid home position (a valid home position is not always a valid launch) +bool condition_airspeed_valid # set to true if airspeed mesurements are valid bool condition_power_input_valid # set if input power is valid bool condition_battery_healthy # set if battery is available and not low bool condition_escs_error # set to true if one or more ESCs reporting esc_status are offline diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index f315c6d007..58c29a91c7 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -342,6 +342,9 @@ int Commander::custom_command(int argc, char *argv[]) } else if (!strcmp(argv[1], "stabilized")) { send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_STABILIZED); + } else if (!strcmp(argv[1], "airspeed")) { + send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AIRSPEED); + } else if (!strcmp(argv[1], "auto:takeoff")) { send_vehicle_command(vehicle_command_s::VEHICLE_CMD_DO_SET_MODE, 1, PX4_CUSTOM_MAIN_MODE_AUTO, PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF); @@ -802,6 +805,11 @@ Commander::handle_command(const vehicle_command_s &cmd) /* OFFBOARD */ main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, _internal_state); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AIRSPEED) { + /* AIRSPEED */ + main_ret = main_state_transition(_status, commander_state_s::MAIN_STATE_AIRSPEED, _status_flags, _internal_state); + printf("main_ret: %i\n", main_ret); } } else { @@ -3227,6 +3235,13 @@ Commander::update_control_mode() _vehicle_control_mode.flag_control_attitude_enabled = true; break; + case vehicle_status_s::NAVIGATION_STATE_AIRSPEED: + _vehicle_control_mode.flag_control_manual_enabled = true; + _vehicle_control_mode.flag_control_rates_enabled = true; + _vehicle_control_mode.flag_control_attitude_enabled = true; + _vehicle_control_mode.flag_control_airspeed_enabled = true; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: _vehicle_control_mode.flag_control_manual_enabled = true; _vehicle_control_mode.flag_control_rates_enabled = true; @@ -3886,6 +3901,8 @@ void Commander::estimator_check() _status_flags.condition_angular_velocity_valid = (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s) && PX4_ISFINITE(angular_velocity.xyz[0]) && PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]); + + _status_flags.condition_airspeed_valid = true; // TODO: check airspeed status } void Commander::UpdateEstimateValidity() @@ -4108,7 +4125,7 @@ The commander module contains the state machine for mode switching and failsafe PRINT_MODULE_USAGE_COMMAND("land"); PRINT_MODULE_USAGE_COMMAND_DESCR("transition", "VTOL transition"); PRINT_MODULE_USAGE_COMMAND_DESCR("mode", "Change flight mode"); - PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland", + PRINT_MODULE_USAGE_ARG("manual|acro|offboard|stabilized|altctl|posctl|airspeed|auto:mission|auto:loiter|auto:rtl|auto:takeoff|auto:land|auto:precland", "Flight mode", false); PRINT_MODULE_USAGE_COMMAND("pair"); PRINT_MODULE_USAGE_COMMAND("lockdown"); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 75a7bb0b9a..3314283c7c 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -51,6 +51,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_OFFBOARD, PX4_CUSTOM_MAIN_MODE_STABILIZED, PX4_CUSTOM_MAIN_MODE_RATTITUDE_LEGACY, + PX4_CUSTOM_MAIN_MODE_AIRSPEED, PX4_CUSTOM_MAIN_MODE_SIMPLE /* unused, but reserved for future use */ }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6cadb0d1b0..d6f9450206 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -300,6 +300,15 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai ret = TRANSITION_CHANGED; break; + case commander_state_s::MAIN_STATE_AIRSPEED: + + /* need at minimum airspeed estimate */ + if (status_flags.condition_airspeed_valid) { + ret = TRANSITION_CHANGED; + } + + break; + case commander_state_s::MAIN_STATE_ALTCTL: /* need at minimum altitude estimate */ @@ -457,6 +466,7 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ case commander_state_s::MAIN_STATE_ACRO: case commander_state_s::MAIN_STATE_MANUAL: case commander_state_s::MAIN_STATE_STAB: + case commander_state_s::MAIN_STATE_AIRSPEED: case commander_state_s::MAIN_STATE_ALTCTL: // Require RC for all manual modes @@ -478,6 +488,10 @@ bool set_nav_state(vehicle_status_s &status, actuator_armed_s &armed, commander_ status.nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; break; + case commander_state_s::MAIN_STATE_AIRSPEED: + status.nav_state = vehicle_status_s::NAVIGATION_STATE_AIRSPEED; + break; + case commander_state_s::MAIN_STATE_ALTCTL: status.nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL; break; diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index 422844b552..6ba42fd28b 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -393,6 +393,10 @@ void FlightModeManager::send_vehicle_cmd_do(uint8_t nav_state) command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; break; + case vehicle_status_s::NAVIGATION_STATE_AIRSPEED: + command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AIRSPEED; + break; + default: //vehicle_status_s::NAVIGATION_STATE_POSCTL command.param2 = (float)PX4_CUSTOM_MAIN_MODE_POSCTL; break; diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 8093fd544b..71875ee750 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -256,6 +256,11 @@ FixedwingPositionControl::manual_control_setpoint_poll() _manual_control_setpoint_altitude = -(math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f) * 2.f - 1.f); _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f; } + + // in airspeed mode the airspeed setpoint is taken from the pitch stick + if (!_control_mode.flag_control_altitude_enabled && _control_mode.flag_control_airspeed_enabled) { + _manual_control_setpoint_airspeed = math::constrain(_manual_control_setpoint.x, -1.0f, 1.0f) / 2.f + 0.5f; + } } @@ -1117,6 +1122,33 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); _att_sp.yaw_body = 0; + } else if (_control_mode.flag_control_airspeed_enabled) { + /* AIRSPEED CONTROL: pitch stick moves airspeed setpoint, throttle stick sets throttle */ + + _control_mode_current = FW_POSCTRL_MODE_AIRSPEED; + + // if we assume that user is taking off then help by demanding altitude setpoint well above ground + // and set limit to pitch angle to prevent steering into ground + // this will only affect planes and not VTOL + float pitch_limit_min = _param_fw_p_lim_min.get(); + // do_takeoff_help(&_hold_alt, &pitch_limit_min); + + _tecs.set_speed_weight(2.0f); + + tecs_update_pitch_throttle(now, _current_altitude, + get_demanded_airspeed(), + radians(_param_fw_p_lim_min.get()), + radians(_param_fw_p_lim_max.get()), + 0.0f, + 1.0f, + _param_fw_thr_cruise.get(), + false, + pitch_limit_min, + tecs_status_s::TECS_MODE_NORMAL); + + _att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get()); + _att_sp.yaw_body = 0; + } else { _control_mode_current = FW_POSCTRL_MODE_OTHER; @@ -1155,6 +1187,9 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 _att_sp.thrust_body[0] = 0.0f; + } else if (_control_mode_current == FW_POSCTRL_MODE_AIRSPEED) { + _att_sp.thrust_body[0] = _manual_control_setpoint.z; + } else if (_control_mode_current == FW_POSCTRL_MODE_OTHER) { _att_sp.thrust_body[0] = min(_att_sp.thrust_body[0], _param_fw_thr_max.get()); @@ -1794,7 +1829,8 @@ FixedwingPositionControl::Run() if (_control_mode.flag_control_position_enabled || _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_acceleration_enabled || - _control_mode.flag_control_altitude_enabled) { + _control_mode.flag_control_altitude_enabled || + _control_mode.flag_control_airspeed_enabled) { const Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -1926,13 +1962,14 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo || mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM)); /* tell TECS to update its state, but let it know when it cannot actually control the plane */ - bool in_air_alt_control = (!_landed && - (_control_mode.flag_control_auto_enabled || - _control_mode.flag_control_velocity_enabled || - _control_mode.flag_control_altitude_enabled)); + const bool in_air_use_tecs = (!_landed && + (_control_mode.flag_control_auto_enabled || + _control_mode.flag_control_velocity_enabled || + _control_mode.flag_control_altitude_enabled || + _control_mode.flag_control_airspeed_enabled)); /* update TECS vehicle state estimates */ - _tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control, + _tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_use_tecs, _current_altitude, _local_pos.vz); /* scale throttle cruise by baro pressure */ diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index ad6b833e95..d040418918 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -261,6 +261,7 @@ private: enum FW_POSCTRL_MODE { FW_POSCTRL_MODE_AUTO, FW_POSCTRL_MODE_POSITION, + FW_POSCTRL_MODE_AIRSPEED, FW_POSCTRL_MODE_ALTITUDE, FW_POSCTRL_MODE_OTHER } _control_mode_current{FW_POSCTRL_MODE_OTHER}; ///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode. diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 1ff58e5b8a..4194547253 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -285,6 +285,10 @@ union px4_custom_mode get_px4_custom_mode(uint8_t nav_state) custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED; break; + case vehicle_status_s::NAVIGATION_STATE_AIRSPEED: + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AIRSPEED; + break; + case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF: custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c0051a0929..8d428bae7c 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -667,6 +667,7 @@ Navigator::run() case vehicle_status_s::NAVIGATION_STATE_TERMINATION: case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: case vehicle_status_s::NAVIGATION_STATE_STAB: + case vehicle_status_s::NAVIGATION_STATE_AIRSPEED: default: navigation_mode_new = nullptr; _can_loiter_at_sp = false;