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

OFFBOARD mode support in commander, mavlink, mc_att_control, WIP

Browse Source
This commit is contained in:
Anton Babushkin
2014-01-22 19:25:14 +01:00
parent 37e28e7fa8
commit f069fe9f61
5 changed files with 110 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+38 -28
View File
@@ -411,42 +411,52 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
if (status->rc_signal_lost) {
/* allow mode switching by command only if no RC signal */
if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
/* use autopilot-specific mode */
if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
/* SEATBELT */
main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
/* SEATBELT */
main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
}
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
} 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_OFFBOARD) {
/* OFFBOARD */
main_res = main_state_transition(status, MAIN_STATE_OFFBOARD);
}
} else {
/* use base mode */
if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
/* AUTO */
main_res = main_state_transition(status, MAIN_STATE_AUTO);
} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
/* EASY */
main_res = main_state_transition(status, MAIN_STATE_EASY);
} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
/* MANUAL */
main_res = main_state_transition(status, MAIN_STATE_MANUAL);
}
}
}
} else {
mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd");
}
if (main_res == TRANSITION_CHANGED)
ret = true;
src/modules/commander/px4_custom_mode.h
+1
View File
@@ -13,6 +13,7 @@ enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_SEATBELT,
PX4_CUSTOM_MAIN_MODE_EASY,
PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_OFFBOARD,
};
enum PX4_CUSTOM_SUB_MODE_AUTO {
src/modules/mavlink/mavlink.c
+2
View File
@@ -231,6 +231,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
} else if (control_mode.nav_state == NAV_STATE_RTL) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
}
} else if (v_status.main_state == MAIN_STATE_OFFBOARD) {
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
}
*mavlink_custom_mode = custom_mode.data;
src/modules/mc_att_control/mc_att_control_main.cpp
+54 -45
View File
@@ -480,64 +480,73 @@ MulticopterAttitudeControl::task_main()
float yaw_sp_move_rate = 0.0f;
bool publish_att_sp = false;
/* define which input is the dominating control input */
if (_control_mode.flag_control_manual_enabled) {
/* manual input */
if (!_control_mode.flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude control mode */
_att_sp.thrust = _manual.throttle;
}
if (_control_mode.flag_control_offboard_enabled) {
/* offboard control */
// TODO set _att_sp or _rates_sp here depending on offboard control mode
// TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode
// but it's not very beautiful, need to think how to do it better
if (!_arming.armed) {
/* reset yaw setpoint when disarmed */
reset_yaw_sp = true;
}
} else {
/* onboard control */
/* define which input is the dominating control input */
if (_control_mode.flag_control_manual_enabled) {
/* manual input */
if (!_control_mode.flag_control_climb_rate_enabled) {
/* pass throttle directly if not in altitude control mode */
_att_sp.thrust = _manual.throttle;
}
if (_control_mode.flag_control_attitude_enabled) {
/* control attitude, update attitude setpoint depending on mode */
if (!_arming.armed) {
/* reset yaw setpoint when disarmed */
reset_yaw_sp = true;
}
if (_att_sp.thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual.yaw;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
if (_control_mode.flag_control_attitude_enabled) {
/* control attitude, update attitude setpoint depending on mode */
if (_att_sp.thrust < 0.1f) {
// TODO
//if (_status.condition_landed) {
/* reset yaw setpoint if on ground */
// reset_yaw_sp = true;
//}
} else {
if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) {
/* move yaw setpoint */
yaw_sp_move_rate = _manual.yaw;
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
_att_sp.R_valid = false;
publish_att_sp = true;
}
}
/* reset yaw setpint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
_att_sp.R_valid = false;
publish_att_sp = true;
}
}
/* reset yaw setpint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp.yaw_body = _att.yaw;
_att_sp.R_valid = false;
publish_att_sp = true;
}
if (!_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_att_sp.roll_body = _manual.roll;
_att_sp.pitch_body = _manual.pitch;
_att_sp.R_valid = false;
publish_att_sp = true;
}
if (!_control_mode.flag_control_velocity_enabled) {
/* update attitude setpoint if not in position control mode */
_att_sp.roll_body = _manual.roll;
_att_sp.pitch_body = _manual.pitch;
_att_sp.R_valid = false;
publish_att_sp = true;
} else {
/* manual rate inputs (ACRO) */
// TODO
/* reset yaw setpoint after ACRO */
reset_yaw_sp = true;
}
} else {
/* manual rate inputs (ACRO) */
// TODO
/* reset yaw setpoint after ACRO */
/* reset yaw setpoint after non-manual control */
reset_yaw_sp = true;
}
} else {
/* reset yaw setpoint after non-manual control */
reset_yaw_sp = true;
}
if (_att_sp.R_valid) {
src/modules/navigator/navigator_main.cpp
+15 -1
View File
@@ -1463,11 +1463,11 @@ Navigator::publish_control_mode()
_control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing;
_control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON;
_control_mode.flag_control_offboard_enabled = false;
_control_mode.flag_control_flighttermination_enabled = false;
switch (_vstatus.main_state) {
case MAIN_STATE_MANUAL:
_control_mode.flag_control_offboard_enabled = false;
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
@@ -1478,6 +1478,7 @@ Navigator::publish_control_mode()
break;
case MAIN_STATE_SEATBELT:
_control_mode.flag_control_offboard_enabled = false;
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
@@ -1488,6 +1489,7 @@ Navigator::publish_control_mode()
break;
case MAIN_STATE_EASY:
_control_mode.flag_control_offboard_enabled = false;
_control_mode.flag_control_manual_enabled = true;
_control_mode.flag_control_rates_enabled = true;
_control_mode.flag_control_attitude_enabled = true;
@@ -1498,6 +1500,7 @@ Navigator::publish_control_mode()
break;
case MAIN_STATE_AUTO:
_control_mode.flag_control_offboard_enabled = false;
_control_mode.flag_control_manual_enabled = false;
if (myState == NAV_STATE_READY) {
/* disable all controllers, armed but idle */
@@ -1517,6 +1520,17 @@ Navigator::publish_control_mode()
}
break;
case MAIN_STATE_OFFBOARD:
_control_mode.flag_control_offboard_enabled = true;
_control_mode.flag_control_manual_enabled = false;
_control_mode.flag_control_rates_enabled = false;
_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;
}