aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ROMFS/px4fmu_common/init.d/100_mpx_easystar14
-rw-r--r--ROMFS/px4fmu_common/init.d/101_hk_bixler12
-rw-r--r--ROMFS/px4fmu_common/init.d/30_io_camflyer12
-rw-r--r--ROMFS/px4fmu_common/init.d/31_io_phantom12
-rw-r--r--nuttx-configs/px4fmu-v2/nsh/defconfig2
-rw-r--r--src/drivers/drv_gps.h3
-rw-r--r--src/drivers/gps/gps.cpp15
-rw-r--r--src/drivers/gps/ubx.cpp1
-rw-r--r--src/drivers/px4io/px4io.cpp5
-rw-r--r--src/modules/commander/commander.cpp44
-rw-r--r--src/modules/px4iofirmware/mixer.cpp2
-rw-r--r--src/modules/systemlib/mixer/mixer.cpp16
-rw-r--r--src/modules/systemlib/mixer/mixer_multirotor.cpp17
-rw-r--r--src/modules/systemlib/mixer/mixer_simple.cpp44
14 files changed, 128 insertions, 71 deletions
diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
index 4ed73909e..1a05f5140 100644
--- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar
+++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar
@@ -29,7 +29,8 @@ if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
- usleep 5000
+
+ commander start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
@@ -37,12 +38,14 @@ then
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
- usleep 5000
+
+ commander start
+
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
fi
-
+
#
# Start the sensors and test them.
#
@@ -54,11 +57,6 @@ sh /etc/init.d/rc.sensors
sh /etc/init.d/rc.logging
#
-# Start the commander.
-#
-commander start
-
-#
# Start GPS interface (depends on orb)
#
gps start
diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler
index b4daa8f5a..89b3185ad 100644
--- a/ROMFS/px4fmu_common/init.d/101_hk_bixler
+++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler
@@ -29,7 +29,8 @@ if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
- usleep 5000
+
+ commander start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
@@ -37,7 +38,9 @@ then
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
- usleep 5000
+
+ commander start
+
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
@@ -54,11 +57,6 @@ sh /etc/init.d/rc.sensors
sh /etc/init.d/rc.logging
#
-# Start the commander.
-#
-commander start
-
-#
# Start GPS interface (depends on orb)
#
gps start
diff --git a/ROMFS/px4fmu_common/init.d/30_io_camflyer b/ROMFS/px4fmu_common/init.d/30_io_camflyer
index ff740b6f2..191d8cd95 100644
--- a/ROMFS/px4fmu_common/init.d/30_io_camflyer
+++ b/ROMFS/px4fmu_common/init.d/30_io_camflyer
@@ -29,7 +29,8 @@ if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
- usleep 5000
+
+ commander start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
@@ -37,7 +38,9 @@ then
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
- usleep 5000
+
+ commander start
+
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
@@ -54,11 +57,6 @@ sh /etc/init.d/rc.sensors
sh /etc/init.d/rc.logging
#
-# Start the commander.
-#
-commander start
-
-#
# Start GPS interface (depends on orb)
#
gps start
diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom
index 838dcb369..7e0127e79 100644
--- a/ROMFS/px4fmu_common/init.d/31_io_phantom
+++ b/ROMFS/px4fmu_common/init.d/31_io_phantom
@@ -29,7 +29,8 @@ if px4io detect
then
# Start MAVLink (depends on orb)
mavlink start
- usleep 5000
+
+ commander start
sh /etc/init.d/rc.io
# Limit to 100 Hz updates and (implicit) 50 Hz PWM
@@ -37,7 +38,9 @@ then
else
# Start MAVLink (on UART1 / ttyS0)
mavlink start -d /dev/ttyS0
- usleep 5000
+
+ commander start
+
fmu mode_pwm
param set BAT_V_SCALING 0.004593
set EXIT_ON_END yes
@@ -54,11 +57,6 @@ sh /etc/init.d/rc.sensors
sh /etc/init.d/rc.logging
#
-# Start the commander.
-#
-commander start
-
-#
# Start GPS interface (depends on orb)
#
gps start
diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig
index 8f9914310..372453fb6 100644
--- a/nuttx-configs/px4fmu-v2/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v2/nsh/defconfig
@@ -410,7 +410,7 @@ CONFIG_START_DAY=1
CONFIG_DEV_CONSOLE=y
# CONFIG_MUTEX_TYPES is not set
CONFIG_PRIORITY_INHERITANCE=y
-CONFIG_SEM_PREALLOCHOLDERS=8
+CONFIG_SEM_PREALLOCHOLDERS=0
CONFIG_SEM_NNESTPRIO=8
# CONFIG_FDCLONE_DISABLE is not set
CONFIG_FDCLONE_STDIO=y
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 1dda8ef0b..398cf4870 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -51,8 +51,7 @@
typedef enum {
GPS_DRIVER_MODE_NONE = 0,
GPS_DRIVER_MODE_UBX,
- GPS_DRIVER_MODE_MTK,
- GPS_DRIVER_MODE_NMEA,
+ GPS_DRIVER_MODE_MTK
} gps_driver_mode_t;
diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index a84cb8e59..fc500a9ec 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -279,10 +279,6 @@ GPS::task_main()
_Helper = new MTK(_serial_fd, &_report);
break;
- case GPS_DRIVER_MODE_NMEA:
- //_Helper = new NMEA(); //TODO: add NMEA
- break;
-
default:
break;
}
@@ -328,10 +324,6 @@ GPS::task_main()
mode_str = "MTK";
break;
- case GPS_DRIVER_MODE_NMEA:
- mode_str = "NMEA";
- break;
-
default:
break;
}
@@ -362,9 +354,6 @@ GPS::task_main()
_mode = GPS_DRIVER_MODE_UBX;
break;
- // case GPS_DRIVER_MODE_NMEA:
- // _mode = GPS_DRIVER_MODE_UBX;
- // break;
default:
break;
}
@@ -400,10 +389,6 @@ GPS::print_info()
warnx("protocol: MTK");
break;
- case GPS_DRIVER_MODE_NMEA:
- warnx("protocol: NMEA");
- break;
-
default:
break;
}
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 0b25b379f..86291901c 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -281,7 +281,6 @@ UBX::receive(unsigned timeout)
return 1;
} else {
- warnx("ubx: timeout - no messages");
return -1;
}
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index b66d425dd..952453a8c 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -591,6 +591,9 @@ PX4IO::init()
if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) &&
(reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+ /* get a status update from IO */
+ io_get_status();
+
mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART");
log("INAIR RESTART RECOVERY (needs commander app running)");
@@ -1509,7 +1512,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries)
* even.
*/
unsigned total_len = sizeof(px4io_mixdata) + count;
- if (total_len % 1) {
+ if (total_len % 2) {
msg->text[count] = '\0';
total_len++;
}
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1e86e8e24..01b7b84d0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -470,8 +470,26 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
}
case VEHICLE_CMD_COMPONENT_ARM_DISARM:
- // XXX implement later
- result = VEHICLE_CMD_RESULT_DENIED;
+ {
+ transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+ if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
+ if (safety->safety_switch_available && !safety->safety_off) {
+ print_reject_arm("NOT ARMING: Press safety switch first.");
+ arming_res = TRANSITION_DENIED;
+
+ } else {
+ arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+ }
+
+ if (arming_res == TRANSITION_CHANGED) {
+ mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
+ result = VEHICLE_CMD_RESULT_ACCEPTED;
+ } else {
+ mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
+ result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+ }
+ }
+ }
break;
default:
@@ -806,16 +824,6 @@ int commander_thread_main(int argc, char *argv[])
check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
- orb_check(cmd_sub, &updated);
-
- if (updated) {
- /* got command */
- orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
- /* handle it */
- handle_command(&status, &safety, &control_mode, &cmd, &armed);
- }
-
/* update safety topic */
orb_check(safety_sub, &updated);
@@ -1147,6 +1155,18 @@ int commander_thread_main(int argc, char *argv[])
}
}
+
+ /* handle commands last, as the system needs to be updated to handle them */
+ orb_check(cmd_sub, &updated);
+
+ if (updated) {
+ /* got command */
+ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+ /* handle it */
+ handle_command(&status, &safety, &control_mode, &cmd, &armed);
+ }
+
/* evaluate the navigation state machine */
transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 0edd91b24..30aff7d20 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -221,7 +221,7 @@ mixer_tick(void)
}
/* do the calculations during the ramp for all at once */
- if(esc_state == ESC_RAMP) {
+ if (esc_state == ESC_RAMP) {
ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US;
}
diff --git a/src/modules/systemlib/mixer/mixer.cpp b/src/modules/systemlib/mixer/mixer.cpp
index df0dfe838..b1bb1a66d 100644
--- a/src/modules/systemlib/mixer/mixer.cpp
+++ b/src/modules/systemlib/mixer/mixer.cpp
@@ -142,6 +142,22 @@ NullMixer *
NullMixer::from_text(const char *buf, unsigned &buflen)
{
NullMixer *nm = nullptr;
+ const char *end = buf + buflen;
+
+ /* enforce that the mixer ends with space or a new line */
+ for (int i = buflen - 1; i >= 0; i--) {
+ if (buf[i] == '\0')
+ continue;
+
+ /* require a space or newline at the end of the buffer, fail on printable chars */
+ if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+ /* found a line ending or space, so no split symbols / numbers. good. */
+ break;
+ } else {
+ return nm;
+ }
+
+ }
if ((buflen >= 2) && (buf[0] == 'Z') && (buf[1] == ':')) {
nm = new NullMixer;
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 8ded0b05c..2446cc3fb 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -181,6 +181,23 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
char geomname[8];
int s[4];
int used;
+ const char *end = buf + buflen;
+
+ /* enforce that the mixer ends with space or a new line */
+ for (int i = buflen - 1; i >= 0; i--) {
+ if (buf[i] == '\0')
+ continue;
+
+ /* require a space or newline at the end of the buffer, fail on printable chars */
+ if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+ /* found a line ending or space, so no split symbols / numbers. good. */
+ break;
+ } else {
+ debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
+ return nullptr;
+ }
+
+ }
if (sscanf(buf, "R: %s %d %d %d %d%n", geomname, &s[0], &s[1], &s[2], &s[3], &used) != 5) {
debug("multirotor parse failed on '%s'", buf);
diff --git a/src/modules/systemlib/mixer/mixer_simple.cpp b/src/modules/systemlib/mixer/mixer_simple.cpp
index 07dc5f37f..c8434f991 100644
--- a/src/modules/systemlib/mixer/mixer_simple.cpp
+++ b/src/modules/systemlib/mixer/mixer_simple.cpp
@@ -55,7 +55,7 @@
#include "mixer.h"
#define debug(fmt, args...) do { } while(0)
-//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
+// #define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
SimpleMixer::SimpleMixer(ControlCallback control_cb,
uintptr_t cb_handle,
@@ -98,14 +98,17 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
{
int ret;
int s[5];
+ int n = -1;
buf = findtag(buf, buflen, 'O');
- if ((buf == nullptr) || (buflen < 12))
+ if ((buf == nullptr) || (buflen < 12)) {
+ debug("output parser failed finding tag, ret: '%s'", buf);
return -1;
+ }
- if ((ret = sscanf(buf, "O: %d %d %d %d %d",
- &s[0], &s[1], &s[2], &s[3], &s[4])) != 5) {
- debug("scaler parse failed on '%s' (got %d)", buf, ret);
+ if ((ret = sscanf(buf, "O: %d %d %d %d %d %n",
+ &s[0], &s[1], &s[2], &s[3], &s[4], &n)) != 5) {
+ debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
return -1;
}
skipline(buf, buflen);
@@ -126,8 +129,10 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
int s[5];
buf = findtag(buf, buflen, 'S');
- if ((buf == nullptr) || (buflen < 16))
+ if ((buf == nullptr) || (buflen < 16)) {
+ debug("contorl parser failed finding tag, ret: '%s'", buf);
return -1;
+ }
if (sscanf(buf, "S: %u %u %d %d %d %d %d",
&u[0], &u[1], &s[0], &s[1], &s[2], &s[3], &s[4]) != 7) {
@@ -156,6 +161,22 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
int used;
const char *end = buf + buflen;
+ /* enforce that the mixer ends with space or a new line */
+ for (int i = buflen - 1; i >= 0; i--) {
+ if (buf[i] == '\0')
+ continue;
+
+ /* require a space or newline at the end of the buffer, fail on printable chars */
+ if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
+ /* found a line ending or space, so no split symbols / numbers. good. */
+ break;
+ } else {
+ debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
+ goto out;
+ }
+
+ }
+
/* get the base info for the mixer */
if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
debug("simple parse failed on '%s'", buf);
@@ -173,22 +194,27 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
mixinfo->control_count = inputs;
- if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler))
+ if (parse_output_scaler(end - buflen, buflen, mixinfo->output_scaler)) {
+ debug("simple mixer parser failed parsing out scaler tag, ret: '%s'", buf);
goto out;
+ }
for (unsigned i = 0; i < inputs; i++) {
if (parse_control_scaler(end - buflen, buflen,
mixinfo->controls[i].scaler,
mixinfo->controls[i].control_group,
- mixinfo->controls[i].control_index))
+ mixinfo->controls[i].control_index)) {
+ debug("simple mixer parser failed parsing ctrl scaler tag, ret: '%s'", buf);
goto out;
+ }
+
}
sm = new SimpleMixer(control_cb, cb_handle, mixinfo);
if (sm != nullptr) {
mixinfo = nullptr;
- debug("loaded mixer with %d inputs", inputs);
+ debug("loaded mixer with %d input(s)", inputs);
} else {
debug("could not allocate memory for mixer");
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 09:09:09 +0000