diff options
76 files changed, 2183 insertions, 489 deletions
diff --git a/NuttX b/NuttX -Subproject 088146b90eee5b614ea6386a64dae343a49a517 +Subproject 41fffa0df192e4a26d2325c7ac5d9b5d7ba0211 diff --git a/ROMFS/px4fmu_common/init.d/3031_phantom b/ROMFS/px4fmu_common/init.d/3031_phantom index 31dfe7100..53c48d8aa 100644 --- a/ROMFS/px4fmu_common/init.d/3031_phantom +++ b/ROMFS/px4fmu_common/init.d/3031_phantom @@ -30,6 +30,9 @@ then param set FW_RR_P 0.08 param set FW_R_LIM 50 param set FW_R_RMAX 0 + # Bottom of bay and nominal zero-pitch attitude differ + # the payload bay is pitched up about 7 degrees + param set SENS_BOARD_Y_OFF 7.0 fi set MIXER phantom diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index a4c1e832d..3714b612f 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -8,3 +8,5 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper + +set FAILSAFE "-c567 -p 1000" diff --git a/ROMFS/px4fmu_common/init.d/rc.interface b/ROMFS/px4fmu_common/init.d/rc.interface index 1de0abb58..e44cd0953 100644 --- a/ROMFS/px4fmu_common/init.d/rc.interface +++ b/ROMFS/px4fmu_common/init.d/rc.interface @@ -77,4 +77,9 @@ then pwm max -c $PWM_OUTPUTS -p $PWM_MAX fi fi + + if [ $FAILSAFE != none ] + then + pwm failsafe -d $OUTPUT_DEV $FAILSAFE + fi fi diff --git a/ROMFS/px4fmu_common/init.d/rc.uavcan b/ROMFS/px4fmu_common/init.d/rc.uavcan new file mode 100644 index 000000000..9a470a6b8 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/rc.uavcan @@ -0,0 +1,18 @@ +#!nsh +# +# UAVCAN initialization script. +# + +if param compare UAVCAN_ENABLE 1 +then + if uavcan start + then + # First sensor publisher to initialize takes lowest instance ID + # This delay ensures that UAVCAN-interfaced sensors would be allocated on lowest instance IDs + sleep 1 + echo "[init] UAVCAN started" + else + echo "[init] ERROR: Could not start UAVCAN" + tone_alarm $TUNE_OUT_ERROR + fi +fi diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index c9e6a27ca..ea04ece34 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -66,6 +66,9 @@ then # sercon + # Try to get an USB console + nshterm /dev/ttyACM0 & + # # Start the ORB (first app to start) # @@ -96,11 +99,9 @@ then # if rgbled start then - echo "[init] RGB Led" else if blinkm start then - echo "[init] BlinkM" blinkm systemstate fi fi @@ -129,6 +130,7 @@ then set LOAD_DEFAULT_APPS yes set GPS yes set GPS_FAKE no + set FAILSAFE none # # Set DO_AUTOCONFIG flag to use it in AUTOSTART scripts @@ -279,9 +281,6 @@ then fi fi - # Try to get an USB console - nshterm /dev/ttyACM0 & - # # Start the datamanager (and do not abort boot if it fails) # @@ -304,11 +303,10 @@ then then if [ $OUTPUT_MODE == uavcan_esc ] then - if uavcan start 1 + if param compare UAVCAN_ENABLE 0 then - echo "CAN UP" - else - echo "CAN ERR" + echo "[init] OVERRIDING UAVCAN_ENABLE = 1" + param set UAVCAN_ENABLE 1 fi fi @@ -448,6 +446,11 @@ then mavlink start $MAVLINK_FLAGS # + # UAVCAN + # + sh /etc/init.d/rc.uavcan + + # # Sensors, Logging, GPS # sh /etc/init.d/rc.sensors diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix index 0ec663e35..7fed488af 100644 --- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix +++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix @@ -64,21 +64,22 @@ O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Gimbal / flaps / payload mixer for last four channels, +using the payload control group ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 2 2 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 +S: 2 3 10000 10000 0 -10000 10000 diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix index 79c4447be..5a0381bd8 100755 --- a/ROMFS/px4fmu_common/mixers/Viper.mix +++ b/ROMFS/px4fmu_common/mixers/Viper.mix @@ -52,21 +52,20 @@ M: 1 O: 10000 10000 0 -10000 10000 S: 0 3 0 20000 -10000 -10000 10000 -Gimbal / flaps / payload mixer for last four channels +Inputs to the mixer come from channel group 2 (payload), channels 0 +(bay servo 1), 1 (bay servo 2) and 3 (drop release). ----------------------------------------------------- M: 1 O: 10000 10000 0 -10000 10000 -S: 0 4 10000 10000 0 -10000 10000 +S: 2 0 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 5 10000 10000 0 -10000 10000 +S: 2 1 10000 10000 0 -10000 10000 M: 1 O: 10000 10000 0 -10000 10000 -S: 0 6 10000 10000 0 -10000 10000 +S: 2 2 -10000 -10000 0 -10000 10000 + -M: 1 -O: 10000 10000 0 -10000 10000 -S: 0 7 10000 10000 0 -10000 10000 diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index d8f4884bc..b46db00b5 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -178,9 +178,9 @@ class uploader(object): 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): + def __init__(self, portname, baudrate, interCharTimeout=0.001, timeout=0.5): # open the port, keep the default timeout short so we can poll quickly - self.port = serial.Serial(portname, baudrate, timeout=2.0) + self.port = serial.Serial(portname, baudrate) self.otp = b'' self.sn = b'' @@ -195,7 +195,7 @@ class uploader(object): def __recv(self, count=1): c = self.port.read(count) if len(c) < 1: - raise RuntimeError("timeout waiting for data") + raise RuntimeError("timeout waiting for data (%u bytes)", count) # print("recv " + binascii.hexlify(c)) return c diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index a7c10f52f..97eddfdd2 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -25,7 +25,6 @@ MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 MODULES += drivers/mb12xx -MODULES += drivers/ll40ls MODULES += drivers/gps MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry @@ -44,7 +43,6 @@ MODULES += modules/sensors # MODULES += systemcmds/mtd MODULES += systemcmds/bl_update -MODULES += systemcmds/i2c MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf @@ -152,5 +150,4 @@ endef # command priority stack entrypoint BUILTIN_COMMANDS := \ $(call _B, sercon, , 2048, sercon_main ) \ - $(call _B, serdis, , 2048, serdis_main ) \ - $(call _B, sysinfo, , 2048, sysinfo_main ) + $(call _B, serdis, , 2048, serdis_main ) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index d0a40445d..5f146686c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -41,6 +41,7 @@ MODULES += drivers/frsky_telemetry MODULES += modules/sensors MODULES += drivers/mkblctrl MODULES += drivers/pca8574 +MODULES += drivers/px4flow # Needs to be burned to the ground and re-written; for now, diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 -Subproject 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd5 +Subproject 2423e47b4f9169e77f7194b36fa2118e018c94e diff --git a/nuttx-configs/aerocore/nsh/defconfig b/nuttx-configs/aerocore/nsh/defconfig index 8d0bae7b9..317194f68 100644 --- a/nuttx-configs/aerocore/nsh/defconfig +++ b/nuttx-configs/aerocore/nsh/defconfig @@ -314,7 +314,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y +# CONFIG_ARCH_IRQPRIO is not set # CONFIG_CUSTOM_STACK is not set # CONFIG_ADDRENV is not set CONFIG_ARCH_HAVE_VFORK=y diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a651faffa..cd9c05b13 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -287,8 +287,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=500 +CONFIG_STM32_I2CTIMEOMS=1 # CONFIG_STM32_I2C_DUTY16_9 is not set # @@ -309,7 +308,7 @@ CONFIG_STM32_I2CTIMEOTICKS=500 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y +# CONFIG_ARCH_IRQPRIO is not set # CONFIG_CUSTOM_STACK is not set # CONFIG_ADDRENV is not set CONFIG_ARCH_HAVE_VFORK=y diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index a55c95a29..e31f40cd2 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -323,8 +323,7 @@ CONFIG_STM32_USART_SINGLEWIRE=y # # CONFIG_STM32_I2C_DYNTIMEO is not set CONFIG_STM32_I2CTIMEOSEC=0 -CONFIG_STM32_I2CTIMEOMS=10 -CONFIG_STM32_I2CTIMEOTICKS=500 +CONFIG_STM32_I2CTIMEOMS=1 # CONFIG_STM32_I2C_DUTY16_9 is not set # @@ -350,7 +349,7 @@ CONFIG_SDIO_PRI=128 # CONFIG_ARCH_NOINTC is not set # CONFIG_ARCH_VECNOTIRQ is not set CONFIG_ARCH_DMA=y -CONFIG_ARCH_IRQPRIO=y +# CONFIG_ARCH_IRQPRIO is not set # CONFIG_CUSTOM_STACK is not set # CONFIG_ADDRENV is not set CONFIG_ARCH_HAVE_VFORK=y diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 3785e0367..7c76be7ec 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -83,7 +83,6 @@ CONFIG_ARCH_BOARD="px4io-v1" CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_DRAM_SIZE=0x00002000 CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_IRQPRIO=y CONFIG_ARCH_INTERRUPTSTACK=n CONFIG_ARCH_STACKDUMP=y CONFIG_ARCH_BOOTLOADER=n @@ -134,6 +133,8 @@ CONFIG_STM32_USART2=y CONFIG_STM32_USART3=y CONFIG_STM32_I2C1=y CONFIG_STM32_I2C2=n +CONFIG_STM32_I2CTIMEOSEC=0 +CONFIG_STM32_I2CTIMEOMS=1 CONFIG_STM32_BKP=n CONFIG_STM32_PWR=n CONFIG_STM32_DAC=n diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index e6563e587..02b51e3d7 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -79,7 +79,6 @@ CONFIG_ARCH_BOARD_PX4IO_V2=y CONFIG_BOARD_LOOPSPERMSEC=2000 CONFIG_DRAM_SIZE=0x00002000 CONFIG_DRAM_START=0x20000000 -CONFIG_ARCH_IRQPRIO=y CONFIG_ARCH_INTERRUPTSTACK=n CONFIG_ARCH_STACKDUMP=y CONFIG_ARCH_BOOTLOADER=n diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 41942aacd..293690d27 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -165,7 +165,7 @@ Airspeed::probe() */ _retries = 4; int ret = measure(); - _retries = 2; + _retries = 0; return ret; } @@ -381,7 +381,10 @@ Airspeed::cycle_trampoline(void *arg) Airspeed *dev = (Airspeed *)arg; dev->cycle(); - dev->update_status(); + // XXX we do not know if this is + // really helping - do not update the + // subsys state right now + //dev->update_status(); } void diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 84815fdfb..5aff6825b 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -95,6 +95,11 @@ __BEGIN_DECLS #define PWM_LOWEST_MAX 1700 /** + * Do not output a channel with this value + */ +#define PWM_IGNORE_THIS_CHANNEL UINT16_MAX + +/** * Servo output signal type, value is actual servo output pulse * width in microseconds. */ diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index 76ec55c3e..ab640837b 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -46,37 +46,6 @@ #define PX4FLOW_DEVICE_PATH "/dev/px4flow" -/** - * @addtogroup topics - * @{ - */ - -/** - * Optical flow in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - * - * @warning If possible the usage of the raw flow and performing rotation-compensation - * using the autopilot angular rate estimate is recommended. - */ -struct px4flow_report { - - uint64_t timestamp; /**< in microseconds since system start */ - - int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ - int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ - -}; - -/** - * @} - */ - /* * ObjDev tag for px4flow data. */ diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index f98d615a2..0f77bb805 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -155,7 +155,6 @@ ETSAirspeed::collect() } uint16_t diff_pres_pa_raw = val[1] << 8 | val[0]; - uint16_t diff_pres_pa; if (diff_pres_pa_raw == 0) { // a zero value means the pressure sensor cannot give us a // value. We need to return, and not report a value or the @@ -166,28 +165,21 @@ ETSAirspeed::collect() return -1; } - if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { - diff_pres_pa = 0; - } else { - diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset; - } - // The raw value still should be compensated for the known offset diff_pres_pa_raw -= _diff_pres_offset; // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_pres_pa; + if (diff_pres_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_pres_pa_raw; } differential_pressure_s report; report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); - report.differential_pressure_pa = (float)diff_pres_pa; // XXX we may want to smooth out the readings to remove noise. - report.differential_pressure_filtered_pa = (float)diff_pres_pa; - report.differential_pressure_raw_pa = (float)diff_pres_pa_raw; + report.differential_pressure_filtered_pa = diff_pres_pa_raw; + report.differential_pressure_raw_pa = diff_pres_pa_raw; report.temperature = -1000.0f; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -369,7 +361,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -394,7 +386,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %f pa", (double)report.differential_pressure_filtered_pa); } /* reset the sensor polling to its default rate */ diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 0e9a961ac..e4ecfa6b5 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -1283,14 +1283,13 @@ int HMC5883::set_excitement(unsigned enable) if (OK != ret) perf_count(_comms_errors); + _conf_reg &= ~0x03; // reset previous excitement mode if (((int)enable) < 0) { _conf_reg |= 0x01; } else if (enable > 0) { _conf_reg |= 0x02; - } else { - _conf_reg &= ~0x03; } // ::printf("set_excitement enable=%d regA=0x%x\n", (int)enable, (unsigned)_conf_reg); diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 159706278..1d9a463ad 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -228,44 +228,23 @@ MEASAirspeed::collect() // the raw value still should be compensated for the known offset diff_press_pa_raw -= _diff_pres_offset; - float diff_press_pa = fabsf(diff_press_pa_raw); - /* - note that we return both the absolute value with offset - applied and a raw value without the offset applied. This - makes it possible for higher level code to detect if the - user has the tubes connected backwards, and also makes it - possible to correctly use offsets calculated by a higher - level airspeed driver. - With the above calculation the MS4525 sensor will produce a positive number when the top port is used as a dynamic port and bottom port is used as the static port - - Also note that the _diff_pres_offset is applied before the - fabsf() not afterwards. It needs to be done this way to - prevent a bias at low speeds, but this also means that when - setting a offset you must set it based on the raw value, not - the offset value */ - + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ - if (diff_press_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_press_pa; + if (diff_press_pa_raw > _max_differential_pressure_pa) { + _max_differential_pressure_pa = diff_press_pa_raw; } report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; - report.differential_pressure_pa = diff_press_pa; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); - - /* the dynamics of the filter can make it overshoot into the negative range */ - if (report.differential_pressure_filtered_pa < 0.0f) { - report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); - } + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw); report.differential_pressure_raw_pa = diff_press_pa_raw; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -345,7 +324,7 @@ MEASAirspeed::cycle() /** correct for 5V rail voltage if the system_power ORB topic is available - + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of offset versus voltage for 3 sensors */ @@ -394,7 +373,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) if (voltage_diff < -1.0f) { voltage_diff = -1.0f; } - temperature -= voltage_diff * temp_slope; + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } @@ -523,7 +502,7 @@ test() } warnx("single read"); - warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -551,7 +530,7 @@ test() } warnx("periodic read %u", i); - warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_filtered_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 873fa62c4..889643d0d 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -130,7 +130,7 @@ protected: float _T; /* altitude conversion calibration */ - unsigned _msl_pressure; /* in kPa */ + unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; @@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg) irqrestore(flags); return -ENOMEM; } - irqrestore(flags); + irqrestore(flags); return OK; } diff --git a/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt new file mode 100644 index 000000000..9c5eb42eb --- /dev/null +++ b/src/drivers/pca9685/arduino_Adafruit_PWM_Servo_Driver_Library_license.txt @@ -0,0 +1,29 @@ +The following license agreement covers re-used code from the arduino driver +for the Adafruit I2C to PWM converter. + +Software License Agreement (BSD License) + +Copyright (c) 2012, Adafruit Industries +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: +1. Redistributions of source code must retain the above copyright +notice, this list of conditions and the following disclaimer. +2. Redistributions in binary form must reproduce the above copyright +notice, this list of conditions and the following disclaimer in the +documentation and/or other materials provided with the distribution. +3. Neither the name of the copyright holders nor the +names of its contributors may be used to endorse or promote products +derived from this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS ''AS IS'' AND ANY +EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY +DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/drivers/pca9685/module.mk b/src/drivers/pca9685/module.mk new file mode 100644 index 000000000..7a5c7996e --- /dev/null +++ b/src/drivers/pca9685/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Driver for the PCA9685 I2C PWM controller +# The chip is used on the adafruit I2C PWM converter, +# which allows to control servos via I2C. +# https://www.adafruit.com/product/815 + +MODULE_COMMAND = pca9685 + +SRCS = pca9685.cpp diff --git a/src/drivers/pca9685/pca9685.cpp b/src/drivers/pca9685/pca9685.cpp new file mode 100644 index 000000000..6f69ce8a1 --- /dev/null +++ b/src/drivers/pca9685/pca9685.cpp @@ -0,0 +1,651 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file pca9685.cpp + * + * Driver for the PCA9685 I2C PWM module + * The chip is used on the Adafruit I2C/PWM converter https://www.adafruit.com/product/815 + * + * Parts of the code are adapted from the arduino library for the board + * https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library + * for the license of these parts see the + * arduino_Adafruit_PWM_Servo_Driver_Library_license.txt file + * see https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library for contributors + * + * @author Thomas Gubler <thomasgubler@gmail.com> + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <string.h> +#include <stdlib.h> +#include <stdbool.h> +#include <fcntl.h> +#include <unistd.h> +#include <stdio.h> +#include <ctype.h> +#include <math.h> + +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> +#include <systemlib/systemlib.h> + +#include <uORB/uORB.h> +#include <uORB/topics/actuator_controls.h> + +#include <board_config.h> +#include <drivers/drv_io_expander.h> + +#define PCA9685_SUBADR1 0x2 +#define PCA9685_SUBADR2 0x3 +#define PCA9685_SUBADR3 0x4 + +#define PCA9685_MODE1 0x0 +#define PCA9685_PRESCALE 0xFE + +#define LED0_ON_L 0x6 +#define LED0_ON_H 0x7 +#define LED0_OFF_L 0x8 +#define LED0_OFF_H 0x9 + +#define ALLLED_ON_L 0xFA +#define ALLLED_ON_H 0xFB +#define ALLLED_OFF_L 0xFC +#define ALLLED_OF + +#define ADDR 0x40 // I2C adress + +#define PCA9685_DEVICE_PATH "/dev/pca9685" +#define PCA9685_BUS PX4_I2C_BUS_EXPANSION +#define PCA9685_PWMFREQ 60.0f +#define PCA9685_NCHANS 16 // total amount of pwm outputs + +#define PCA9685_PWMMIN 150 // this is the 'minimum' pulse length count (out of 4096) +#define PCA9685_PWMMAX 600 // this is the 'maximum' pulse length count (out of 4096)_PWMFREQ 60.0f + +#define PCA9685_PWMCENTER ((PCA9685_PWMMAX + PCA9685_PWMMIN)/2) +#define PCA9685_MAXSERVODEG 90.0f /* maximal servo deflection in degrees + PCA9685_PWMMIN <--> -PCA9685_MAXSERVODEG + PCA9685_PWMMAX <--> PCA9685_MAXSERVODEG + */ +#define PCA9685_SCALE ((PCA9685_PWMMAX - PCA9685_PWMCENTER)/(M_DEG_TO_RAD_F * PCA9685_MAXSERVODEG)) // scales from rad to PWM + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +class PCA9685 : public device::I2C +{ +public: + PCA9685(int bus=PCA9685_BUS, uint8_t address=ADDR); + virtual ~PCA9685(); + + + virtual int init(); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + virtual int info(); + virtual int reset(); + bool is_running() { return _running; } + +private: + work_s _work; + + + enum IOX_MODE _mode; + bool _running; + int _i2cpwm_interval; + bool _should_run; + perf_counter_t _comms_errors; + + uint8_t _msg[6]; + + int _actuator_controls_sub; + struct actuator_controls_s _actuator_controls; + uint16_t _current_values[NUM_ACTUATOR_CONTROLS]; /**< stores the current pwm output + values as sent to the setPin() */ + + bool _mode_on_initialized; /** Set to true after the first call of i2cpwm in mode IOX_MODE_ON */ + + static void i2cpwm_trampoline(void *arg); + void i2cpwm(); + + /** + * Helper function to set the pwm frequency + */ + int setPWMFreq(float freq); + + /** + * Helper function to set the demanded pwm value + * @param num pwm output number + */ + int setPWM(uint8_t num, uint16_t on, uint16_t off); + + /** + * Sets pin without having to deal with on/off tick placement and properly handles + * a zero value as completely off. Optional invert parameter supports inverting + * the pulse for sinking to ground. + * @param num pwm output number + * @param val should be a value from 0 to 4095 inclusive. + */ + int setPin(uint8_t num, uint16_t val, bool invert = false); + + + /* Wrapper to read a byte from addr */ + int read8(uint8_t addr, uint8_t &value); + + /* Wrapper to wite a byte to addr */ + int write8(uint8_t addr, uint8_t value); + +}; + +/* for now, we only support one board */ +namespace +{ +PCA9685 *g_pca9685; +} + +void pca9685_usage(); + +extern "C" __EXPORT int pca9685_main(int argc, char *argv[]); + +PCA9685::PCA9685(int bus, uint8_t address) : + I2C("pca9685", PCA9685_DEVICE_PATH, bus, address, 100000), + _mode(IOX_MODE_OFF), + _running(false), + _i2cpwm_interval(SEC2TICK(1.0f/60.0f)), + _should_run(false), + _comms_errors(perf_alloc(PC_COUNT, "actuator_controls_2_comms_errors")), + _actuator_controls_sub(-1), + _actuator_controls(), + _mode_on_initialized(false) +{ + memset(&_work, 0, sizeof(_work)); + memset(_msg, 0, sizeof(_msg)); + memset(_current_values, 0, sizeof(_current_values)); +} + +PCA9685::~PCA9685() +{ +} + +int +PCA9685::init() +{ + int ret; + ret = I2C::init(); + if (ret != OK) { + return ret; + } + + ret = reset(); + if (ret != OK) { + return ret; + } + + ret = setPWMFreq(PCA9685_PWMFREQ); + + return ret; +} + +int +PCA9685::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + int ret = -EINVAL; + switch (cmd) { + + case IOX_SET_MODE: + + if (_mode != (IOX_MODE)arg) { + + switch ((IOX_MODE)arg) { + case IOX_MODE_OFF: + warnx("shutting down"); + break; + case IOX_MODE_ON: + warnx("starting"); + break; + case IOX_MODE_TEST_OUT: + warnx("test starting"); + break; + + default: + return -1; + } + + _mode = (IOX_MODE)arg; + } + + // if not active, kick it + if (!_running) { + _running = true; + work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, 1); + } + + + return OK; + + default: + // see if the parent class can make any use of it + ret = CDev::ioctl(filp, cmd, arg); + break; + } + + return ret; +} + +int +PCA9685::info() +{ + int ret = OK; + + if (is_running()) { + warnx("Driver is running, mode: %u", _mode); + } else { + warnx("Driver started but not running"); + } + + return ret; +} + +void +PCA9685::i2cpwm_trampoline(void *arg) +{ + PCA9685 *i2cpwm = reinterpret_cast<PCA9685 *>(arg); + + i2cpwm->i2cpwm(); +} + +/** + * Main loop function + */ +void +PCA9685::i2cpwm() +{ + if (_mode == IOX_MODE_TEST_OUT) { + setPin(0, PCA9685_PWMCENTER); + _should_run = true; + } else if (_mode == IOX_MODE_OFF) { + _should_run = false; + } else { + if (!_mode_on_initialized) { + /* Subscribe to actuator control 2 (payload group for gimbal) */ + _actuator_controls_sub = orb_subscribe(ORB_ID(actuator_controls_2)); + /* set the uorb update interval lower than the driver pwm interval */ + orb_set_interval(_actuator_controls_sub, 1000.0f / PCA9685_PWMFREQ - 5); + + _mode_on_initialized = true; + } + + /* Read the servo setpoints from the actuator control topics (gimbal) */ + bool updated; + orb_check(_actuator_controls_sub, &updated); + if (updated) { + orb_copy(ORB_ID(actuator_controls_2), _actuator_controls_sub, &_actuator_controls); + for (int i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { + /* Scale the controls to PWM, first multiply by pi to get rad, + * the control[i] values are on the range -1 ... 1 */ + uint16_t new_value = PCA9685_PWMCENTER + + (_actuator_controls.control[i] * M_PI_F * PCA9685_SCALE); + debug("%d: current: %u, new %u, control %.2f", i, _current_values[i], new_value, + (double)_actuator_controls.control[i]); + if (new_value != _current_values[i] && + isfinite(new_value) && + new_value >= PCA9685_PWMMIN && + new_value <= PCA9685_PWMMAX) { + /* This value was updated, send the command to adjust the PWM value */ + setPin(i, new_value); + _current_values[i] = new_value; + } + } + } + _should_run = true; + } + + // check if any activity remains, else stop + if (!_should_run) { + _running = false; + return; + } + + // re-queue ourselves to run again later + _running = true; + work_queue(LPWORK, &_work, (worker_t)&PCA9685::i2cpwm_trampoline, this, _i2cpwm_interval); +} + +int +PCA9685::setPWM(uint8_t num, uint16_t on, uint16_t off) +{ + int ret; + /* convert to correct message */ + _msg[0] = LED0_ON_L + 4 * num; + _msg[1] = on; + _msg[2] = on >> 8; + _msg[3] = off; + _msg[4] = off >> 8; + + /* try i2c transfer */ + ret = transfer(_msg, 5, nullptr, 0); + + if (OK != ret) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + + return ret; +} + +int +PCA9685::setPin(uint8_t num, uint16_t val, bool invert) +{ + // Clamp value between 0 and 4095 inclusive. + if (val > 4095) { + val = 4095; + } + if (invert) { + if (val == 0) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 4095) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, 4095-val); + } + } else { + if (val == 4095) { + // Special value for signal fully on. + return setPWM(num, 4096, 0); + } else if (val == 0) { + // Special value for signal fully off. + return setPWM(num, 0, 4096); + } else { + return setPWM(num, 0, val); + } + } + + return ERROR; +} + +int +PCA9685::setPWMFreq(float freq) +{ + int ret = OK; + freq *= 0.9f; /* Correct for overshoot in the frequency setting (see issue + https://github.com/adafruit/Adafruit-PWM-Servo-Driver-Library/issues/11). */ + float prescaleval = 25000000; + prescaleval /= 4096; + prescaleval /= freq; + prescaleval -= 1; + uint8_t prescale = uint8_t(prescaleval + 0.5f); //implicit floor() + uint8_t oldmode; + ret = read8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + uint8_t newmode = (oldmode&0x7F) | 0x10; // sleep + + ret = write8(PCA9685_MODE1, newmode); // go to sleep + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_PRESCALE, prescale); // set the prescaler + if (ret != OK) { + return ret; + } + ret = write8(PCA9685_MODE1, oldmode); + if (ret != OK) { + return ret; + } + + usleep(5000); //5ms delay (from arduino driver) + + ret = write8(PCA9685_MODE1, oldmode | 0xa1); // This sets the MODE1 register to turn on auto increment. + if (ret != OK) { + return ret; + } + + return ret; +} + +/* Wrapper to read a byte from addr */ +int +PCA9685::read8(uint8_t addr, uint8_t &value) +{ + int ret = OK; + + /* send addr */ + ret = transfer(&addr, sizeof(addr), nullptr, 0); + if (ret != OK) { + goto fail_read; + } + + /* get value */ + ret = transfer(nullptr, 0, &value, 1); + if (ret != OK) { + goto fail_read; + } + + return ret; + +fail_read: + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + + return ret; +} + +int PCA9685::reset(void) { + warnx("resetting"); + return write8(PCA9685_MODE1, 0x0); +} + +/* Wrapper to wite a byte to addr */ +int +PCA9685::write8(uint8_t addr, uint8_t value) { + int ret = OK; + _msg[0] = addr; + _msg[1] = value; + /* send addr and value */ + ret = transfer(_msg, 2, nullptr, 0); + if (ret != OK) { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + } + return ret; +} + +void +pca9685_usage() +{ + warnx("missing command: try 'start', 'test', 'stop', 'info'"); + warnx("options:"); + warnx(" -b i2cbus (%d)", PX4_I2C_BUS_EXPANSION); + warnx(" -a addr (0x%x)", ADDR); +} + +int +pca9685_main(int argc, char *argv[]) +{ + int i2cdevice = -1; + int i2caddr = ADDR; // 7bit + + int ch; + + // jump over start/off/etc and look at options first + while ((ch = getopt(argc, argv, "a:b:")) != EOF) { + switch (ch) { + case 'a': + i2caddr = strtol(optarg, NULL, 0); + break; + + case 'b': + i2cdevice = strtol(optarg, NULL, 0); + break; + + default: + pca9685_usage(); + exit(0); + } + } + + if (optind >= argc) { + pca9685_usage(); + exit(1); + } + + const char *verb = argv[optind]; + + int fd; + int ret; + + if (!strcmp(verb, "start")) { + if (g_pca9685 != nullptr) { + errx(1, "already started"); + } + + if (i2cdevice == -1) { + // try the external bus first + i2cdevice = PX4_I2C_BUS_EXPANSION; + g_pca9685 = new PCA9685(PX4_I2C_BUS_EXPANSION, i2caddr); + + if (g_pca9685 != nullptr && OK != g_pca9685->init()) { + delete g_pca9685; + g_pca9685 = nullptr; + } + + if (g_pca9685 == nullptr) { + errx(1, "init failed"); + } + } + + if (g_pca9685 == nullptr) { + g_pca9685 = new PCA9685(i2cdevice, i2caddr); + + if (g_pca9685 == nullptr) { + errx(1, "new failed"); + } + + if (OK != g_pca9685->init()) { + delete g_pca9685; + g_pca9685 = nullptr; + errx(1, "init failed"); + } + } + fd = open(PCA9685_DEVICE_PATH, 0); + if (fd == -1) { + errx(1, "Unable to open " PCA9685_DEVICE_PATH); + } + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_ON); + close(fd); + + + exit(0); + } + + // need the driver past this point + if (g_pca9685 == nullptr) { + warnx("not started, run pca9685 start"); + exit(1); + } + + if (!strcmp(verb, "info")) { + g_pca9685->info(); + exit(0); + } + + if (!strcmp(verb, "reset")) { + g_pca9685->reset(); + exit(0); + } + + + if (!strcmp(verb, "test")) { + fd = open(PCA9685_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA9685_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_TEST_OUT); + + close(fd); + exit(ret); + } + + if (!strcmp(verb, "stop")) { + fd = open(PCA9685_DEVICE_PATH, 0); + + if (fd == -1) { + errx(1, "Unable to open " PCA9685_DEVICE_PATH); + } + + ret = ioctl(fd, IOX_SET_MODE, (unsigned long)IOX_MODE_OFF); + close(fd); + + // wait until we're not running any more + for (unsigned i = 0; i < 15; i++) { + if (!g_pca9685->is_running()) { + break; + } + + usleep(50000); + printf("."); + fflush(stdout); + } + printf("\n"); + fflush(stdout); + + if (!g_pca9685->is_running()) { + delete g_pca9685; + g_pca9685= nullptr; + warnx("stopped, exiting"); + exit(0); + } else { + warnx("stop failed."); + exit(1); + } + } + + pca9685_usage(); + exit(0); +} diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index f214b5964..60ad3c1af 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -37,7 +37,7 @@ * * Driver for the PX4FLOW module connected via I2C. */ - + #include <nuttx/config.h> #include <drivers/device/i2c.h> @@ -68,7 +68,7 @@ #include <uORB/uORB.h> #include <uORB/topics/subsystem_info.h> -//#include <uORB/topics/optical_flow.h> +#include <uORB/topics/optical_flow.h> #include <board_config.h> @@ -80,7 +80,7 @@ /* PX4FLOW Registers addresses */ #define PX4FLOW_REG 0x00 /* Measure Register */ -#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz +#define PX4FLOW_CONVERSION_INTERVAL 8000 /* 8ms 125Hz */ /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -115,17 +115,17 @@ class PX4FLOW : public device::I2C public: PX4FLOW(int bus = PX4FLOW_BUS, int address = I2C_FLOW_ADDRESS); virtual ~PX4FLOW(); - + virtual int init(); - + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); - + /** * Diagnostics - print some basic information about the driver. */ void print_info(); - + protected: virtual int probe(); @@ -136,13 +136,13 @@ private: bool _sensor_ok; int _measure_ticks; bool _collect_phase; - + orb_advert_t _px4flow_topic; perf_counter_t _sample_perf; perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; - + /** * Test whether the device supported by the driver is present at a * specific address. @@ -151,7 +151,7 @@ private: * @return True if the device is present. */ int probe_address(uint8_t address); - + /** * Initialise the automatic measurement state machine and start it. * @@ -159,12 +159,12 @@ private: * to make it more aggressive about resetting the bus in case of errors. */ void start(); - + /** * Stop the automatic measurement state machine. */ void stop(); - + /** * Perform a poll cycle; collect from the previous measurement * and start a new one. @@ -179,8 +179,8 @@ private: * @param arg Instance pointer for the driver that is polling. */ static void cycle_trampoline(void *arg); - - + + }; /* @@ -201,7 +201,7 @@ PX4FLOW::PX4FLOW(int bus, int address) : { // enable debug() calls _debug_enabled = true; - + // work_cancel in the dtor will explode if we don't do this... memset(&_work, 0, sizeof(_work)); } @@ -226,13 +226,13 @@ PX4FLOW::init() goto out; /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(px4flow_report)); + _reports = new RingBuffer(2, sizeof(struct optical_flow_s)); if (_reports == nullptr) goto out; /* get a publish handle on the px4flow topic */ - struct px4flow_report zero_report; + struct optical_flow_s zero_report; memset(&zero_report, 0, sizeof(zero_report)); _px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report); @@ -323,24 +323,24 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } case SENSORIOCGQUEUEDEPTH: return _reports->size(); - + case SENSORIOCRESET: /* XXX implement this */ return -EINVAL; - + default: /* give it to the superclass */ return I2C::ioctl(filp, cmd, arg); @@ -350,8 +350,8 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg) ssize_t PX4FLOW::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct px4flow_report); - struct px4flow_report *rbuf = reinterpret_cast<struct px4flow_report *>(buffer); + unsigned count = buflen / sizeof(struct optical_flow_s); + struct optical_flow_s *rbuf = reinterpret_cast<struct optical_flow_s *>(buffer); int ret = 0; /* buffer must be large enough */ @@ -425,7 +425,7 @@ PX4FLOW::measure() return ret; } ret = OK; - + return ret; } @@ -433,14 +433,14 @@ int PX4FLOW::collect() { int ret = -EIO; - + /* read from the sensor */ uint8_t val[22] = {0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0,0, 0}; - + perf_begin(_sample_perf); - + ret = transfer(nullptr, 0, &val[0], 22); - + if (ret < 0) { log("error reading from sensor: %d", ret); @@ -448,7 +448,7 @@ PX4FLOW::collect() perf_end(_sample_perf); return ret; } - + // f.frame_count = val[1] << 8 | val[0]; // f.pixel_flow_x_sum= val[3] << 8 | val[2]; // f.pixel_flow_y_sum= val[5] << 8 | val[4]; @@ -466,7 +466,7 @@ PX4FLOW::collect() int16_t flowcy = val[9] << 8 | val[8]; int16_t gdist = val[21] << 8 | val[20]; - struct px4flow_report report; + struct optical_flow_s report; report.flow_comp_x_m = float(flowcx)/1000.0f; report.flow_comp_y_m = float(flowcy)/1000.0f; report.flow_raw_x= val[3] << 8 | val[2]; @@ -503,7 +503,7 @@ PX4FLOW::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&PX4FLOW::cycle_trampoline, this, 1); - + /* notify about state change */ struct subsystem_info_s info = { true, @@ -644,7 +644,7 @@ start() fail: - if (g_dev != nullptr) + if (g_dev != nullptr) { delete g_dev; g_dev = nullptr; @@ -678,7 +678,7 @@ void stop() void test() { - struct px4flow_report report; + struct optical_flow_s report; ssize_t sz; int ret; @@ -777,7 +777,7 @@ px4flow_main(int argc, char *argv[]) */ if (!strcmp(argv[1], "start")) px4flow::start(); - + /* * Stop the driver */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 82977a032..122a3cd17 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1272,7 +1272,9 @@ PX4FMU::write(file *filp, const char *buffer, size_t len) memcpy(values, buffer, count * 2); for (uint8_t i = 0; i < count; i++) { - up_pwm_servo_set(i, values[i]); + if (values[i] != PWM_IGNORE_THIS_CHANNEL) { + up_pwm_servo_set(i, values[i]); + } } return count * 2; diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 32069cf09..97919538f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1968,7 +1968,7 @@ PX4IO::print_status(bool extended_status) printf("actuators"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i))); printf("\n"); printf("servos"); diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index bca1715fa..d382d08d0 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -520,11 +520,9 @@ SF0X::collect() /* clear buffer if last read was too long ago */ uint64_t read_elapsed = hrt_elapsed_time(&_last_read); + /* timed out - retry */ if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) { _linebuf_index = 0; - } else if (_linebuf_index > 0) { - /* increment to next read position */ - _linebuf_index++; } /* the buffer for read chars is buflen minus null termination */ @@ -550,18 +548,19 @@ SF0X::collect() return -EAGAIN; } - /* we did increment the index to the next position already, so just add the additional fields */ - _linebuf_index += (ret - 1); + /* let the write pointer point to the next free entry */ + _linebuf_index += ret; _last_read = hrt_absolute_time(); - if (_linebuf_index < 1) { - /* we need at least the two end bytes to make sense of this string */ + /* require a reasonable amount of minimum bytes */ + if (_linebuf_index < 6) { + /* we need at this format: x.xx\r\n */ return -EAGAIN; - } else if (_linebuf[_linebuf_index - 1] != '\r' || _linebuf[_linebuf_index] != '\n') { + } else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') { - if (_linebuf_index >= readlen - 1) { + if (_linebuf_index == readlen) { /* we have a full buffer, but no line ending - abort */ _linebuf_index = 0; perf_count(_comms_errors); @@ -577,9 +576,7 @@ SF0X::collect() bool valid; /* enforce line ending */ - unsigned lend = (_linebuf_index < (sizeof(_linebuf) - 1)) ? _linebuf_index : (sizeof(_linebuf) - 1); - - _linebuf[lend] = '\0'; + _linebuf[_linebuf_index] = '\0'; if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') { si_units = -1.0f; @@ -591,14 +588,16 @@ SF0X::collect() valid = false; /* wipe out partially read content from last cycle(s), check for dot */ - for (unsigned i = 0; i < (lend - 2); i++) { + for (unsigned i = 0; i < (_linebuf_index - 2); i++) { if (_linebuf[i] == '\n') { - char buf[sizeof(_linebuf)]; - memcpy(buf, &_linebuf[i+1], (lend + 1) - (i + 1)); - memcpy(_linebuf, buf, (lend + 1) - (i + 1)); + /* wipe out any partial measurements */ + for (unsigned j = 0; j <= i; j++) { + _linebuf[j] = ' '; + } } - if (_linebuf[i] == '.') { + /* we need a digit before the dot and a dot for a valid number */ + if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) { valid = true; } } @@ -606,7 +605,7 @@ SF0X::collect() if (valid) { si_units = strtod(_linebuf, &end); - /* we require at least 3 characters for a valid number */ + /* we require at least four characters for a valid number */ if (end > _linebuf + 3) { valid = true; } else { @@ -616,7 +615,7 @@ SF0X::collect() } } - debug("val (float): %8.4f, raw: %s, valid: %s\n", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); + debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO")); /* done with this chunk, resetting - even if invalid */ _linebuf_index = 0; @@ -708,12 +707,12 @@ SF0X::cycle() int collect_ret = collect(); if (collect_ret == -EAGAIN) { - /* reschedule to grab the missing bits, time to transmit 10 bytes @9600 bps */ + /* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */ work_queue(HPWORK, &_work, (worker_t)&SF0X::cycle_trampoline, this, - USEC2TICK(1100)); + USEC2TICK(1042 * 8)); return; } diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 46db788a6..926a8db2a 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -169,7 +169,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -190,7 +190,9 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = _bodyrate_setpoint * _k_ff * scaler + + _rate_error * _k_p * scaler * scaler + + integrator_constrained; //scaler is proportional to 1/airspeed // warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p); // warnx("roll: _last_output %.4f", (double)_last_output); return math::constrain(_last_output, -1.0f, 1.0f); diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 9894a34d7..94bd26f03 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -135,7 +135,7 @@ float ECL_RollController::control_bodyrate(float pitch, if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) { - float id = _rate_error * dt; + float id = _rate_error * dt * scaler; /* * anti-windup: do not allow integrator to increase if actuator is at limit @@ -157,7 +157,9 @@ float ECL_RollController::control_bodyrate(float pitch, //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max); /* Apply PI rate controller and store non-limited output */ - _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed + _last_output = _bodyrate_setpoint * _k_ff * scaler + + _rate_error * _k_p * scaler * scaler + + integrator_constrained; //scaler is proportional to 1/airspeed return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index a57a0481a..d27bf776f 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -252,6 +252,11 @@ void TECS::_update_height_demand(float demand, float state) void TECS::_detect_underspeed(void) { + if (!_detect_underspeed_enabled) { + _underspeed = false; + return; + } + if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) { _underspeed = true; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index bcc2d90e5..36ae4ecaf 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -66,6 +66,9 @@ public: _hgt_dem_prev(0.0f), _TAS_dem_adj(0.0f), _STEdotErrLast(0.0f), + _underspeed(false), + _detect_underspeed_enabled(true), + _badDescent(false), _climbOutDem(false), _SPE_dem(0.0f), _SKE_dem(0.0f), @@ -221,6 +224,10 @@ public: _speedrate_p = speedrate_p; } + void set_detect_underspeed_enabled(bool enabled) { + _detect_underspeed_enabled = enabled; + } + private: struct tecs_state _tecs_state; @@ -323,6 +330,9 @@ private: // Underspeed condition bool _underspeed; + // Underspeed detection enabled + bool _detect_underspeed_enabled; + // Bad descent condition caused by unachievable airspeed demand bool _badDescent; diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp index c555a0a69..9d479832f 100644 --- a/src/lib/launchdetection/CatapultLaunchMethod.cpp +++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp @@ -66,10 +66,10 @@ void CatapultLaunchMethod::update(float accel_x) last_timestamp = hrt_absolute_time(); if (accel_x > threshold_accel.get()) { - integrator += accel_x * dt; + integrator += dt; // warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f", // (double)integrator, (double)threshold_accel.get(), (double)threshold_time.get(), (double)accel_x, (double)dt); - if (integrator > threshold_accel.get() * threshold_time.get()) { + if (integrator > threshold_time.get()) { launchDetected = true; } diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 0e58c68b6..339b11bbe 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -180,11 +180,13 @@ int do_airspeed_calibration(int mavlink_fd) return ERROR; } - mavlink_log_critical(mavlink_fd, "Offset of %d Pa, create airflow now!", (int)diff_pres_offset); + mavlink_log_critical(mavlink_fd, "Offset of %d Pascal", (int)diff_pres_offset); /* wait 500 ms to ensure parameter propagated through the system */ usleep(500 * 1000); + mavlink_log_critical(mavlink_fd, "Create airflow now"); + calibration_counter = 0; const unsigned maxcount = 3000; @@ -204,18 +206,18 @@ int do_airspeed_calibration(int mavlink_fd) calibration_counter++; if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { - if (calibration_counter % 100 == 0) { - mavlink_log_critical(mavlink_fd, "Missing airflow! (%d, wanted: 50 Pa, #h101)", - (int)diff_pres.differential_pressure_raw_pa); + if (calibration_counter % 500 == 0) { + mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", + (int)diff_pres.differential_pressure_raw_pa); } continue; } /* do not allow negative values */ if (diff_pres.differential_pressure_raw_pa < 0.0f) { - mavlink_log_info(mavlink_fd, "negative pressure: ERROR (%d Pa)", - (int)diff_pres.differential_pressure_raw_pa); - mavlink_log_critical(mavlink_fd, "%d Pa: swap static and dynamic ports!", (int)diff_pres.differential_pressure_raw_pa); + mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!"); + mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", + (int)diff_pres.differential_pressure_raw_pa); close(diff_pres_sub); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ @@ -236,7 +238,7 @@ int do_airspeed_calibration(int mavlink_fd) mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); return ERROR; } else { - mavlink_log_info(mavlink_fd, "positive pressure: OK (%d Pa)", + mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", (int)diff_pres.differential_pressure_raw_pa); break; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6c2c03070..a5a772825 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -129,7 +129,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 -#define DL_TIMEOUT 5 * 1000* 1000 +#define DL_TIMEOUT (10 * 1000 * 1000) #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -615,6 +615,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: case VEHICLE_CMD_PREFLIGHT_STORAGE: + case VEHICLE_CMD_CUSTOM_0: + case VEHICLE_CMD_CUSTOM_1: + case VEHICLE_CMD_CUSTOM_2: + case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY: + case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY: /* ignore commands that handled in low prio loop */ break; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f8589d24b..684c61a17 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -182,12 +182,19 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s // Fail transition if power levels on the avionics rail // are measured but are insufficient - if (status->condition_power_input_valid && ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.9f))) { - - mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage); - feedback_provided = true; - valid_transition = false; + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f)) { + // Check avionics rail voltages + if (status->avionics_power_rail_voltage < 4.75f) { + mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + valid_transition = false; + } else if (status->avionics_power_rail_voltage < 4.9f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power low: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } else if (status->avionics_power_rail_voltage > 5.4f) { + mavlink_log_critical(mavlink_fd, "CAUTION: Avionics power high: %6.2f Volt", (double)status->avionics_power_rail_voltage); + feedback_provided = true; + } } } diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 0cea13cc4..ad873203e 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -145,6 +145,7 @@ private: perf_counter_t _nonfinite_output_perf; /**< performance counter for non finite output */ bool _setpoint_valid; /**< flag if the position control setpoint is valid */ + bool _debug; /**< if set to true, print debug output */ struct { float tconst; @@ -324,7 +325,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() : _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")), _nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")), /* states */ - _setpoint_valid(false) + _setpoint_valid(false), + _debug(false) { /* safely initialize structs */ _att = {}; @@ -700,7 +702,8 @@ FixedwingAttitudeControl::task_main() perf_count(_nonfinite_input_perf); } } else { - airspeed = _airspeed.true_airspeed_m_s; + /* prevent numerical drama by requiring 0.5 m/s minimal speed */ + airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s); } /* @@ -785,7 +788,7 @@ FixedwingAttitudeControl::task_main() speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d; speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d; } else { - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("Did not get a valid R\n"); } } @@ -808,7 +811,7 @@ FixedwingAttitudeControl::task_main() _roll_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("roll_u %.4f", (double)roll_u); } } @@ -821,7 +824,7 @@ FixedwingAttitudeControl::task_main() if (!isfinite(pitch_u)) { _pitch_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f," " airspeed %.4f, airspeed_scaling %.4f," " roll_sp %.4f, pitch_sp %.4f," @@ -845,7 +848,7 @@ FixedwingAttitudeControl::task_main() if (!isfinite(yaw_u)) { _yaw_ctrl.reset_integrator(); perf_count(_nonfinite_output_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("yaw_u %.4f", (double)yaw_u); } } @@ -853,13 +856,13 @@ FixedwingAttitudeControl::task_main() /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); } } } else { perf_count(_nonfinite_input_perf); - if (loop_counter % 10 == 0) { + if (_debug && loop_counter % 10 == 0) { warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 350ce6dec..deccab482 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -569,6 +569,12 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); + + /* check if negative value for 2/3 of flare altitude is set for throttle cut */ + if (_parameters.land_thrust_lim_alt_relative < 0.0f) { + _parameters.land_thrust_lim_alt_relative = 0.66f * _parameters.land_flare_alt_relative; + } + param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); @@ -823,7 +829,7 @@ float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, * the measurement is valid * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt */ - if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) { + if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) { return rel_alt_estimated; } @@ -1380,6 +1386,9 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_ _mTecs.updateAltitudeSpeed(flightPathAngle, altitude, alt_sp, _airspeed.true_airspeed_m_s, v_sp, mode, limitOverride); } else { + /* No underspeed protection in landing mode */ + _tecs.set_detect_underspeed_enabled(!(mode == TECS_MODE_LAND || mode == TECS_MODE_LAND_THROTTLELIM)); + /* Using tecs library */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, altitude, alt_sp, v_sp, _airspeed.indicated_airspeed_m_s, eas2tas, diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 41c374407..890ab3bad 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -379,18 +379,23 @@ PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HVIRT, 10.0f); /** - * Landing flare altitude (relative) + * Landing flare altitude (relative to landing altitude) * + * @unit meter * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_FLALT, 15.0f); +PARAM_DEFINE_FLOAT(FW_LND_FLALT, 8.0f); /** - * Landing throttle limit altitude (relative) + * Landing throttle limit altitude (relative landing altitude) * + * Default of -1.0f lets the system default to applying throttle + * limiting at 2/3 of the flare altitude. + * + * @unit meter * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); +PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); /** * Landing heading hold horizontal distance diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 00c8df18c..5b65dc369 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -114,7 +114,7 @@ MavlinkFTP::_worker(Request *req) uint32_t messageCRC; // basic sanity checks; must validate length before use - if ((hdr->magic != kProtocolMagic) || (hdr->size > kMaxDataLength)) { + if (hdr->size > kMaxDataLength) { errorCode = kErrNoRequest; goto out; } @@ -122,6 +122,9 @@ MavlinkFTP::_worker(Request *req) // check request CRC to make sure this is one of ours messageCRC = hdr->crc32; hdr->crc32 = 0; + hdr->padding[0] = 0; + hdr->padding[1] = 0; + hdr->padding[2] = 0; if (crc32(req->rawData(), req->dataSize()) != messageCRC) { errorCode = kErrNoRequest; goto out; @@ -199,10 +202,13 @@ MavlinkFTP::_reply(Request *req) { auto hdr = req->header(); - hdr->magic = kProtocolMagic; + hdr->seqNumber = req->header()->seqNumber + 1; // message is assumed to be already constructed in the request buffer, so generate the CRC hdr->crc32 = 0; + hdr->padding[0] = 0; + hdr->padding[1] = 0; + hdr->padding[2] = 0; hdr->crc32 = crc32(req->rawData(), req->dataSize()); // then pack and send the reply back to the request source diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 43de89de9..1dd8f102e 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -38,9 +38,6 @@ * * MAVLink remote file server. * - * Messages are wrapped in ENCAPSULATED_DATA messages. Every message includes - * a session ID and sequence number. - * * A limited number of requests (currently 2) may be outstanding at a time. * Additional messages will be discarded. * @@ -74,16 +71,19 @@ private: static MavlinkFTP *_server; + /// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the + /// structure ourselves to 32 bit alignment which should get us what we want. struct RequestHeader - { - uint8_t magic; - uint8_t session; - uint8_t opcode; - uint8_t size; - uint32_t crc32; - uint32_t offset; + { + uint16_t seqNumber; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t padding[3]; + uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; - }; + }; enum Opcode : uint8_t { @@ -131,10 +131,11 @@ private: }; bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { - if (fromMessage->msgid == MAVLINK_MSG_ID_ENCAPSULATED_DATA) { + if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + _systemId = fromMessage->sysid; _mavlink = mavlink; - mavlink_msg_encapsulated_data_decode(fromMessage, &_message); - return true; + mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message); + return _message.target_system == _mavlink->get_system_id(); } return false; } @@ -145,8 +146,14 @@ private: // flat memory architecture, as we're operating between threads here. mavlink_message_t msg; msg.checksum = 0; - unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(), - _mavlink->get_channel(), &msg, sequence()+1, rawData()); + unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id + _mavlink->get_component_id(), // Sender component id + _mavlink->get_channel(), // Channel to send on + &msg, // Message to pack payload into + 0, // Target network + _systemId, // Target system id + 0, // Target component id + rawData()); // Payload to pack into message _mavlink->lockMessageBufferMutex(); bool success = _mavlink->message_buffer_write(&msg, len); @@ -167,26 +174,25 @@ private: #endif } - uint8_t *rawData() { return &_message.data[0]; } - RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.data[0]); } + uint8_t *rawData() { return &_message.payload[0]; } + RequestHeader *header() { return reinterpret_cast<RequestHeader *>(&_message.payload[0]); } uint8_t *requestData() { return &(header()->data[0]); } unsigned dataSize() { return header()->size + sizeof(RequestHeader); } - uint16_t sequence() const { return _message.seqnr; } mavlink_channel_t channel() { return _mavlink->get_channel(); } char *dataAsCString(); private: Mavlink *_mavlink; - mavlink_encapsulated_data_t _message; + mavlink_file_transfer_protocol_t _message; + uint8_t _systemId; }; - static const uint8_t kProtocolMagic = 'f'; static const char kDirentFile = 'F'; static const char kDirentDir = 'D'; static const char kDirentUnknown = 'U'; - static const uint8_t kMaxDataLength = MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN - sizeof(RequestHeader); + static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader); /// Request worker; runs on the low-priority work queue to service /// remote requests. diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 408605939..940e64144 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -134,44 +134,44 @@ Mavlink::Mavlink() : _mode(MAVLINK_MODE_NORMAL), _channel(MAVLINK_COMM_0), _logbuffer {}, - _total_counter(0), - _receive_thread {}, - _verbose(false), - _forwarding_on(false), - _passing_on(false), - _ftp_on(false), - _uart_fd(-1), - _baudrate(57600), - _datarate(1000), - _datarate_events(500), - _rate_mult(1.0f), - _mavlink_param_queue_index(0), - mavlink_link_termination_allowed(false), - _subscribe_to_stream(nullptr), - _subscribe_to_stream_rate(0.0f), - _flow_control_enabled(true), - _last_write_success_time(0), - _last_write_try_time(0), - _bytes_tx(0), - _bytes_txerr(0), - _bytes_rx(0), - _bytes_timestamp(0), - _rate_tx(0.0f), - _rate_txerr(0.0f), - _rate_rx(0.0f), - _rstatus {}, - _message_buffer {}, - _message_buffer_mutex {}, - _send_mutex {}, - _param_initialized(false), - _param_system_id(0), - _param_component_id(0), - _param_system_type(0), - _param_use_hil_gps(0), - - /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), - _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) + _total_counter(0), + _receive_thread {}, + _verbose(false), + _forwarding_on(false), + _passing_on(false), + _ftp_on(false), + _uart_fd(-1), + _baudrate(57600), + _datarate(1000), + _datarate_events(500), + _rate_mult(1.0f), + _mavlink_param_queue_index(0), + mavlink_link_termination_allowed(false), + _subscribe_to_stream(nullptr), + _subscribe_to_stream_rate(0.0f), + _flow_control_enabled(true), + _last_write_success_time(0), + _last_write_try_time(0), + _bytes_tx(0), + _bytes_txerr(0), + _bytes_rx(0), + _bytes_timestamp(0), + _rate_tx(0.0f), + _rate_txerr(0.0f), + _rate_rx(0.0f), + _rstatus {}, + _message_buffer {}, + _message_buffer_mutex {}, + _send_mutex {}, + _param_initialized(false), + _param_system_id(0), + _param_component_id(0), + _param_system_type(0), + _param_use_hil_gps(0), + + /* performance counters */ + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl; @@ -217,6 +217,8 @@ Mavlink::Mavlink() : errx(1, "instance ID is out of range"); break; } + + _rstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; } Mavlink::~Mavlink() @@ -1227,7 +1229,10 @@ Mavlink::task_main(int argc, char *argv[]) _mode = MAVLINK_MODE_CUSTOM; } else if (strcmp(optarg, "camera") == 0) { - _mode = MAVLINK_MODE_CAMERA; + // left in here for compatibility + _mode = MAVLINK_MODE_ONBOARD; + } else if (strcmp(optarg, "onboard") == 0) { + _mode = MAVLINK_MODE_ONBOARD; } break; @@ -1287,8 +1292,8 @@ Mavlink::task_main(int argc, char *argv[]) warnx("mode: CUSTOM"); break; - case MAVLINK_MODE_CAMERA: - warnx("mode: CAMERA"); + case MAVLINK_MODE_ONBOARD: + warnx("mode: ONBOARD"); break; default: @@ -1391,13 +1396,14 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); + configure_stream("OPTICAL_FLOW", 20.0f); break; - case MAVLINK_MODE_CAMERA: + case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - configure_stream("ATTITUDE", 15.0f); - configure_stream("GLOBAL_POSITION_INT", 15.0f); - configure_stream("CAMERA_CAPTURE", 1.0f); + configure_stream("ATTITUDE", 50.0f); + configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("CAMERA_CAPTURE", 2.0f); break; default: @@ -1653,6 +1659,8 @@ Mavlink::display_status() printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time)); } + printf("\tmavlink chan: #%u\n", _channel); + if (_rstatus.timestamp > 0) { printf("\ttype:\t\t"); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 1e2e94cef..7f9d225bb 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -127,7 +127,7 @@ public: enum MAVLINK_MODE { MAVLINK_MODE_NORMAL = 0, MAVLINK_MODE_CUSTOM, - MAVLINK_MODE_CAMERA + MAVLINK_MODE_ONBOARD }; void set_mode(enum MAVLINK_MODE); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index af4d46a36..a2f3828ff 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -774,6 +774,9 @@ private: MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; + MavlinkOrbSubscription *_sensor_combined_sub; + uint64_t _sensor_combined_time; + /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); @@ -789,7 +792,9 @@ protected: _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act_time(0), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), - _airspeed_time(0) + _airspeed_time(0), + _sensor_combined_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_combined_time(0) {} void send(const hrt_abstime t) @@ -799,12 +804,14 @@ protected: struct actuator_armed_s armed; struct actuator_controls_s act; struct airspeed_s airspeed; + struct sensor_combined_s sensor_combined; bool updated = _att_sub->update(&_att_time, &att); updated |= _pos_sub->update(&_pos_time, &pos); updated |= _armed_sub->update(&_armed_time, &armed); updated |= _act_sub->update(&_act_time, &act); updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); + updated |= _sensor_combined_sub->update(&_sensor_combined_time, &sensor_combined); if (updated) { mavlink_vfr_hud_t msg; @@ -813,7 +820,7 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = pos.alt; + msg.alt = sensor_combined.baro_alt_meter; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); @@ -879,8 +886,8 @@ protected: msg.eph = cm_uint16_from_m_float(gps.eph); msg.epv = cm_uint16_from_m_float(gps.epv); msg.vel = cm_uint16_from_m_float(gps.vel_m_s), - msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, - msg.satellites_visible = gps.satellites_used; + msg.cog = _wrap_2pi(gps.cog_rad) * M_RAD_TO_DEG_F * 1e2f, + msg.satellites_visible = gps.satellites_used; _mavlink->send_message(MAVLINK_MSG_ID_GPS_RAW_INT, &msg); } @@ -950,11 +957,11 @@ protected: msg.lat = pos.lat * 1e7; msg.lon = pos.lon * 1e7; msg.alt = pos.alt * 1000.0f; - msg.relative_alt = (pos.alt - home.alt) * 1000.0f; - msg.vx = pos.vel_n * 100.0f; - msg.vy = pos.vel_e * 100.0f; - msg.vz = pos.vel_d * 100.0f; - msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; + msg.relative_alt = (pos.alt - home.alt) * 1000.0f; + msg.vx = pos.vel_n * 100.0f; + msg.vy = pos.vel_e * 100.0f; + msg.vz = pos.vel_d * 100.0f; + msg.hdg = _wrap_2pi(pos.yaw) * M_RAD_TO_DEG_F * 100.0f; _mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &msg); } @@ -1015,9 +1022,9 @@ protected: msg.x = pos.x; msg.y = pos.y; msg.z = pos.z; - msg.vx = pos.vx; - msg.vy = pos.vy; - msg.vz = pos.vz; + msg.vx = pos.vx; + msg.vy = pos.vy; + msg.vz = pos.vz; _mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_NED, &msg); } @@ -1078,9 +1085,9 @@ protected: msg.x = pos.x; msg.y = pos.y; msg.z = pos.z; - msg.roll = pos.roll; - msg.pitch = pos.pitch; - msg.yaw = pos.yaw; + msg.roll = pos.roll; + msg.pitch = pos.pitch; + msg.yaw = pos.yaw; _mavlink->send_message(MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, &msg); } diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7a761588a..d436c95e9 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -58,6 +58,10 @@ #endif static const int ERROR = -1; +#define CHECK_SYSID_COMPID_MISSION(_msg) (_msg.target_system == mavlink_system.sysid && \ + ((_msg.target_component == mavlink_system.compid) || \ + (_msg.target_component == MAV_COMP_ID_MISSIONPLANNER) || \ + (_msg.target_component == MAV_COMP_ID_ALL))) MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(mavlink), _state(MAVLINK_WPM_STATE_IDLE), @@ -79,8 +83,7 @@ MavlinkMissionManager::MavlinkMissionManager(Mavlink *mavlink) : MavlinkStream(m _mission_result_sub(-1), _offboard_mission_pub(-1), _slow_rate_limiter(_interval / 10.0f), - _verbose(false), - _comp_id(MAV_COMP_ID_MISSIONPLANNER) + _verbose(false) { _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _mission_result_sub = orb_subscribe(ORB_ID(mission_result)); @@ -384,7 +387,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); - if (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpa)) { if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -416,7 +419,7 @@ MavlinkMissionManager::handle_mission_set_current(const mavlink_message_t *msg) mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); - if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -451,7 +454,7 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); - if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wprl)) { if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -487,7 +490,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); - if (wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpr)) { if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -558,7 +561,7 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); - if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { + if (CHECK_SYSID_COMPID_MISSION(wpc)) { if (_state == MAVLINK_WPM_STATE_IDLE) { _time_last_recv = hrt_absolute_time(); @@ -624,7 +627,7 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); - if (wp.target_system == mavlink_system.sysid && wp.target_component == _comp_id) { + if (CHECK_SYSID_COMPID_MISSION(wp)) { if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); @@ -710,7 +713,7 @@ MavlinkMissionManager::handle_mission_clear_all(const mavlink_message_t *msg) mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); - if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */) { + if (CHECK_SYSID_COMPID_MISSION(wpca)) { if (_state == MAVLINK_WPM_STATE_IDLE) { /* don't touch mission items storage itself, but only items count in mission state */ diff --git a/src/modules/mavlink/mavlink_mission.h b/src/modules/mavlink/mavlink_mission.h index 8ff27f87d..a7bb94c22 100644 --- a/src/modules/mavlink/mavlink_mission.h +++ b/src/modules/mavlink/mavlink_mission.h @@ -126,8 +126,6 @@ private: bool _verbose; - uint8_t _comp_id; - /* do not allow top copying this class */ MavlinkMissionManager(MavlinkMissionManager &); MavlinkMissionManager& operator = (const MavlinkMissionManager &); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c0fae0a2f..bed1bd789 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), - _radio_status_available(false), _control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))), _hil_frames(0), _old_timestamp(0), @@ -137,6 +136,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_command_long(msg); break; + case MAVLINK_MSG_ID_COMMAND_INT: + handle_message_command_int(msg); + break; + case MAVLINK_MSG_ID_OPTICAL_FLOW: handle_message_optical_flow(msg); break; @@ -169,7 +172,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_request_data_stream(msg); break; - case MAVLINK_MSG_ID_ENCAPSULATED_DATA: + case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: MavlinkFTP::getServer()->handle_message(_mavlink, msg); break; @@ -277,6 +280,62 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) } void +MavlinkReceiver::handle_message_command_int(mavlink_message_t *msg) +{ + /* command */ + mavlink_command_int_t cmd_mavlink; + mavlink_msg_command_int_decode(msg, &cmd_mavlink); + + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + //check for MAVLINK terminate command + if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { + /* This is the link shutdown command, terminate mavlink */ + warnx("terminated by remote command"); + fflush(stdout); + usleep(50000); + + /* terminate other threads and this thread */ + _mavlink->_task_should_exit = true; + + } else { + + if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { + warnx("ignoring CMD spoofed with same SYS/COMP (%d/%d) ID", + mavlink_system.sysid, mavlink_system.compid); + return; + } + + struct vehicle_command_s vcmd; + memset(&vcmd, 0, sizeof(vcmd)); + + /* Copy the content of mavlink_command_int_t cmd_mavlink into command_t cmd */ + vcmd.param1 = cmd_mavlink.param1; + vcmd.param2 = cmd_mavlink.param2; + vcmd.param3 = cmd_mavlink.param3; + vcmd.param4 = cmd_mavlink.param4; + /* these are coordinates as 1e7 scaled integers to work around the 32 bit floating point limits */ + vcmd.param5 = ((double)cmd_mavlink.x) / 1e7; + vcmd.param6 = ((double)cmd_mavlink.y) / 1e7; + vcmd.param7 = cmd_mavlink.z; + // XXX do proper translation + vcmd.command = (enum VEHICLE_CMD)cmd_mavlink.command; + vcmd.target_system = cmd_mavlink.target_system; + vcmd.target_component = cmd_mavlink.target_component; + vcmd.source_system = msg->sysid; + vcmd.source_component = msg->compid; + + if (_cmd_pub < 0) { + _cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + + } else { + orb_publish(ORB_ID(vehicle_command), _cmd_pub, &vcmd); + } + } + } +} + +void MavlinkReceiver::handle_message_optical_flow(mavlink_message_t *msg) { /* optical flow */ @@ -430,9 +489,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) } else { orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } - - /* this means that heartbeats alone won't be published to the radio status no more */ - _radio_status_available = true; } } @@ -474,25 +530,17 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) struct telemetry_status_s &tstatus = _mavlink->get_rx_status(); - hrt_abstime tnow = hrt_absolute_time(); - - /* always set heartbeat, publish only if telemetry link not up */ - tstatus.heartbeat_time = tnow; - - /* if no radio status messages arrive, lets at least publish that heartbeats were received */ - if (!_radio_status_available) { + /* set heartbeat time and topic time and publish - + * the telem status also gets updated on telemetry events + */ + tstatus.timestamp = hrt_absolute_time(); + tstatus.heartbeat_time = tstatus.timestamp; - tstatus.timestamp = tnow; - /* telem_time indicates the timestamp of a telemetry status packet and we got none */ - tstatus.telem_time = 0; - tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC; + if (_telemetry_status_pub < 0) { + _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - if (_telemetry_status_pub < 0) { - _telemetry_status_pub = orb_advertise(telemetry_status_orb_id[_mavlink->get_channel()], &tstatus); - - } else { - orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); - } + } else { + orb_publish(telemetry_status_orb_id[_mavlink->get_channel()], _telemetry_status_pub, &tstatus); } } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 014193100..91125736c 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -110,6 +110,7 @@ private: void handle_message(mavlink_message_t *msg); void handle_message_command_long(mavlink_message_t *msg); + void handle_message_command_int(mavlink_message_t *msg); void handle_message_optical_flow(mavlink_message_t *msg); void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); @@ -151,7 +152,6 @@ private: orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; orb_advert_t _manual_pub; - bool _radio_status_available; int _control_mode_sub; int _hil_frames; uint64_t _old_timestamp; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728..042c46afd 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -353,6 +353,8 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; @@ -368,8 +370,6 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: case NAVIGATION_STATE_OFFBOARD: _navigation_mode = &_offboard; break; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 43161aa70..0da778b6f 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -285,7 +285,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { /* XXX range-check value? */ - r_page_servos[offset] = *values; + if (*values != PWM_IGNORE_THIS_CHANNEL) { + r_page_servos[offset] = *values; + } offset++; num_values--; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6c4b49452..dbda8ea6f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -76,6 +76,7 @@ #include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/satellite_info.h> #include <uORB/topics/vehicle_vicon_position.h> +#include <uORB/topics/vision_position_estimate.h> #include <uORB/topics/vehicle_global_velocity_setpoint.h> #include <uORB/topics/optical_flow.h> #include <uORB/topics/battery_status.h> @@ -934,6 +935,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct position_setpoint_triplet_s triplet; struct vehicle_vicon_position_s vicon_pos; + struct vision_position_estimate vision_pos; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -984,6 +986,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_EST1_s log_EST1; struct log_PWR_s log_PWR; struct log_VICN_s log_VICN; + struct log_VISN_s log_VISN; struct log_GS0A_s log_GS0A; struct log_GS0B_s log_GS0B; struct log_GS1A_s log_GS1A; @@ -1013,6 +1016,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int gps_pos_sub; int sat_info_sub; int vicon_pos_sub; + int vision_pos_sub; int flow_sub; int rc_sub; int airspeed_sub; @@ -1043,6 +1047,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); subs.triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet)); subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); + subs.vision_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate)); subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); subs.rc_sub = orb_subscribe(ORB_ID(rc_channels)); subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); @@ -1459,6 +1464,22 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_VICN.yaw = buf.vicon_pos.yaw; LOGBUFFER_WRITE_AND_COUNT(VICN); } + + /* --- VISION POSITION --- */ + if (copy_if_updated(ORB_ID(vision_position_estimate), subs.vision_pos_sub, &buf.vision_pos)) { + log_msg.msg_type = LOG_VISN_MSG; + log_msg.body.log_VISN.x = buf.vision_pos.x; + log_msg.body.log_VISN.y = buf.vision_pos.y; + log_msg.body.log_VISN.z = buf.vision_pos.z; + log_msg.body.log_VISN.vx = buf.vision_pos.vx; + log_msg.body.log_VISN.vy = buf.vision_pos.vy; + log_msg.body.log_VISN.vz = buf.vision_pos.vz; + log_msg.body.log_VISN.qx = buf.vision_pos.q[0]; + log_msg.body.log_VISN.qy = buf.vision_pos.q[1]; + log_msg.body.log_VISN.qz = buf.vision_pos.q[2]; + log_msg.body.log_VISN.qw = buf.vision_pos.q[3]; + LOGBUFFER_WRITE_AND_COUNT(VISN); + } /* --- FLOW --- */ if (copy_if_updated(ORB_ID(optical_flow), subs.flow_sub, &buf.flow)) { diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fb7609435..6741c7e25 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -391,6 +391,20 @@ struct log_TEL_s { uint64_t heartbeat_time; }; +/* --- VISN - VISION POSITION --- */ +#define LOG_VISN_MSG 38 +struct log_VISN_s { + float x; + float y; + float z; + float vx; + float vy; + float vz; + float qx; + float qy; + float qz; + float qw; +}; /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -449,6 +463,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(VICN, "ffffff", "X,Y,Z,Roll,Pitch,Yaw"), + LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatX,QuatY,QuatZ,QuatW"), LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB", "s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"), diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 7ce6ef5ef..229bfe3ce 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f); */ PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0); +/** + * QNH for barometer + * + * @min 500 + * @max 1500 + * @group Sensor Calibration + * @unit hPa + */ +PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); + /** * Board rotation diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4e8a8c01d..cdcb428dd 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -143,6 +143,12 @@ #define STICK_ON_OFF_LIMIT 0.75f +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + /** * Sensor app start / stop handling function * @@ -235,7 +241,7 @@ private: math::Matrix<3, 3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */ math::Matrix<3, 3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */ bool _mag_is_external; /**< true if the active mag is on an external board */ - + uint64_t _battery_discharged; /**< battery discharged current in mA*ms */ hrt_abstime _battery_current_timestamp; /**< timestamp of last battery current reading */ @@ -258,7 +264,7 @@ private: int board_rotation; int external_mag_rotation; - + float board_offset[3]; int rc_map_roll; @@ -301,6 +307,8 @@ private: float battery_voltage_scaling; float battery_current_scaling; + float baro_qnh; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -354,9 +362,11 @@ private: param_t board_rotation; param_t external_mag_rotation; - + param_t board_offset[3]; + param_t baro_qnh; + } _parameter_handles; /**< handles for interesting parameters */ @@ -462,12 +472,6 @@ private: namespace sensors { -/* oddly, ERROR is not defined for c++ */ -#ifdef ERROR -# undef ERROR -#endif -static const int ERROR = -1; - Sensors *g_sensors = nullptr; } @@ -611,12 +615,15 @@ Sensors::Sensors() : /* rotations */ _parameter_handles.board_rotation = param_find("SENS_BOARD_ROT"); _parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT"); - + /* rotation offsets */ _parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF"); _parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF"); _parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF"); + /* Barometer QNH */ + _parameter_handles.baro_qnh = param_find("SENS_BARO_QNH"); + /* fetch initial parameter values */ parameters_update(); } @@ -828,19 +835,37 @@ Sensors::parameters_update() get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation); get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation); - + param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0])); param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1])); param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2])); - + /** fine tune board offset on parameter update **/ - math::Matrix<3, 3> board_rotation_offset; + math::Matrix<3, 3> board_rotation_offset; board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0], M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - + _board_rotation = _board_rotation * board_rotation_offset; + /* update barometer qnh setting */ + param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); + int fd; + fd = open(BARO_DEVICE_PATH, 0); + if (fd < 0) { + warn("%s", BARO_DEVICE_PATH); + errx(1, "FATAL: no barometer found"); + + } else { + int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); + if (ret) { + warnx("qnh could not be set"); + close(fd); + return ERROR; + } + close(fd); + } + return OK; } @@ -1204,16 +1229,18 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) if (updated) { orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); - raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; + raw.differential_pressure_pa = _diff_pres.differential_pressure_raw_pa; raw.differential_pressure_timestamp = _diff_pres.timestamp; raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; float air_temperature_celsius = (_diff_pres.temperature > -300.0f) ? _diff_pres.temperature : (raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG); _airspeed.timestamp = _diff_pres.timestamp; - _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa); - _airspeed.true_airspeed_m_s = calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, - raw.baro_pres_mbar * 1e2f, air_temperature_celsius); + + /* don't risk to feed negative airspeed into the system */ + _airspeed.indicated_airspeed_m_s = math::max(0.0f, calc_indicated_airspeed(_diff_pres.differential_pressure_filtered_pa)); + _airspeed.true_airspeed_m_s = math::max(0.0f, calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f, + raw.baro_pres_mbar * 1e2f, air_temperature_celsius)); _airspeed.air_temperature_celsius = air_temperature_celsius; /* announce the airspeed if needed, just publish else */ @@ -1432,12 +1459,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw) if (voltage > 0.4f && (_parameters.diff_pres_analog_scale > 0.0f)) { float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; - float diff_pres_pa = (diff_pres_pa_raw > 0.0f) ? diff_pres_pa_raw : 0.0f; _diff_pres.timestamp = t; - _diff_pres.differential_pressure_pa = diff_pres_pa; _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; - _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa * 0.1f); + _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + (diff_pres_pa_raw * 0.1f); _diff_pres.temperature = -1000.0f; /* announce the airspeed if needed, just publish else */ diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index cd48d3cb2..7342fcf04 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,7 +54,6 @@ struct differential_pressure_s { uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ uint64_t error_count; /**< Number of errors detected by driver */ - float differential_pressure_pa; /**< Differential pressure reading */ float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index 7db33d98b..44aa50572 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler <thomasgubler@student.ethz.ch> - * @author Julian Oes <joes@student.ethz.ch> - * @author Lorenz Meier <lm@inf.ethz.ch> + * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,6 +34,10 @@ /** * @file vehicle_command.h * Definition of the vehicle command uORB topic. + * + * @author Thomas Gubler <thomasgubler@student.ethz.ch> + * @author Julian Oes <joes@student.ethz.ch> + * @author Lorenz Meier <lm@inf.ethz.ch> */ #ifndef TOPIC_VEHICLE_COMMAND_H_ @@ -52,6 +53,9 @@ * but can contain additional ones. */ enum VEHICLE_CMD { + VEHICLE_CMD_CUSTOM_0 = 0, /* test command */ + VEHICLE_CMD_CUSTOM_1 = 1, /* test command */ + VEHICLE_CMD_CUSTOM_2 = 2, /* test command */ VEHICLE_CMD_NAV_WAYPOINT = 16, /* Navigate to MISSION. |Hold time in decimal seconds. (ignored by fixed wing, time to stay at MISSION for rotary wing)| Acceptance radius in meters (if the sphere with this radius is hit, the MISSION counts as reached)| 0 to pass through the WP, if > 0 radius in meters to pass by WP. Positive value for clockwise orbit, negative value for counter-clockwise orbit. Allows trajectory control.| Desired yaw angle at MISSION (rotary wing)| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_UNLIM = 17, /* Loiter around this MISSION an unlimited amount of time |Empty| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_LOITER_TURNS = 18, /* Loiter around this MISSION for X turns |Turns| Empty| Radius around MISSION, in meters. If positive loiter clockwise, else counter-clockwise| Desired yaw angle.| Latitude| Longitude| Altitude| */ @@ -87,7 +91,8 @@ enum VEHICLE_CMD { VEHICLE_CMD_MISSION_START = 300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| */ VEHICLE_CMD_COMPONENT_ARM_DISARM = 400, /* Arms / Disarms a component |1 to arm, 0 to disarm| */ VEHICLE_CMD_START_RX_PAIR = 500, /* Starts receiver pairing |0:Spektrum| 0:Spektrum DSM2, 1:Spektrum DSMX| */ - VEHICLE_CMD_ENUM_END = 501, /* | */ + VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001, /**< Prepare a payload deployment in the flight plan */ + VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002 /**< Control a pre-programmed payload deployment */ }; /** @@ -115,8 +120,8 @@ struct vehicle_command_s { float param2; /**< Parameter 2, as defined by MAVLink VEHICLE_CMD enum. */ float param3; /**< Parameter 3, as defined by MAVLink VEHICLE_CMD enum. */ float param4; /**< Parameter 4, as defined by MAVLink VEHICLE_CMD enum. */ - float param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ - float param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ + double param5; /**< Parameter 5, as defined by MAVLink VEHICLE_CMD enum. */ + double param6; /**< Parameter 6, as defined by MAVLink VEHICLE_CMD enum. */ float param7; /**< Parameter 7, as defined by MAVLink VEHICLE_CMD enum. */ enum VEHICLE_CMD command; /**< Command ID, as defined MAVLink by VEHICLE_CMD enum. */ uint8_t target_system; /**< System which should execute the command */ diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/actuators/esc.cpp index 406eba88c..223d94731 100644 --- a/src/modules/uavcan/esc_controller.cpp +++ b/src/modules/uavcan/actuators/esc.cpp @@ -32,12 +32,12 @@ ****************************************************************************/ /** - * @file esc_controller.cpp + * @file esc.cpp * * @author Pavel Kirienko <pavel.kirienko@gmail.com> */ -#include "esc_controller.hpp" +#include "esc.hpp" #include <systemlib/err.h> UavcanEscController::UavcanEscController(uavcan::INode &node) : diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/actuators/esc.hpp index 559ede561..cf0988210 100644 --- a/src/modules/uavcan/esc_controller.hpp +++ b/src/modules/uavcan/actuators/esc.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file esc_controller.hpp + * @file esc.hpp * * UAVCAN <--> ORB bridge for ESC messages: * uavcan.equipment.esc.RawCommand diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 3865f2468..f92bc754f 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -40,10 +40,19 @@ MODULE_COMMAND = uavcan MAXOPTIMIZATION = -Os -SRCS += uavcan_main.cpp \ - uavcan_clock.cpp \ - esc_controller.cpp \ - gnss_receiver.cpp +# Main +SRCS += uavcan_main.cpp \ + uavcan_clock.cpp \ + uavcan_params.c + +# Actuators +SRCS += actuators/esc.cpp + +# Sensors +SRCS += sensors/sensor_bridge.cpp \ + sensors/gnss.cpp \ + sensors/mag.cpp \ + sensors/baro.cpp # # libuavcan diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp new file mode 100644 index 000000000..80c5e3828 --- /dev/null +++ b/src/modules/uavcan/sensors/baro.cpp @@ -0,0 +1,117 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#include "baro.hpp" +#include <cmath> + +static const orb_id_t BARO_TOPICS[2] = { + ORB_ID(sensor_baro0), + ORB_ID(sensor_baro1) +}; + +const char *const UavcanBarometerBridge::NAME = "baro"; + +UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : +UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS), +_sub_air_data(node) +{ +} + +int UavcanBarometerBridge::init() +{ + int res = device::CDev::init(); + if (res < 0) { + return res; + } + + res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + return 0; +} + +int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case BAROIOCSMSLPRESSURE: { + if ((arg < 80000) || (arg > 120000)) { + return -EINVAL; + } else { + log("new msl pressure %u", _msl_pressure); + _msl_pressure = arg; + return OK; + } + } + case BAROIOCGMSLPRESSURE: { + return _msl_pressure; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg) +{ + auto report = ::baro_report(); + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.temperature = msg.static_temperature; + report.pressure = msg.static_pressure / 100.0F; // Convert to millibar + + /* + * Altitude computation + * Refer to the MS5611 driver for details + */ + const double T1 = 15.0 + 273.15; // temperature at base height in Kelvin + const double a = -6.5 / 1000; // temperature gradient in degrees per metre + const double g = 9.80665; // gravity constant in m/s/s + const double R = 287.05; // ideal gas constant in J/kg/K + + const double p1 = _msl_pressure / 1000.0; // current pressure at MSL in kPa + const double p = double(msg.static_pressure) / 1000.0; // measured pressure in kPa + + report.altitude = (((std::pow((p / p1), (-(a * R) / g))) * T1) - T1) / a; + + publish(msg.getSrcNodeID().get(), &report); +} diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp new file mode 100644 index 000000000..9d470219e --- /dev/null +++ b/src/modules/uavcan/sensors/baro.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include <drivers/drv_baro.h> + +#include <uavcan/equipment/air_data/StaticAirData.hpp> + +class UavcanBarometerBridge : public UavcanCDevSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanBarometerBridge(uavcan::INode& node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg); + + typedef uavcan::MethodBinder<UavcanBarometerBridge*, + void (UavcanBarometerBridge::*) + (const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData>&)> + AirDataCbBinder; + + uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data; + unsigned _msl_pressure = 101325; +}; diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/sensors/gnss.cpp index ba1fe5e49..0d67aad47 100644 --- a/src/modules/uavcan/gnss_receiver.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -32,52 +32,74 @@ ****************************************************************************/ /** - * @file gnss_receiver.cpp + * @file gnss.cpp * * @author Pavel Kirienko <pavel.kirienko@gmail.com> * @author Andrew Chambers <achamber@gmail.com> * */ -#include "gnss_receiver.hpp" +#include "gnss.hpp" #include <systemlib/err.h> #include <mathlib/mathlib.h> #define MM_PER_CM 10 // Millimeters per centimeter -UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) : +const char *const UavcanGnssBridge::NAME = "gnss"; + +UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : _node(node), -_uavcan_sub_status(node), +_sub_fix(node), _report_pub(-1) { } -int UavcanGnssReceiver::init() +int UavcanGnssBridge::init() { - int res = -1; - - // GNSS fix subscription - res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb)); + int res = _sub_fix.start(FixCbBinder(this, &UavcanGnssBridge::gnss_fix_sub_cb)); if (res < 0) { warnx("GNSS fix sub failed %i", res); return res; } + return res; +} - // Clear the uORB GPS report - memset(&_report, 0, sizeof(_report)); +unsigned UavcanGnssBridge::get_num_redundant_channels() const +{ + return (_receiver_node_id < 0) ? 0 : 1; +} - return res; +void UavcanGnssBridge::print_status() const +{ + printf("RX errors: %d, receiver node id: ", _sub_fix.getFailureCount()); + if (_receiver_node_id < 0) { + printf("N/A\n"); + } else { + printf("%d\n", _receiver_node_id); + } } -void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg) +void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg) { - _report.timestamp_position = hrt_absolute_time(); - _report.lat = msg.lat_1e7; - _report.lon = msg.lon_1e7; - _report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) + // This bridge does not support redundant GNSS receivers yet. + if (_receiver_node_id < 0) { + _receiver_node_id = msg.getSrcNodeID().get(); + warnx("GNSS receiver node ID: %d", _receiver_node_id); + } else { + if (_receiver_node_id != msg.getSrcNodeID().get()) { + return; // This GNSS receiver is the redundant one, ignore it. + } + } + + auto report = ::vehicle_gps_position_s(); + + report.timestamp_position = hrt_absolute_time(); + report.lat = msg.lat_1e7; + report.lon = msg.lon_1e7; + report.alt = msg.alt_1e2 * MM_PER_CM; // Convert from centimeter (1e2) to millimeters (1e3) - _report.timestamp_variance = _report.timestamp_position; + report.timestamp_variance = report.timestamp_position; // Check if the msg contains valid covariance information @@ -90,19 +112,19 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav // Horizontal position uncertainty const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]); - _report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; + report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F; // Vertical position uncertainty - _report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; + report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F; } else { - _report.eph = -1.0F; - _report.epv = -1.0F; + report.eph = -1.0F; + report.epv = -1.0F; } if (valid_velocity_covariance) { float vel_cov[9]; msg.velocity_covariance.unpackSquareMatrix(vel_cov); - _report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); + report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]); /* There is a nonlinear relationship between the velocity vector and the heading. * Use Jacobian to transform velocity covariance to heading covariance @@ -118,36 +140,36 @@ void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uav float vel_e = msg.ned_velocity[1]; float vel_n_sq = vel_n * vel_n; float vel_e_sq = vel_e * vel_e; - _report.c_variance_rad = + report.c_variance_rad = (vel_e_sq * vel_cov[0] + -2 * vel_n * vel_e * vel_cov[1] + // Covariance matrix is symmetric vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq)); } else { - _report.s_variance_m_s = -1.0F; - _report.c_variance_rad = -1.0F; + report.s_variance_m_s = -1.0F; + report.c_variance_rad = -1.0F; } - _report.fix_type = msg.status; + report.fix_type = msg.status; - _report.timestamp_velocity = _report.timestamp_position; - _report.vel_n_m_s = msg.ned_velocity[0]; - _report.vel_e_m_s = msg.ned_velocity[1]; - _report.vel_d_m_s = msg.ned_velocity[2]; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s); - _report.vel_ned_valid = true; + report.timestamp_velocity = report.timestamp_position; + report.vel_n_m_s = msg.ned_velocity[0]; + report.vel_e_m_s = msg.ned_velocity[1]; + report.vel_d_m_s = msg.ned_velocity[2]; + report.vel_m_s = sqrtf(report.vel_n_m_s * report.vel_n_m_s + report.vel_e_m_s * report.vel_e_m_s + report.vel_d_m_s * report.vel_d_m_s); + report.cog_rad = atan2f(report.vel_e_m_s, report.vel_n_m_s); + report.vel_ned_valid = true; - _report.timestamp_time = _report.timestamp_position; - _report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds + report.timestamp_time = report.timestamp_position; + report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec(); // Convert to microseconds - _report.satellites_used = msg.sats_used; + report.satellites_used = msg.sats_used; if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &report); } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &report); } } diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/sensors/gnss.hpp index 18df8da2f..e8466b401 100644 --- a/src/modules/uavcan/gnss_receiver.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file gnss_receiver.hpp + * @file gnss.hpp * * UAVCAN --> ORB bridge for GNSS messages: * uavcan.equipment.gnss.Fix @@ -51,12 +51,22 @@ #include <uavcan/uavcan.hpp> #include <uavcan/equipment/gnss/Fix.hpp> -class UavcanGnssReceiver +#include "sensor_bridge.hpp" + +class UavcanGnssBridge : public IUavcanSensorBridge { public: - UavcanGnssReceiver(uavcan::INode& node); + static const char *const NAME; + + UavcanGnssBridge(uavcan::INode& node); + + const char *get_name() const override { return NAME; } + + int init() override; - int init(); + unsigned get_num_redundant_channels() const override; + + void print_status() const override; private: /** @@ -64,21 +74,14 @@ private: */ void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg); - - typedef uavcan::MethodBinder<UavcanGnssReceiver*, - void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)> + typedef uavcan::MethodBinder<UavcanGnssBridge*, + void (UavcanGnssBridge::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)> FixCbBinder; - /* - * libuavcan related things - */ - uavcan::INode &_node; - uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _uavcan_sub_status; + uavcan::INode &_node; + uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder> _sub_fix; + int _receiver_node_id = -1; - /* - * uORB - */ - struct vehicle_gps_position_s _report; ///< uORB topic for gnss position - orb_advert_t _report_pub; ///< uORB pub for gnss position + orb_advert_t _report_pub; ///< uORB pub for gnss position }; diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp new file mode 100644 index 000000000..8e6a9a22f --- /dev/null +++ b/src/modules/uavcan/sensors/mag.cpp @@ -0,0 +1,123 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#include "mag.hpp" + +static const orb_id_t MAG_TOPICS[3] = { + ORB_ID(sensor_mag0), + ORB_ID(sensor_mag1), + ORB_ID(sensor_mag2) +}; + +const char *const UavcanMagnetometerBridge::NAME = "mag"; + +UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : +UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), +_sub_mag(node) +{ + _scale.x_scale = 1.0F; + _scale.y_scale = 1.0F; + _scale.z_scale = 1.0F; +} + +int UavcanMagnetometerBridge::init() +{ + int res = device::CDev::init(); + if (res < 0) { + return res; + } + + res = _sub_mag.start(MagCbBinder(this, &UavcanMagnetometerBridge::mag_sub_cb)); + if (res < 0) { + log("failed to start uavcan sub: %d", res); + return res; + } + return 0; +} + +int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + case MAGIOCSSCALE: { + std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale)); + return 0; + } + case MAGIOCGSCALE: { + std::memcpy(reinterpret_cast<void*>(arg), &_scale, sizeof(_scale)); + return 0; + } + case MAGIOCSELFTEST: { + return 0; // Nothing to do + } + case MAGIOCGEXTERNAL: { + return 0; // We don't want anyone to transform the coordinate frame, so we declare it onboard + } + case MAGIOCSSAMPLERATE: { + return 0; // Pretend that this stuff is supported to keep the sensor app happy + } + case MAGIOCCALIBRATE: + case MAGIOCGSAMPLERATE: + case MAGIOCSRANGE: + case MAGIOCGRANGE: + case MAGIOCSLOWPASS: + case MAGIOCEXSTRAP: + case MAGIOCGLOWPASS: { + return -EINVAL; + } + default: { + return CDev::ioctl(filp, cmd, arg); + } + } +} + +void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg) +{ + auto report = ::mag_report(); + + report.range_ga = 1.3F; // Arbitrary number, doesn't really mean anything + + report.timestamp = msg.getUtcTimestamp().toUSec(); + if (report.timestamp == 0) { + report.timestamp = msg.getMonotonicTimestamp().toUSec(); + } + + report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale; + report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale; + report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale; + + publish(msg.getSrcNodeID().get(), &report); +} diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp new file mode 100644 index 000000000..6d413a8f7 --- /dev/null +++ b/src/modules/uavcan/sensors/mag.hpp @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include "sensor_bridge.hpp" +#include <drivers/drv_mag.h> + +#include <uavcan/equipment/ahrs/Magnetometer.hpp> + +class UavcanMagnetometerBridge : public UavcanCDevSensorBridgeBase +{ +public: + static const char *const NAME; + + UavcanMagnetometerBridge(uavcan::INode& node); + + const char *get_name() const override { return NAME; } + + int init() override; + +private: + int ioctl(struct file *filp, int cmd, unsigned long arg) override; + + void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg); + + typedef uavcan::MethodBinder<UavcanMagnetometerBridge*, + void (UavcanMagnetometerBridge::*) + (const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer>&)> + MagCbBinder; + + uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag; + mag_scale _scale = {}; +}; diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp new file mode 100644 index 000000000..9608ce680 --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -0,0 +1,158 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#include "sensor_bridge.hpp" +#include <cassert> + +#include "gnss.hpp" +#include "mag.hpp" +#include "baro.hpp" + +/* + * IUavcanSensorBridge + */ +void IUavcanSensorBridge::make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list) +{ + list.add(new UavcanBarometerBridge(node)); + list.add(new UavcanMagnetometerBridge(node)); + list.add(new UavcanGnssBridge(node)); +} + +/* + * UavcanCDevSensorBridgeBase + */ +UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() +{ + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id >= 0) { + (void)unregister_class_devname(_class_devname, _channels[i].class_instance); + } + } + delete [] _orb_topics; + delete [] _channels; +} + +void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) +{ + assert(report != nullptr); + + Channel *channel = nullptr; + + // Checking if such channel already exists + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id == node_id) { + channel = _channels + i; + break; + } + } + + // No such channel - try to create one + if (channel == nullptr) { + if (_out_of_channels) { + return; // Give up immediately - saves some CPU time + } + + log("adding channel %d...", node_id); + + // Search for the first free channel + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id < 0) { + channel = _channels + i; + break; + } + } + + // No free channels left + if (channel == nullptr) { + _out_of_channels = true; + log("out of channels"); + return; + } + + // Ask the CDev helper which class instance we can take + const int class_instance = register_class_devname(_class_devname); + if (class_instance < 0 || class_instance >= int(_max_channels)) { + _out_of_channels = true; + log("out of class instances"); + (void)unregister_class_devname(_class_devname, class_instance); + return; + } + + // Publish to the appropriate topic, abort on failure + channel->orb_id = _orb_topics[class_instance]; + channel->node_id = node_id; + channel->class_instance = class_instance; + + channel->orb_advert = orb_advertise(channel->orb_id, report); + if (channel->orb_advert < 0) { + log("ADVERTISE FAILED"); + (void)unregister_class_devname(_class_devname, class_instance); + *channel = Channel(); + return; + } + + log("channel %d class instance %d ok", channel->node_id, channel->class_instance); + } + assert(channel != nullptr); + + (void)orb_publish(channel->orb_id, channel->orb_advert, report); +} + +unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const +{ + unsigned out = 0; + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id >= 0) { + out += 1; + } + } + return out; +} + +void UavcanCDevSensorBridgeBase::print_status() const +{ + printf("devname: %s\n", _class_devname); + + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id >= 0) { + printf("channel %d: node id %d --> class instance %d\n", + i, _channels[i].node_id, _channels[i].class_instance); + } else { + printf("channel %d: empty\n", i); + } + } +} diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp new file mode 100644 index 000000000..1316f7ecc --- /dev/null +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -0,0 +1,131 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#pragma once + +#include <containers/List.hpp> +#include <uavcan/uavcan.hpp> +#include <drivers/device/device.h> +#include <drivers/drv_orb_dev.h> + +/** + * A sensor bridge class must implement this interface. + */ +class IUavcanSensorBridge : uavcan::Noncopyable, public ListNode<IUavcanSensorBridge*> +{ +public: + static constexpr unsigned MAX_NAME_LEN = 20; + + virtual ~IUavcanSensorBridge() { } + + /** + * Returns ASCII name of the bridge. + */ + virtual const char *get_name() const = 0; + + /** + * Starts the bridge. + * @return Non-negative value on success, negative on error. + */ + virtual int init() = 0; + + /** + * Returns number of active redundancy channels. + */ + virtual unsigned get_num_redundant_channels() const = 0; + + /** + * Prints current status in a human readable format to stdout. + */ + virtual void print_status() const = 0; + + /** + * Sensor bridge factory. + * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". + * @return nullptr if such bridge can't be created. + */ + static void make_all(uavcan::INode &node, List<IUavcanSensorBridge*> &list); +}; + +/** + * This is the base class for redundant sensors with an independent ORB topic per each redundancy channel. + * For example, sensor_mag0, sensor_mag1, etc. + */ +class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CDev +{ + struct Channel + { + int node_id = -1; + orb_id_t orb_id = nullptr; + orb_advert_t orb_advert = -1; + int class_instance = -1; + }; + + const unsigned _max_channels; + const char *const _class_devname; + orb_id_t *const _orb_topics; + Channel *const _channels; + bool _out_of_channels = false; + +protected: + template <unsigned MaxChannels> + UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, + const orb_id_t (&orb_topics)[MaxChannels]) : + device::CDev(name, devname), + _max_channels(MaxChannels), + _class_devname(class_devname), + _orb_topics(new orb_id_t[MaxChannels]), + _channels(new Channel[MaxChannels]) + { + memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); + } + + /** + * Sends one measurement into appropriate ORB topic. + * New redundancy channels will be registered automatically. + * @param node_id Sensor's Node ID + * @param report Pointer to ORB message object + */ + void publish(const int node_id, const void *report); + +public: + virtual ~UavcanCDevSensorBridgeBase(); + + unsigned get_num_redundant_channels() const override; + + void print_status() const override; +}; diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp index e41d5f953..fe8ba406a 100644 --- a/src/modules/uavcan/uavcan_clock.cpp +++ b/src/modules/uavcan/uavcan_clock.cpp @@ -62,6 +62,8 @@ void adjustUtc(uavcan::UtcDuration adjustment) (void)adjustment; } +uavcan::uint64_t getUtcUSecFromCanInterrupt(); + uavcan::uint64_t getUtcUSecFromCanInterrupt() { return 0; diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index 4535b6d6a..a8485ee44 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -38,8 +38,10 @@ #include <fcntl.h> #include <systemlib/err.h> #include <systemlib/systemlib.h> +#include <systemlib/param/param.h> #include <systemlib/mixer/mixer.h> #include <systemlib/board_serial.h> +#include <systemlib/scheduling_priorities.h> #include <version/version.h> #include <arch/board/board.h> #include <arch/chip/chip.h> @@ -65,16 +67,18 @@ UavcanNode *UavcanNode::_instance; UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) : CDev("uavcan", UAVCAN_DEVICE_PATH), _node(can_driver, system_clock), - _esc_controller(_node), - _gnss_receiver(_node) + _node_mutex(), + _esc_controller(_node) { _control_topics[0] = ORB_ID(actuator_controls_0); _control_topics[1] = ORB_ID(actuator_controls_1); _control_topics[2] = ORB_ID(actuator_controls_2); _control_topics[3] = ORB_ID(actuator_controls_3); - // memset(_controls, 0, sizeof(_controls)); - // memset(_poll_fds, 0, sizeof(_poll_fds)); + const int res = pthread_mutex_init(&_node_mutex, nullptr); + if (res < 0) { + std::abort(); + } } UavcanNode::~UavcanNode() @@ -99,10 +103,18 @@ UavcanNode::~UavcanNode() } /* clean up the alternate device node */ - // unregister_driver(PWM_OUTPUT_DEVICE_PATH); + // unregister_driver(PWM_OUTPUT_DEVICE_PATH); ::close(_armed_sub); + // Removing the sensor bridges + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + auto next = br->getSibling(); + delete br; + br = next; + } + _instance = nullptr; } @@ -164,7 +176,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate) * Start the task. Normally it should never exit. */ static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();}; - _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize, + _instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize, static_cast<main_t>(run_trampoline), nullptr); if (_instance->_task < 0) { @@ -214,11 +226,12 @@ int UavcanNode::init(uavcan::NodeID node_id) { int ret = -1; - /* do regular cdev init */ + // Do regular cdev init ret = CDev::init(); - if (ret != OK) + if (ret != OK) { return ret; + } _node.setName("org.pixhawk.pixhawk"); @@ -226,14 +239,24 @@ int UavcanNode::init(uavcan::NodeID node_id) fill_node_info(); - /* initializing the bridges UAVCAN <--> uORB */ + // Actuators ret = _esc_controller.init(); - if (ret < 0) + if (ret < 0) { return ret; + } - ret = _gnss_receiver.init(); - if (ret < 0) - return ret; + // Sensor bridges + IUavcanSensorBridge::make_all(_node, _sensor_bridges); + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + ret = br->init(); + if (ret < 0) { + warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret); + return ret; + } + warnx("sensor bridge '%s' init ok", br->get_name()); + br = br->getSibling(); + } return _node.start(); } @@ -248,6 +271,8 @@ void UavcanNode::node_spin_once() int UavcanNode::run() { + (void)pthread_mutex_lock(&_node_mutex); + const unsigned PollTimeoutMs = 50; // XXX figure out the output count @@ -291,8 +316,13 @@ int UavcanNode::run() _groups_subscribed = _groups_required; } + // Mutex is unlocked while the thread is blocked on IO multiplexing + (void)pthread_mutex_unlock(&_node_mutex); + const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs); + (void)pthread_mutex_lock(&_node_mutex); + node_spin_once(); // Non-blocking // this would be bad... @@ -352,7 +382,6 @@ int UavcanNode::run() // Output to the bus _esc_controller.update_outputs(outputs.output, outputs.noutputs); } - } // Check arming state @@ -376,10 +405,7 @@ int UavcanNode::run() } int -UavcanNode::control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input) +UavcanNode::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) { const actuator_controls_s *controls = (actuator_controls_s *)handle; @@ -520,8 +546,23 @@ UavcanNode::print_info() warnx("not running, start first"); } - warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); - warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK"); + (void)pthread_mutex_lock(&_node_mutex); + + // ESC mixer status + printf("ESC actuators control groups: sub: %u / req: %u / fds: %u\n", + (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num); + printf("ESC mixer: %s\n", (_mixers == nullptr) ? "NONE" : "OK"); + + // Sensor bridges + auto br = _sensor_bridges.getHead(); + while (br != nullptr) { + printf("Sensor '%s':\n", br->get_name()); + br->print_status(); + printf("\n"); + br = br->getSibling(); + } + + (void)pthread_mutex_unlock(&_node_mutex); } /* @@ -529,79 +570,57 @@ UavcanNode::print_info() */ static void print_usage() { - warnx("usage: uavcan start <node_id> [can_bitrate]"); + warnx("usage: \n" + "\tuavcan {start|status|stop}"); } extern "C" __EXPORT int uavcan_main(int argc, char *argv[]); int uavcan_main(int argc, char *argv[]) { - constexpr unsigned DEFAULT_CAN_BITRATE = 1000000; - if (argc < 2) { print_usage(); ::exit(1); } if (!std::strcmp(argv[1], "start")) { - if (argc < 3) { - print_usage(); - ::exit(1); + if (UavcanNode::instance()) { + errx(1, "already started"); } - /* - * Node ID - */ - const int node_id = atoi(argv[2]); + // Node ID + int32_t node_id = 0; + (void)param_get(param_find("UAVCAN_NODE_ID"), &node_id); if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) { warnx("Invalid Node ID %i", node_id); ::exit(1); } - /* - * CAN bitrate - */ - unsigned bitrate = 0; - - if (argc > 3) { - bitrate = atol(argv[3]); - } - - if (bitrate <= 0) { - bitrate = DEFAULT_CAN_BITRATE; - } + // CAN bitrate + int32_t bitrate = 0; + (void)param_get(param_find("UAVCAN_BITRATE"), &bitrate); - if (UavcanNode::instance()) { - errx(1, "already started"); - } - - /* - * Start - */ + // Start warnx("Node ID %u, bitrate %u", node_id, bitrate); return UavcanNode::start(node_id, bitrate); - } /* commands below require the app to be started */ - UavcanNode *inst = UavcanNode::instance(); + UavcanNode *const inst = UavcanNode::instance(); if (!inst) { errx(1, "application not running"); } if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) { - - inst->print_info(); - return OK; + inst->print_info(); + ::exit(0); } if (!std::strcmp(argv[1], "stop")) { - - delete inst; - inst = nullptr; - return OK; + delete inst; + ::exit(0); } print_usage(); diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp index 05b66fd7b..be7db9741 100644 --- a/src/modules/uavcan/uavcan_main.hpp +++ b/src/modules/uavcan/uavcan_main.hpp @@ -42,8 +42,8 @@ #include <uORB/topics/actuator_outputs.h> #include <uORB/topics/actuator_armed.h> -#include "esc_controller.hpp" -#include "gnss_receiver.hpp" +#include "actuators/esc.hpp" +#include "sensors/sensor_bridge.hpp" /** * @file uavcan_main.hpp @@ -77,12 +77,10 @@ public: static int start(uavcan::NodeID node_id, uint32_t bitrate); - Node& getNode() { return _node; } + Node& get_node() { return _node; } - static int control_callback(uintptr_t handle, - uint8_t control_group, - uint8_t control_index, - float &input); + // TODO: move the actuator mixing stuff into the ESC controller class + static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); void subscribe(); @@ -109,8 +107,11 @@ private: static UavcanNode *_instance; ///< singleton pointer Node _node; ///< library instance + pthread_mutex_t _node_mutex; + UavcanEscController _esc_controller; - UavcanGnssReceiver _gnss_receiver; + + List<IUavcanSensorBridge*> _sensor_bridges; ///< List of active sensor bridges MixerGroup *_mixers = nullptr; diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c new file mode 100644 index 000000000..e6ea8a8fb --- /dev/null +++ b/src/modules/uavcan/uavcan_params.c @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @author Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#include <nuttx/config.h> +#include <systemlib/param/param.h> + +/** + * Enable UAVCAN. + * + * Enables support for UAVCAN-interfaced actuators and sensors. + * + * @min 0 + * @max 1 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); + +/** + * UAVCAN Node ID. + * + * Read the specs at http://uavcan.org to learn more about Node ID. + * + * @min 1 + * @max 125 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1); + +/** + * UAVCAN CAN bus bitrate. + * + * @min 20000 + * @max 1000000 + * @group UAVCAN + */ +PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000); + + + diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index a4f17eebd..e005bf9c1 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -331,7 +331,7 @@ mag(int argc, char *argv[]) float len = sqrtf(buf.x * buf.x + buf.y * buf.y + buf.z * buf.z); - if (len < 1.0f || len > 3.0f) { + if (len < 0.25f || len > 3.0f) { warnx("MAG scale error!"); return ERROR; } diff --git a/uavcan b/uavcan -Subproject 6980ee824074aa2f7a62221bf6040ee41111952 +Subproject c4c14c60fbbd9acd281ee97d5bb2a4027d0ae2d |