aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/drivers/hmc5883/hmc5883.cpp11
-rw-r--r--src/modules/commander/commander.cpp25
-rw-r--r--src/modules/commander/rc_calibration.cpp14
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp1
-rw-r--r--src/modules/mavlink/missionlib.c6
-rw-r--r--src/modules/mavlink/waypoints.c6
-rw-r--r--src/modules/position_estimator_inav/position_estimator_inav_main.c10
-rw-r--r--src/modules/systemlib/param/param.c4
8 files changed, 50 insertions, 27 deletions
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index a5229b237..3ede90a17 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -962,11 +962,12 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
warnx("sampling 500 samples for scaling offset");
/* set the queue depth to 10 */
- if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
- warn("failed to set queue depth");
- ret = 1;
- goto out;
- }
+ /* don't do this for now, it can lead to a crash in start() respectively work_queue() */
+// if (OK != ioctl(filp, SENSORIOCSQUEUEDEPTH, 10)) {
+// warn("failed to set queue depth");
+// ret = 1;
+// goto out;
+// }
/* start the sensor polling at 50 Hz */
if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 8e8226f09..333fe30ae 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -50,6 +50,7 @@
#include <unistd.h>
#include <fcntl.h>
#include <errno.h>
+#include <systemlib/err.h>
#include <debug.h>
#include <sys/prctl.h>
#include <sys/stat.h>
@@ -599,6 +600,7 @@ int commander_thread_main(int argc, char *argv[])
pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
struct sched_param param;
+ (void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
/* low priority */
param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
@@ -1655,13 +1657,13 @@ void *commander_low_prio_loop(void *arg)
if (((int)(cmd.param1)) == 1) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
- usleep(1000000);
+ usleep(100000);
/* reboot */
systemreset(false);
} else if (((int)(cmd.param1)) == 3) {
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
- usleep(1000000);
+ usleep(100000);
/* reboot to bootloader */
systemreset(true);
@@ -1704,6 +1706,7 @@ void *commander_low_prio_loop(void *arg)
} else if ((int)(cmd.param4) == 1) {
/* RC calibration */
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+ calib_ret = do_rc_calibration(mavlink_fd);
} else if ((int)(cmd.param5) == 1) {
/* accelerometer calibration */
@@ -1729,22 +1732,36 @@ void *commander_low_prio_loop(void *arg)
case VEHICLE_CMD_PREFLIGHT_STORAGE: {
if (((int)(cmd.param1)) == 0) {
- if (0 == param_load_default()) {
+ int ret = param_load_default();
+ if (ret == OK) {
mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+ /* convenience as many parts of NuttX use negative errno */
+ if (ret < 0)
+ ret = -ret;
+
+ if (ret < 1000)
+ mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
} else if (((int)(cmd.param1)) == 1) {
- if (0 == param_save_default()) {
+ int ret = param_save_default();
+ if (ret == OK) {
mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
} else {
mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+ /* convenience as many parts of NuttX use negative errno */
+ if (ret < 0)
+ ret = -ret;
+
+ if (ret < 1000)
+ mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
}
}
diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
index 9fc1d6470..90ede499a 100644
--- a/src/modules/commander/rc_calibration.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -57,14 +57,16 @@ int do_rc_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "trim calibration starting");
- /* XXX fix this */
- // if (current_status.rc_signal) {
- // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
- // return;
- // }
-
int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
struct manual_control_setpoint_s sp;
+ bool changed;
+ orb_check(sub_man, &changed);
+
+ if (!changed) {
+ mavlink_log_critical(mavlink_fd, "no manual control, aborting");
+ return ERROR;
+ }
+
orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
/* set parameters */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index af43542da..4674f7a24 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -747,6 +747,7 @@ receive_start(int uart)
fcntl(uart, F_SETFL, flags | O_NONBLOCK);
struct sched_param param;
+ (void)pthread_attr_getschedparam(&receiveloop_attr, &param);
param.sched_priority = SCHED_PRIORITY_MAX - 40;
(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index be88b8794..3175e64ce 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -167,9 +167,9 @@ bool set_special_fields(float param1, float param2, float param3, float param4,
sp->loiter_direction = (param3 >= 0) ? 1 : -1;
sp->param1 = param1;
- sp->param1 = param2;
- sp->param1 = param3;
- sp->param1 = param4;
+ sp->param2 = param2;
+ sp->param3 = param3;
+ sp->param4 = param4;
}
/**
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 16a7c2d35..c6c2eac68 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -417,6 +417,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
}
if (time_elapsed) {
+
if (cur_wp->autocontinue) {
cur_wp->current = 0;
@@ -425,9 +426,10 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
/* the last waypoint was reached, if auto continue is
- * activated restart the waypoint list from the beginning
+ * activated keep the system loitering there.
*/
- wpm->current_active_wp_id = 0;
+ cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
+ cur_wp->param3 = 15.0f; // XXX magic number 15 m loiter radius
} else {
if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 88057b323..932f61088 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -53,7 +53,7 @@
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
-#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
@@ -181,7 +181,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
uint32_t baro_counter = 0;
/* declare and safely initialize all structs */
- struct actuator_controls_effective_s actuator;
+ struct actuator_controls_s actuator;
memset(&actuator, 0, sizeof(actuator));
struct actuator_armed_s armed;
memset(&armed, 0, sizeof(armed));
@@ -200,7 +200,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* subscribe */
int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
- int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+ int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -337,7 +337,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* actuator */
if (fds[1].revents & POLLIN) {
- orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator);
+ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
}
/* armed */
@@ -546,7 +546,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
alt_disp = alt_disp * alt_disp;
float land_disp2 = params.land_disp * params.land_disp;
/* get actual thrust output */
- float thrust = armed.armed ? actuator.control_effective[3] : 0.0f;
+ float thrust = armed.armed ? actuator.control[3] : 0.0f;
if (landed) {
if (alt_disp > land_disp2 && thrust > params.land_thr) {
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 24b52d1a9..c69de52b7 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -520,7 +520,7 @@ param_save_default(void)
if (fd < 0) {
warn("opening '%s' for writing failed", param_get_default_file());
- return -1;
+ return fd;
}
result = param_export(fd, false);
@@ -529,7 +529,7 @@ param_save_default(void)
if (result != 0) {
warn("error exporting parameters to '%s'", param_get_default_file());
unlink(param_get_default_file());
- return -2;
+ return result;
}
return 0;
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-18 01:44:39 +0000