diff options
57 files changed, 1086 insertions, 307 deletions
diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil index 11318023c..6e29bd6f8 100644 --- a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil +++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil @@ -32,6 +32,7 @@ then param set FW_T_SINK_MAX 5.0 param set FW_T_SINK_MIN 4.0 param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/100_mpx_easystar b/ROMFS/px4fmu_common/init.d/100_mpx_easystar index 75437f404..828540ad9 100644 --- a/ROMFS/px4fmu_common/init.d/100_mpx_easystar +++ b/ROMFS/px4fmu_common/init.d/100_mpx_easystar @@ -8,7 +8,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on EasyStar" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/101_hk_bixler b/ROMFS/px4fmu_common/init.d/101_hk_bixler index 89b3185ad..5a9cb2171 100644 --- a/ROMFS/px4fmu_common/init.d/101_hk_bixler +++ b/ROMFS/px4fmu_common/init.d/101_hk_bixler @@ -8,7 +8,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on HK Bixler" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 16 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/31_io_phantom b/ROMFS/px4fmu_common/init.d/31_io_phantom index 7e0127e79..8fe94452f 100644 --- a/ROMFS/px4fmu_common/init.d/31_io_phantom +++ b/ROMFS/px4fmu_common/init.d/31_io_phantom @@ -8,7 +8,27 @@ echo "[init] PX4FMU v1, v2 with or without IO on Phantom FPV" if param compare SYS_AUTOCONFIG 1 then # Set all params here, then disable autoconfig - # TODO + param set FW_P_D 0 + param set FW_P_I 0 + param set FW_P_IMAX 15 + param set FW_P_LIM_MAX 50 + param set FW_P_LIM_MIN -50 + param set FW_P_P 60 + param set FW_P_RMAX_NEG 0 + param set FW_P_RMAX_POS 0 + param set FW_P_ROLLFF 1.1 + param set FW_R_D 0 + param set FW_R_I 5 + param set FW_R_IMAX 20 + param set FW_R_P 100 + param set FW_R_RMAX 100 + param set FW_THR_CRUISE 0.65 + param set FW_THR_MAX 1 + param set FW_THR_MIN 0 + param set FW_T_SINK_MAX 5.0 + param set FW_T_SINK_MIN 4.0 + param set FW_Y_ROLLFF 1.1 + param set FW_L1_PERIOD 17 param set SYS_AUTOCONFIG 0 param save diff --git a/ROMFS/px4fmu_common/init.d/cmp_test b/ROMFS/px4fmu_common/init.d/cmp_test new file mode 100644 index 000000000..f86f4f85b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/cmp_test @@ -0,0 +1,9 @@ +#!nsh + +cp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded +if cmp /etc/extras/px4io-v2_default.bin /fs/microsd/px4io.loaded +then + echo "CMP returned true" +else + echo "CMP returned false" +fi
\ No newline at end of file diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 1dd949076..46640f3c5 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -134,7 +134,10 @@ MODULES += lib/geo # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control -MODULES += examples/fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest # # Transitional support - add commands from the NuttX export archive. diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e12d61a53..bb589cb9f 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -128,6 +128,13 @@ MODULES += lib/geo # https://pixhawk.ethz.ch/px4/dev/debug_values #MODULES += examples/px4_mavlink_debug +# Tutorial code from +# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control +#MODULES += examples/fixedwing_control + +# Hardware test +#MODULES += examples/hwtest + # # Transitional support - add commands from the NuttX export archive. # diff --git a/src/drivers/bma180/bma180.cpp b/src/drivers/bma180/bma180.cpp index f0044d36f..1590cc182 100644 --- a/src/drivers/bma180/bma180.cpp +++ b/src/drivers/bma180/bma180.cpp @@ -690,6 +690,7 @@ BMA180::measure() * measurement flow without using the external interrupt. */ report.timestamp = hrt_absolute_time(); + report.error_count = 0; /* * y of board is x of sensor and x of board is -y of sensor * perform only the axis assignment here. diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 794de584b..eff5e7349 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -52,6 +52,7 @@ */ struct accel_report { uint64_t timestamp; + uint64_t error_count; float x; /**< acceleration in the NED X board axis in m/s^2 */ float y; /**< acceleration in the NED Y board axis in m/s^2 */ float z; /**< acceleration in the NED Z board axis in m/s^2 */ diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 176f798c0..aa251889f 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -55,6 +55,7 @@ struct baro_report { float altitude; float temperature; uint64_t timestamp; + uint64_t error_count; }; /* diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 1d0fef6fc..fefcae50b 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -52,6 +52,7 @@ */ struct gyro_report { uint64_t timestamp; + uint64_t error_count; float x; /**< angular velocity in the NED X board axis in rad/s */ float y; /**< angular velocity in the NED Y board axis in rad/s */ float z; /**< angular velocity in the NED Z board axis in rad/s */ diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index 3de5625fd..06107bd3d 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -54,6 +54,7 @@ */ struct mag_report { uint64_t timestamp; + uint64_t error_count; float x; float y; float z; diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 94e923d71..fc96bd848 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -120,6 +120,7 @@ ORB_DECLARE(output_pwm); #define DSM2_BIND_PULSES 3 /* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */ #define DSMX_BIND_PULSES 7 /* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */ +#define DSMX8_BIND_PULSES 10 /* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */ /** Power up DSM receiver */ #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index 03a82ec5d..cf91f7030 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -52,6 +52,7 @@ */ struct range_finder_report { uint64_t timestamp; + uint64_t error_count; float distance; /** in meters */ uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ }; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index dd8436b10..084671ae2 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -153,35 +153,36 @@ ETSAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { - log("error reading from sensor: %d", ret); + perf_count(_comms_errors); return ret; } uint16_t diff_pres_pa = val[1] << 8 | val[0]; if (diff_pres_pa == 0) { - // a zero value means the pressure sensor cannot give us a - // value. We need to return, and not report a value or the - // caller could end up using this value as part of an - // average - log("zero value from sensor"); - return -1; + // a zero value means the pressure sensor cannot give us a + // value. We need to return, and not report a value or the + // caller could end up using this value as part of an + // average + perf_count(_comms_errors); + log("zero value from sensor"); + return -1; } if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) { diff_pres_pa = 0; - } else { diff_pres_pa -= _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; + _max_differential_pressure_pa = diff_pres_pa; } // XXX we may want to smooth out the readings to remove noise. differential_pressure_s report; report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); report.differential_pressure_pa = diff_pres_pa; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -209,7 +210,7 @@ ETSAirspeed::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); + perf_count(_comms_errors); /* restart the measurement state machine */ start(); return; diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index 5e891d7bb..5f0ce4ff8 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -791,6 +791,7 @@ HMC5883::collect() /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); + new_report.error_count = perf_event_count(_comms_errors); /* * @note We could read the status register here, which could tell us that diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 748809d3f..8f5674823 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -771,6 +771,7 @@ L3GD20::measure() * 74 from all measurements centers them around zero. */ report.timestamp = hrt_absolute_time(); + report.error_count = 0; // not recorded switch (_orientation) { diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 2c56b6035..8bed8a8df 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1248,6 +1248,7 @@ LSM303D::measure() accel_report.timestamp = hrt_absolute_time(); + accel_report.error_count = 0; // not reported accel_report.x_raw = raw_accel_report.x; accel_report.y_raw = raw_accel_report.y; diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index ccc5bc15e..c910e2890 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -490,6 +490,7 @@ MB12XX::collect() /* this should be fairly close to the end of the measurement, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); report.distance = si_units; report.valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 03d7bbfb9..ee45d46ac 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -138,7 +138,6 @@ MEASAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); return ret; } @@ -161,7 +160,6 @@ MEASAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { - log("error reading from sensor: %d", ret); perf_count(_comms_errors); perf_end(_sample_perf); return ret; @@ -207,6 +205,7 @@ MEASAirspeed::collect() } report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.voltage = 0; @@ -235,7 +234,6 @@ MEASAirspeed::cycle() /* perform collection */ if (OK != collect()) { - log("collection error"); /* restart the measurement state machine */ start(); return; diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 14a3571de..70359110e 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1186,7 +1186,7 @@ MPU6000::measure() * Adjust and scale results to m/s^2. */ grb.timestamp = arb.timestamp = hrt_absolute_time(); - + grb.error_count = arb.error_count = 0; // not reported /* * 1) Scale raw value to SI units using scaling from datasheet. diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 1c8a4d776..938786d3f 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -573,6 +573,7 @@ MS5611::collect() struct baro_report report; /* this should be fairly close to the end of the conversion, so the best approximation of the time */ report.timestamp = hrt_absolute_time(); + report.error_count = perf_event_count(_comms_errors); /* read the most recent measurement - read offset/size are hardcoded in the interface */ ret = _interface->read(0, (void *)&raw, 0); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index cba193208..d9869bf94 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -80,7 +80,9 @@ #include <uORB/topics/vehicle_command.h> #include <uORB/topics/rc_channels.h> #include <uORB/topics/battery_status.h> +#include <uORB/topics/servorail_status.h> #include <uORB/topics/parameter_update.h> + #include <debug.h> #include <mavlink/mavlink_log.h> @@ -195,6 +197,11 @@ public: int set_idle_values(const uint16_t *vals, unsigned len); /** + * Disable RC input handling + */ + int disable_rc_handling(); + + /** * Print IO status. * * Print all relevant IO status information @@ -202,9 +209,9 @@ public: void print_status(); /** - * Disable RC input handling + * Fetch and print debug console output. */ - int disable_rc_handling(); + int print_debug(); #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 /** @@ -268,6 +275,7 @@ private: orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage + orb_advert_t _to_servorail; ///< servorail status orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; ///<mixed outputs @@ -426,8 +434,27 @@ private: */ void dsm_bind_ioctl(int dsmMode); -}; + /** + * Handle a battery update from IO. + * + * Publish IO battery information if necessary. + * + * @param vbatt vbatt register + * @param ibatt ibatt register + */ + void io_handle_battery(uint16_t vbatt, uint16_t ibatt); + /** + * Handle a servorail update from IO. + * + * Publish servo rail information if necessary. + * + * @param vservo vservo register + * @param vrssi vrssi register + */ + void io_handle_vservo(uint16_t vbatt, uint16_t ibatt); + +}; namespace { @@ -463,6 +490,7 @@ PX4IO::PX4IO(device::Device *interface) : _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _to_servorail(0), _to_safety(0), _primary_pwm_device(false), _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor @@ -682,7 +710,6 @@ PX4IO::init() /* regular boot, no in-air restart, init IO */ } else { - /* dis-arm IO before touching anything */ io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | @@ -1183,13 +1210,13 @@ PX4IO::dsm_bind_ioctl(int dsmMode) if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { /* 0: dsm2, 1:dsmx */ if ((dsmMode == 0) || (dsmMode == 1)) { - mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%c rx", (dsmMode == 0) ? '2' : 'x'); - ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : DSMX_BIND_PULSES); + mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8")); + ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES)); } else { mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected"); } } else { - mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); + mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected"); } } @@ -1207,10 +1234,67 @@ PX4IO::io_handle_alarms(uint16_t alarms) return 0; } +void +PX4IO::io_handle_battery(uint16_t vbatt, uint16_t ibatt) +{ + /* only publish if battery has a valid minimum voltage */ + if (vbatt <= 3300) { + return; + } + + battery_status_s battery_status; + battery_status.timestamp = hrt_absolute_time(); + + /* voltage is scaled to mV */ + battery_status.voltage_v = vbatt / 1000.0f; + + /* + ibatt contains the raw ADC count, as 12 bit ADC + value, with full range being 3.3v + */ + battery_status.current_a = ibatt * (3.3f/4096.0f) * _battery_amp_per_volt; + battery_status.current_a += _battery_amp_bias; + + /* + integrate battery over time to get total mAh used + */ + if (_battery_last_timestamp != 0) { + _battery_mamphour_total += battery_status.current_a * + (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; + } + battery_status.discharged_mah = _battery_mamphour_total; + _battery_last_timestamp = battery_status.timestamp; + + /* lazily publish the battery voltage */ + if (_to_battery > 0) { + orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); + } else { + _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); + } +} + +void +PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi) +{ + servorail_status_s servorail_status; + servorail_status.timestamp = hrt_absolute_time(); + + /* voltage is scaled to mV */ + servorail_status.voltage_v = vservo * 0.001f; + servorail_status.rssi_v = vrssi * 0.001f; + + /* lazily publish the servorail voltages */ + if (_to_servorail > 0) { + orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status); + } else { + _to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status); + } +} + int PX4IO::io_get_status() { - uint16_t regs[4]; + uint16_t regs[6]; int ret; /* get STATUS_FLAGS, STATUS_ALARMS, STATUS_VBATT, STATUS_IBATT in that order */ @@ -1220,40 +1304,14 @@ PX4IO::io_get_status() io_handle_status(regs[0]); io_handle_alarms(regs[1]); - - /* only publish if battery has a valid minimum voltage */ - if (regs[2] > 3300) { - battery_status_s battery_status; - - battery_status.timestamp = hrt_absolute_time(); - /* voltage is scaled to mV */ - battery_status.voltage_v = regs[2] / 1000.0f; - - /* - regs[3] contains the raw ADC count, as 12 bit ADC - value, with full range being 3.3v - */ - battery_status.current_a = regs[3] * (3.3f/4096.0f) * _battery_amp_per_volt; - battery_status.current_a += _battery_amp_bias; - - /* - integrate battery over time to get total mAh used - */ - if (_battery_last_timestamp != 0) { - _battery_mamphour_total += battery_status.current_a * - (battery_status.timestamp - _battery_last_timestamp) * 1.0e-3f / 3600; - } - battery_status.discharged_mah = _battery_mamphour_total; - _battery_last_timestamp = battery_status.timestamp; +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 + io_handle_battery(regs[2], regs[3]); +#endif - /* lazily publish the battery voltage */ - if (_to_battery > 0) { - orb_publish(ORB_ID(battery_status), _to_battery, &battery_status); - } else { - _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); - } - } +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + io_handle_vservo(regs[4], regs[5]); +#endif return ret; } @@ -1481,8 +1539,52 @@ PX4IO::io_reg_modify(uint8_t page, uint8_t offset, uint16_t clearbits, uint16_t } int +PX4IO::print_debug() +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + int io_fd = -1; + + if (io_fd < 0) { + io_fd = ::open("/dev/ttyS0", O_RDONLY | O_NONBLOCK | O_NOCTTY); + } + + /* read IO's output */ + if (io_fd > 0) { + pollfd fds[1]; + fds[0].fd = io_fd; + fds[0].events = POLLIN; + + usleep(500); + int pret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), 0); + + if (pret > 0) { + int count; + char buf[65]; + + do { + count = ::read(io_fd, buf, sizeof(buf) - 1); + if (count > 0) { + /* enforce null termination */ + buf[count] = '\0'; + warnx("IO CONSOLE: %s", buf); + } + + } while (count > 0); + } + + ::close(io_fd); + return 0; + } +#endif + return 1; + +} + +int PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) { + /* get debug level */ + int debuglevel = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG); uint8_t frame[_max_transfer]; @@ -1504,6 +1606,8 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) memcpy(&msg->text[0], buf, count); buf += count; buflen -= count; + } else { + continue; } /* @@ -1521,6 +1625,15 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, total_len / 2); + /* print mixer chunk */ + if (debuglevel > 5 || ret) { + + warnx("fmu sent: \"%s\"", msg->text); + + /* read IO's output */ + print_debug(); + } + if (ret) { log("mixer send error %d", ret); return ret; @@ -1530,6 +1643,12 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } while (buflen > 0); + /* ensure a closing newline */ + msg->text[0] = '\n'; + msg->text[1] = '\0'; + + int ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); + retries--; log("mixer sent"); @@ -1751,7 +1870,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) usleep(500000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); - usleep(50000); + usleep(72000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); @@ -2094,8 +2213,13 @@ bind(int argc, char *argv[]) pulses = DSM2_BIND_PULSES; else if (!strcmp(argv[2], "dsmx")) pulses = DSMX_BIND_PULSES; + else if (!strcmp(argv[2], "dsmx8")) + pulses = DSMX8_BIND_PULSES; else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + // Test for custom pulse parameter + if (argc > 3) + pulses = atoi(argv[3]); if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) errx(1, "system must not be armed"); @@ -2191,28 +2315,37 @@ test(void) void monitor(void) { + /* clear screen */ + printf("\033[2J"); + unsigned cancels = 3; - printf("Hit <enter> three times to exit monitor mode\n"); for (;;) { pollfd fds[1]; fds[0].fd = 0; fds[0].events = POLLIN; - poll(fds, 1, 500); + poll(fds, 1, 2000); if (fds[0].revents == POLLIN) { int c; read(0, &c, 1); - if (cancels-- == 0) + if (cancels-- == 0) { + printf("\033[H"); /* move cursor home and clear screen */ exit(0); + } } -#warning implement this + if (g_dev != nullptr) { -// if (g_dev != nullptr) -// g_dev->dump_one = true; + printf("\033[H"); /* move cursor home and clear screen */ + (void)g_dev->print_status(); + (void)g_dev->print_debug(); + printf("[ Use 'px4io debug <N>' for more output. Hit <enter> three times to exit monitor mode ]\n"); + } else { + errx(1, "driver not loaded, exiting"); + } } } diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index fe8561a0b..d01dedb0d 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -237,7 +237,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout) fds[0].fd = _io_fd; fds[0].events = POLLIN; - /* wait 100 ms for a character */ + /* wait <timout> ms for a character */ int ret = ::poll(&fds[0], 1, timeout); if (ret < 1) { diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c new file mode 100644 index 000000000..06337be32 --- /dev/null +++ b/src/examples/hwtest/hwtest.c @@ -0,0 +1,74 @@ +/**************************************************************************** + * + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier <lm@inf.ethz.ch> + * + * 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 hwtest.c + * + * Simple functional hardware test. + * + * @author Lorenz Meier <lm@inf.ethz.ch> + */ + +#include <nuttx/config.h> +#include <stdio.h> +#include <systemlib/err.h> +#include <drivers/drv_hrt.h> +#include <uORB/uORB.h> +#include <uORB/topics/actuator_controls.h> + +__EXPORT int ex_hwtest_main(int argc, char *argv[]); + +int ex_hwtest_main(int argc, char *argv[]) +{ + struct actuator_controls_s actuators; + memset(&actuators, 0, sizeof(actuators)); + orb_advert_t actuator_pub_fd = orb_advertise(ORB_ID(actuator_controls_0), &actuators); + + int i; + float rcvalue = -1.0f; + hrt_abstime stime; + + while (true) { + stime = hrt_absolute_time(); + while (hrt_absolute_time() - stime < 1000000) { + for (i=0; i<8; i++) + actuators.control[i] = rcvalue; + actuators.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_controls_0), actuator_pub_fd, &actuators); + } + warnx("servos set to %.1f", rcvalue); + rcvalue *= -1.0f; + } + + return OK; +} diff --git a/src/examples/hwtest/module.mk b/src/examples/hwtest/module.mk new file mode 100644 index 000000000..6e70863ed --- /dev/null +++ b/src/examples/hwtest/module.mk @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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. +# +############################################################################ + +# +# Hardware test example application +# + +MODULE_COMMAND = ex_hwtest + +SRCS = hwtest.c diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp index 87eb99f16..c5c0c7a3c 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp @@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing() float ECL_L1_Pos_Controller::switch_distance(float wp_radius) { /* following [2], switching on L1 distance */ - return math::min(wp_radius, _L1_distance); + return math::max(wp_radius, _L1_distance); } bool ECL_L1_Pos_Controller::reached_loiter_target(void) @@ -86,6 +86,7 @@ float ECL_L1_Pos_Controller::crosstrack_error(void) void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position, const math::Vector2f &ground_speed_vector) { + /* this follows the logic presented in [1] */ float eta; @@ -128,7 +129,11 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c float distance_A_to_airplane = vector_A_to_airplane.length(); float alongTrackDist = vector_A_to_airplane * vector_AB; - /* extension from [2] */ + /* estimate airplane position WRT to B */ + math::Vector2f vector_B_to_airplane_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized(); + float bearing_wp_b = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX()); + + /* extension from [2], fly directly to A */ if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) { /* calculate eta to fly to waypoint A */ @@ -143,6 +148,21 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c /* bearing from current position to L1 point */ _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX()); +// XXX this can be useful as last-resort guard, but is currently not needed +#if 0 + } else if (absf(bearing_wp_b) > math::radians(80.0f)) { + /* extension, fly back to waypoint */ + + /* calculate eta to fly to waypoint B */ + + /* velocity across / orthogonal to line */ + xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit); + /* velocity along line */ + ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit); + eta = atan2f(xtrack_vel, ltrack_vel); + /* bearing from current position to L1 point */ + _nav_bearing = bearing_wp_b; +#endif } else { /* calculate eta to fly along the line between A and B */ @@ -162,6 +182,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c eta = eta1 + eta2; /* bearing from current position to L1 point */ _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1; + } /* limit angle to +-90 degrees */ diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h index a6a2c0156..5a17346cb 100644 --- a/src/lib/ecl/l1/ecl_l1_pos_controller.h +++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h @@ -131,6 +131,14 @@ public: /** + * Returns true if following a circle (loiter) + */ + bool circle_mode() { + return _circle_mode; + } + + + /** * Get the switch distance * * This is the distance at which the system will diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index f01ee0355..799fc2fb9 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -166,19 +166,19 @@ protected: double lat, lon; /**< lat, lon radians */ float alt; /**< altitude, meters */ // parameters - control::BlockParam<float> _vGyro; /**< gyro process noise */ - control::BlockParam<float> _vAccel; /**< accelerometer process noise */ - control::BlockParam<float> _rMag; /**< magnetometer measurement noise */ - control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */ - control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParam<float> _rPressAlt; /**< press altitude measurement noise */ - control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */ - control::BlockParam<float> _magDip; /**< magnetic inclination with level */ - control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParam<float> _g; /**< gravitational constant */ - control::BlockParam<float> _faultPos; /**< fault detection threshold for position */ - control::BlockParam<float> _faultAtt; /**< fault detection threshold for attitude */ + control::BlockParamFloat _vGyro; /**< gyro process noise */ + control::BlockParamFloat _vAccel; /**< accelerometer process noise */ + control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ + control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ + control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ + control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ + control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ + control::BlockParamFloat _magDip; /**< magnetic inclination with level */ + control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ + control::BlockParamFloat _g; /**< gravitational constant */ + control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ + control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ // status bool _attitudeInitialized; bool _positionInitialized; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 01b7b84d0..2ef509980 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -359,23 +359,35 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe case VEHICLE_CMD_DO_SET_MODE: { uint8_t base_mode = (uint8_t) cmd->param1; uint8_t custom_main_mode = (uint8_t) cmd->param2; + transition_result_t arming_res = TRANSITION_NOT_CHANGED; /* set HIL state */ hil_state_t new_hil_state = (base_mode & MAV_MODE_FLAG_HIL_ENABLED) ? HIL_STATE_ON : HIL_STATE_OFF; - hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + int hil_ret = hil_state_transition(new_hil_state, status_pub, status, control_mode_pub, control_mode, mavlink_fd); + + /* if HIL got enabled, reset battery status state */ + if (hil_ret == OK && control_mode->flag_system_hil_enabled) { + /* reset the arming mode to disarmed */ + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed); + if (arming_res != TRANSITION_DENIED) { + mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby"); + } else { + mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state"); + } + } // TODO remove debug code //mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode); /* set arming state */ - transition_result_t arming_res = TRANSITION_NOT_CHANGED; + arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if (safety->safety_switch_available && !safety->safety_off) { + if ((safety->safety_switch_available && !safety->safety_off) && !control_mode->flag_system_hil_enabled) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -385,7 +397,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, safety, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, control_mode, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -478,7 +490,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_DENIED; } else { - arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed); } if (arming_res == TRANSITION_CHANGED) { @@ -532,6 +544,9 @@ static struct actuator_armed_s armed; static struct safety_s safety; +/* flags for control apps */ +struct vehicle_control_mode_s control_mode; + int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ @@ -584,10 +599,6 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); - /* flags for control apps */ - struct vehicle_control_mode_s control_mode; - - /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); @@ -876,8 +887,8 @@ int commander_thread_main(int argc, char *argv[]) // warnx("bat v: %2.2f", battery.voltage_v); - /* only consider battery voltage if system has been running 2s and battery voltage is not 0 */ - if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 0.001f) { + /* only consider battery voltage if system has been running 2s and battery voltage is higher than 4V */ + if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_v > 4.0f) { status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); @@ -958,9 +969,9 @@ int commander_thread_main(int argc, char *argv[]) battery_tune_played = false; if (armed.armed) { - arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed); } else { - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed); } status_changed = true; @@ -969,6 +980,7 @@ int commander_thread_main(int argc, char *argv[]) critical_voltage_counter++; } else { + low_voltage_counter = 0; critical_voltage_counter = 0; } @@ -978,7 +990,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1082,7 +1094,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, &safety, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, &control_mode, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1104,7 +1116,7 @@ int commander_thread_main(int argc, char *argv[]) print_reject_arm("NOT ARMING: Switch to MANUAL mode first."); } else { - res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED, &armed); } stick_on_counter = 0; @@ -1181,31 +1193,19 @@ int commander_thread_main(int argc, char *argv[]) bool main_state_changed = check_main_state_changed(); bool navigation_state_changed = check_navigation_state_changed(); - if (arming_state_changed || main_state_changed || navigation_state_changed) { - mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); - status_changed = true; - } - hrt_abstime t1 = hrt_absolute_time(); - /* publish arming state */ - if (arming_state_changed) { - armed.timestamp = t1; - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); - } - - /* publish control mode */ if (navigation_state_changed || arming_state_changed) { - /* publish new navigation state */ control_mode.flag_armed = armed.armed; // copy armed state to vehicle_control_mode topic - control_mode.counter++; - control_mode.timestamp = t1; - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } + + if (arming_state_changed || main_state_changed || navigation_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + status_changed = true; } /* publish states (armed, control mode, vehicle status) at least with 5 Hz */ if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { - status.counter++; status.timestamp = t1; orb_publish(ORB_ID(vehicle_status), status_pub, &status); control_mode.timestamp = t1; @@ -1752,7 +1752,7 @@ void *commander_low_prio_loop(void *arg) /* try to go to INIT/PREFLIGHT arming state */ // XXX disable interrupts in arming_state_transition - if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed)) { + if (TRANSITION_DENIED == arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_INIT, &armed)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; } @@ -1792,7 +1792,7 @@ void *commander_low_prio_loop(void *arg) else tune_negative(); - arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed); break; } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 8ce31550f..490fc8fc6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,9 @@ static bool main_state_changed = true; static bool navigation_state_changed = true; transition_result_t -arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, + const struct vehicle_control_mode_s *control_mode, + arming_state_t new_arming_state, struct actuator_armed_s *armed) { /* * Perform an atomic state update @@ -82,6 +84,13 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * } else { + /* enforce lockdown in HIL */ + if (control_mode->flag_system_hil_enabled) { + armed->lockdown = true; + } else { + armed->lockdown = false; + } + switch (new_arming_state) { case ARMING_STATE_INIT: @@ -98,7 +107,8 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow coming from INIT and disarming from ARMED */ if (status->arming_state == ARMING_STATE_INIT - || status->arming_state == ARMING_STATE_ARMED) { + || status->arming_state == ARMING_STATE_ARMED + || control_mode->flag_system_hil_enabled) { /* sensors need to be initialized for STANDBY state */ if (status->condition_system_sensors_initialized) { @@ -115,7 +125,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s * /* allow arming from STANDBY and IN-AIR-RESTORE */ if ((status->arming_state == ARMING_STATE_STANDBY || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) - && (!safety->safety_switch_available || safety->safety_off)) { /* only allow arming if safety is off */ + && (!safety->safety_switch_available || safety->safety_off || control_mode->flag_system_hil_enabled)) { /* only allow arming if safety is off */ ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = true; @@ -466,20 +476,17 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s case HIL_STATE_OFF: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } + /* we're in HIL and unexpected things can happen if we disable HIL now */ + mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)"); + valid_transition = false; break; case HIL_STATE_ON: if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + || current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_STANDBY_ERROR) { current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); @@ -497,7 +504,6 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s if (valid_transition) { current_status->hil_state = new_state; - current_status->counter++; current_status->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_status); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 1641b6f60..0bfdf36a8 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -58,7 +58,7 @@ typedef enum { } transition_result_t; transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, - arming_state_t new_arming_state, struct actuator_armed_s *armed); + const struct vehicle_control_mode_s *control_mode, arming_state_t new_arming_state, struct actuator_armed_s *armed); bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 58a9bfc0d..36bc8c24b 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -69,22 +69,39 @@ protected: /** * Parameters that are tied to blocks for updating and nameing. */ -template<class T> -class __EXPORT BlockParam : public BlockParamBase + +class __EXPORT BlockParamFloat : public BlockParamBase +{ +public: + BlockParamFloat(Block *block, const char *name, bool parent_prefix=true) : + BlockParamBase(block, name, parent_prefix), + _val() { + update(); + } + float get() { return _val; } + void set(float val) { _val = val; } + void update() { + if (_handle != PARAM_INVALID) param_get(_handle, &_val); + } +protected: + float _val; +}; + +class __EXPORT BlockParamInt : public BlockParamBase { public: - BlockParam(Block *block, const char *name, bool parent_prefix=true) : + BlockParamInt(Block *block, const char *name, bool parent_prefix=true) : BlockParamBase(block, name, parent_prefix), _val() { update(); } - T get() { return _val; } - void set(T val) { _val = val; } + int get() { return _val; } + void set(int val) { _val = val; } void update() { if (_handle != PARAM_INVALID) param_get(_handle, &_val); } protected: - T _val; + int _val; }; } // namespace control diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp index fefe99702..66e929038 100644 --- a/src/modules/controllib/blocks.hpp +++ b/src/modules/controllib/blocks.hpp @@ -74,8 +74,8 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam<float> _min; - BlockParam<float> _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockLimitTest(); @@ -99,7 +99,7 @@ public: float getMax() { return _max.get(); } protected: // attributes - BlockParam<float> _max; + control::BlockParamFloat _max; }; int __EXPORT blockLimitSymTest(); @@ -126,7 +126,7 @@ public: protected: // attributes float _state; - BlockParam<float> _fCut; + control::BlockParamFloat _fCut; }; int __EXPORT blockLowPassTest(); @@ -157,7 +157,7 @@ protected: // attributes float _u; /**< previous input */ float _y; /**< previous output */ - BlockParam<float> _fCut; /**< cut-off frequency, Hz */ + control::BlockParamFloat _fCut; /**< cut-off frequency, Hz */ }; int __EXPORT blockHighPassTest(); @@ -273,7 +273,7 @@ public: // accessors float getKP() { return _kP.get(); } protected: - BlockParam<float> _kP; + control::BlockParamFloat _kP; }; int __EXPORT blockPTest(); @@ -303,8 +303,8 @@ public: BlockIntegral &getIntegral() { return _integral; } private: BlockIntegral _integral; - BlockParam<float> _kP; - BlockParam<float> _kI; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; }; int __EXPORT blockPITest(); @@ -334,8 +334,8 @@ public: BlockDerivative &getDerivative() { return _derivative; } private: BlockDerivative _derivative; - BlockParam<float> _kP; - BlockParam<float> _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kD; }; int __EXPORT blockPDTest(); @@ -372,9 +372,9 @@ private: // attributes BlockIntegral _integral; BlockDerivative _derivative; - BlockParam<float> _kP; - BlockParam<float> _kI; - BlockParam<float> _kD; + control::BlockParamFloat _kP; + control::BlockParamFloat _kI; + control::BlockParamFloat _kD; }; int __EXPORT blockPIDTest(); @@ -404,7 +404,7 @@ public: float get() { return _val; } private: // attributes - BlockParam<float> _trim; + control::BlockParamFloat _trim; BlockLimit _limit; float _val; }; @@ -439,8 +439,8 @@ public: float getMax() { return _max.get(); } private: // attributes - BlockParam<float> _min; - BlockParam<float> _max; + control::BlockParamFloat _min; + control::BlockParamFloat _max; }; int __EXPORT blockRandUniformTest(); @@ -486,8 +486,8 @@ public: float getStdDev() { return _stdDev.get(); } private: // attributes - BlockParam<float> _mean; - BlockParam<float> _stdDev; + control::BlockParamFloat _mean; + control::BlockParamFloat _stdDev; }; int __EXPORT blockRandGaussTest(); 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 6b98003fd..ae9aaa2da 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -593,6 +593,10 @@ FixedwingAttitudeControl::task_main() pitch_sp = _att_sp.pitch_body; throttle_sp = _att_sp.thrust; + /* reset integrals where needed */ + if (_att_sp.roll_reset_integral) + _roll_ctrl.reset_integrator(); + } else { /* * Scale down roll and pitch as the setpoints are radians 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 3d5bce134..cd4a0d58e 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 @@ -74,6 +74,7 @@ #include <uORB/topics/vehicle_rates_setpoint.h> #include <uORB/topics/vehicle_attitude.h> #include <uORB/topics/vehicle_control_mode.h> +#include <uORB/topics/navigation_capabilities.h> #include <uORB/topics/parameter_update.h> #include <systemlib/param/param.h> #include <systemlib/err.h> @@ -129,9 +130,11 @@ private: int _accel_sub; /**< body frame accelerations */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ + orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ struct vehicle_attitude_s _att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _att_sp; /**< vehicle attitude setpoint */ + struct navigation_capabilities_s _nav_capabilities; /**< navigation capabilities */ struct manual_control_setpoint_s _manual; /**< r/c channel data */ struct airspeed_s _airspeed; /**< airspeed */ struct vehicle_control_mode_s _control_mode; /**< vehicle status */ @@ -312,6 +315,7 @@ FixedwingPositionControl::FixedwingPositionControl() : /* publications */ _attitude_sp_pub(-1), + _nav_capabilities_pub(-1), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -323,6 +327,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _groundspeed_undershoot(0.0f), _global_pos_valid(false) { + _nav_capabilities.turn_distance = 0.0f; + _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); _parameter_handles.l1_damping = param_find("FW_L1_DAMPING"); _parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R"); @@ -625,6 +631,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // else integrators should be constantly reset. if (_control_mode.flag_control_position_enabled) { + /* get circle mode */ + bool was_circle_mode = _l1_control.circle_mode(); + /* execute navigation once we have a setpoint */ if (_setpoint_valid) { @@ -642,7 +651,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { /* - * No valid next waypoint, go for heading hold. + * No valid previous waypoint, go for the current wp. * This is automatically handled by the L1 library. */ prev_wp.setX(global_triplet.current.lat / 1e7f); @@ -810,6 +819,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } + if (was_circle_mode && !_l1_control.circle_mode()) { + /* just kicked out of loiter, reset roll integrals */ + _att_sp.roll_reset_integral = true; + } + } else if (0/* seatbelt mode enabled */) { /** SEATBELT FLIGHT **/ @@ -968,6 +982,21 @@ FixedwingPositionControl::task_main() _attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &_att_sp); } + float turn_distance = _l1_control.switch_distance(_global_triplet.current.turn_distance_xy); + + /* lazily publish navigation capabilities */ + if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) { + + /* set new turn distance */ + _nav_capabilities.turn_distance = turn_distance; + + if (_nav_capabilities_pub > 0) { + orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities); + } else { + _nav_capabilities_pub = orb_advertise(ORB_ID(navigation_capabilities), &_nav_capabilities); + } + } + } } diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 7a5c2dab2..7c10e297b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -690,25 +690,25 @@ int mavlink_thread_main(int argc, char *argv[]) lowspeed_counter++; - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* check if waypoint has been reached against the last positions */ - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); /* send parameters at 20 Hz (if queued for sending) */ mavlink_pm_queued_send(); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); /* sleep quarter the time */ usleep(25000); - mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos); + mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos, &nav_cap); if (baudrate > 57600) { mavlink_pm_queued_send(); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7dd9e321f..8243748dc 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -167,6 +167,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); @@ -437,9 +438,11 @@ handle_message(mavlink_message_t *msg) if (pub_hil_airspeed < 0) { pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } + //warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s); /* individual sensor publications */ @@ -455,49 +458,72 @@ handle_message(mavlink_message_t *msg) if (pub_hil_gyro < 0) { pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro); + } else { orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro); } struct accel_report accel; + accel.x_raw = imu.xacc / mg2ms2; + accel.y_raw = imu.yacc / mg2ms2; + accel.z_raw = imu.zacc / mg2ms2; + accel.x = imu.xacc; + accel.y = imu.yacc; + accel.z = imu.zacc; + accel.temperature = imu.temperature; + accel.timestamp = hrt_absolute_time(); if (pub_hil_accel < 0) { pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); } struct mag_report mag; + mag.x_raw = imu.xmag / mga2ga; + mag.y_raw = imu.ymag / mga2ga; + mag.z_raw = imu.zmag / mga2ga; + mag.x = imu.xmag; + mag.y = imu.ymag; + mag.z = imu.zmag; + mag.timestamp = hrt_absolute_time(); if (pub_hil_mag < 0) { pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag); + } else { orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag); } struct baro_report baro; + baro.pressure = imu.abs_pressure; + baro.altitude = imu.pressure_alt; + baro.temperature = imu.temperature; + baro.timestamp = hrt_absolute_time(); if (pub_hil_baro < 0) { pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro); + } else { orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro); } @@ -505,6 +531,7 @@ handle_message(mavlink_message_t *msg) /* publish combined sensor topic */ if (pub_hil_sensors > 0) { orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors); + } else { pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors); } @@ -517,6 +544,7 @@ handle_message(mavlink_message_t *msg) /* lazily publish the battery voltage */ if (pub_hil_battery > 0) { orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + } else { pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } @@ -527,7 +555,7 @@ handle_message(mavlink_message_t *msg) // output if ((timestamp - old_timestamp) > 10000000) { - printf("receiving hil sensor at %d hz\n", hil_frames/10); + printf("receiving hil sensor at %d hz\n", hil_frames / 10); old_timestamp = timestamp; hil_frames = 0; } @@ -552,9 +580,11 @@ handle_message(mavlink_message_t *msg) /* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */ float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; + /* go back to -PI..PI */ if (heading_rad > M_PI_F) heading_rad -= 2.0f * M_PI_F; + hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m @@ -567,6 +597,7 @@ handle_message(mavlink_message_t *msg) /* publish GPS measurement data */ if (pub_hil_gps > 0) { orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps); + } else { pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps); } @@ -585,6 +616,7 @@ handle_message(mavlink_message_t *msg) if (pub_hil_airspeed < 0) { pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed); + } else { orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } @@ -602,6 +634,7 @@ handle_message(mavlink_message_t *msg) if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } @@ -613,8 +646,8 @@ handle_message(mavlink_message_t *msg) /* set rotation matrix */ for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - hil_attitude.R[i][j] = C_nb(i, j); - + hil_attitude.R[i][j] = C_nb(i, j); + hil_attitude.R_valid = true; /* set quaternion */ @@ -636,22 +669,32 @@ handle_message(mavlink_message_t *msg) if (pub_hil_attitude > 0) { orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude); + } else { pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude); } struct accel_report accel; + accel.x_raw = hil_state.xacc / 9.81f * 1e3f; + accel.y_raw = hil_state.yacc / 9.81f * 1e3f; + accel.z_raw = hil_state.zacc / 9.81f * 1e3f; + accel.x = hil_state.xacc; + accel.y = hil_state.yacc; + accel.z = hil_state.zacc; + accel.temperature = 25.0f; + accel.timestamp = hrt_absolute_time(); if (pub_hil_accel < 0) { pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel); + } else { orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel); } @@ -664,6 +707,7 @@ handle_message(mavlink_message_t *msg) /* lazily publish the battery voltage */ if (pub_hil_battery > 0) { orb_publish(ORB_ID(battery_status), pub_hil_battery, &hil_battery_status); + } else { pub_hil_battery = orb_advertise(ORB_ID(battery_status), &hil_battery_status); } @@ -733,17 +777,23 @@ receive_thread(void *arg) mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink uart rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_uart_rcv", getpid()); - while (!thread_should_exit) { + struct pollfd fds[1]; + fds[0].fd = uart_fd; + fds[0].events = POLLIN; - struct pollfd fds[1]; - fds[0].fd = uart_fd; - fds[0].events = POLLIN; + ssize_t nread = 0; + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { + if (nread < sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } + /* non-blocking read. read may return negative values */ - ssize_t nread = read(uart_fd, buf, sizeof(buf)); + nread = read(uart_fd, buf, sizeof(buf)); /* if read failed, this loop won't execute */ for (ssize_t i = 0; i < nread; i++) { @@ -751,10 +801,10 @@ receive_thread(void *arg) /* handle generic messages and commands */ handle_message(&msg); - /* Handle packet with waypoint component */ + /* handle packet with waypoint component */ mavlink_wpm_message_handler(&msg, &global_pos, &local_pos); - /* Handle packet with parameter component */ + /* handle packet with parameter component */ mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c index 3175e64ce..e8d707948 100644 --- a/src/modules/mavlink/missionlib.c +++ b/src/modules/mavlink/missionlib.c @@ -170,6 +170,28 @@ bool set_special_fields(float param1, float param2, float param3, float param4, sp->param2 = param2; sp->param3 = param3; sp->param4 = param4; + + + /* define the turn distance */ + float orbit = 15.0f; + + if (command == (int)MAV_CMD_NAV_WAYPOINT) { + + orbit = param2; + + } else if (command == (int)MAV_CMD_NAV_LOITER_TURNS || + command == (int)MAV_CMD_NAV_LOITER_TIME || + command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + + orbit = param3; + } else { + + // XXX set default orbit via param + // 15 initialized above + } + + sp->turn_distance_xy = orbit; + sp->turn_distance_z = orbit; } /** @@ -223,10 +245,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int last_setpoint_index = -1; bool last_setpoint_valid = false; - /* at first waypoint, but cycled once through mission */ - if (index == 0 && last_waypoint_index > 0) { - last_setpoint_index = last_waypoint_index; - } else { + if (index > 0) { last_setpoint_index = index - 1; } @@ -251,10 +270,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1, int next_setpoint_index = -1; bool next_setpoint_valid = false; - /* at last waypoint, try to re-loop through mission as default */ - if (index == (wpm->size - 1) && wpm->size > 1) { - next_setpoint_index = 0; - } else if (wpm->size > 1) { + /* next waypoint */ + if (wpm->size > 1) { next_setpoint_index = index + 1; } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index cec2fdc43..f6860930c 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -67,6 +67,7 @@ extern bool gcs_link; struct vehicle_global_position_s global_pos; struct vehicle_local_position_s local_pos; +struct navigation_capabilities_s nav_cap; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; @@ -122,6 +123,7 @@ static void l_optical_flow(const struct listener *l); static void l_vehicle_rates_setpoint(const struct listener *l); static void l_home(const struct listener *l); static void l_airspeed(const struct listener *l); +static void l_nav_cap(const struct listener *l); static const struct listener listeners[] = { {l_sensor_combined, &mavlink_subs.sensor_sub, 0}, @@ -148,6 +150,7 @@ static const struct listener listeners[] = { {l_vehicle_rates_setpoint, &mavlink_subs.rates_setpoint_sub, 0}, {l_home, &mavlink_subs.home_sub, 0}, {l_airspeed, &mavlink_subs.airspeed_sub, 0}, + {l_nav_cap, &mavlink_subs.navigation_capabilities_sub, 0}, }; static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]); @@ -691,11 +694,24 @@ l_airspeed(const struct listener *l) mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } +void +l_nav_cap(const struct listener *l) +{ + + orb_copy(ORB_ID(navigation_capabilities), mavlink_subs.navigation_capabilities_sub, &nav_cap); + + mavlink_msg_named_value_float_send(MAVLINK_COMM_0, + hrt_absolute_time() / 1000, + "turn dist", + nav_cap.turn_distance); + +} + static void * uorb_receive_thread(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "mavlink orb rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_orb_rcv", getpid()); /* * set up poll to block for new data, @@ -837,6 +853,11 @@ uorb_receive_start(void) mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed)); orb_set_interval(mavlink_subs.airspeed_sub, 200); /* 5Hz updates */ + /* --- NAVIGATION CAPABILITIES --- */ + mavlink_subs.navigation_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities)); + orb_set_interval(mavlink_subs.navigation_capabilities_sub, 500); /* 2Hz updates */ + nav_cap.turn_distance = 0.0f; + /* start the listener loop */ pthread_attr_t uorb_attr; pthread_attr_init(&uorb_attr); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index c5cd0d03e..91773843b 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -67,6 +67,7 @@ #include <uORB/topics/airspeed.h> #include <uORB/topics/battery_status.h> #include <drivers/drv_rc_input.h> +#include <uORB/topics/navigation_capabilities.h> struct mavlink_subscriptions { int sensor_sub; @@ -92,6 +93,7 @@ struct mavlink_subscriptions { int rates_setpoint_sub; int home_sub; int airspeed_sub; + int navigation_capabilities_sub; }; extern struct mavlink_subscriptions mavlink_subs; @@ -102,6 +104,9 @@ extern struct vehicle_global_position_s global_pos; /** Local position */ extern struct vehicle_local_position_s local_pos; +/** navigation capabilities */ +extern struct navigation_capabilities_s nav_cap; + /** Vehicle status */ extern struct vehicle_status_s v_status; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 97472c233..ddad5f0df 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -251,9 +251,8 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) * * The distance calculation is based on the WGS84 geoid (GPS) */ -float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt) +float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float lon, float alt, float *dist_xy, float *dist_z) { - static uint16_t counter; if (seq < wpm->size) { mavlink_mission_item_t *wp = &(wpm->waypoints[seq]); @@ -274,20 +273,21 @@ float mavlink_wpm_distance_to_point_global_wgs84(uint16_t seq, float lat, float float dxy = radius_earth * c; float dz = alt - wp->z; + *dist_xy = fabsf(dxy); + *dist_z = fabsf(dz); + return sqrtf(dxy * dxy + dz * dz); } else { return -1.0f; } - counter++; - } /* * Calculate distance in local frame (NED) */ -float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z) +float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float z, float *dist_xy, float *dist_z) { if (seq < wpm->size) { mavlink_mission_item_t *cur = &(wpm->waypoints[seq]); @@ -296,6 +296,9 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float float dy = (cur->y - y); float dz = (cur->z - z); + *dist_xy = sqrtf(dx * dx + dy * dy); + *dist_z = fabsf(dz); + return sqrtf(dx * dx + dy * dy + dz * dz); } else { @@ -303,11 +306,13 @@ float mavlink_wpm_distance_to_point_local(uint16_t seq, float x, float y, float } } -void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos) +void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_s *global_pos, struct vehicle_local_position_s *local_pos, float turn_distance) { static uint16_t counter; - if (!global_pos->valid && !local_pos->xy_valid) { + if ((!global_pos->valid && !local_pos->xy_valid) || + /* no waypoint */ + wpm->size == 0) { /* nothing to check here, return */ return; } @@ -330,26 +335,37 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ orbit = 15.0f; } + /* keep vertical orbit */ + float vertical_switch_distance = orbit; + + /* Take the larger turn distance - orbit or turn_distance */ + if (orbit < turn_distance) + orbit = turn_distance; + int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { - dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); + dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z, &dist_xy, &dist_z); } else if (coordinate_frame == (int)MAV_FRAME_MISSION) { /* Check if conditions of mission item are satisfied */ // XXX TODO } - if (dist >= 0.f && dist <= orbit) { + if (dist >= 0.f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { wpm->pos_reached = true; } + // check if required yaw reached float yaw_sp = _wrap_pi(wpm->waypoints[wpm->current_active_wp_id].param4 / 180.0f * FM_PI); float yaw_err = _wrap_pi(yaw_sp - local_pos->yaw); @@ -463,7 +479,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ } -int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position) +int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, struct vehicle_local_position_s *local_position, struct navigation_capabilities_s *nav_cap) { /* check for timed-out operations */ if (now - wpm->timestamp_lastaction > wpm->timeout && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { @@ -486,7 +502,7 @@ int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_positio } } - check_waypoints_reached(now, global_position, local_position); + check_waypoints_reached(now, global_position, local_position, nav_cap->turn_distance); return OK; } @@ -1009,5 +1025,5 @@ void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehi } } - check_waypoints_reached(now, global_pos, local_pos); + // check_waypoints_reached(now, global_pos, local_pos); } diff --git a/src/modules/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h index 96a0ecd30..d7d6b31dc 100644 --- a/src/modules/mavlink/waypoints.h +++ b/src/modules/mavlink/waypoints.h @@ -55,6 +55,7 @@ #include <stdbool.h> #include <uORB/topics/vehicle_global_position.h> #include <uORB/topics/vehicle_local_position.h> +#include <uORB/topics/navigation_capabilities.h> // FIXME XXX - TO BE MOVED TO XML enum MAVLINK_WPM_STATES { @@ -120,7 +121,7 @@ typedef struct mavlink_wpm_storage mavlink_wpm_storage; void mavlink_wpm_init(mavlink_wpm_storage *state); int mavlink_waypoint_eventloop(uint64_t now, const struct vehicle_global_position_s *global_position, - struct vehicle_local_position_s *local_pos); + struct vehicle_local_position_s *local_pos, struct navigation_capabilities_s *nav_cap); void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos); diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index e71344982..0edb53a3e 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -167,12 +167,12 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf case 921600: speed = B921600; break; default: - fprintf(stderr, "[mavlink] ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600\n\n", baud); + warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\n\t9600\n19200\n38400\n57600\n115200\n230400\n460800\n921600", baud); return -EINVAL; } /* open uart */ - printf("[mavlink] UART is %s, baudrate is %d\n", uart_name, baud); + warnx("UART is %s, baudrate is %d", uart_name, baud); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ @@ -183,7 +183,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf if (strcmp(uart_name, "/dev/ttyACM0") != OK) { /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); + warnx("ERROR getting baudrate / termios config for %s: %d", uart_name, termios_state); close(uart); return -1; } @@ -196,14 +196,14 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf /* Set baud rate */ if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", uart_name, termios_state); close(uart); return -1; } if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - fprintf(stderr, "[mavlink] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name); + warnx("ERROR setting baudrate / termios config for %s (tcsetattr)", uart_name); close(uart); return -1; } @@ -481,9 +481,9 @@ int mavlink_thread_main(int argc, char *argv[]) static void usage() { - fprintf(stderr, "usage: mavlink start [-d <devicename>] [-b <baud rate>]\n" - " mavlink stop\n" - " mavlink status\n"); + fprintf(stderr, "usage: mavlink_onboard start [-d <devicename>] [-b <baud rate>]\n" + " mavlink_onboard stop\n" + " mavlink_onboard status\n"); exit(1); } @@ -499,7 +499,7 @@ int mavlink_onboard_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) - errx(0, "mavlink already running\n"); + errx(0, "already running"); thread_should_exit = false; mavlink_task = task_spawn_cmd("mavlink_onboard", @@ -516,7 +516,7 @@ int mavlink_onboard_main(int argc, char *argv[]) while (thread_running) { usleep(200000); } - warnx("terminated."); + warnx("terminated"); exit(0); } diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c index 0236e6126..4658bcc1d 100644 --- a/src/modules/mavlink_onboard/mavlink_receiver.c +++ b/src/modules/mavlink_onboard/mavlink_receiver.c @@ -100,11 +100,11 @@ handle_message(mavlink_message_t *msg) mavlink_msg_command_long_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))) { + || (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 */ - printf("[mavlink] Terminating .. \n"); + warnx("terminating..."); fflush(stdout); usleep(50000); @@ -132,6 +132,7 @@ handle_message(mavlink_message_t *msg) if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); } + /* publish */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } @@ -156,6 +157,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (flow_pub <= 0) { flow_pub = orb_advertise(ORB_ID(optical_flow), &f); + } else { /* publish */ orb_publish(ORB_ID(optical_flow), flow_pub, &f); @@ -186,6 +188,7 @@ handle_message(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { /* create command */ orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); @@ -203,6 +206,7 @@ handle_message(mavlink_message_t *msg) if (vicon_position_pub <= 0) { vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { orb_publish(ORB_ID(vehicle_vicon_position), vicon_position_pub, &vicon_position); } @@ -219,7 +223,7 @@ handle_message(mavlink_message_t *msg) /* switch to a receiving link mode */ gcs_link = false; - /* + /* * rate control mode - defined by MAVLink */ @@ -227,33 +231,37 @@ handle_message(mavlink_message_t *msg) bool ml_armed = false; switch (quad_motors_setpoint.mode) { - case 0: - ml_armed = false; - break; - case 1: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; - ml_armed = true; - - break; - case 2: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; - ml_armed = true; - - break; - case 3: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; - break; - case 4: - ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; - break; + case 0: + ml_armed = false; + break; + + case 1: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_RATES; + ml_armed = true; + + break; + + case 2: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; + ml_armed = true; + + break; + + case 3: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY; + break; + + case 4: + ml_mode = OFFBOARD_CONTROL_MODE_DIRECT_POSITION; + break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p3= (float)quad_motors_setpoint.yaw[mavlink_system.sysid-1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid-1]/(float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid-1] == 0) { + if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; } @@ -265,6 +273,7 @@ handle_message(mavlink_message_t *msg) /* check if topic has to be advertised */ if (offboard_control_sp_pub <= 0) { offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + } else { /* Publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); @@ -281,31 +290,36 @@ handle_message(mavlink_message_t *msg) static void * receive_thread(void *arg) { - int uart_fd = *((int*)arg); + int uart_fd = *((int *)arg); const int timeout = 1000; - uint8_t ch; + uint8_t buf[32]; mavlink_message_t msg; - prctl(PR_SET_NAME, "mavlink offb rcv", getpid()); + prctl(PR_SET_NAME, "mavlink_onboard_rcv", getpid()); - while (!thread_should_exit) { + struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; - struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } }; + ssize_t nread = 0; + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { - /* non-blocking read until buffer is empty */ - int nread = 0; + if (nread < sizeof(buf)) { + /* to avoid reading very small chunks wait for data before reading */ + usleep(1000); + } - do { - nread = read(uart_fd, &ch, 1); + /* non-blocking read. read may return negative values */ + nread = read(uart_fd, buf, sizeof(buf)); - if (mavlink_parse_char(chan, ch, &msg, &status)) { //parse the char + /* if read failed, this loop won't execute */ + for (ssize_t i = 0; i < nread; i++) { + if (mavlink_parse_char(chan, buf[i], &msg, &status)) { /* handle generic messages and commands */ handle_message(&msg); } - } while (nread > 0); + } } } @@ -319,8 +333,8 @@ receive_start(int uart) pthread_attr_init(&receiveloop_attr); struct sched_param param; - param.sched_priority = SCHED_PRIORITY_MAX - 40; - (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); + param.sched_priority = SCHED_PRIORITY_MAX - 40; + (void)pthread_attr_setschedparam(&receiveloop_attr, ¶m); pthread_attr_setstacksize(&receiveloop_attr, 2048); diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 433976656..fd3b72015 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -285,10 +285,10 @@ dsm_bind(uint16_t cmd, int pulses) /*Pulse RX pin a number of times*/ for (int i = 0; i < pulses; i++) { + up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, false); up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, true); - up_udelay(25); } break; diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 8cb21e54f..9d9ef7c6d 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -674,27 +674,25 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val #ifdef ADC_VSERVO /* PX4IO_P_STATUS_VSERVO */ { - /* - * Coefficients here derived by measurement of the 5-16V - * range on one unit: - * - * XXX pending measurements - * - * slope = xxx - * intercept = xxx - * - * Intercept corrected for best results @ 5.0V. - */ unsigned counts = adc_measure(ADC_VSERVO); if (counts != 0xffff) { - unsigned mV = (4150 + (counts * 46)) / 10 - 200; - unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VSERVO_SCALE]) / 10000; - - r_page_status[PX4IO_P_STATUS_VSERVO] = corrected; + // use 3:1 scaling on 3.3V ADC input + unsigned mV = counts * 9900 / 4096; + r_page_status[PX4IO_P_STATUS_VSERVO] = mV; + } + } +#endif +#ifdef ADC_RSSI + /* PX4IO_P_STATUS_VRSSI */ + { + unsigned counts = adc_measure(ADC_RSSI); + if (counts != 0xffff) { + // use 1:1 scaling on 3.3V ADC input + unsigned mV = counts * 3300 / 4096; + r_page_status[PX4IO_P_STATUS_VRSSI] = mV; } } #endif - /* XXX PX4IO_P_STATUS_VRSSI */ /* XXX PX4IO_P_STATUS_PRSSI */ SELECT_PAGE(r_page_status); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index be599762f..3fcacaf8f 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1224,6 +1224,9 @@ Sensors::parameter_update_poll(bool forced) void Sensors::adc_poll(struct sensor_combined_s &raw) { + /* only read if publishing */ + if (!_publishing) + return; /* rate limit to 100 Hz */ if (hrt_absolute_time() - _last_adc >= 10000) { diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index 3c1e10287..bf84b7945 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -321,6 +321,32 @@ perf_print_counter(perf_counter_t handle) } } +uint64_t +perf_event_count(perf_counter_t handle) +{ + if (handle == NULL) + return 0; + + switch (handle->type) { + case PC_COUNT: + return ((struct perf_ctr_count *)handle)->event_count; + + case PC_ELAPSED: { + struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle; + return pce->event_count; + } + + case PC_INTERVAL: { + struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + return pci->event_count; + } + + default: + break; + } + return 0; +} + void perf_print_all(void) { diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 4cd8b67a1..e1e3cbe95 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -135,6 +135,14 @@ __EXPORT extern void perf_print_all(void); */ __EXPORT extern void perf_reset_all(void); +/** + * Return current event_count + * + * @param handle The counter returned from perf_alloc. + * @return event_count + */ +__EXPORT extern uint64_t perf_event_count(perf_counter_t handle); + __END_DECLS #endif diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 1eb63a799..3514dca24 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -87,6 +87,9 @@ ORB_DEFINE(safety, struct safety_s); #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); +#include "topics/servorail_status.h" +ORB_DEFINE(servorail_status, struct servorail_status_s); + #include "topics/vehicle_global_position.h" ORB_DEFINE(vehicle_global_position, struct vehicle_global_position_s); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 1ffeda764..e4d2c92ce 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -53,6 +53,7 @@ */ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t error_count; uint16_t differential_pressure_pa; /**< Differential pressure reading */ uint16_t max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ diff --git a/src/modules/uORB/topics/servorail_status.h b/src/modules/uORB/topics/servorail_status.h new file mode 100644 index 000000000..55668790b --- /dev/null +++ b/src/modules/uORB/topics/servorail_status.h @@ -0,0 +1,67 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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 servorail_status.h + * + * Definition of the servorail status uORB topic. + */ + +#ifndef SERVORAIL_STATUS_H_ +#define SERVORAIL_STATUS_H_ + +#include "../uORB.h" +#include <stdint.h> + +/** + * @addtogroup topics + * @{ + */ + +/** + * Servorail voltages and status + */ +struct servorail_status_s { + uint64_t timestamp; /**< microseconds since system boot */ + float voltage_v; /**< Servo rail voltage in volts */ + float rssi_v; /**< RSSI pin voltage in volts */ +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(servorail_status); + +#endif diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h index 686fd765c..1a245132a 100644 --- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h +++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_attitude_setpoint_s float thrust; /**< Thrust in Newton the power system should generate */ + bool roll_reset_integral; /**< Reset roll integral part (navigation logic change) */ + }; /** diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 093c6775d..38fb74c9b 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,7 +61,6 @@ */ struct vehicle_control_mode_s { - uint16_t counter; /**< incremented by the writing thread every time new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_armed; diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index 5c8ce1e4d..a56434d3b 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -72,6 +72,8 @@ struct vehicle_global_position_setpoint_s float param2; float param3; float param4; + float turn_distance_xy; /**< The distance on the plane which will mark this as reached */ + float turn_distance_z; /**< The distance in Z direction which will mark this as reached */ }; /** diff --git a/src/systemcmds/tests/tests_file.c b/src/systemcmds/tests/tests_file.c index f36c28061..7f3a654bf 100644 --- a/src/systemcmds/tests/tests_file.c +++ b/src/systemcmds/tests/tests_file.c @@ -54,7 +54,8 @@ int test_file(int argc, char *argv[]) { - const iterations = 200; + const unsigned iterations = 100; + const unsigned alignments = 65; /* check if microSD card is mounted */ struct stat buffer; @@ -63,68 +64,190 @@ test_file(int argc, char *argv[]) return 1; } - uint8_t buf[512]; - hrt_abstime start, end; - perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); + /* perform tests for a range of chunk sizes */ + unsigned chunk_sizes[] = {1, 5, 8, 13, 16, 32, 33, 64, 70, 128, 133, 256, 300, 512, 555, 1024, 1500}; - int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - memset(buf, 0, sizeof(buf)); + for (unsigned c = 0; c < (sizeof(chunk_sizes) / sizeof(chunk_sizes[0])); c++) { - warnx("aligned write - please wait.."); + printf("\n====== FILE TEST: %u bytes chunks ======\n", chunk_sizes[c]); - if ((0x3 & (uintptr_t)buf)) - warnx("memory is unaligned!"); + for (unsigned a = 0; a < alignments; a++) { - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - write(fd, buf, sizeof(buf)); - fsync(fd); - perf_end(wperf); - } - end = hrt_absolute_time(); + printf("\n"); + warnx("----- alignment test: %u bytes -----", a); - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); - perf_print_counter(wperf); - perf_free(wperf); + /* fill write buffer with known values */ + for (int i = 0; i < sizeof(write_buf); i++) { + /* this will wrap, but we just need a known value with spacing */ + write_buf[i] = i+11; + } - int ret = unlink("/fs/microsd/testfile"); + uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64))); + hrt_abstime start, end; + //perf_counter_t wperf = perf_alloc(PC_ELAPSED, "SD writes (aligned)"); - if (ret) - err(1, "UNLINKING FILE FAILED"); + int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); - warnx("unaligned write - please wait.."); + warnx("testing unaligned writes - please wait.."); - struct { - uint8_t byte; - uint8_t unaligned[512]; - } unaligned_buf; + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + //perf_begin(wperf); + int wret = write(fd, write_buf + a, chunk_sizes[c]); - if ((0x3 & (uintptr_t)unaligned_buf.unaligned) == 0) - warnx("creating unaligned memory failed."); + if (wret != chunk_sizes[c]) { + warn("WRITE ERROR!"); - wperf = perf_alloc(PC_ELAPSED, "SD writes (unaligned)"); + if ((0x3 & (uintptr_t)(write_buf + a))) + errx(1, "memory is unaligned, align shift: %d", a); - start = hrt_absolute_time(); - for (unsigned i = 0; i < iterations; i++) { - perf_begin(wperf); - write(fd, unaligned_buf.unaligned, sizeof(unaligned_buf.unaligned)); - fsync(fd); - perf_end(wperf); - } - end = hrt_absolute_time(); + } - close(fd); + fsync(fd); + //perf_end(wperf); - warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + } + end = hrt_absolute_time(); - perf_print_counter(wperf); - perf_free(wperf); + //warnx("%dKiB in %llu microseconds", iterations / 2, end - start); + + //perf_print_counter(wperf); + //perf_free(wperf); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + /* read back data for validation */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + errx(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j + a]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a); + compare_ok = false; + break; + } + } + + if (!compare_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + /* + * ALIGNED WRITES AND UNALIGNED READS + */ + + close(fd); + int ret = unlink("/fs/microsd/testfile"); + fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT); + + warnx("testing aligned writes - please wait.."); + + start = hrt_absolute_time(); + for (unsigned i = 0; i < iterations; i++) { + int wret = write(fd, write_buf, chunk_sizes[c]); + + if (wret != chunk_sizes[c]) { + err(1, "WRITE ERROR!"); + } + + } + + fsync(fd); + + warnx("reading data aligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool align_read_ok = true; + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + /* compare value */ + bool compare_ok = true; + + for (int j = 0; j < chunk_sizes[c]; j++) { + if (read_buf[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d: %u != %u", j, (unsigned int)read_buf[j], (unsigned int)write_buf[j]); + align_read_ok = false; + break; + } + } + + if (!align_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + warnx("align read result: %s\n", (align_read_ok) ? "OK" : "ERROR"); + + warnx("reading data unaligned.."); + + close(fd); + fd = open("/fs/microsd/testfile", O_RDONLY); + + bool unalign_read_ok = true; + int unalign_read_err_count = 0; + + memset(read_buf, 0, sizeof(read_buf)); + + /* read back data unaligned */ + for (unsigned i = 0; i < iterations; i++) { + int rret = read(fd, read_buf + a, chunk_sizes[c]); + + if (rret != chunk_sizes[c]) { + err(1, "READ ERROR!"); + } + + for (int j = 0; j < chunk_sizes[c]; j++) { + + if ((read_buf + a)[j] != write_buf[j]) { + warnx("COMPARISON ERROR: byte %d, align shift: %d: %u != %u", j, a, (unsigned int)read_buf[j + a], (unsigned int)write_buf[j]); + unalign_read_ok = false; + unalign_read_err_count++; + + if (unalign_read_err_count > 10) + break; + } + } + + if (!unalign_read_ok) { + errx(1, "ABORTING FURTHER COMPARISON DUE TO ERROR"); + } + + } + + ret = unlink("/fs/microsd/testfile"); + close(fd); + + if (ret) + err(1, "UNLINKING FILE FAILED"); + + } + } /* list directory */ - DIR *d; - struct dirent *dir; + DIR *d; + struct dirent *dir; d = opendir("/fs/microsd"); if (d) { |