aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ROMFS/px4fmu_common/init.d/10_dji_f33011
-rw-r--r--ROMFS/px4fmu_common/init.d/11_dji_f4501
-rw-r--r--ROMFS/px4fmu_common/init.d/15_tbs_discovery1
-rw-r--r--ROMFS/px4fmu_common/init.d/16_3dr_iris1
-rwxr-xr-xTools/px_uploader.py97
-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
13 files changed, 127 insertions, 61 deletions
diff --git a/ROMFS/px4fmu_common/init.d/10_dji_f330 b/ROMFS/px4fmu_common/init.d/10_dji_f330
index 709e17083..d21759507 100644
--- a/ROMFS/px4fmu_common/init.d/10_dji_f330
+++ b/ROMFS/px4fmu_common/init.d/10_dji_f330
@@ -10,12 +10,12 @@ then
# Set all params here, then disable autoconfig
param set SYS_AUTOCONFIG 0
- param set MC_ATTRATE_D 0.004
+ param set MC_ATTRATE_D 0.002
param set MC_ATTRATE_I 0.0
- param set MC_ATTRATE_P 0.12
+ param set MC_ATTRATE_P 0.09
param set MC_ATT_D 0.0
param set MC_ATT_I 0.0
- param set MC_ATT_P 7.0
+ param set MC_ATT_P 6.8
param set MC_YAWPOS_D 0.0
param set MC_YAWPOS_I 0.0
param set MC_YAWPOS_P 2.0
@@ -30,12 +30,12 @@ then
param set MPC_XY_P 0.5
param set MPC_XY_VEL_D 0
param set MPC_XY_VEL_I 0
- param set MPC_XY_VEL_MAX 2
+ param set MPC_XY_VEL_MAX 3
param set MPC_XY_VEL_P 0.2
param set MPC_Z_D 0
param set MPC_Z_P 1
param set MPC_Z_VEL_D 0
- param set MPC_Z_VEL_I 0.10
+ param set MPC_Z_VEL_I 0.1
param set MPC_Z_VEL_MAX 2
param set MPC_Z_VEL_P 0.20
@@ -69,6 +69,7 @@ else
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
+ param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
diff --git a/ROMFS/px4fmu_common/init.d/11_dji_f450 b/ROMFS/px4fmu_common/init.d/11_dji_f450
index 73af1465c..388f40a47 100644
--- a/ROMFS/px4fmu_common/init.d/11_dji_f450
+++ b/ROMFS/px4fmu_common/init.d/11_dji_f450
@@ -53,6 +53,7 @@ else
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
+ param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
diff --git a/ROMFS/px4fmu_common/init.d/15_tbs_discovery b/ROMFS/px4fmu_common/init.d/15_tbs_discovery
index e4b9aee34..cbfde6d3c 100644
--- a/ROMFS/px4fmu_common/init.d/15_tbs_discovery
+++ b/ROMFS/px4fmu_common/init.d/15_tbs_discovery
@@ -53,6 +53,7 @@ else
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
+ param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris
index 3fbc84899..b6c5fbdea 100644
--- a/ROMFS/px4fmu_common/init.d/16_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris
@@ -53,6 +53,7 @@ else
mavlink start -d /dev/ttyS0
usleep 5000
fmu mode_pwm
+ param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index 8a7e5e250..cab3c2fd0 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -137,34 +137,40 @@ class uploader(object):
'''Uploads a firmware file to the PX FMU bootloader'''
# protocol bytes
- INSYNC = chr(0x12)
- EOC = chr(0x20)
+ INSYNC = b'\x12'
+ EOC = b'\x20'
# reply bytes
- OK = chr(0x10)
- FAILED = chr(0x11)
- INVALID = chr(0x13) # rev3+
+ OK = b'\x10'
+ FAILED = b'\x11'
+ INVALID = b'\x13' # rev3+
# command bytes
- NOP = chr(0x00) # guaranteed to be discarded by the bootloader
- GET_SYNC = chr(0x21)
- GET_DEVICE = chr(0x22)
- CHIP_ERASE = chr(0x23)
- CHIP_VERIFY = chr(0x24) # rev2 only
- PROG_MULTI = chr(0x27)
- READ_MULTI = chr(0x28) # rev2 only
- GET_CRC = chr(0x29) # rev3+
- REBOOT = chr(0x30)
+ NOP = b'\x00' # guaranteed to be discarded by the bootloader
+ GET_SYNC = b'\x21'
+ GET_DEVICE = b'\x22'
+ CHIP_ERASE = b'\x23'
+ CHIP_VERIFY = b'\x24' # rev2 only
+ PROG_MULTI = b'\x27'
+ READ_MULTI = b'\x28' # rev2 only
+ GET_CRC = b'\x29' # rev3+
+ REBOOT = b'\x30'
- INFO_BL_REV = chr(1) # bootloader protocol revision
+ INFO_BL_REV = b'\x01' # bootloader protocol revision
BL_REV_MIN = 2 # minimum supported bootloader protocol
BL_REV_MAX = 4 # maximum supported bootloader protocol
- INFO_BOARD_ID = chr(2) # board type
- INFO_BOARD_REV = chr(3) # board revision
- INFO_FLASH_SIZE = chr(4) # max firmware size in bytes
+ INFO_BOARD_ID = b'\x02' # board type
+ INFO_BOARD_REV = b'\x03' # board revision
+ INFO_FLASH_SIZE = b'\x04' # max firmware size in bytes
PROG_MULTI_MAX = 60 # protocol max is 255, must be multiple of 4
READ_MULTI_MAX = 60 # protocol max is 255, something overflows with >= 64
+
+ NSH_INIT = bytearray(b'\x0d\x0d\x0d')
+ NSH_REBOOT_BL = b"reboot -b\n"
+ NSH_REBOOT = b"reboot\n"
+ MAVLINK_REBOOT_ID1 = bytearray(b'\xfe\x21\x72\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x01\x00\x00\x48\xf0')
+ MAVLINK_REBOOT_ID0 = bytearray(b'\xfe\x21\x45\xff\x00\x4c\x00\x00\x80\x3f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf6\x00\x00\x00\x00\xd7\xac')
def __init__(self, portname, baudrate):
# open the port, keep the default timeout short so we can poll quickly
@@ -176,7 +182,7 @@ class uploader(object):
def __send(self, c):
# print("send " + binascii.hexlify(c))
- self.port.write(str(c))
+ self.port.write(c)
def __recv(self, count=1):
c = self.port.read(count)
@@ -192,9 +198,9 @@ class uploader(object):
def __getSync(self):
self.port.flush()
- c = self.__recv()
- if c is not self.INSYNC:
- raise RuntimeError("unexpected 0x%x instead of INSYNC" % ord(c))
+ c = bytes(self.__recv())
+ if c != self.INSYNC:
+ raise RuntimeError("unexpected %s instead of INSYNC" % c)
c = self.__recv()
if c == self.INVALID:
raise RuntimeError("bootloader reports INVALID OPERATION")
@@ -249,17 +255,29 @@ class uploader(object):
# send a PROG_MULTI command to write a collection of bytes
def __program_multi(self, data):
- self.__send(uploader.PROG_MULTI
- + chr(len(data)))
+
+ if runningPython3 == True:
+ length = len(data).to_bytes(1, byteorder='big')
+ else:
+ length = chr(len(data))
+
+ self.__send(uploader.PROG_MULTI)
+ self.__send(length)
self.__send(data)
self.__send(uploader.EOC)
self.__getSync()
# verify multiple bytes in flash
def __verify_multi(self, data):
- self.__send(uploader.READ_MULTI
- + chr(len(data))
- + uploader.EOC)
+
+ if runningPython3 == True:
+ length = len(data).to_bytes(1, byteorder='big')
+ else:
+ length = chr(len(data))
+
+ self.__send(uploader.READ_MULTI)
+ self.__send(length)
+ self.__send(uploader.EOC)
self.port.flush()
programmed = self.__recv(len(data))
if programmed != data:
@@ -350,7 +368,24 @@ class uploader(object):
print("done, rebooting.")
self.__reboot()
self.port.close()
-
+
+ def send_reboot(self):
+ # try reboot via NSH first
+ self.__send(uploader.NSH_INIT)
+ self.__send(uploader.NSH_REBOOT_BL)
+ self.__send(uploader.NSH_INIT)
+ self.__send(uploader.NSH_REBOOT)
+ # then try MAVLINK command
+ self.__send(uploader.MAVLINK_REBOOT_ID1)
+ self.__send(uploader.MAVLINK_REBOOT_ID0)
+
+
+
+# Detect python version
+if sys.version_info[0] < 3:
+ runningPython3 = False
+else:
+ runningPython3 = True
# Parse commandline arguments
parser = argparse.ArgumentParser(description="Firmware uploader for the PX autopilot system.")
@@ -397,7 +432,11 @@ while True:
print("Found board %x,%x bootloader rev %x on %s" % (up.board_type, up.board_rev, up.bl_rev, port))
except:
- # most probably a timeout talking to the port, no bootloader
+ # most probably a timeout talking to the port, no bootloader, try to reboot the board
+ print("attempting reboot on %s..." % port)
+ up.send_reboot()
+ # wait for the reboot, without we might run into Serial I/O Error 5
+ time.sleep(1.5)
continue
try:
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 04:44:35 +0000