From 318abfabcb1e0d8d23df3df8015d4c7bdc011b2a Mon Sep 17 00:00:00 2001 From: marco Date: Fri, 13 Sep 2013 21:05:41 +0200 Subject: [PATCH 01/13] mkblctrl fix and qgroundcontrol2 startup script for different frametypes --- .../init.d/rc.custom_dji_f330_mkblctrl | 122 ++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 36 ++++++ src/drivers/mkblctrl/mkblctrl.cpp | 7 +- 3 files changed, 164 insertions(+), 1 deletion(-) create mode 100644 ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl new file mode 100644 index 0000000000..51bc61c9e8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.custom_dji_f330_mkblctrl @@ -0,0 +1,122 @@ +#!nsh + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.002 + param set MC_ATTRATE_I 0.0 + 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 6.8 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + 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 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.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 2 = quadrotor +# +param set MAV_TYPE 2 + +set EXIT_ON_END no + +# +# Start the Mikrokopter ESC driver +# +if [ $MKBLCTRL_MODE == yes ] +then + if [ $MKBLCTRL_FRAME == x ] + then + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing" + mkblctrl -mkmode x + else + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing" + mkblctrl -mkmode + + fi +else + if [ $MKBLCTRL_FRAME == x ] + then + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame" + else + echo "[init] PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame" + fi + mkblctrl +fi + +usleep 10000 + +# +# Start and configure PX4IO or FMU interface +# +if px4io detect +then + # Start MAVLink (depends on orb) + mavlink start + usleep 5000 + + sh /etc/init.d/rc.io + # Set PWM values for DJI ESCs + px4io idle 900 900 900 900 + px4io min 1200 1200 1200 1200 + px4io max 1800 1800 1800 1800 +else + fmu mode_pwm + # Start MAVLink (on UART1 / ttyS0) + mavlink start -d /dev/ttyS0 + usleep 5000 + param set BAT_V_SCALING 0.004593 + set EXIT_ON_END yes +fi + +# +# Load mixer +# +if [ $MKBLCTRL_FRAME == x ] +then + mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix +else + mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix +fi + +# +# Set PWM output frequency +# +#pwm -u 400 -m 0xff + +# +# Start common for all multirotors apps +# +sh /etc/init.d/rc.multirotor + +if [ $EXIT_ON_END == yes ] +then + exit +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index cd4d4487d9..b7f1329dd2 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -172,6 +172,42 @@ then sh /etc/init.d/16_3dr_iris set MODE custom fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame + if param compare SYS_AUTOSTART 17 + then + set MKBLCTRL_MODE no + set MKBLCTRL_FRAME x + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame + if param compare SYS_AUTOSTART 18 + then + set MKBLCTRL_MODE no + set MKBLCTRL_FRAME + + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 X Frame Mikrokopter-Addressing + if param compare SYS_AUTOSTART 19 + then + set MKBLCTRL_MODE yes + set MKBLCTRL_FRAME x + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi + + # PX4FMU v1, v2 with or without IO and Mikrokopter I2C ESCs on DJI F330 + Frame Mikrokopter-Addressing + if param compare SYS_AUTOSTART 20 + then + set MKBLCTRL_MODE yes + set MKBLCTRL_FRAME + + sh /etc/init.d/rc.custom_dji_f330_mkblctrl + set MODE custom + fi if param compare SYS_AUTOSTART 30 then diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1bc3e97a41..d0de26a1ad 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -96,9 +96,10 @@ class MK : public device::I2C { public: enum Mode { + MODE_NONE, MODE_2PWM, MODE_4PWM, - MODE_NONE + MODE_6PWM, }; enum MappingMode { @@ -1023,9 +1024,11 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) return ret; /* if we are in valid PWM mode, try it as a PWM ioctl as well */ + /* switch (_mode) { case MODE_2PWM: case MODE_4PWM: + case MODE_6PWM: ret = pwm_ioctl(filp, cmd, arg); break; @@ -1033,6 +1036,8 @@ MK::ioctl(file *filp, int cmd, unsigned long arg) debug("not in a PWM mode"); break; } + */ + ret = pwm_ioctl(filp, cmd, arg); /* if nobody wants it, let CDev have it */ if (ret == -ENOTTY) From ac00100cb800423347ad81967a7731ab766be629 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Sep 2013 08:43:38 +0200 Subject: [PATCH 02/13] Hotfix: Disable gyro scale calibration to prevent people from wrongly using it --- src/modules/commander/gyro_calibration.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 82a0349c9e..369e6da624 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -163,7 +163,9 @@ int do_gyro_calibration(int mavlink_fd) return ERROR; } + mavlink_log_info(mavlink_fd, "offset calibration done."); +#if 0 /*** --- SCALING --- ***/ mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip."); @@ -241,9 +243,14 @@ int do_gyro_calibration(int mavlink_fd) } float gyro_scale = baseline_integral / gyro_integral; - float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; + warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale); +#else + float gyro_scales[] = { 1.0f, 1.0f, 1.0f }; +#endif + + if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { @@ -277,9 +284,6 @@ int do_gyro_calibration(int mavlink_fd) warn("WARNING: auto-save of params to storage failed"); } - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); /* third beep by cal end routine */ From 210e7f92454e2b99a448c1563e22fed6f0220ea7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Sep 2013 08:44:02 +0200 Subject: [PATCH 03/13] Make param save command tolerant of FS timing --- src/modules/systemlib/param/param.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index c69de52b70..e7c96fe54f 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -519,6 +519,10 @@ param_save_default(void) int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); if (fd < 0) { + /* do another attempt in case the unlink call is not synced yet */ + usleep(5000); + fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL); + warn("opening '%s' for writing failed", param_get_default_file()); return fd; } @@ -528,7 +532,7 @@ param_save_default(void) if (result != 0) { warn("error exporting parameters to '%s'", param_get_default_file()); - unlink(param_get_default_file()); + (void)unlink(param_get_default_file()); return result; } From 6624c8f1c40d08d594fd2f180bff1088eaff82d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 18 Sep 2013 11:49:22 +0200 Subject: [PATCH 04/13] Hotfix: Make voltage scaling for standalone default --- ROMFS/px4fmu_common/init.d/08_ardrone | 1 + ROMFS/px4fmu_common/init.d/09_ardrone_flow | 1 + src/modules/sensors/sensor_params.c | 3 ++- 3 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/08_ardrone b/ROMFS/px4fmu_common/init.d/08_ardrone index 7dbbb82847..f6f09ac220 100644 --- a/ROMFS/px4fmu_common/init.d/08_ardrone +++ b/ROMFS/px4fmu_common/init.d/08_ardrone @@ -30,6 +30,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +param set BAT_V_SCALING 0.008381 # # Start MAVLink diff --git a/ROMFS/px4fmu_common/init.d/09_ardrone_flow b/ROMFS/px4fmu_common/init.d/09_ardrone_flow index 6333aae564..9b739f2455 100644 --- a/ROMFS/px4fmu_common/init.d/09_ardrone_flow +++ b/ROMFS/px4fmu_common/init.d/09_ardrone_flow @@ -30,6 +30,7 @@ fi # MAV_TYPE 2 = quadrotor # param set MAV_TYPE 2 +param set BAT_V_SCALING 0.008381 # # Start MAVLink and MAVLink Onboard (PX4FLOW Sensor) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 950af2eba2..ed4fcdd462 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -174,7 +174,8 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f); /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ /* PX4IOAR: 0.00838095238 */ /* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */ -PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); +/* FMU with PX4IOAR: (3.3f * 52.0f / 5.0f / 4095.0f) */ +PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.00459340659f); #endif PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); From 36ffe2d69404ff62c4899b6f9f20709243a88b43 Mon Sep 17 00:00:00 2001 From: "Hyon Lim (Retina)" Date: Thu, 19 Sep 2013 04:29:46 +0900 Subject: [PATCH 05/13] Quadrotor HILS related RC script has been added --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 110 ++++++++++++++++++++ ROMFS/px4fmu_common/init.d/rcS | 9 +- 2 files changed, 117 insertions(+), 2 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d/1001_rc_quad.hil diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil new file mode 100644 index 0000000000..82efeb2e12 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -0,0 +1,110 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] HILS quadrotor starting.." + +# +# Load default params for this platform +# +if param compare SYS_AUTOCONFIG 1 +then + # Set all params here, then disable autoconfig + param set SYS_AUTOCONFIG 0 + + param set MC_ATTRATE_D 0.002 + param set MC_ATTRATE_I 0.0 + 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 6.8 + param set MC_YAWPOS_D 0.0 + param set MC_YAWPOS_I 0.0 + param set MC_YAWPOS_P 2.0 + param set MC_YAWRATE_D 0.005 + param set MC_YAWRATE_I 0.2 + param set MC_YAWRATE_P 0.3 + param set NAV_TAKEOFF_ALT 3.0 + param set MPC_TILT_MAX 0.5 + param set MPC_THR_MAX 0.7 + param set MPC_THR_MIN 0.3 + param set MPC_XY_D 0 + 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 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.1 + param set MPC_Z_VEL_MAX 2 + param set MPC_Z_VEL_P 0.20 + + param save +fi + +# Allow USB some time to come up +sleep 1 +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/ttyACM0 + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 2 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Check if we got an IO +# +if px4io start +then + echo "IO started" +else + fmu mode_serial + echo "FMU started" +fi + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +att_pos_estimator_ekf start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix + +# +# Start position estimator +# +position_estimator_inav start + +# +# Start attitude control +# +multirotor_att_control start + +# +# Start position control +# +multirotor_pos_control start + +echo "[HIL] setup done, running" + diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 040b5d594a..89cba5cdfe 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -106,8 +106,13 @@ then sh /etc/init.d/1000_rc.hil set MODE custom else - # Try to get an USB console - nshterm /dev/ttyACM0 & + if param compare SYS_AUTOSTART 1001 + then + sh /etc/init.d/1001_rc_quad.hil + else + # Try to get an USB console + nshterm /dev/ttyACM0 & + fi fi # From a32b25eb52fd9575873fc3b9c78e24736749a004 Mon Sep 17 00:00:00 2001 From: Hyon Lim Date: Thu, 19 Sep 2013 05:33:57 +1000 Subject: [PATCH 06/13] Update 1001_rc_quad.hil Default gain set --- ROMFS/px4fmu_common/init.d/1001_rc_quad.hil | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil index 82efeb2e12..bbe3b7e288 100644 --- a/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil +++ b/ROMFS/px4fmu_common/init.d/1001_rc_quad.hil @@ -13,22 +13,22 @@ then # Set all params here, then disable autoconfig param set SYS_AUTOCONFIG 0 - param set MC_ATTRATE_D 0.002 + param set MC_ATTRATE_D 0.0 param set MC_ATTRATE_I 0.0 - param set MC_ATTRATE_P 0.09 + param set MC_ATTRATE_P 0.05 param set MC_ATT_D 0.0 param set MC_ATT_I 0.0 - param set MC_ATT_P 6.8 + param set MC_ATT_P 3.0 param set MC_YAWPOS_D 0.0 param set MC_YAWPOS_I 0.0 - param set MC_YAWPOS_P 2.0 - param set MC_YAWRATE_D 0.005 - param set MC_YAWRATE_I 0.2 - param set MC_YAWRATE_P 0.3 + param set MC_YAWPOS_P 2.1 + param set MC_YAWRATE_D 0.0 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_P 0.05 param set NAV_TAKEOFF_ALT 3.0 param set MPC_TILT_MAX 0.5 - param set MPC_THR_MAX 0.7 - param set MPC_THR_MIN 0.3 + param set MPC_THR_MAX 0.5 + param set MPC_THR_MIN 0.1 param set MPC_XY_D 0 param set MPC_XY_P 0.5 param set MPC_XY_VEL_D 0 From 89d3e1db281414571fb55b87fb87385a97263cf1 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 18 Sep 2013 20:04:22 -0400 Subject: [PATCH 07/13] Implement Spektrum DSM pairing in V2 - Bind control for V2 - Relays and accessory power not supported on V2 hardware --- src/drivers/boards/px4io-v2/board_config.h | 2 +- src/drivers/boards/px4io-v2/px4iov2_init.c | 4 +- src/drivers/px4io/px4io.cpp | 67 ++++++++++++++++++---- src/modules/px4iofirmware/dsm.c | 21 +++++-- src/modules/px4iofirmware/protocol.h | 2 + src/modules/px4iofirmware/px4io.h | 2 + src/modules/px4iofirmware/registers.c | 12 ++-- src/modules/sensors/sensor_params.c | 3 +- 8 files changed, 83 insertions(+), 30 deletions(-) diff --git a/src/drivers/boards/px4io-v2/board_config.h b/src/drivers/boards/px4io-v2/board_config.h index 818b644362..4d41d0d07e 100644 --- a/src/drivers/boards/px4io-v2/board_config.h +++ b/src/drivers/boards/px4io-v2/board_config.h @@ -84,7 +84,7 @@ /* Power switch controls ******************************************************/ -#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN13) +#define GPIO_SPEKTRUM_PWR_EN (GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN13) #define GPIO_SERVO_FAULT_DETECT (GPIO_INPUT|GPIO_CNF_INPULLUP|GPIO_MODE_INPUT|GPIO_PORTA|GPIO_PIN15) diff --git a/src/drivers/boards/px4io-v2/px4iov2_init.c b/src/drivers/boards/px4io-v2/px4iov2_init.c index 0ea95bded9..ccd01edf56 100644 --- a/src/drivers/boards/px4io-v2/px4iov2_init.c +++ b/src/drivers/boards/px4io-v2/px4iov2_init.c @@ -111,9 +111,7 @@ __EXPORT void stm32_boardinitialize(void) stm32_configgpio(GPIO_BTN_SAFETY); - /* spektrum power enable is active high - disable it by default */ - /* XXX might not want to do this on warm restart? */ - stm32_gpiowrite(GPIO_SPEKTRUM_PWR_EN, false); + /* spektrum power enable is active high - enable it by default */ stm32_configgpio(GPIO_SPEKTRUM_PWR_EN); stm32_configgpio(GPIO_SERVO_FAULT_DETECT); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 5fff1feac5..f9fb9eea96 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -204,6 +204,7 @@ public: */ int disable_rc_handling(); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /** * Set the DSM VCC is controlled by relay one flag * @@ -223,6 +224,9 @@ public: { return _dsm_vcc_ctl; }; +#endif + + inline uint16_t system_status() const {return _status;} private: device::Device *_interface; @@ -274,8 +278,9 @@ private: float _battery_mamphour_total;///= 0) { + if (dsm_bind_val > -1) { dsm_bind_ioctl(dsm_bind_val); dsm_bind_val = -1; param_set(dsm_bind_param, &dsm_bind_val); @@ -1169,7 +1177,7 @@ PX4IO::dsm_bind_ioctl(int dsmMode) { if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { /* 0: dsm2, 1:dsmx */ - if ((dsmMode >= 0) && (dsmMode <= 1)) { + if ((dsmMode == 0) || (dsmMode == 1)) { mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); } else { @@ -1638,11 +1646,19 @@ PX4IO::print_status() ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + printf("rates 0x%04x default %u alt %u\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_ALTRATE)); +#endif printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); printf("controls"); for (unsigned i = 0; i < _max_controls; i++) @@ -1783,36 +1799,58 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) } case GPIO_RESET: { +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl) bits &= ~PX4IO_P_SETUP_RELAYS_POWER1; ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; } case GPIO_SET: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; - else - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); + break; + } + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case GPIO_CLEAR: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) + if (_dsm_vcc_ctl & (arg & PX4IO_P_SETUP_RELAYS_POWER1)) { ret = -EINVAL; - else + break; + } ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case GPIO_GET: +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 *(uint32_t *)arg = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS); if (*(uint32_t *)arg == _io_reg_get_error) ret = -EIO; +#endif +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + ret = -EINVAL; +#endif break; case MIXERIOCGETOUTPUTCOUNT: @@ -1990,6 +2028,7 @@ start(int argc, char *argv[]) } } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 int dsm_vcc_ctl; if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { @@ -1998,6 +2037,7 @@ start(int argc, char *argv[]) g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); } } +#endif exit(0); } @@ -2037,8 +2077,10 @@ bind(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "px4io must be started first"); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 if (!g_dev->get_dsm_vcc_ctl()) errx(1, "DSM bind feature not enabled"); +#endif if (argc < 3) errx(0, "needs argument, use dsm2 or dsmx"); @@ -2049,9 +2091,12 @@ bind(int argc, char *argv[]) pulses = DSMX_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + errx(1, "system must not be armed"); +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); - +#endif g_dev->ioctl(nullptr, DSM_BIND_START, pulses); exit(0); diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 206e27db5a..fd6bca62a2 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -243,28 +243,35 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { +#if !(defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4IO_V2)) + #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM +#else const uint32_t usart1RxAsOutp = GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10; if (dsm_fd < 0) return; -#ifdef CONFIG_ARCH_BOARD_PX4IO_V2 - // XXX implement - #warning DSM BIND NOT IMPLEMENTED ON PX4IO V2 -#else switch (cmd) { case dsm_bind_power_down: /*power down DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 POWER_RELAY1(0); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(0); +#endif break; case dsm_bind_power_up: /*power up DSM satellite*/ +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 POWER_RELAY1(1); +#else /* CONFIG_ARCH_BOARD_PX4IO_V2 */ + POWER_SPEKTRUM(1); +#endif dsm_guess_format(true); break; @@ -387,8 +394,10 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } - if (dsm_channel_shift == 11) + if (dsm_channel_shift == 11) { + /* Set the 11-bit data indicator */ *num_values |= 0x8000; + } /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. @@ -412,7 +421,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) * Upon receiving a full dsm frame we attempt to decode it. * * @param[out] values pointer to per channel array of decoded values - * @param[out] num_values pointer to number of raw channel values returned + * @param[out] num_values pointer to number of raw channel values returned, high order bit 0:10 bit data, 1:11 bit data * @return true=decoded raw channel values updated, false=no update */ bool diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index f5fa0ba751..0e2cd16898 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -165,11 +165,13 @@ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ +#if defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V1) #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ #define PX4IO_P_SETUP_RELAYS_POWER1 (1<<0) /* hardware rev [1] power relay 1 */ #define PX4IO_P_SETUP_RELAYS_POWER2 (1<<1) /* hardware rev [1] power relay 2 */ #define PX4IO_P_SETUP_RELAYS_ACC1 (1<<2) /* hardware rev [1] accessory power 1 */ #define PX4IO_P_SETUP_RELAYS_ACC2 (1<<3) /* hardware rev [1] accessory power 2 */ +#endif #define PX4IO_P_SETUP_VBATT_SCALE 6 /* hardware rev [1] battery voltage correction factor (float) */ #define PX4IO_P_SETUP_VSERVO_SCALE 6 /* hardware rev [2] servo voltage correction factor (float) */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 11f4d053d5..66c4ca9066 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -100,7 +100,9 @@ extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ #define r_setup_pwm_rates r_page_setup[PX4IO_P_SETUP_PWM_RATES] #define r_setup_pwm_defaultrate r_page_setup[PX4IO_P_SETUP_PWM_DEFAULTRATE] #define r_setup_pwm_altrate r_page_setup[PX4IO_P_SETUP_PWM_ALTRATE] +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 #define r_setup_relays r_page_setup[PX4IO_P_SETUP_RELAYS] +#endif #define r_control_values (&r_page_controls[0]) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 9c95fd1c52..8cb21e54fb 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -145,7 +145,9 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_PWM_RATES] = 0, [PX4IO_P_SETUP_PWM_DEFAULTRATE] = 50, [PX4IO_P_SETUP_PWM_ALTRATE] = 200, +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 [PX4IO_P_SETUP_RELAYS] = 0, +#endif #ifdef ADC_VSERVO [PX4IO_P_SETUP_VSERVO_SCALE] = 10000, #else @@ -462,22 +464,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value); break; +#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; -#ifdef POWER_RELAY1 POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0); -#endif -#ifdef POWER_RELAY2 POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0); -#endif -#ifdef POWER_ACC1 POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0); -#endif -#ifdef POWER_ACC2 POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0); -#endif break; +#endif case PX4IO_P_SETUP_VBATT_SCALE: r_page_setup[PX4IO_P_SETUP_VBATT_SCALE] = value; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index ed4fcdd462..4d719e0e15 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -164,8 +164,9 @@ PARAM_DEFINE_FLOAT(RC15_MAX, 2000); PARAM_DEFINE_FLOAT(RC15_REV, 1.0f); PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f); - +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ +#endif PARAM_DEFINE_INT32(RC_DSM_BIND, -1); /* -1 = Idle, 0 = Start DSM2 bind, 1 = Start DSMX bind */ #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 From 390d5b34de7dc595340d71c1554fec78c0d9423e Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 18 Sep 2013 20:14:53 -0400 Subject: [PATCH 08/13] Fix backwards ifdef in dsm.c --- src/modules/px4iofirmware/dsm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index fd6bca62a2..dc544d991e 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -243,7 +243,7 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { -#if !(defined(CONFIG_ARCH_BOARD_PX4IO_V1) || defined(CONFIG_ARCH_BOARD_PX4IO_V2)) +#if !(defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)) #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else const uint32_t usart1RxAsOutp = From 72df378577a7d4dbe590f6ef2e1a119924786fa2 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Wed, 18 Sep 2013 20:24:33 -0400 Subject: [PATCH 09/13] Finally get the #if right!!! --- src/modules/px4iofirmware/dsm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index dc544d991e..4339766568 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -243,7 +243,7 @@ dsm_init(const char *device) void dsm_bind(uint16_t cmd, int pulses) { -#if !(defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2)) +#if !defined(CONFIG_ARCH_BOARD_PX4IO_V1) && !defined(CONFIG_ARCH_BOARD_PX4IO_V2) #warning DSM BIND NOT IMPLEMENTED ON UNKNOWN PLATFORM #else const uint32_t usart1RxAsOutp = From 7e0da345f06babd2b99670b559c5c7faf96b6997 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 18 Sep 2013 21:47:10 -0700 Subject: [PATCH 10/13] The parameter system is supposed to have a lock; implement one. --- src/modules/systemlib/param/param.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index e7c96fe54f..2832d87ef1 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -48,6 +48,7 @@ #include #include #include +#include #include @@ -95,18 +96,20 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; +static sem_t param_lock = { .semcount = 1 }; + /** lock the parameter store */ static void param_lock(void) { - /* XXX */ + do {} while (sem_wait(¶m_lock) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - /* XXX */ + sem_post(¶m_lock); } /** assert that the parameter store is locked */ From 978a234d2903b34df7d13fcf0980db283a6f1166 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 18 Sep 2013 22:40:07 -0700 Subject: [PATCH 11/13] Lock name should not equal locking function name. Urr. --- src/modules/systemlib/param/param.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 2832d87ef1..97c547848d 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -96,20 +96,20 @@ ORB_DEFINE(parameter_update, struct parameter_update_s); /** parameter update topic handle */ static orb_advert_t param_topic = -1; -static sem_t param_lock = { .semcount = 1 }; +static sem_t param_sem = { .semcount = 1 }; /** lock the parameter store */ static void param_lock(void) { - do {} while (sem_wait(¶m_lock) != 0); + do {} while (sem_wait(¶m_sem) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - sem_post(¶m_lock); + sem_post(¶m_sem); } /** assert that the parameter store is locked */ From fdc45219493560487d95cdf88a66117ba43fa854 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Sep 2013 07:51:37 +0200 Subject: [PATCH 12/13] Hotfix: Disabling param lock, not operational yet --- src/modules/systemlib/param/param.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 97c547848d..59cbcf5fb0 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -102,14 +102,14 @@ static sem_t param_sem = { .semcount = 1 }; static void param_lock(void) { - do {} while (sem_wait(¶m_sem) != 0); + //do {} while (sem_wait(¶m_sem) != 0); } /** unlock the parameter store */ static void param_unlock(void) { - sem_post(¶m_sem); + //sem_post(¶m_sem); } /** assert that the parameter store is locked */ From 3a49ec9eb1c5daa8b2c9cd3e9f88d18f19ef66a8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 19 Sep 2013 07:55:34 +0200 Subject: [PATCH 13/13] Hotfix: Guard against corrupted param files, still boot the system if they occur --- ROMFS/px4fmu_common/init.d/rcS | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 040b5d594a..07ccbb3f48 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -84,7 +84,12 @@ then param select /fs/microsd/params if [ -f /fs/microsd/params ] then - param load /fs/microsd/params + if param load /fs/microsd/params + then + echo "Parameters loaded" + else + echo "Parameter file corrupt - ignoring" + fi fi #fi