forked from Archive/PX4-Autopilot
commander, navigator: use updated manual_control_setpoint
This commit is contained in:
Anton Babushkin
parent
1d5f62d890
commit
6f38ed3b4b
|
|
@ -67,6 +67,7 @@
|
|||
#include <uORB/topics/home_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/position_setpoint_triplet.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/subsystem_info.h>
|
||||
|
|
@ -112,8 +113,7 @@ extern struct system_load_s system_load;
|
|||
|
||||
#define MAVLINK_OPEN_INTERVAL 50000
|
||||
|
||||
#define STICK_ON_OFF_LIMIT 0.75f
|
||||
#define STICK_THRUST_RANGE 1.0f
|
||||
#define STICK_ON_OFF_LIMIT 0.9f
|
||||
#define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
|
||||
#define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
|
||||
|
||||
|
|
@ -207,7 +207,7 @@ void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool
|
|||
|
||||
void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
|
||||
|
||||
transition_result_t set_main_state_rc(struct vehicle_status_s *status);
|
||||
transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
|
||||
|
||||
void set_control_mode();
|
||||
|
||||
|
|
@ -814,6 +814,11 @@ int commander_thread_main(int argc, char *argv[])
|
|||
struct subsystem_info_s info;
|
||||
memset(&info, 0, sizeof(info));
|
||||
|
||||
/* Subscribe to position setpoint triplet */
|
||||
int pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
|
||||
struct position_setpoint_triplet_s pos_sp_triplet;
|
||||
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
|
||||
|
||||
control_status_leds(&status, &armed, true);
|
||||
|
||||
/* now initialized */
|
||||
|
|
@ -1005,6 +1010,13 @@ int commander_thread_main(int argc, char *argv[])
|
|||
status_changed = true;
|
||||
}
|
||||
|
||||
/* update subsystem */
|
||||
orb_check(pos_sp_triplet_sub, &updated);
|
||||
|
||||
if (updated) {
|
||||
orb_copy(ORB_ID(position_setpoint_triplet), pos_sp_triplet_sub, &pos_sp_triplet);
|
||||
}
|
||||
|
||||
if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
|
||||
/* compute system load */
|
||||
uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;
|
||||
|
|
@ -1135,7 +1147,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
if (status.is_rotary_wing &&
|
||||
(status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
|
||||
(status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
|
||||
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
|
||||
|
||||
if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
|
||||
|
|
@ -1153,7 +1165,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
|
||||
if (status.arming_state == ARMING_STATE_STANDBY &&
|
||||
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
|
||||
sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
|
||||
if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
|
||||
if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
|
||||
print_reject_arm("NOT ARMING: Press safety switch first.");
|
||||
|
|
@ -1193,11 +1205,8 @@ int commander_thread_main(int argc, char *argv[])
|
|||
transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
|
||||
}
|
||||
|
||||
/* fill status according to mode switches */
|
||||
check_mode_switches(&sp_man, &status);
|
||||
|
||||
/* evaluate the main state machine according to mode switches */
|
||||
res = set_main_state_rc(&status);
|
||||
res = set_main_state_rc(&status, &sp_man);
|
||||
|
||||
/* play tune on mode change only if armed, blink LED always */
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
|
|
@ -1208,6 +1217,33 @@ int commander_thread_main(int argc, char *argv[])
|
|||
mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
|
||||
}
|
||||
|
||||
/* set navigation state */
|
||||
/* RETURN switch, overrides MISSION switch */
|
||||
if (sp_man.return_switch == SWITCH_POS_ON) {
|
||||
/* switch to RTL if not already landed after RTL and home position set */
|
||||
status.set_nav_state = NAV_STATE_RTL;
|
||||
status.set_nav_state_timestamp = hrt_absolute_time();
|
||||
|
||||
} else {
|
||||
/* MISSION switch */
|
||||
if (sp_man.mission_switch == SWITCH_POS_ON) {
|
||||
/* stick is in LOITER position */
|
||||
status.set_nav_state = NAV_STATE_LOITER;
|
||||
status.set_nav_state_timestamp = hrt_absolute_time();
|
||||
|
||||
} else if (sp_man.mission_switch != SWITCH_POS_NONE) {
|
||||
/* stick is in MISSION position */
|
||||
status.set_nav_state = NAV_STATE_MISSION;
|
||||
status.set_nav_state_timestamp = hrt_absolute_time();
|
||||
|
||||
} else if (sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE &&
|
||||
pos_sp_triplet.nav_state == NAV_STATE_RTL) {
|
||||
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
|
||||
status.set_nav_state = NAV_STATE_MISSION;
|
||||
status.set_nav_state_timestamp = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!status.rc_signal_lost) {
|
||||
mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
|
||||
|
|
@ -1255,7 +1291,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
// TODO remove this hack
|
||||
/* flight termination in manual mode if assisted switch is on easy position */
|
||||
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
|
||||
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
|
||||
if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
|
||||
tune_positive(armed.armed);
|
||||
}
|
||||
|
|
@ -1470,76 +1506,26 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
|
|||
leds_counter++;
|
||||
}
|
||||
|
||||
void
|
||||
check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status)
|
||||
{
|
||||
/* main mode switch */
|
||||
if (!isfinite(sp_man->mode_switch)) {
|
||||
/* default to manual if signal is invalid */
|
||||
status->mode_switch = MODE_SWITCH_MANUAL;
|
||||
|
||||
} else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) {
|
||||
status->mode_switch = MODE_SWITCH_AUTO;
|
||||
|
||||
} else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) {
|
||||
status->mode_switch = MODE_SWITCH_MANUAL;
|
||||
|
||||
} else {
|
||||
status->mode_switch = MODE_SWITCH_ASSISTED;
|
||||
}
|
||||
|
||||
/* return switch */
|
||||
if (!isfinite(sp_man->return_switch)) {
|
||||
status->return_switch = RETURN_SWITCH_NONE;
|
||||
|
||||
} else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) {
|
||||
status->return_switch = RETURN_SWITCH_RETURN;
|
||||
|
||||
} else {
|
||||
status->return_switch = RETURN_SWITCH_NORMAL;
|
||||
}
|
||||
|
||||
/* assisted switch */
|
||||
if (!isfinite(sp_man->assisted_switch)) {
|
||||
status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
|
||||
|
||||
} else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) {
|
||||
status->assisted_switch = ASSISTED_SWITCH_EASY;
|
||||
|
||||
} else {
|
||||
status->assisted_switch = ASSISTED_SWITCH_SEATBELT;
|
||||
}
|
||||
|
||||
/* mission switch */
|
||||
if (!isfinite(sp_man->mission_switch)) {
|
||||
status->mission_switch = MISSION_SWITCH_NONE;
|
||||
|
||||
} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
|
||||
status->mission_switch = MISSION_SWITCH_LOITER;
|
||||
|
||||
} else {
|
||||
status->mission_switch = MISSION_SWITCH_MISSION;
|
||||
}
|
||||
}
|
||||
|
||||
transition_result_t
|
||||
set_main_state_rc(struct vehicle_status_s *status)
|
||||
set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man)
|
||||
{
|
||||
/* set main state according to RC switches */
|
||||
transition_result_t res = TRANSITION_DENIED;
|
||||
|
||||
switch (status->mode_switch) {
|
||||
case MODE_SWITCH_MANUAL:
|
||||
switch (sp_man->mode_switch) {
|
||||
case SWITCH_POS_NONE:
|
||||
case SWITCH_POS_OFF: // MANUAL
|
||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
case MODE_SWITCH_ASSISTED:
|
||||
if (status->assisted_switch == ASSISTED_SWITCH_EASY) {
|
||||
case SWITCH_POS_MIDDLE: // ASSISTED
|
||||
if (sp_man->assisted_switch == SWITCH_POS_ON) {
|
||||
res = main_state_transition(status, MAIN_STATE_EASY);
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
// else fallback to SEATBELT
|
||||
print_reject_mode(status, "EASY");
|
||||
|
|
@ -1547,29 +1533,33 @@ set_main_state_rc(struct vehicle_status_s *status)
|
|||
|
||||
res = main_state_transition(status, MAIN_STATE_SEATBELT);
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this mode
|
||||
}
|
||||
|
||||
if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages
|
||||
if (sp_man->assisted_switch != SWITCH_POS_ON) {
|
||||
print_reject_mode(status, "SEATBELT");
|
||||
}
|
||||
|
||||
// else fallback to MANUAL
|
||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
// TRANSITION_DENIED is not possible here
|
||||
break;
|
||||
|
||||
case MODE_SWITCH_AUTO:
|
||||
case SWITCH_POS_ON: // AUTO
|
||||
res = main_state_transition(status, MAIN_STATE_AUTO);
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
// else fallback to SEATBELT (EASY likely will not work too)
|
||||
print_reject_mode(status, "AUTO");
|
||||
res = main_state_transition(status, MAIN_STATE_SEATBELT);
|
||||
|
||||
if (res != TRANSITION_DENIED)
|
||||
if (res != TRANSITION_DENIED) {
|
||||
break; // changed successfully or already in this state
|
||||
}
|
||||
|
||||
// else fallback to MANUAL
|
||||
res = main_state_transition(status, MAIN_STATE_MANUAL);
|
||||
|
|
|
|||
|
|
@ -690,84 +690,46 @@ Navigator::task_main()
|
|||
if (fds[6].revents & POLLIN) {
|
||||
vehicle_status_update();
|
||||
|
||||
/* evaluate state machine from commander and set the navigator mode accordingly */
|
||||
/* evaluate state requested by commander */
|
||||
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
|
||||
bool stick_mode = false;
|
||||
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
|
||||
/* commander requested new navigation mode, try to set it */
|
||||
switch (_vstatus.set_nav_state) {
|
||||
case NAV_STATE_NONE:
|
||||
/* nothing to do */
|
||||
break;
|
||||
|
||||
if (!_vstatus.rc_signal_lost) {
|
||||
/* RC signal available, use control switches to set mode */
|
||||
/* RETURN switch, overrides MISSION switch */
|
||||
if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
|
||||
/* switch to RTL if not already landed after RTL and home position set */
|
||||
case NAV_STATE_LOITER:
|
||||
request_loiter_or_ready();
|
||||
break;
|
||||
|
||||
case NAV_STATE_MISSION:
|
||||
request_mission_if_available();
|
||||
break;
|
||||
|
||||
case NAV_STATE_RTL:
|
||||
if (!(_rtl_state == RTL_STATE_DESCEND &&
|
||||
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
|
||||
_vstatus.condition_home_position_valid) {
|
||||
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
|
||||
_vstatus.condition_home_position_valid) {
|
||||
dispatch(EVENT_RTL_REQUESTED);
|
||||
}
|
||||
|
||||
stick_mode = true;
|
||||
break;
|
||||
|
||||
} else {
|
||||
/* MISSION switch */
|
||||
if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
|
||||
request_loiter_or_ready();
|
||||
stick_mode = true;
|
||||
case NAV_STATE_LAND:
|
||||
dispatch(EVENT_LAND_REQUESTED);
|
||||
|
||||
} else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
|
||||
request_mission_if_available();
|
||||
stick_mode = true;
|
||||
}
|
||||
break;
|
||||
|
||||
if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) {
|
||||
/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
|
||||
request_mission_if_available();
|
||||
stick_mode = true;
|
||||
}
|
||||
default:
|
||||
warnx("ERROR: Requested navigation state not supported");
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!stick_mode) {
|
||||
if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
|
||||
/* commander requested new navigation mode, try to set it */
|
||||
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
|
||||
|
||||
switch (_vstatus.set_nav_state) {
|
||||
case NAV_STATE_NONE:
|
||||
/* nothing to do */
|
||||
break;
|
||||
|
||||
case NAV_STATE_LOITER:
|
||||
request_loiter_or_ready();
|
||||
break;
|
||||
|
||||
case NAV_STATE_MISSION:
|
||||
request_mission_if_available();
|
||||
break;
|
||||
|
||||
case NAV_STATE_RTL:
|
||||
if (!(_rtl_state == RTL_STATE_DESCEND &&
|
||||
(myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
|
||||
_vstatus.condition_home_position_valid) {
|
||||
dispatch(EVENT_RTL_REQUESTED);
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case NAV_STATE_LAND:
|
||||
dispatch(EVENT_LAND_REQUESTED);
|
||||
|
||||
break;
|
||||
|
||||
default:
|
||||
warnx("ERROR: Requested navigation state not supported");
|
||||
break;
|
||||
}
|
||||
|
||||
} else {
|
||||
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
|
||||
if (myState == NAV_STATE_NONE) {
|
||||
request_mission_if_available();
|
||||
}
|
||||
} else {
|
||||
/* on first switch to AUTO try mission by default, if none is available fallback to loiter */
|
||||
if (myState == NAV_STATE_NONE) {
|
||||
request_mission_if_available();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -775,6 +737,8 @@ Navigator::task_main()
|
|||
/* navigator shouldn't act */
|
||||
dispatch(EVENT_NONE_REQUESTED);
|
||||
}
|
||||
|
||||
_set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
|
||||
}
|
||||
|
||||
/* parameters updated */
|
||||
|
|
|
|||
|
|
@ -93,29 +93,6 @@ typedef enum {
|
|||
FAILSAFE_STATE_MAX
|
||||
} failsafe_state_t;
|
||||
|
||||
typedef enum {
|
||||
MODE_SWITCH_MANUAL = 0,
|
||||
MODE_SWITCH_ASSISTED,
|
||||
MODE_SWITCH_AUTO
|
||||
} mode_switch_pos_t;
|
||||
|
||||
typedef enum {
|
||||
ASSISTED_SWITCH_SEATBELT = 0,
|
||||
ASSISTED_SWITCH_EASY
|
||||
} assisted_switch_pos_t;
|
||||
|
||||
typedef enum {
|
||||
RETURN_SWITCH_NONE = 0,
|
||||
RETURN_SWITCH_NORMAL,
|
||||
RETURN_SWITCH_RETURN
|
||||
} return_switch_pos_t;
|
||||
|
||||
typedef enum {
|
||||
MISSION_SWITCH_NONE = 0,
|
||||
MISSION_SWITCH_LOITER,
|
||||
MISSION_SWITCH_MISSION
|
||||
} mission_switch_pos_t;
|
||||
|
||||
enum VEHICLE_MODE_FLAG {
|
||||
VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
|
||||
VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
|
||||
|
|
@ -187,11 +164,6 @@ struct vehicle_status_s {
|
|||
|
||||
bool is_rotary_wing;
|
||||
|
||||
mode_switch_pos_t mode_switch;
|
||||
return_switch_pos_t return_switch;
|
||||
assisted_switch_pos_t assisted_switch;
|
||||
mission_switch_pos_t mission_switch;
|
||||
|
||||
bool condition_battery_voltage_valid;
|
||||
bool condition_system_in_air_restore; /**< true if we can restore in mid air */
|
||||
bool condition_system_sensors_initialized;
|
||||
|
|
|
|||
Loading…
Reference in New Issue