dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Add new Airspeed flight mode (only fixed-wing) 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2020-05-22 11:33:04 +02:00
parent 4a0fa08953
commit 9624fbad1b
12 changed files with 91 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/commander_state.msg
View File

@ -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

1
msg/vehicle_control_mode.msg
View File

@ -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)

2
msg/vehicle_status.msg
View File

@ -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

1
msg/vehicle_status_flags.msg
View File

@ -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

19
src/modules/commander/Commander.cpp
View File

@ -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");

1
src/modules/commander/px4_custom_mode.h
View File

@ -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 */
};

14
src/modules/commander/state_machine_helper.cpp
View File

@ -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;

4
src/modules/flight_mode_manager/FlightModeManager.cpp
View File

@ -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;

49
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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 */

1
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -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.

4
src/modules/mavlink/mavlink_messages.cpp
View File

@ -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;

1
src/modules/navigator/navigator_main.cpp
View File

@ -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;