aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/commander/commander.cpp2
-rw-r--r--src/modules/navigator/mission.cpp27
-rw-r--r--src/modules/px4iofirmware/sbus.c40
-rw-r--r--src/modules/uORB/topics/manual_control_setpoint.h16
4 files changed, 61 insertions, 24 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 002a33811..7917bc9f2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
-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, struct manual_control_setpoint_s *sp_man);
void set_control_mode();
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index b8e3274fd..0765e9b7c 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -371,7 +371,7 @@ Mission::set_mission_items()
} else {
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
- mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
@@ -595,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
}
- if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
- /* mission item index out of bounds */
- return false;
- }
-
- /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+ /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
+ * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
for (int i = 0; i < 10; i++) {
+
+ if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+ /* mission item index out of bounds */
+ return false;
+ }
+
const ssize_t len = sizeof(struct mission_item_s);
/* read mission item to temp storage first to not overwrite current mission item if data damaged */
@@ -626,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (is_current) {
(mission_item_tmp.do_jump_current_count)++;
/* save repeat count */
- if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
+ if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
+ &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
- "ERROR DO JUMP waypoint could not be written");
+ "ERROR DO JUMP waypoint could not be written");
return false;
}
}
@@ -639,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
- mavlink_log_critical(_navigator->get_mavlink_fd(),
- "DO JUMP repetitions completed");
+ if (is_current) {
+ mavlink_log_critical(_navigator->get_mavlink_fd(),
+ "DO JUMP repetitions completed");
+ }
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
}
diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 6ead38d61..d76ec55f0 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -57,6 +57,7 @@
#define SBUS_FLAGS_BYTE 23
#define SBUS_FAILSAFE_BIT 3
#define SBUS_FRAMELOST_BIT 2
+#define SBUS1_FRAME_DELAY 14000
/*
Measured values with Futaba FX-30/R6108SB:
@@ -80,6 +81,7 @@ static int sbus_fd = -1;
static hrt_abstime last_rx_time;
static hrt_abstime last_frame_time;
+static hrt_abstime last_txframe_time = 0;
static uint8_t frame[SBUS_FRAME_SIZE];
@@ -122,10 +124,42 @@ sbus_init(const char *device)
void
sbus1_output(uint16_t *values, uint16_t num_values)
{
- char a = 'A';
- write(sbus_fd, &a, 1);
-}
+ uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
+ uint8_t offset = 0;
+ uint16_t value;
+ hrt_abstime now;
+
+ now = hrt_absolute_time();
+
+ if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
+ last_txframe_time = now;
+ uint8_t oframe[SBUS_FRAME_SIZE] = { 0x0f };
+
+ /* 16 is sbus number of servos/channels minus 2 single bit channels.
+ * currently ignoring single bit channels. */
+
+ for (unsigned i = 0; (i < num_values) && (i < 16); ++i) {
+ value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
+
+ /*protect from out of bounds values and limit to 11 bits*/
+ if (value > 0x07ff ) {
+ value = 0x07ff;
+ }
+
+ while (offset >= 8) {
+ ++byteindex;
+ offset -= 8;
+ }
+ oframe[byteindex] |= (value << (offset)) & 0xff;
+ oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff;
+ oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff;
+ offset += 11;
+ }
+
+ write(sbus_fd, oframe, SBUS_FRAME_SIZE);
+ }
+}
void
sbus2_output(uint16_t *values, uint16_t num_values)
{
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index dde237adc..50b7bd9e5 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -47,7 +47,7 @@
* Switch position
*/
typedef enum {
- SWITCH_POS_NONE = 0, /**< switch is not mapped */
+ SWITCH_POS_NONE = 0, /**< switch is not mapped */
SWITCH_POS_ON, /**< switch activated (value = 1) */
SWITCH_POS_MIDDLE, /**< middle position (value = 0) */
SWITCH_POS_OFF /**< switch not activated (value = -1) */
@@ -93,13 +93,13 @@ struct manual_control_setpoint_s {
float aux4; /**< default function: camera roll */
float aux5; /**< default function: payload drop */
- switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
- switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
- 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 */
+ switch_pos_t mode_switch; /**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
+ switch_pos_t return_switch; /**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
+ 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 */
+};
/**
* @}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-05 05:35:16 +0000