aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/modules/commander/commander.cpp110
-rw-r--r--src/modules/commander/px4_custom_mode.h1
-rw-r--r--src/modules/commander/state_machine_helper.cpp12
-rw-r--r--src/modules/sensors/sensor_params.c9
-rw-r--r--src/modules/sensors/sensors.cpp16
-rw-r--r--src/modules/uORB/topics/manual_control_setpoint.h1
-rw-r--r--src/modules/uORB/topics/rc_channels.h2
-rw-r--r--src/modules/uORB/topics/vehicle_control_mode.h1
-rw-r--r--src/modules/uORB/topics/vehicle_status.h1
9 files changed, 139 insertions, 14 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c26200048..90efd5115 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -120,6 +120,7 @@ extern struct system_load_s system_load;
#define POSITION_TIMEOUT (600 * 1000) /**< consider the local or global position estimate invalid after 600ms */
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
+#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
@@ -160,6 +161,7 @@ static struct vehicle_status_s status;
static struct actuator_armed_s armed;
static struct safety_s safety;
static struct vehicle_control_mode_s control_mode;
+static struct offboard_control_setpoint_s sp_offboard;
/* tasks waiting for low prio thread */
typedef enum {
@@ -457,6 +459,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_res = main_state_transition(status, MAIN_STATE_ACRO);
+
+ } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
+ /* OFFBOARD */
+ main_res = main_state_transition(status, MAIN_STATE_OFFBOARD);
}
} else {
@@ -657,6 +663,7 @@ int commander_thread_main(int argc, char *argv[])
main_states_str[2] = "POSCTL";
main_states_str[3] = "AUTO";
main_states_str[4] = "ACRO";
+ main_states_str[5] = "OFFBOARD";
char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[0] = "INIT";
@@ -791,7 +798,6 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to offboard control data */
int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
- struct offboard_control_setpoint_s sp_offboard;
memset(&sp_offboard, 0, sizeof(sp_offboard));
/* Subscribe to global position */
@@ -1338,21 +1344,29 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
- /* failsafe for manual modes */
- transition_result_t manual_res = TRANSITION_DENIED;
-
- if (!status.condition_landed) {
- /* vehicle is not landed, try to perform RTL */
- manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+ transition_result_t offboard_res = TRANSITION_DENIED;
+ if (status.main_state == MAIN_STATE_OFFBOARD) {
+ /* check if OFFBOARD mode still allowed */
+ offboard_res = main_state_transition(&status, MAIN_STATE_OFFBOARD);
}
- if (manual_res == TRANSITION_DENIED) {
- /* RTL not allowed (no global position estimate) or not wanted, try LAND */
- manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+ if (offboard_res == TRANSITION_DENIED) {
+ /* not in OFFBOARD mode or OFFBOARD is not allowed anymore, switch to failsafe */
+ transition_result_t failsafe_res = TRANSITION_DENIED;
- if (manual_res == TRANSITION_DENIED) {
- /* LAND not allowed, set TERMINATION state */
- (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+ if (!status.condition_landed) {
+ /* vehicle is not landed, try to perform RTL */
+ failsafe_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
+ }
+
+ if (failsafe_res == TRANSITION_DENIED) {
+ /* RTL not allowed (no global position estimate) or not wanted, try LAND */
+ failsafe_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+
+ if (failsafe_res == TRANSITION_DENIED) {
+ /* LAND not allowed, set TERMINATION state */
+ (void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
+ }
}
}
}
@@ -1365,6 +1379,30 @@ int commander_thread_main(int argc, char *argv[])
}
}
+ /* check offboard signal */
+ if (sp_offboard.timestamp != 0 && hrt_absolute_time() < sp_offboard.timestamp + OFFBOARD_TIMEOUT) {
+ if (!status.offboard_control_signal_found_once) {
+ status.offboard_control_signal_found_once = true;
+ mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time");
+ status_changed = true;
+
+ } else {
+ if (status.offboard_control_signal_lost) {
+ mavlink_log_info(mavlink_fd, "[cmd] offboard signal regained");
+ status_changed = true;
+ }
+ }
+
+ status.offboard_control_signal_lost = false;
+
+ } else {
+ if (!status.offboard_control_signal_lost) {
+ mavlink_log_critical(mavlink_fd, "[cmd[ CRITICAL: OFFBOARD SIGNAL LOST");
+ status.offboard_control_signal_lost = true;
+ status_changed = true;
+ }
+ }
+
// TODO remove this hack
/* flight termination in manual mode if assist switch is on POSCTL position */
if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
@@ -1618,6 +1656,18 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
+ /* offboard switch overrides main switch */
+ if (sp_man->offboard_switch == SWITCH_POS_ON) {
+ res = main_state_transition(status, MAIN_STATE_OFFBOARD);
+ if (res == TRANSITION_DENIED) {
+ print_reject_mode(status, "OFFBOARD");
+
+ } else {
+ return res;
+ }
+ }
+
+ /* offboard switched off or denied, check main mode switch */
switch (sp_man->mode_switch) {
case SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
@@ -1695,6 +1745,7 @@ set_control_mode()
control_mode.flag_armed = armed.armed;
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
+ control_mode.flag_control_offboard_enabled = false;
control_mode.flag_control_termination_enabled = false;
@@ -1752,6 +1803,39 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
break;
+ case MAIN_STATE_OFFBOARD:
+ control_mode.flag_control_manual_enabled = false;
+ control_mode.flag_control_auto_enabled = false;
+ control_mode.flag_control_offboard_enabled = true;
+
+ switch (sp_offboard.mode) {
+ case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
+ control_mode.flag_control_rates_enabled = true;
+ control_mode.flag_control_attitude_enabled = true;
+ 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;
+ case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
+ control_mode.flag_control_rates_enabled = true;
+ 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:
+ 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;
}
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index e0f8dc95d..04532d09a 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -16,6 +16,7 @@ enum PX4_CUSTOM_MAIN_MODE {
PX4_CUSTOM_MAIN_MODE_POSCTL,
PX4_CUSTOM_MAIN_MODE_AUTO,
PX4_CUSTOM_MAIN_MODE_ACRO,
+ PX4_CUSTOM_MAIN_MODE_OFFBOARD,
};
enum PX4_CUSTOM_SUB_MODE_AUTO {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index f1a353d5b..55f73e043 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -255,6 +255,18 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
}
break;
+
+ case MAIN_STATE_OFFBOARD:
+
+ /* need offboard signal */
+ if (!status->offboard_control_signal_lost) {
+ ret = TRANSITION_CHANGED;
+ }
+
+ break;
+
+ default:
+ break;
}
if (ret == TRANSITION_CHANGED) {
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 999cf8bb3..65e2376ce 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -635,6 +635,15 @@ PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
/**
+ * Offboard switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+
+/**
* Flaps channel mapping.
*
* @min 0
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b268b1b36..1a1d153f7 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -264,6 +264,7 @@ private:
int rc_map_posctl_sw;
int rc_map_loiter_sw;
int rc_map_acro_sw;
+ int rc_map_offboard_sw;
int rc_map_flaps;
@@ -280,12 +281,14 @@ private:
float rc_return_th;
float rc_loiter_th;
float rc_acro_th;
+ float rc_offboard_th;
bool rc_assist_inv;
bool rc_auto_inv;
bool rc_posctl_inv;
bool rc_return_inv;
bool rc_loiter_inv;
bool rc_acro_inv;
+ bool rc_offboard_inv;
float battery_voltage_scaling;
float battery_current_scaling;
@@ -319,6 +322,7 @@ private:
param_t rc_map_posctl_sw;
param_t rc_map_loiter_sw;
param_t rc_map_acro_sw;
+ param_t rc_map_offboard_sw;
param_t rc_map_flaps;
@@ -335,6 +339,7 @@ private:
param_t rc_return_th;
param_t rc_loiter_th;
param_t rc_acro_th;
+ param_t rc_offboard_th;
param_t battery_voltage_scaling;
param_t battery_current_scaling;
@@ -536,6 +541,7 @@ Sensors::Sensors() :
_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
+ _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW");
_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -551,6 +557,7 @@ Sensors::Sensors() :
_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
+ _parameter_handles.rc_offboard_th = param_find("RC_OFFB_TH");
/* gyro offsets */
_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -698,6 +705,10 @@ Sensors::parameters_update()
warnx("%s", paramerr);
}
+ if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) {
+ warnx("%s", paramerr);
+ }
+
if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
warnx("%s", paramerr);
}
@@ -726,6 +737,9 @@ Sensors::parameters_update()
param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th));
_parameters.rc_acro_inv = (_parameters.rc_acro_th < 0);
_parameters.rc_acro_th = fabs(_parameters.rc_acro_th);
+ param_get(_parameter_handles.rc_offboard_th, &(_parameters.rc_offboard_th));
+ _parameters.rc_offboard_inv = (_parameters.rc_offboard_th < 0);
+ _parameters.rc_offboard_th = fabs(_parameters.rc_offboard_th);
/* update RC function mappings */
_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -738,6 +752,7 @@ Sensors::parameters_update()
_rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
+ _rc.function[OFFBOARD] = _parameters.rc_map_offboard_sw - 1;
_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
@@ -1524,6 +1539,7 @@ Sensors::rc_poll()
manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
+ manual.offboard_switch = get_rc_sw2pos_position(OFFBOARD, _parameters.rc_offboard_th, _parameters.rc_offboard_inv);
/* publish manual_control_setpoint topic */
if (_manual_control_pub > 0) {
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 910b8a623..dde237adc 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -98,6 +98,7 @@ struct manual_control_setpoint_s {
switch_pos_t posctl_switch; /**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
switch_pos_t loiter_switch; /**< loiter 2 position switch (optional): _MISSION_, LOITER */
switch_pos_t acro_switch; /**< acro 2 position switch (optional): _MANUAL_, ACRO */
+ switch_pos_t offboard_switch; /**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
}; /**< manual control inputs */
/**
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 370c54442..f8048a159 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -66,7 +66,7 @@ enum RC_CHANNELS_FUNCTION {
RETURN = 5,
POSCTL = 6,
LOITER = 7,
- OFFBOARD_MODE = 8,
+ OFFBOARD = 8,
ACRO = 9,
FLAPS = 10,
AUX_1 = 11,
diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h
index ea554006f..49e2ba4b5 100644
--- a/src/modules/uORB/topics/vehicle_control_mode.h
+++ b/src/modules/uORB/topics/vehicle_control_mode.h
@@ -74,6 +74,7 @@ struct vehicle_control_mode_s {
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_auto_enabled; /**< true if onboard autopilot should act */
+ bool flag_control_offboard_enabled; /**< true if offboard control should be used */
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index ea20a317a..ef621fcb2 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -67,6 +67,7 @@ typedef enum {
MAIN_STATE_POSCTL,
MAIN_STATE_AUTO,
MAIN_STATE_ACRO,
+ MAIN_STATE_OFFBOARD,
MAIN_STATE_MAX
} main_state_t;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-09 11:42:16 +0000