diff options
-rw-r--r-- | ROMFS/px4fmu_common/init.d/3033_wingwing | 2 | ||||
-rw-r--r-- | ROMFS/px4fmu_common/mixers/wingwing.mix | 51 | ||||
-rwxr-xr-x | Tools/check_submodules.sh | 26 | ||||
-rw-r--r-- | src/drivers/device/spi.cpp | 62 | ||||
-rw-r--r-- | src/drivers/device/spi.h | 2 | ||||
-rw-r--r-- | src/drivers/drv_px4flow.h | 16 | ||||
-rw-r--r-- | src/drivers/drv_range_finder.h | 9 | ||||
-rw-r--r-- | src/drivers/drv_rc_input.h | 9 | ||||
-rw-r--r-- | src/drivers/gps/gps.cpp | 4 | ||||
-rw-r--r-- | src/drivers/px4fmu/fmu.cpp | 2 | ||||
-rw-r--r-- | src/drivers/px4io/px4io_uploader.cpp | 22 | ||||
-rw-r--r-- | src/modules/commander/commander.cpp | 24 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 35 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.h | 2 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 55 |
15 files changed, 214 insertions, 107 deletions
diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index fff4b58df..3cbbd555e 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -32,7 +32,7 @@ then param set FW_THR_CRUISE 0.65 fi -set MIXER FMU_Q +set MIXER wingwing # Provide ESC a constant 1000 us pulse set PWM_OUTPUTS 4 set PWM_DISARMED 1000 diff --git a/ROMFS/px4fmu_common/mixers/wingwing.mix b/ROMFS/px4fmu_common/mixers/wingwing.mix new file mode 100644 index 000000000..08333ba5c --- /dev/null +++ b/ROMFS/px4fmu_common/mixers/wingwing.mix @@ -0,0 +1,51 @@ +Delta-wing mixer for PX4FMU +=========================== + +Designed for Wing Wing Z-84 + +This file defines mixers suitable for controlling a delta wing aircraft using +PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU +servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is +assumed to be unused. + +Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0 +(roll), 1 (pitch) and 3 (thrust). + +See the README for more information on the scaler format. + +Elevon mixers +------------- +Three scalers total (output, roll, pitch). + +On the assumption that the two elevon servos are physically reversed, the pitch +input is inverted between the two servos. + +The scaling factor for roll inputs is adjusted to implement differential travel +for the elevons. + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 6500 6500 0 -10000 10000 + +M: 2 +O: 10000 10000 0 -10000 10000 +S: 0 0 -6000 -6000 0 -10000 10000 +S: 0 1 -6500 -6500 0 -10000 10000 + +Output 2 +-------- +This mixer is empty. + +Z: + +Motor speed mixer +----------------- +Two scalers total (output, thrust). + +This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1) +range. Inputs below zero are treated as zero. + +M: 1 +O: 10000 10000 0 -10000 10000 +S: 0 3 0 20000 -10000 -10000 10000 diff --git a/Tools/check_submodules.sh b/Tools/check_submodules.sh index a178b4a38..abe3088c7 100755 --- a/Tools/check_submodules.sh +++ b/Tools/check_submodules.sh @@ -2,12 +2,19 @@ if [ -d NuttX/nuttx ]; then - STATUSRETVAL=$(git status --porcelain | grep -i "NuttX") - if [ "$STATUSRETVAL" == "" ]; then + STATUSRETVAL=$(git submodule summary | grep -A20 -i "NuttX" | grep "<") + if [ -z "$STATUSRETVAL" ]; then echo "Checked NuttX submodule, correct version found" else - echo "NuttX sub repo not at correct version. Try 'make updatesubmodules'" + echo "" + echo "" + echo "NuttX sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" exit 1 fi else @@ -18,12 +25,19 @@ fi if [ -d mavlink/include/mavlink/v1.0 ]; then - STATUSRETVAL=$(git status --porcelain | grep -i "mavlink/include/mavlink/v1.0") - if [ "$STATUSRETVAL" == "" ]; then + STATUSRETVAL=$(git submodule summary | grep -A20 -i "mavlink/include/mavlink/v1.0" | grep "<") + if [ -z "$STATUSRETVAL" ]; then echo "Checked mavlink submodule, correct version found" else - echo "mavlink sub repo not at correct version. Try 'make updatesubmodules'" + echo "" + echo "" + echo "mavlink sub repo not at correct version. Try 'git submodule update'" echo "or follow instructions on http://pixhawk.org/dev/git/submodules" + echo "" + echo "" + echo "New commits required:" + echo "$(git submodule summary)" + echo "" exit 1 fi else diff --git a/src/drivers/device/spi.cpp b/src/drivers/device/spi.cpp index fed6c312c..57cef34d2 100644 --- a/src/drivers/device/spi.cpp +++ b/src/drivers/device/spi.cpp @@ -133,26 +133,44 @@ SPI::probe() int SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) { - irqstate_t state; + int result; if ((send == nullptr) && (recv == nullptr)) return -EINVAL; + LockMode mode = up_interrupt_context() ? LOCK_NONE : locking_mode; + /* lock the bus as required */ - if (!up_interrupt_context()) { - switch (locking_mode) { - default: - case LOCK_PREEMPTION: - state = irqsave(); - break; - case LOCK_THREADS: - SPI_LOCK(_dev, true); - break; - case LOCK_NONE: - break; + switch (mode) { + default: + case LOCK_PREEMPTION: + { + irqstate_t state = irqsave(); + result = _transfer(send, recv, len); + irqrestore(state); } + break; + case LOCK_THREADS: + SPI_LOCK(_dev, true); + result = _transfer(send, recv, len); + SPI_LOCK(_dev, false); + break; + case LOCK_NONE: + result = _transfer(send, recv, len); + break; } + return result; +} +void +SPI::set_frequency(uint32_t frequency) +{ + _frequency = frequency; +} + +int +SPI::_transfer(uint8_t *send, uint8_t *recv, unsigned len) +{ SPI_SETFREQUENCY(_dev, _frequency); SPI_SETMODE(_dev, _mode); SPI_SETBITS(_dev, 8); @@ -164,27 +182,7 @@ SPI::transfer(uint8_t *send, uint8_t *recv, unsigned len) /* and clean up */ SPI_SELECT(_dev, _device, false); - if (!up_interrupt_context()) { - switch (locking_mode) { - default: - case LOCK_PREEMPTION: - irqrestore(state); - break; - case LOCK_THREADS: - SPI_LOCK(_dev, false); - break; - case LOCK_NONE: - break; - } - } - return OK; } -void -SPI::set_frequency(uint32_t frequency) -{ - _frequency = frequency; -} - } // namespace device diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 299575dc5..8e943b3f2 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -129,6 +129,8 @@ private: enum spi_mode_e _mode; uint32_t _frequency; struct spi_dev_s *_dev; + + int _transfer(uint8_t *send, uint8_t *recv, unsigned len); }; } // namespace device diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h index bef02d54e..76ec55c3e 100644 --- a/src/drivers/drv_px4flow.h +++ b/src/drivers/drv_px4flow.h @@ -47,9 +47,17 @@ #define PX4FLOW_DEVICE_PATH "/dev/px4flow" /** + * @addtogroup topics + * @{ + */ + +/** * Optical flow in NED body frame in SI units. * * @see http://en.wikipedia.org/wiki/International_System_of_Units + * + * @warning If possible the usage of the raw flow and performing rotation-compensation + * using the autopilot angular rate estimate is recommended. */ struct px4flow_report { @@ -57,14 +65,18 @@ struct px4flow_report { int16_t flow_raw_x; /**< flow in pixels in X direction, not rotation-compensated */ int16_t flow_raw_y; /**< flow in pixels in Y direction, not rotation-compensated */ - float flow_comp_x_m; /**< speed over ground in meters, rotation-compensated */ - float flow_comp_y_m; /**< speed over ground in meters, rotation-compensated */ + float flow_comp_x_m; /**< speed over ground in meters per second, rotation-compensated */ + float flow_comp_y_m; /**< speed over ground in meters per second, rotation-compensated */ float ground_distance_m; /**< Altitude / distance to ground in meters */ uint8_t quality; /**< Quality of the measurement, 0: bad quality, 255: maximum quality */ uint8_t sensor_id; /**< id of the sensor emitting the flow value */ }; +/** + * @} + */ + /* * ObjDev tag for px4flow data. */ diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index e45939b37..0f8362f58 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -51,6 +51,11 @@ enum RANGE_FINDER_TYPE { }; /** + * @addtogroup topics + * @{ + */ + +/** * range finder report structure. Reads from the device must be in multiples of this * structure. */ @@ -64,6 +69,10 @@ struct range_finder_report { uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ }; +/** + * @} + */ + /* * ObjDev tag for raw range finder data. */ diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h index 20763e265..47fa8fa59 100644 --- a/src/drivers/drv_rc_input.h +++ b/src/drivers/drv_rc_input.h @@ -68,6 +68,11 @@ #define RC_INPUT_RSSI_MAX 255 /** + * @addtogroup topics + * @{ + */ + +/** * Input signal type, value is a control position from zero to 100 * percent. */ @@ -141,6 +146,10 @@ struct rc_input_values { rc_input_t values[RC_INPUT_MAX_CHANNELS]; }; +/** + * @} + */ + /* * ObjDev tag for R/C inputs. */ diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 401b65dd4..34dd63086 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -127,7 +127,7 @@ private: /** * Try to configure the GPS, handle outgoing communication to the GPS */ - void config(); + void config(); /** * Trampoline to the worker task @@ -486,6 +486,8 @@ GPS::print_info() warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); + warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 0a4635728..d103935ae 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -329,7 +329,7 @@ PX4FMU::init() _task = task_spawn_cmd("fmuservo", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2048, + 1600, (main_t)&PX4FMU::task_main_trampoline, nullptr); diff --git a/src/drivers/px4io/px4io_uploader.cpp b/src/drivers/px4io/px4io_uploader.cpp index 7b6361a7c..d134c0246 100644 --- a/src/drivers/px4io/px4io_uploader.cpp +++ b/src/drivers/px4io/px4io_uploader.cpp @@ -39,6 +39,7 @@ #include <nuttx/config.h> #include <sys/types.h> +#include <stdlib.h> #include <stdint.h> #include <stdbool.h> #include <assert.h> @@ -413,11 +414,17 @@ static int read_with_retry(int fd, void *buf, size_t n) int PX4IO_Uploader::program(size_t fw_size) { - uint8_t file_buf[PROG_MULTI_MAX]; + uint8_t *file_buf; ssize_t count; int ret; size_t sent = 0; + file_buf = (uint8_t *)malloc(PROG_MULTI_MAX); + if (!file_buf) { + log("Can't allocate program buffer"); + return -ENOMEM; + } + log("programming %u bytes...", (unsigned)fw_size); ret = lseek(_fw_fd, 0, SEEK_SET); @@ -425,8 +432,8 @@ PX4IO_Uploader::program(size_t fw_size) while (sent < fw_size) { /* get more bytes to program */ size_t n = fw_size - sent; - if (n > sizeof(file_buf)) { - n = sizeof(file_buf); + if (n > PROG_MULTI_MAX) { + n = PROG_MULTI_MAX; } count = read_with_retry(_fw_fd, file_buf, n); @@ -438,8 +445,10 @@ PX4IO_Uploader::program(size_t fw_size) (int)errno); } - if (count == 0) + if (count == 0) { + free(file_buf); return OK; + } sent += count; @@ -455,9 +464,12 @@ PX4IO_Uploader::program(size_t fw_size) ret = get_sync(1000); - if (ret != OK) + if (ret != OK) { + free(file_buf); return ret; + } } + free(file_buf); return OK; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2058cd92e..18761665a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -286,15 +286,22 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("\tcommander is running"); - print_status(); + /* commands needing the app to run below */ + if (!thread_running) { + warnx("\tcommander not started"); + exit(1); + } - } else { - warnx("\tcommander not started"); - } + if (!strcmp(argv[1], "status")) { + print_status(); + exit(0); + } + if (!strcmp(argv[1], "check")) { + int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0); + int checkres = prearm_check(&status, mavlink_fd_local); + close(mavlink_fd_local); + warnx("FINAL RESULT: %s", (checkres == 0) ? "OK" : "FAILED"); exit(0); } @@ -303,7 +310,7 @@ int commander_main(int argc, char *argv[]) exit(0); } - if (!strcmp(argv[1], "2")) { + if (!strcmp(argv[1], "disarm")) { arm_disarm(false, mavlink_fd, "command line"); exit(0); } @@ -324,6 +331,7 @@ void usage(const char *reason) void print_status() { + warnx("type: %s", (status.is_rotary_wing) ? "ROTARY" : "PLANE"); warnx("usb powered: %s", (on_usb_power) ? "yes" : "no"); /* read all relevant states */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f..363f5e8eb 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -70,8 +70,6 @@ #endif static const int ERROR = -1; -static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); - // This array defines the arming state transitions. The rows are the new state, and the columns // are the current state. Using new state and current state you can index into the array which // will be true for a valid transition or false for a invalid transition. In some cases even @@ -622,12 +620,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) { int ret; + bool failed = false; int fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING"); - ret = fd; + failed = true; goto system_eval; } @@ -635,6 +634,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret != OK) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION"); + failed = true; goto system_eval; } @@ -644,33 +644,31 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) if (ret == sizeof(acc)) { /* evaluate values */ - float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - - if (accel_scale < 9.78f || accel_scale > 9.83f) { - mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended."); - } + float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z); - if (accel_scale > 30.0f /* m/s^2 */) { + if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE"); + mavlink_log_info(mavlink_fd, "#audio: hold still while arming"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; - } else { - ret = OK; } } else { mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } if (!status->is_rotary_wing) { + + /* accel done, close it */ + close(fd); fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + if (fd <= 0) { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING"); - ret = fd; + failed = true; goto system_eval; } @@ -679,20 +677,19 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) ret = read(fd, &diff_pres, sizeof(diff_pres)); if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { + if (fabsf(diff_pres.differential_pressure_filtered_pa > 6.0f)) { mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); // XXX do not make this fatal yet - ret = OK; } } else { mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); /* this is frickin' fatal */ - ret = ERROR; + failed = true; goto system_eval; } } system_eval: close(fd); - return ret; + return (failed); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 11072403e..ddc5bf154 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -67,4 +67,6 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished); +int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd); + #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 667be87b7..c7ad605c5 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1051,10 +1051,16 @@ protected: uint32_t mavlink_custom_mode; get_mavlink_mode_state(&status, &pos_sp_triplet, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); + float out[8]; + + const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + + /* scale outputs depending on system type */ if (mavlink_system.type == MAV_TYPE_QUADROTOR || mavlink_system.type == MAV_TYPE_HEXAROTOR || mavlink_system.type == MAV_TYPE_OCTOROTOR) { - /* set number of valid outputs depending on vehicle type */ + /* multirotors: set number of rotor outputs depending on type */ + unsigned n; switch (mavlink_system.type) { @@ -1071,59 +1077,44 @@ protected: break; } - /* scale / assign outputs depending on system type */ - float out[8]; - for (unsigned i = 0; i < 8; i++) { - if (i < n) { - if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - /* scale fake PWM out 900..2100 us to 0..1 for normal multirotors */ + if (mavlink_base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + if (i < n) { + /* scale PWM out 900..2100 us to 0..1 for rotors */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } else { - /* send 0 when disarmed */ - out[i] = 0.0f; + /* scale PWM out 900..2100 us to -1..1 for other channels */ + out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } } else { - out[i] = -1.0f; + /* send 0 when disarmed */ + out[i] = 0.0f; } } - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); } else { - - /* fixed wing: scale all channels except throttle -1 .. 1 - * because we know that we set the mixers up this way - */ - - float out[8]; - - const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2; + /* fixed wing: scale throttle to 0..1 and other channels to -1..1 */ for (unsigned i = 0; i < 8; i++) { if (i != 3) { - /* scale fake PWM out 900..2100 us to -1..+1 for normal channels */ + /* scale PWM out 900..2100 us to -1..1 for normal channels */ out[i] = (act.output[i] - pwm_center) / ((PWM_HIGHEST_MAX - PWM_LOWEST_MIN) / 2); } else { - - /* scale fake PWM out 900..2100 us to 0..1 for throttle */ + /* scale PWM out 900..2100 us to 0..1 for throttle */ out[i] = (act.output[i] - PWM_LOWEST_MIN) / (PWM_HIGHEST_MAX - PWM_LOWEST_MIN); } } - - mavlink_msg_hil_controls_send(_channel, - hrt_absolute_time(), - out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], - mavlink_base_mode, - 0); } + + mavlink_msg_hil_controls_send(_channel, + hrt_absolute_time(), + out[0], out[1], out[2], out[3], out[4], out[5], out[6], out[7], + mavlink_base_mode, + 0); } } }; |