aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/modules/commander/commander.cpp47
-rw-r--r--src/modules/mavlink/mavlink.c20
-rw-r--r--src/modules/mavlink/orb_listener.c27
-rw-r--r--src/modules/mavlink/orb_topics.h4
-rw-r--r--src/modules/navigator/navigator_main.cpp96
-rw-r--r--src/modules/sensors/sensors.cpp24
-rw-r--r--src/modules/uORB/topics/vehicle_status.h2
7 files changed, 161 insertions, 59 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 199bfb0da..03d3c02d1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -193,7 +193,7 @@ void usage(const char *reason);
/**
* React to commands that are sent e.g. from the mavlink module.
*/
-void handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
+bool handle_command(struct vehicle_status_s *status, struct vehicle_command_s *cmd, struct actuator_armed_s *armed);
/**
* Mainloop of commander.
@@ -345,10 +345,11 @@ void print_status()
static orb_advert_t status_pub;
-void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
+bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
{
/* result of the command */
enum VEHICLE_CMD_RESULT result = VEHICLE_CMD_RESULT_UNSUPPORTED;
+ bool ret = false;
/* only handle high-priority commands here */
@@ -375,6 +376,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
}
}
+ if (hil_ret == OK)
+ ret = true;
// TODO remove debug code
//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
@@ -407,6 +410,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
arming_res = TRANSITION_NOT_CHANGED;
}
}
+ if (arming_res == TRANSITION_CHANGED)
+ ret = true;
/* set main state */
transition_result_t main_res = TRANSITION_DENIED;
@@ -447,6 +452,8 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
}
}
+ if (main_res == TRANSITION_CHANGED)
+ ret = true;
if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -473,6 +480,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
result = VEHICLE_CMD_RESULT_ACCEPTED;
+ ret = true;
} else {
mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
@@ -482,12 +490,36 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
break;
+ case VEHICLE_CMD_OVERRIDE_GOTO: {
+ // TODO listen vehicle_command topic directly from navigator (?)
+ unsigned int mav_goto = cmd->param1;
+ if (mav_goto == 0) { // MAV_GOTO_DO_HOLD
+ status->set_nav_state = NAV_STATE_LOITER;
+ status->set_nav_state_timestamp = hrt_absolute_time();
+ mavlink_log_critical(mavlink_fd, "#audio: pause mission cmd");
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ ret = true;
+
+ } else if (mav_goto == 1) { // MAV_GOTO_DO_CONTINUE
+ status->set_nav_state = NAV_STATE_MISSION;
+ status->set_nav_state_timestamp = hrt_absolute_time();
+ mavlink_log_critical(mavlink_fd, "#audio: continue mission cmd");
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ ret = true;
+
+ } else {
+ mavlink_log_info(mavlink_fd, "Unsupported OVERRIDE_GOTO: %f %f %f %f %f %f %f %f", cmd->param1, cmd->param2, cmd->param3, cmd->param4, cmd->param5, cmd->param6, cmd->param7);
+ }
+ }
+ break;
+
/* Flight termination */
case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
if (armed->armed && cmd->param3 > 0.5) { //xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
transition_result_t flighttermination_res = flighttermination_state_transition(status, FLIGHTTERMINATION_STATE_ON);
result = VEHICLE_CMD_RESULT_ACCEPTED;
+ ret = true;
} else {
/* reject parachute depoyment not armed */
@@ -1151,7 +1183,8 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
/* handle it */
- handle_command(&status, &safety, &cmd, &armed);
+ if (handle_command(&status, &safety, &cmd, &armed))
+ status_changed = true;
}
/* check which state machines for changes, clear "changed" flag */
@@ -1359,7 +1392,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
current_status->mode_switch = MODE_SWITCH_ASSISTED;
}
- /* land switch */
+ /* return switch */
if (!isfinite(sp_man->return_switch)) {
current_status->return_switch = RETURN_SWITCH_NONE;
@@ -1367,7 +1400,7 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
current_status->return_switch = RETURN_SWITCH_RETURN;
} else {
- current_status->return_switch = RETURN_SWITCH_NONE;
+ current_status->return_switch = RETURN_SWITCH_NORMAL;
}
/* assisted switch */
@@ -1383,10 +1416,10 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
/* mission switch */
if (!isfinite(sp_man->mission_switch)) {
- current_status->mission_switch = MISSION_SWITCH_MISSION;
+ current_status->mission_switch = MISSION_SWITCH_NONE;
} else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) {
- current_status->mission_switch = MISSION_SWITCH_NONE;
+ current_status->mission_switch = MISSION_SWITCH_LOITER;
} else {
current_status->mission_switch = MISSION_SWITCH_MISSION;
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 4c38cf35a..bc191f30d 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -220,22 +220,18 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
} else if (v_status.main_state == MAIN_STATE_AUTO) {
*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
- // TODO use control_mode topic
- /*
- if (v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) {
+ // TODO review
+ if (control_mode.nav_state == NAV_STATE_NONE) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
- } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
- custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
- } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER) {
+ } else if (control_mode.nav_state == NAV_STATE_LOITER) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
- } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {
+ } else if (control_mode.nav_state == NAV_STATE_MISSION_LOITER) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+ } else if (control_mode.nav_state == NAV_STATE_RTL_LOITER) {
+ custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+ } else if (control_mode.nav_state == NAV_STATE_MISSION) {
custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
- } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL) {
- custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
- } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) {
- custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND;
}
- */
}
*mavlink_custom_mode = custom_mode.data;
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 28478a803..e168dded3 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -69,6 +69,7 @@ struct vehicle_global_position_s global_pos;
struct vehicle_local_position_s local_pos;
struct navigation_capabilities_s nav_cap;
struct vehicle_status_s v_status;
+struct vehicle_control_mode_s control_mode;
struct rc_channels_s rc;
struct rc_input_values rc_raw;
struct actuator_armed_s armed;
@@ -125,6 +126,7 @@ static void l_vehicle_rates_setpoint(const struct listener *l);
static void l_home(const struct listener *l);
static void l_airspeed(const struct listener *l);
static void l_nav_cap(const struct listener *l);
+static void l_control_mode(const struct listener *l);
static const struct listener listeners[] = {
{l_sensor_combined, &mavlink_subs.sensor_sub, 0},
@@ -151,6 +153,7 @@ static const struct listener listeners[] = {
{l_home, &mavlink_subs.home_sub, 0},
{l_airspeed, &mavlink_subs.airspeed_sub, 0},
{l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0},
+ {l_control_mode, &mavlink_subs.control_mode_sub, 0},
};
static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -678,6 +681,26 @@ l_nav_cap(const struct listener *l)
}
+void
+l_control_mode(const struct listener *l)
+{
+ orb_copy(ORB_ID(vehicle_control_mode), mavlink_subs.control_mode_sub, &control_mode);
+
+ /* translate the current syste state to mavlink state and mode */
+ uint8_t mavlink_state = 0;
+ uint8_t mavlink_base_mode = 0;
+ uint32_t mavlink_custom_mode = 0;
+ get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);
+
+ /* send heartbeat */
+ mavlink_msg_heartbeat_send(chan,
+ mavlink_system.type,
+ MAV_AUTOPILOT_PX4,
+ mavlink_base_mode,
+ mavlink_custom_mode,
+ mavlink_state);
+}
+
static void *
uorb_receive_thread(void *arg)
{
@@ -753,6 +776,10 @@ uorb_receive_start(void)
status_sub = orb_subscribe(ORB_ID(vehicle_status));
orb_set_interval(status_sub, 300); /* max 3.33 Hz updates */
+ /* --- CONTROL MODE --- */
+ mavlink_subs.control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
+ orb_set_interval(mavlink_subs.control_mode_sub, 300); /* max 3.33 Hz updates */
+
/* --- RC CHANNELS VALUE --- */
rc_sub = orb_subscribe(ORB_ID(rc_channels));
orb_set_interval(rc_sub, 100); /* 10Hz updates */
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 9000728cb..4d428406a 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -95,6 +95,7 @@ struct mavlink_subscriptions {
int home_sub;
int airspeed_sub;
int navigation_capabilities_sub;
+ int control_mode_sub;
};
extern struct mavlink_subscriptions mavlink_subs;
@@ -111,6 +112,9 @@ extern struct navigation_capabilities_s nav_cap;
/** Vehicle status */
extern struct vehicle_status_s v_status;
+/** Vehicle control mode */
+extern struct vehicle_control_mode_s control_mode;
+
/** RC channels */
extern struct rc_channels_s rc;
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 0a0ee2541..4176cee1b 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -526,6 +526,8 @@ Navigator::task_main()
/* rate limit position updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
+ unsigned prevState = 0;
+
/* wakeup source(s) */
struct pollfd fds[7];
@@ -545,7 +547,6 @@ Navigator::task_main()
fds[6].fd = _vstatus_sub;
fds[6].events = POLLIN;
-
while (!_task_should_exit) {
/* wait for up to 100ms for data */
@@ -571,41 +572,75 @@ Navigator::task_main()
/* Evaluate state machine from commander and set the navigator mode accordingly */
if (_vstatus.main_state == MAIN_STATE_AUTO) {
- 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;
+ bool stick_mode = false;
+ 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) {
+ dispatch(EVENT_RTL_REQUESTED);
+ stick_mode = true;
+ } else {
+ /* MISSION switch */
+ if (!stick_mode) {
+ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
+ dispatch(EVENT_LOITER_REQUESTED);
+ stick_mode = true;
+ } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
+ /* switch to mission only if available */
+ if (_mission.current_mission_available()) {
+ dispatch(EVENT_MISSION_REQUESTED);
+ } else {
+ dispatch(EVENT_LOITER_REQUESTED);
+ }
+ stick_mode = true;
+ }
+ }
+ 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 */
+ dispatch(EVENT_LOITER_REQUESTED);
+ stick_mode = true;
+ }
+ }
+ }
- switch (_vstatus.set_nav_state) {
- case NAV_STATE_INIT:
- case NAV_STATE_NONE:
- /* nothing to do */
- 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;
- case NAV_STATE_LOITER:
- dispatch(EVENT_LOITER_REQUESTED);
- break;
+ switch (_vstatus.set_nav_state) {
+ case NAV_STATE_INIT:
+ case NAV_STATE_NONE:
+ /* nothing to do */
+ break;
- case NAV_STATE_MISSION:
- dispatch(EVENT_MISSION_REQUESTED);
- break;
+ case NAV_STATE_LOITER:
+ dispatch(EVENT_LOITER_REQUESTED);
+ break;
- case NAV_STATE_RTL:
- dispatch(EVENT_RTL_REQUESTED);
- break;
+ case NAV_STATE_MISSION:
+ dispatch(EVENT_MISSION_REQUESTED);
+ break;
- default:
- warnx("ERROR: Requested navigation state not supported");
- break;
- }
+ case NAV_STATE_RTL:
+ dispatch(EVENT_RTL_REQUESTED);
+ break;
+
+ default:
+ warnx("ERROR: Requested navigation state not supported");
+ break;
+ }
- } else {
- /* try mission, if none is available fallback to loiter instead */
- if (_mission.current_mission_available()) {
- dispatch(EVENT_MISSION_REQUESTED);
} else {
- dispatch(EVENT_LOITER_REQUESTED);
+ /* on first switch to AUTO try mission, if none is available fallback to loiter instead */
+ if (myState == NAV_STATE_INIT || myState == NAV_STATE_NONE) {
+ if (_mission.current_mission_available()) {
+ dispatch(EVENT_MISSION_REQUESTED);
+ } else {
+ dispatch(EVENT_LOITER_REQUESTED);
+ }
+ }
}
- break;
}
} else {
@@ -673,6 +708,11 @@ Navigator::task_main()
}
}
+ if (myState != prevState) {
+ mavlink_log_info(_mavlink_fd, "[navigator] nav state %d -> %d", prevState, myState);
+ prevState = myState;
+ }
+
publish_control_mode();
perf_end(_loop_perf);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 66491500e..7a2f4c6da 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -1370,18 +1370,6 @@ Sensors::rc_poll()
manual_control.yaw *= _parameters.rc_scale_yaw;
}
- /* mode switch input */
- manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
-
- /* land switch input */
- manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
-
- /* assisted switch input */
- manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
-
- /* mission switch input */
- manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
-
/* flaps */
if (_rc.function[FLAPS] >= 0) {
@@ -1392,14 +1380,26 @@ Sensors::rc_poll()
}
}
+ /* mode switch input */
if (_rc.function[MODE] >= 0) {
manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled);
}
+ /* assisted switch input */
+ if (_rc.function[ASSISTED] >= 0) {
+ manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled);
+ }
+
+ /* mission switch input */
if (_rc.function[MISSION] >= 0) {
manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
}
+ /* return switch input */
+ if (_rc.function[RETURN] >= 0) {
+ manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
+ }
+
// if (_rc.function[OFFBOARD_MODE] >= 0) {
// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled);
// }
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index ae3a24a1b..1a9dec5f5 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -99,11 +99,13 @@ typedef enum {
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;
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-30 23:26:45 +0000