aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp49
-rw-r--r--src/drivers/px4io/px4io.cpp47
-rw-r--r--src/lib/external_lgpl/tecs/tecs.cpp14
-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
7 files changed, 106 insertions, 89 deletions
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index e4ecfa6b5..81f767965 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
* LSM/Ga, giving 1.16 and 1.08 */
float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
- warnx("starting mag scale calibration");
-
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
- warn("failed to set 2Hz poll rate");
+ warn("FAILED: SENSORIOCSPOLLRATE 2Hz");
ret = 1;
goto out;
}
@@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
/* Set to 2.5 Gauss. We ask for 3 to get the right part of
* the chained if statement above. */
if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
- warnx("failed to set 2.5 Ga range");
+ warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
- warnx("failed to enable sensor calibration mode");
+ warnx("FAILED: MAGIOCEXSTRAP 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
- warn("WARNING: failed to get scale / offsets for mag");
+ warn("FAILED: MAGIOCGSCALE 1");
ret = 1;
goto out;
}
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
- warn("WARNING: failed to set null scale / offsets for mag");
+ warn("FAILED: MAGIOCSSCALE 1");
ret = 1;
goto out;
}
@@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
- warn("timed out waiting for sensor data");
+ warn("ERROR: TIMEOUT 1");
goto out;
}
@@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
- warn("periodic read failed");
+ warn("ERROR: READ 1");
ret = -EIO;
goto out;
}
@@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
ret = ::poll(&fds, 1, 2000);
if (ret != 1) {
- warn("timed out waiting for sensor data");
+ warn("ERROR: TIMEOUT 2");
goto out;
}
@@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sz = ::read(fd, &report, sizeof(report));
if (sz != sizeof(report)) {
- warn("periodic read failed");
+ warn("ERROR: READ 2");
ret = -EIO;
goto out;
}
@@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
sum_excited[1] += cal[1];
sum_excited[2] += cal[2];
}
-
- //warnx("periodic read %u", i);
- //warnx("measurement: %.6f %.6f %.6f", (double)report.x, (double)report.y, (double)report.z);
- //warnx("cal: %.6f %.6f %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
}
if (good_count < 5) {
- warn("failed calibration");
ret = -EIO;
goto out;
}
-#if 0
- warnx("measurement avg: %.6f %.6f %.6f",
- (double)sum_excited[0]/good_count,
- (double)sum_excited[1]/good_count,
- (double)sum_excited[2]/good_count);
-#endif
-
float scaling[3];
scaling[0] = sum_excited[0] / good_count;
scaling[1] = sum_excited[1] / good_count;
scaling[2] = sum_excited[2] / good_count;
- warnx("axes scaling: %.6f %.6f %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
-
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
@@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
out:
if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
- warn("failed to set new scale / offsets for mag");
+ warn("FAILED: MAGIOCSSCALE 2");
}
/* set back to normal mode */
/* Set to 1.1 Gauss */
if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
- warnx("failed to set 1.1 Ga range");
+ warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
}
if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
- warnx("failed to disable sensor calibration mode");
+ warnx("FAILED: MAGIOCEXSTRAP 0");
}
if (ret == OK) {
- if (!check_scale()) {
- warnx("mag scale calibration successfully finished.");
- } else {
- warnx("mag scale calibration finished with invalid results.");
+ if (check_scale()) {
+ /* failed */
+ warnx("FAILED: SCALE");
ret = ERROR;
}
- } else {
- warnx("mag scale calibration failed.");
}
return ret;
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 6d68d9f60..06faf49a4 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1245,41 +1245,42 @@ PX4IO::io_set_rc_config()
* for compatibility reasons with existing
* autopilots / GCS'.
*/
- param_get(param_find("RC_MAP_ROLL"), &ichan);
-
- /* subtract one from 1-based index - this might be
- * a negative number now
- */
- ichan -= 1;
- if ((ichan >= 0) && (ichan < (int)_max_rc_input))
- input_map[ichan] = 0;
+ /* ROLL */
+ param_get(param_find("RC_MAP_ROLL"), &ichan);
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+ input_map[ichan - 1] = 0;
+ }
+ /* PITCH */
param_get(param_find("RC_MAP_PITCH"), &ichan);
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+ input_map[ichan - 1] = 1;
+ }
- if ((ichan >= 0) && (ichan < (int)_max_rc_input))
- input_map[ichan] = 1;
-
+ /* YAW */
param_get(param_find("RC_MAP_YAW"), &ichan);
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+ input_map[ichan - 1] = 2;
+ }
- if ((ichan >= 0) && (ichan < (int)_max_rc_input))
- input_map[ichan] = 2;
-
+ /* THROTTLE */
param_get(param_find("RC_MAP_THROTTLE"), &ichan);
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+ input_map[ichan - 1] = 3;
+ }
- if ((ichan >= 0) && (ichan < (int)_max_rc_input))
- input_map[ichan] = 3;
-
+ /* FLAPS */
param_get(param_find("RC_MAP_FLAPS"), &ichan);
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+ input_map[ichan - 1] = 4;
+ }
- if ((ichan >= 0) && (ichan < (int)_max_rc_input))
- input_map[ichan] = 4;
-
+ /* MAIN MODE SWITCH */
param_get(param_find("RC_MAP_MODE_SW"), &ichan);
-
- if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
+ if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
/* use out of normal bounds index to indicate special channel */
- input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
+ input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
}
/*
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 0ed210cf4..cfcc48b62 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
}
- // Calculate PD + FF throttle
+ // Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+ _throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
// Rate limit PD + FF throttle
// Calculate the throttle increment from the specified slew time
if (fabsf(_throttle_slewrate) > 0.01f) {
float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
-
_throttle_dem = constrain(_throttle_dem,
- _last_throttle_dem - thrRateIncr,
- _last_throttle_dem + thrRateIncr);
+ _last_throttle_dem - thrRateIncr,
+ _last_throttle_dem + thrRateIncr);
}
// Ensure _last_throttle_dem is always initialized properly
- // Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
_last_throttle_dem = _throttle_dem;
-
// Calculate integrator state upper and lower limits
- // Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+ // Set to a value that will allow 0.1 (10%) throttle saturation to allow for noise on the demand
float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
float integ_min = (_THRminf - _throttle_dem - 0.1f);
// Calculate integrator state, constraining state
- // Set integrator to a max throttle value dduring climbout
+ // Set integrator to a max throttle value during climbout
_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
if (_climbOutDem) {
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 08:46:16 +0000