diff options
55 files changed, 2014 insertions, 678 deletions
diff --git a/Debug/ARMv7M b/Debug/ARMv7M index 3d96da682..803d96453 100644 --- a/Debug/ARMv7M +++ b/Debug/ARMv7M @@ -19,120 +19,132 @@ end define vecstate - set $icsr = *(uint32_t *)0xe000ed04 + set $icsr = *(unsigned *)0xe000ed04 set $vect = $icsr & 0x1ff set $pend = ($icsr & 0x1ff000) >> 12 - set $shcsr = *(uint32_t *)0xe000ed24 - set $cfsr = *(uint32_t *)0xe000ed28 + set $shcsr = *(unsigned *)0xe000ed24 + set $cfsr = *(unsigned *)0xe000ed28 set $mmfsr = $cfsr & 0xff set $bfsr = ($cfsr >> 8) & 0xff set $ufsr = ($cfsr >> 16) & 0xffff - set $hfsr = *(uint32_t *)0xe000ed2c - set $bfar = *(uint32_t *)0xe000ed38 - set $mmfar = *(uint32_t *)0xe000ed34 + set $hfsr = *(unsigned *)0xe000ed2c + set $bfar = *(unsigned *)0xe000ed38 + set $mmfar = *(unsigned *)0xe000ed34 - # XXX Currently, rather than look at $vect, we just decode the - # fault status registers directly. + if $vect < 15 - if $hfsr != 0 - printf "HardFault:" - if $hfsr & (1<<1) - printf " due to vector table read fault\n" + if $hfsr != 0 + printf "HardFault:" + if $hfsr & (1<<1) + printf " due to vector table read fault\n" + end + if $hfsr & (1<<30) + printf " forced due to escalated or disabled configurable fault (see below)\n" + end + if $hfsr & (1<<31) + printf " due to an unexpected debug event\n" + end + end + if $mmfsr != 0 + printf "MemManage:" + if $mmfsr & (1<<5) + printf " during lazy FP state save" + end + if $mmfsr & (1<<4) + printf " during exception entry" + end + if $mmfsr & (1<<3) + printf " during exception return" + end + if $mmfsr & (1<<0) + printf " during data access" + end + if $mmfsr & (1<<0) + printf " during instruction prefetch" + end + if $mmfsr & (1<<7) + printf " accessing 0x%08x", $mmfar + end + printf "\n" + end + if $bfsr != 0 + printf "BusFault:" + if $bfsr & (1<<2) + printf " (imprecise)" + end + if $bfsr & (1<<1) + printf " (precise)" + end + if $bfsr & (1<<5) + printf " during lazy FP state save" + end + if $bfsr & (1<<4) + printf " during exception entry" + end + if $bfsr & (1<<3) + printf " during exception return" + end + if $bfsr & (1<<0) + printf " during instruction prefetch" + end + if $bfsr & (1<<7) + printf " accessing 0x%08x", $bfar + end + printf "\n" + end + if $ufsr != 0 + printf "UsageFault" + if $ufsr & (1<<9) + printf " due to divide-by-zero" + end + if $ufsr & (1<<8) + printf " due to unaligned memory access" + end + if $ufsr & (1<<3) + printf " due to access to disabled/absent coprocessor" + end + if $ufsr & (1<<2) + printf " due to a bad EXC_RETURN value" + end + if $ufsr & (1<<1) + printf " due to bad T or IT bits in EPSR" + end + if $ufsr & (1<<0) + printf " due to executing an undefined instruction" + end + printf "\n" end - if $hfsr & (1<<30) - printf " forced ue to escalated or disabled configurable fault (see below)\n" - end - if $hfsr & (1<<31) - printf " due to an unexpected debug event\n" - end - end - if $mmfsr != 0 - printf "MemManage:" - if $mmfsr & (1<<5) - printf " during lazy FP state save" - end - if $mmfsr & (1<<4) - printf " during exception entry" - end - if $mmfsr & (1<<3) - printf " during exception return" - end - if $mmfsr & (1<<0) - printf " during data access" - end - if $mmfsr & (1<<0) - printf " during instruction prefetch" - end - if $mmfsr & (1<<7) - printf " accessing 0x%08x", $mmfar - end - printf "\n" - end - if $bfsr != 0 - printf "BusFault:" - if $bfsr & (1<<2) - printf " (imprecise)" - end - if $bfsr & (1<<1) - printf " (precise)" - end - if $bfsr & (1<<5) - printf " during lazy FP state save" - end - if $bfsr & (1<<4) - printf " during exception entry" - end - if $bfsr & (1<<3) - printf " during exception return" - end - if $bfsr & (1<<0) - printf " during instruction prefetch" - end - if $bfsr & (1<<7) - printf " accessing 0x%08x", $bfar - end - printf "\n" - end - if $ufsr != 0 - printf "UsageFault" - if $ufsr & (1<<9) - printf " due to divide-by-zero" - end - if $ufsr & (1<<8) - printf " due to unaligned memory access" - end - if $ufsr & (1<<3) - printf " due to access to disabled/absent coprocessor" - end - if $ufsr & (1<<2) - printf " due to a bad EXC_RETURN value" - end - if $ufsr & (1<<1) - printf " due to bad T or IT bits in EPSR" - end - if $ufsr & (1<<0) - printf " due to executing an undefined instruction" + else + if $vect >= 15 + printf "Handling vector %u\n", $vect end - printf "\n" end - if ((uint32_t)$lr & 0xf0000000) == 0xf0000000 + if ((unsigned)$lr & 0xf0000000) == 0xf0000000 if ($lr & 1) - set $frame_ptr = (uint32_t *)$msp + printf "exception frame is on MSP\n" + #set $frame_ptr = (unsigned *)$msp + set $frame_ptr = (unsigned *)$sp else - set $frame_ptr = (uint32_t *)$psp + printf "exception frame is on PSP, backtrace may not be possible\n" + #set $frame_ptr = (unsigned *)$psp + set $frame_ptr = (unsigned *)$sp end - printf " r0: %08x r1: %08x r2: %08x r3: %08x\n, $frame_ptr[0], $frame_ptr[1], $frame_ptr[2], $frame_ptr[3] + if $lr & 0x10 + set $fault_sp = $frame_ptr + (8 * 4) + else + set $fault_sp = $frame_ptr + (26 * 4) + end + + + printf " r0: %08x r1: %08x r2: %08x r3: %08x\n", $frame_ptr[0], $frame_ptr[1], $frame_ptr[2], $frame_ptr[3] printf " r4: %08x r5: %08x r6: %08x r7: %08x\n", $r4, $r5, $r6, $r7 printf " r8: %08x r9: %08x r10: %08x r11: %08x\n", $r8, $r9, $r10, $r11 - printf " r12: $08x lr: %08x pc: %08xx PSR: %08x\n", $frame_ptr[4], $frame_ptr[5], $frame_ptr[6], $frame_ptr[7] + printf " r12: %08x\n", $frame_ptr[4] + printf " sp: %08x lr: %08x pc: %08x PSR: %08x\n", $fault_sp, $frame_ptr[5], $frame_ptr[6], $frame_ptr[7] # Swap to the context of the faulting code and try to print a backtrace set $saved_sp = $sp - if $lr & 0x10 - set $sp = $frame_ptr + (8 * 4) - else - set $sp = $frame_ptr + (26 * 4) - end + set $sp = $fault_sp set $saved_lr = $lr set $lr = $frame_ptr[5] set $saved_pc = $pc @@ -142,7 +154,7 @@ define vecstate set $lr = $saved_lr set $pc = $saved_pc else - printf "(not currently in exception state)\n" + printf "(not currently in exception handler)\n" end end diff --git a/ROMFS/logging/logconv.m b/ROMFS/logging/logconv.m index 5ea2aeb95..92ee01413 100755 --- a/ROMFS/logging/logconv.m +++ b/ROMFS/logging/logconv.m @@ -33,7 +33,7 @@ end % float vbat; //battery voltage in [volt] % float bat_current - current drawn from battery at this time instant % float bat_discharged - discharged energy in mAh -% float adc[3]; //remaining auxiliary ADC ports [volt] +% float adc[4]; //ADC ports [volt] % float local_position[3]; //tangent plane mapping into x,y,z [m] % int32_t gps_raw_position[3]; //latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] % float attitude[3]; //pitch, roll, yaw [rad] @@ -57,7 +57,7 @@ logFormat{9} = struct('name', 'actuators', 'bytes', 4, 'array', 8, ' logFormat{10} = struct('name', 'vbat', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{11} = struct('name', 'bat_current', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{12} = struct('name', 'bat_discharged', 'bytes', 4, 'array', 1, 'precision', 'float', 'machineformat', 'ieee-le'); -logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); +logFormat{13} = struct('name', 'adc', 'bytes', 4, 'array', 4, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{14} = struct('name', 'local_position', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); logFormat{15} = struct('name', 'gps_raw_position', 'bytes', 4, 'array', 3, 'precision', 'uint32', 'machineformat', 'ieee-le'); logFormat{16} = struct('name', 'attitude', 'bytes', 4, 'array', 3, 'precision', 'float', 'machineformat', 'ieee-le'); diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a3e8fb745..aaabe7f4b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -76,6 +76,8 @@ #include <uORB/topics/subsystem_info.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/parameter_update.h> +#include <uORB/topics/differential_pressure.h> +#include <mavlink/mavlink_log.h> #include <drivers/drv_led.h> #include <drivers/drv_hrt.h> @@ -799,6 +801,72 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } +void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +{ + /* announce change */ + + mavlink_log_info(mavlink_fd, "keep it still"); + + const int calibration_count = 2500; + + int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + + int calibration_counter = 0; + float airspeed_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure); + airspeed_offset += differential_pressure.voltage; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + airspeed_offset = airspeed_offset / calibration_count; + + if (isfinite(airspeed_offset)) { + + if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + close(sub_differential_pressure); +} + void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) @@ -1018,6 +1086,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta handled = true; } + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { + + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + mavlink_log_info(mavlink_fd, "starting airspeed cal"); + tune_confirm(); + do_airspeed_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished airspeed cal"); + tune_confirm(); + // XXX if this fails, go to ERROR + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + /* none found */ if (!handled) { //warnx("refusing unsupported calibration request\n"); @@ -1438,6 +1528,11 @@ int commander_thread_main(int argc, char *argv[]) struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s differential_pressure; + memset(&differential_pressure, 0, sizeof(differential_pressure)); + uint64_t last_differential_pressure_time = 0; + /* Subscribe to command topic */ int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); struct vehicle_command_s cmd; @@ -1491,6 +1586,13 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); } + orb_check(differential_pressure_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure); + last_differential_pressure_time = differential_pressure.timestamp; + } + orb_check(cmd_sub, &new_data); if (new_data) { @@ -1691,6 +1793,8 @@ int commander_thread_main(int argc, char *argv[]) // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; bool global_pos_valid = current_status.condition_global_position_valid; bool local_pos_valid = current_status.condition_local_position_valid; + bool airspeed_valid = current_status.condition_airspeed_valid; + /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { @@ -1709,6 +1813,14 @@ int commander_thread_main(int argc, char *argv[]) current_status.condition_local_position_valid = false; } + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_differential_pressure_time < 2000000) { + current_status.condition_airspeed_valid = true; + + } else { + current_status.condition_airspeed_valid = false; + } + /* * Consolidate global position and local position valid flags * for vector flight mode. @@ -1721,12 +1833,13 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_vector_flight_mode_ok = false; // } - // /* consolidate state change, flag as changed if required */ - // if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || - // global_pos_valid != current_status.flag_global_position_valid || - // local_pos_valid != current_status.flag_local_position_valid) { - // state_changed = true; - // } + // XXX why is this needed? + /* consolidate state change, flag as changed if required */ + if (global_pos_valid != current_status.condition_global_position_valid || + local_pos_valid != current_status.condition_local_position_valid || + airspeed_valid != current_status.condition_airspeed_valid) { + state_changed = true; + } /* * Mark the position of the first position lock as return to launch (RTL) diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 584ca2306..1768bfee4 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -325,7 +325,7 @@ void BlockMultiModeBacksideAutopilot::update() _att.roll, _att.pitch, _att.yaw, _att.rollspeed, _att.pitchspeed, _att.yawspeed ); - _actuators.control[CH_AIL] = - _backsideAutopilot.getAileron(); + _actuators.control[CH_AIL] = _backsideAutopilot.getAileron(); _actuators.control[CH_ELV] = - _backsideAutopilot.getElevator(); _actuators.control[CH_RDR] = _backsideAutopilot.getRudder(); _actuators.control[CH_THR] = _backsideAutopilot.getThrottle(); diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index ceb2d987d..9a1407d65 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -127,7 +127,7 @@ class BlinkM : public device::I2C { public: BlinkM(int bus, int blinkm); - ~BlinkM(); + virtual ~BlinkM(); virtual int init(); diff --git a/apps/drivers/bma180/bma180.cpp b/apps/drivers/bma180/bma180.cpp index 32eb5333e..4409a8a9c 100644 --- a/apps/drivers/bma180/bma180.cpp +++ b/apps/drivers/bma180/bma180.cpp @@ -126,7 +126,7 @@ class BMA180 : public device::SPI { public: BMA180(int bus, spi_dev_e device); - ~BMA180(); + virtual ~BMA180(); virtual int init(); diff --git a/apps/drivers/drv_hrt.h b/apps/drivers/drv_hrt.h index 3b493a81a..8a99eeca7 100644 --- a/apps/drivers/drv_hrt.h +++ b/apps/drivers/drv_hrt.h @@ -92,6 +92,22 @@ __EXPORT extern hrt_abstime ts_to_abstime(struct timespec *ts); __EXPORT extern void abstime_to_ts(struct timespec *ts, hrt_abstime abstime); /* + * Compute the delta between a timestamp taken in the past + * and now. + * + * This function is safe to use even if the timestamp is updated + * by an interrupt during execution. + */ +__EXPORT extern hrt_abstime hrt_elapsed_time(const volatile hrt_abstime *then); + +/* + * Store the absolute time in an interrupt-safe fashion. + * + * This function ensures that the timestamp cannot be seen half-written by an interrupt handler. + */ +__EXPORT extern hrt_abstime hrt_store_absolute_time(volatile hrt_abstime *now); + +/* * Call callout(arg) after delay has elapsed. * * If callout is NULL, this can be used to implement a timeout by testing the call diff --git a/apps/drivers/drv_range_finder.h b/apps/drivers/drv_range_finder.h new file mode 100644 index 000000000..03a82ec5d --- /dev/null +++ b/apps/drivers/drv_range_finder.h @@ -0,0 +1,81 @@ +/**************************************************************************** + * + * Copyright (C) 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 Rangefinder driver interface. + */ + +#ifndef _DRV_RANGEFINDER_H +#define _DRV_RANGEFINDER_H + +#include <stdint.h> +#include <sys/ioctl.h> + +#include "drv_sensor.h" +#include "drv_orb_dev.h" + +#define RANGE_FINDER_DEVICE_PATH "/dev/range_finder" + +/** + * range finder report structure. Reads from the device must be in multiples of this + * structure. + */ +struct range_finder_report { + uint64_t timestamp; + float distance; /** in meters */ + uint8_t valid; /** 1 == within sensor range, 0 = outside sensor range */ +}; + +/* + * ObjDev tag for raw range finder data. + */ +ORB_DECLARE(sensor_range_finder); + +/* + * ioctl() definitions + * + * Rangefinder drivers also implement the generic sensor driver + * interfaces from drv_sensor.h + */ + +#define _RANGEFINDERIOCBASE (0x7900) +#define __RANGEFINDERIOC(_n) (_IOC(_RANGEFINDERIOCBASE, _n)) + +/** set the minimum effective distance of the device */ +#define RANGEFINDERIOCSETMINIUMDISTANCE __RANGEFINDERIOC(1) + +/** set the maximum effective distance of the device */ +#define RANGEFINDERIOCSETMAXIUMDISTANCE __RANGEFINDERIOC(2) + + +#endif /* _DRV_RANGEFINDER_H */ diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 6d7cfcc68..e35bdb944 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -67,7 +67,7 @@ #include "mtk.h" -#define TIMEOUT_5HZ 400 +#define TIMEOUT_5HZ 500 #define RATE_MEASUREMENT_PERIOD 5000000 /* oddly, ERROR is not defined for c++ */ @@ -86,7 +86,7 @@ class GPS : public device::CDev { public: GPS(const char* uart_path); - ~GPS(); + virtual ~GPS(); virtual int init(); diff --git a/apps/drivers/hil/hil.cpp b/apps/drivers/hil/hil.cpp index fe9b281f6..861ed7924 100644 --- a/apps/drivers/hil/hil.cpp +++ b/apps/drivers/hil/hil.cpp @@ -91,7 +91,7 @@ public: MODE_NONE }; HIL(); - ~HIL(); + virtual ~HIL(); virtual int ioctl(file *filp, int cmd, unsigned long arg); diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 4a201b98c..8ab568282 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -130,7 +130,7 @@ class HMC5883 : public device::I2C { public: HMC5883(int bus); - ~HMC5883(); + virtual ~HMC5883(); virtual int init(); diff --git a/apps/drivers/l3gd20/l3gd20.cpp b/apps/drivers/l3gd20/l3gd20.cpp index f2f585f42..6227df72a 100644 --- a/apps/drivers/l3gd20/l3gd20.cpp +++ b/apps/drivers/l3gd20/l3gd20.cpp @@ -152,7 +152,7 @@ class L3GD20 : public device::SPI { public: L3GD20(int bus, const char* path, spi_dev_e device); - ~L3GD20(); + virtual ~L3GD20(); virtual int init(); diff --git a/apps/drivers/led/led.cpp b/apps/drivers/led/led.cpp index 12d864be2..c7c45525e 100644 --- a/apps/drivers/led/led.cpp +++ b/apps/drivers/led/led.cpp @@ -53,7 +53,7 @@ class LED : device::CDev { public: LED(); - ~LED(); + virtual ~LED(); virtual int init(); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); diff --git a/apps/drivers/mb12xx/Makefile b/apps/drivers/mb12xx/Makefile new file mode 100644 index 000000000..0d2405787 --- /dev/null +++ b/apps/drivers/mb12xx/Makefile @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (C) 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. +# +############################################################################ + +# +# Makefile to build the Maxbotix Sonar driver. +# + +APPNAME = mb12xx +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/mb12xx/mb12xx.cpp b/apps/drivers/mb12xx/mb12xx.cpp new file mode 100644 index 000000000..9d0f6bddc --- /dev/null +++ b/apps/drivers/mb12xx/mb12xx.cpp @@ -0,0 +1,840 @@ +/**************************************************************************** + * + * Copyright (C) 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 mb12xx.cpp + * @author Greg Hulands + * + * Driver for the Maxbotix sonar range finders connected via I2C. + */ + +#include <nuttx/config.h> + +#include <drivers/device/i2c.h> + +#include <sys/types.h> +#include <stdint.h> +#include <stdlib.h> +#include <stdbool.h> +#include <semaphore.h> +#include <string.h> +#include <fcntl.h> +#include <poll.h> +#include <errno.h> +#include <stdio.h> +#include <math.h> +#include <unistd.h> + +#include <nuttx/arch.h> +#include <nuttx/wqueue.h> +#include <nuttx/clock.h> + +#include <arch/board/board.h> + +#include <systemlib/perf_counter.h> +#include <systemlib/err.h> + +#include <drivers/drv_hrt.h> +#include <drivers/drv_range_finder.h> + +#include <uORB/uORB.h> +#include <uORB/topics/subsystem_info.h> + +/* Configuration Constants */ +#define MB12XX_BUS PX4_I2C_BUS_EXPANSION +#define MB12XX_BASEADDR 0x70 /* 7-bit address. 8-bit address is 0xE0 */ + +/* MB12xx Registers addresses */ + +#define MB12XX_TAKE_RANGE_REG 0x51 /* Measure range Register */ +#define MB12XX_SET_ADDRESS_1 0xAA /* Change address 1 Register */ +#define MB12XX_SET_ADDRESS_2 0xA5 /* Change address 2 Register */ + +/* Device limits */ +#define MB12XX_MIN_DISTANCE (0.20f) +#define MB12XX_MAX_DISTANCE (7.65f) + +#define MB12XX_CONVERSION_INTERVAL 60000 /* 60ms */ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + +#ifndef CONFIG_SCHED_WORKQUEUE +# error This requires CONFIG_SCHED_WORKQUEUE. +#endif + +class MB12XX : public device::I2C +{ +public: + MB12XX(int bus = MB12XX_BUS, int address = MB12XX_BASEADDR); + virtual ~MB12XX(); + + virtual int init(); + + virtual ssize_t read(struct file *filp, char *buffer, size_t buflen); + virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + + /** + * Diagnostics - print some basic information about the driver. + */ + void print_info(); + +protected: + virtual int probe(); + +private: + float _min_distance; + float _max_distance; + work_s _work; + unsigned _num_reports; + volatile unsigned _next_report; + volatile unsigned _oldest_report; + range_finder_report *_reports; + bool _sensor_ok; + int _measure_ticks; + bool _collect_phase; + + orb_advert_t _range_finder_topic; + + perf_counter_t _sample_perf; + perf_counter_t _comms_errors; + perf_counter_t _buffer_overflows; + + /** + * Test whether the device supported by the driver is present at a + * specific address. + * + * @param address The I2C bus address to probe. + * @return True if the device is present. + */ + int probe_address(uint8_t address); + + /** + * Initialise the automatic measurement state machine and start it. + * + * @note This function is called at open and error time. It might make sense + * to make it more aggressive about resetting the bus in case of errors. + */ + void start(); + + /** + * Stop the automatic measurement state machine. + */ + void stop(); + + /** + * Set the min and max distance thresholds if you want the end points of the sensors + * range to be brought in at all, otherwise it will use the defaults MB12XX_MIN_DISTANCE + * and MB12XX_MAX_DISTANCE + */ + void set_minimum_distance(float min); + void set_maximum_distance(float max); + float get_minimum_distance(); + float get_maximum_distance(); + + /** + * Perform a poll cycle; collect from the previous measurement + * and start a new one. + */ + void cycle(); + int measure(); + int collect(); + /** + * Static trampoline from the workq context; because we don't have a + * generic workq wrapper yet. + * + * @param arg Instance pointer for the driver that is polling. + */ + static void cycle_trampoline(void *arg); + + +}; + +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + +/* + * Driver 'main' command. + */ +extern "C" __EXPORT int mb12xx_main(int argc, char *argv[]); + +MB12XX::MB12XX(int bus, int address) : + I2C("MB12xx", RANGE_FINDER_DEVICE_PATH, bus, address, 100000), + _min_distance(MB12XX_MIN_DISTANCE), + _max_distance(MB12XX_MAX_DISTANCE), + _num_reports(0), + _next_report(0), + _oldest_report(0), + _reports(nullptr), + _sensor_ok(false), + _measure_ticks(0), + _collect_phase(false), + _range_finder_topic(-1), + _sample_perf(perf_alloc(PC_ELAPSED, "mb12xx_read")), + _comms_errors(perf_alloc(PC_COUNT, "mb12xx_comms_errors")), + _buffer_overflows(perf_alloc(PC_COUNT, "mb12xx_buffer_overflows")) +{ + // enable debug() calls + _debug_enabled = true; + + // work_cancel in the dtor will explode if we don't do this... + memset(&_work, 0, sizeof(_work)); +} + +MB12XX::~MB12XX() +{ + /* make sure we are truly inactive */ + stop(); + + /* free any existing reports */ + if (_reports != nullptr) + delete[] _reports; +} + +int +MB12XX::init() +{ + int ret = ERROR; + + /* do I2C init (and probe) first */ + if (I2C::init() != OK) + goto out; + + /* allocate basic report buffers */ + _num_reports = 2; + _reports = new struct range_finder_report[_num_reports]; + + if (_reports == nullptr) + goto out; + + _oldest_report = _next_report = 0; + + /* get a publish handle on the range finder topic */ + memset(&_reports[0], 0, sizeof(_reports[0])); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &_reports[0]); + + if (_range_finder_topic < 0) + debug("failed to create sensor_range_finder object. Did you start uOrb?"); + + ret = OK; + /* sensor is ok, but we don't really know if it is within range */ + _sensor_ok = true; +out: + return ret; +} + +int +MB12XX::probe() +{ + return measure(); +} + +void +MB12XX::set_minimum_distance(float min) +{ + _min_distance = min; +} + +void +MB12XX::set_maximum_distance(float max) +{ + _max_distance = max; +} + +float +MB12XX::get_minimum_distance() +{ + return _min_distance; +} + +float +MB12XX::get_maximum_distance() +{ + return _max_distance; +} + +int +MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg) +{ + switch (cmd) { + + case SENSORIOCSPOLLRATE: { + switch (arg) { + + /* switching to manual polling */ + case SENSOR_POLLRATE_MANUAL: + stop(); + _measure_ticks = 0; + return OK; + + /* external signalling (DRDY) not supported */ + case SENSOR_POLLRATE_EXTERNAL: + + /* zero would be bad */ + case 0: + return -EINVAL; + + /* set default/max polling rate */ + case SENSOR_POLLRATE_MAX: + case SENSOR_POLLRATE_DEFAULT: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* set interval for next measurement to minimum legal value */ + _measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL); + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + + /* adjust to a legal polling interval in Hz */ + default: { + /* do we need to start internal polling? */ + bool want_start = (_measure_ticks == 0); + + /* convert hz to tick interval via microseconds */ + unsigned ticks = USEC2TICK(1000000 / arg); + + /* check against maximum rate */ + if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL)) + return -EINVAL; + + /* update interval for next measurement */ + _measure_ticks = ticks; + + /* if we need to start the poll state machine, do it */ + if (want_start) + start(); + + return OK; + } + } + } + + case SENSORIOCGPOLLRATE: + if (_measure_ticks == 0) + return SENSOR_POLLRATE_MANUAL; + + return (1000 / _measure_ticks); + + case SENSORIOCSQUEUEDEPTH: { + /* add one to account for the sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct range_finder_report *buf = new struct range_finder_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _reports; + _num_reports = arg; + _reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_reports - 1; + + case SENSORIOCRESET: + /* XXX implement this */ + return -EINVAL; + + case RANGEFINDERIOCSETMINIUMDISTANCE: + { + set_minimum_distance(*(float *)arg); + return 0; + } + break; + case RANGEFINDERIOCSETMAXIUMDISTANCE: + { + set_maximum_distance(*(float *)arg); + return 0; + } + break; + default: + /* give it to the superclass */ + return I2C::ioctl(filp, cmd, arg); + } +} + +ssize_t +MB12XX::read(struct file *filp, char *buffer, size_t buflen) +{ + unsigned count = buflen / sizeof(struct range_finder_report); + int ret = 0; + + /* buffer must be large enough */ + if (count < 1) + return -ENOSPC; + + /* if automatic measurement is enabled */ + if (_measure_ticks > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the workq thread while we are doing this; + * we are careful to avoid racing with them. + */ + while (count--) { + if (_oldest_report != _next_report) { + memcpy(buffer, _reports + _oldest_report, sizeof(*_reports)); + ret += sizeof(_reports[0]); + INCREMENT(_oldest_report, _num_reports); + } + } + + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement - run one conversion */ + /* XXX really it'd be nice to lock against other readers here */ + do { + _oldest_report = _next_report = 0; + + /* trigger a measurement */ + if (OK != measure()) { + ret = -EIO; + break; + } + + /* wait for it to complete */ + usleep(MB12XX_CONVERSION_INTERVAL); + + /* run the collection phase */ + if (OK != collect()) { + ret = -EIO; + break; + } + + /* state machine will have generated a report, copy it out */ + memcpy(buffer, _reports, sizeof(*_reports)); + ret = sizeof(*_reports); + + } while (0); + + return ret; +} + +int +MB12XX::measure() +{ + int ret; + + /* + * Send the command to begin a measurement. + */ + uint8_t cmd = MB12XX_TAKE_RANGE_REG; + ret = transfer(&cmd, 1, nullptr, 0); + + if (OK != ret) + { + perf_count(_comms_errors); + log("i2c::transfer returned %d", ret); + return ret; + } + ret = OK; + + return ret; +} + +int +MB12XX::collect() +{ + int ret = -EIO; + + /* read from the sensor */ + uint8_t val[2] = {0, 0}; + + perf_begin(_sample_perf); + + ret = transfer(nullptr, 0, &val[0], 2); + + if (ret < 0) + { + log("error reading from sensor: %d", ret); + return ret; + } + + uint16_t distance = val[0] << 8 | val[1]; + float si_units = (distance * 1.0f)/ 100.0f; /* cm to m */ + /* this should be fairly close to the end of the measurement, so the best approximation of the time */ + _reports[_next_report].timestamp = hrt_absolute_time(); + _reports[_next_report].distance = si_units; + _reports[_next_report].valid = si_units > get_minimum_distance() && si_units < get_maximum_distance() ? 1 : 0; + + /* publish it */ + orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &_reports[_next_report]); + + /* post a report to the ring - note, not locked */ + INCREMENT(_next_report, _num_reports); + + /* if we are running up against the oldest report, toss it */ + if (_next_report == _oldest_report) { + perf_count(_buffer_overflows); + INCREMENT(_oldest_report, _num_reports); + } + + /* notify anyone waiting for data */ + poll_notify(POLLIN); + + ret = OK; + +out: + perf_end(_sample_perf); + return ret; + + return ret; +} + +void +MB12XX::start() +{ + /* reset the report ring and state machine */ + _collect_phase = false; + _oldest_report = _next_report = 0; + + /* schedule a cycle to start things */ + work_queue(HPWORK, &_work, (worker_t)&MB12XX::cycle_trampoline, this, 1); + + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + true, + SUBSYSTEM_TYPE_RANGEFINDER}; + static orb_advert_t pub = -1; + + if (pub > 0) { + orb_publish(ORB_ID(subsystem_info), pub, &info); + } else { + pub = orb_advertise(ORB_ID(subsystem_info), &info); + } +} + +void +MB12XX::stop() +{ + work_cancel(HPWORK, &_work); +} + +void +MB12XX::cycle_trampoline(void *arg) +{ + MB12XX *dev = (MB12XX *)arg; + + dev->cycle(); +} + +void +MB12XX::cycle() +{ + /* collection phase? */ + if (_collect_phase) { + + /* perform collection */ + if (OK != collect()) { + log("collection error"); + /* restart the measurement state machine */ + start(); + return; + } + + /* next phase is measurement */ + _collect_phase = false; + + /* + * Is there a collect->measure gap? + */ + if (_measure_ticks > USEC2TICK(MB12XX_CONVERSION_INTERVAL)) { + + /* schedule a fresh cycle call when we are ready to measure again */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + _measure_ticks - USEC2TICK(MB12XX_CONVERSION_INTERVAL)); + + return; + } + } + + /* measurement phase */ + if (OK != measure()) + log("measure error"); + + /* next phase is collection */ + _collect_phase = true; + + /* schedule a fresh cycle call when the measurement is done */ + work_queue(HPWORK, + &_work, + (worker_t)&MB12XX::cycle_trampoline, + this, + USEC2TICK(MB12XX_CONVERSION_INTERVAL)); +} + +void +MB12XX::print_info() +{ + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + perf_print_counter(_buffer_overflows); + printf("poll interval: %u ticks\n", _measure_ticks); + printf("report queue: %u (%u/%u @ %p)\n", + _num_reports, _oldest_report, _next_report, _reports); +} + +/** + * Local functions in support of the shell command. + */ +namespace mb12xx +{ + +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +const int ERROR = -1; + +MB12XX *g_dev; + +void start(); +void stop(); +void test(); +void reset(); +void info(); + +/** + * Start the driver. + */ +void +start() +{ + int fd; + + if (g_dev != nullptr) + errx(1, "already started"); + + /* create the driver */ + g_dev = new MB12XX(MB12XX_BUS); + + if (g_dev == nullptr) + goto fail; + + if (OK != g_dev->init()) + goto fail; + + /* set the poll rate to default, starts automatic data collection */ + fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + goto fail; + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + goto fail; + + exit(0); + +fail: + + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + + errx(1, "driver start failed"); +} + +/** + * Stop the driver + */ +void stop() +{ + if (g_dev != nullptr) + { + delete g_dev; + g_dev = nullptr; + } + else + { + errx(1, "driver not running"); + } + exit(0); +} + +/** + * Perform some basic functional tests on the driver; + * make sure we can collect data from the sensor in polled + * and automatic modes. + */ +void +test() +{ + struct range_finder_report report; + ssize_t sz; + int ret; + + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "%s open failed (try 'mb12xx start' if the driver is not running", RANGE_FINDER_DEVICE_PATH); + + /* do a simple demand read */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "immediate read failed"); + + warnx("single read"); + warnx("measurement: %0.2f m", (double)report.distance); + warnx("time: %lld", report.timestamp); + + /* start the sensor polling at 2Hz */ + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + errx(1, "failed to set 2Hz poll rate"); + + /* read the sensor 5x and report each value */ + for (unsigned i = 0; i < 5; i++) { + struct pollfd fds; + + /* wait for data to be ready */ + fds.fd = fd; + fds.events = POLLIN; + ret = poll(&fds, 1, 2000); + + if (ret != 1) + errx(1, "timed out waiting for sensor data"); + + /* now go get it */ + sz = read(fd, &report, sizeof(report)); + + if (sz != sizeof(report)) + err(1, "periodic read failed"); + + warnx("periodic read %u", i); + warnx("measurement: %0.3f", (double)report.distance); + warnx("time: %lld", report.timestamp); + } + + errx(0, "PASS"); +} + +/** + * Reset the driver. + */ +void +reset() +{ + int fd = open(RANGE_FINDER_DEVICE_PATH, O_RDONLY); + + if (fd < 0) + err(1, "failed "); + + if (ioctl(fd, SENSORIOCRESET, 0) < 0) + err(1, "driver reset failed"); + + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + err(1, "driver poll restart failed"); + + exit(0); +} + +/** + * Print a little info about the driver. + */ +void +info() +{ + if (g_dev == nullptr) + errx(1, "driver not running"); + + printf("state @ %p\n", g_dev); + g_dev->print_info(); + + exit(0); +} + +} // namespace + +int +mb12xx_main(int argc, char *argv[]) +{ + /* + * Start/load the driver. + */ + if (!strcmp(argv[1], "start")) + mb12xx::start(); + + /* + * Stop the driver + */ + if (!strcmp(argv[1], "stop")) + mb12xx::stop(); + + /* + * Test the driver/device. + */ + if (!strcmp(argv[1], "test")) + mb12xx::test(); + + /* + * Reset the driver. + */ + if (!strcmp(argv[1], "reset")) + mb12xx::reset(); + + /* + * Print driver information. + */ + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + mb12xx::info(); + + errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'"); +} diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 27e200e40..ce7062046 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -151,7 +151,7 @@ class MPU6000 : public device::SPI { public: MPU6000(int bus, spi_dev_e device); - ~MPU6000(); + virtual ~MPU6000(); virtual int init(); diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 44014d969..08420822a 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -104,7 +104,7 @@ class MS5611 : public device::I2C { public: MS5611(int bus); - ~MS5611(); + virtual ~MS5611(); virtual int init(); diff --git a/apps/drivers/px4fmu/fmu.cpp b/apps/drivers/px4fmu/fmu.cpp index c29fe0ba3..8e13f7c62 100644 --- a/apps/drivers/px4fmu/fmu.cpp +++ b/apps/drivers/px4fmu/fmu.cpp @@ -82,7 +82,7 @@ public: MODE_NONE }; PX4FMU(); - ~PX4FMU(); + virtual ~PX4FMU(); virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index 99f0f35b2..925212cc8 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -82,23 +82,28 @@ #include <uORB/topics/parameter_update.h> #include <px4io/protocol.h> +#include <mavlink/mavlink_log.h> #include "uploader.h" +#include <debug.h> class PX4IO : public device::I2C { public: PX4IO(); - ~PX4IO(); + virtual ~PX4IO(); virtual int init(); virtual int ioctl(file *filp, int cmd, unsigned long arg); virtual ssize_t write(file *filp, const char *buffer, size_t len); + void print_status(); + private: // XXX unsigned _max_actuators; + unsigned _max_controls; unsigned _max_rc_input; unsigned _max_relays; unsigned _max_transfer; @@ -108,6 +113,8 @@ private: volatile int _task; ///< worker task volatile bool _task_should_exit; + int _mavlink_fd; + perf_counter_t _perf_update; /* cached IO state */ @@ -115,7 +122,7 @@ private: uint16_t _alarms; /* subscribed topics */ - int _t_actuators; ///< actuator output topic + int _t_actuators; ///< actuator controls topic int _t_armed; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -275,18 +282,21 @@ PX4IO *g_dev; PX4IO::PX4IO() : I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), + _max_controls(0), _max_rc_input(0), _max_relays(0), _max_transfer(16), /* sensible default */ _update_interval(0), - _status(0), - _alarms(0), _task(-1), _task_should_exit(false), + _mavlink_fd(-1), _perf_update(perf_alloc(PC_ELAPSED, "px4io update")), + _status(0), + _alarms(0), _t_actuators(-1), _t_armed(-1), _t_vstatus(-1), + _t_param(-1), _to_input_rc(0), _to_actuators_effective(0), _to_outputs(0), @@ -296,6 +306,9 @@ PX4IO::PX4IO() : /* we need this potentially before it could be set in task_main */ g_dev = this; + /* open MAVLink text channel */ + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + _debug_enabled = true; } @@ -339,6 +352,7 @@ PX4IO::init() /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); + _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); _max_relays = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT); _max_transfer = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER) - 2; _max_rc_input = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT); @@ -348,6 +362,7 @@ PX4IO::init() (_max_rc_input < 1) || (_max_rc_input > 255)) { log("failed getting parameters from PX4IO"); + mavlink_log_emergency(_mavlink_fd, "[IO] param read fail, abort."); return -1; } if (_max_rc_input > RC_INPUT_MAX_CHANNELS) @@ -374,6 +389,8 @@ PX4IO::init() if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && (reg & PX4IO_P_SETUP_ARMING_ARM_OK)) { + mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO * remains untouched (so manual override is still available). @@ -459,10 +476,11 @@ PX4IO::init() PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK, 0); - /* publish RC config to IO */ + /* publish RC config to IO */ ret = io_set_rc_config(); if (ret != OK) { log("failed to update RC input config"); + mavlink_log_info(_mavlink_fd, "[IO] RC config upload fail"); return ret; } @@ -484,6 +502,8 @@ PX4IO::init() return -errno; } + mavlink_log_info(_mavlink_fd, "[IO] init ok"); + return OK; } @@ -634,11 +654,11 @@ PX4IO::io_set_control_state() orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &controls); - for (unsigned i = 0; i < _max_actuators; i++) + for (unsigned i = 0; i < _max_controls; i++) regs[i] = FLOAT_TO_REG(controls.control[i]); /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_actuators); + return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); } int @@ -689,21 +709,26 @@ PX4IO::io_set_rc_config() for (unsigned i = 0; i < _max_rc_input; i++) input_map[i] = -1; + /* + * NOTE: The indices for mapped channels are 1-based + * for compatibility reasons with existing + * autopilots / GCS'. + */ param_get(param_find("RC_MAP_ROLL"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 0; + input_map[ichan - 1] = 0; param_get(param_find("RC_MAP_PITCH"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 1; + input_map[ichan - 1] = 1; param_get(param_find("RC_MAP_YAW"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 2; + input_map[ichan - 1] = 2; param_get(param_find("RC_MAP_THROTTLE"), &ichan); if ((ichan >= 0) && (ichan < (int)_max_rc_input)) - input_map[ichan] = 3; + input_map[ichan - 1] = 3; ichan = 4; for (unsigned i = 0; i < _max_rc_input; i++) @@ -761,9 +786,16 @@ PX4IO::io_set_rc_config() /* send channel config to IO */ ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE); if (ret != OK) { - log("RC config update failed"); + log("rc config upload failed"); + break; + } + + /* check the IO initialisation flag */ + if (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + log("config for RC%d rejected by IO", i + 1); break; } + offset += PX4IO_P_RC_CONFIG_STRIDE; } @@ -813,6 +845,8 @@ PX4IO::io_handle_alarms(uint16_t alarms) /* set new alarms state */ _alarms = alarms; + + return 0; } int @@ -990,7 +1024,7 @@ PX4IO::io_publish_pwm_outputs() /* convert from register format to float */ for (unsigned i = 0; i < _max_actuators; i++) - outputs.output[i] = REG_TO_FLOAT(ctl[i]); + outputs.output[i] = ctl[i]; outputs.noutputs = _max_actuators; /* lazily advertise on first publication */ @@ -1142,18 +1176,134 @@ PX4IO::mixer_send(const char *buf, unsigned buflen) } while (buflen > 0); - debug("mixer upload OK"); - /* check for the mixer-OK flag */ - if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) + if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { + debug("mixer upload OK"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); return 0; - - debug("mixer rejected by IO"); + } else { + debug("mixer rejected by IO"); + mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); + } /* load must have failed for some reason */ return -EINVAL; } +void +PX4IO::print_status() +{ + /* basic configuration */ + printf("protocol %u software %u bootloader %u buffer %uB\n", + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_BOOTLOADER_VERSION), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_MAX_TRANSFER)); + printf("%u controls %u actuators %u R/C inputs %u analog inputs %u relays\n", + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT), + io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_RELAY_COUNT)); + + /* status */ + printf("%u bytes free\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); + uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n", + flags, + ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); + uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); + printf("alarms 0x%04x%s%s%s%s%s%s\n", + alarms, + ((alarms & PX4IO_P_STATUS_ALARMS_VBATT_LOW) ? " VBATT_LOW" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_TEMPERATURE) ? " TEMPERATURE" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_SERVO_CURRENT) ? " SERVO_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_ACC_CURRENT) ? " ACC_CURRENT" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_FMU_LOST) ? " FMU_LOST" : ""), + ((alarms & PX4IO_P_STATUS_ALARMS_RC_LOST) ? " RC_LOST" : "")); + printf("vbatt %u ibatt %u\n", + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VBATT), + io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_IBATT)); + printf("actuators"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_ACTUATORS, i)); + printf("\n"); + printf("servos"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i)); + printf("\n"); + uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT); + printf("%d raw R/C inputs", raw_inputs); + for (unsigned i = 0; i < raw_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_BASE + i)); + printf("\n"); + uint16_t mapped_inputs = io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_VALID); + printf("mapped R/C inputs 0x%04x", mapped_inputs); + for (unsigned i = 0; i < _max_rc_input; i++) { + if (mapped_inputs & (1 << i)) + printf(" %u:%d", i, REG_TO_SIGNED(io_reg_get(PX4IO_PAGE_RC_INPUT, PX4IO_P_RC_BASE + i))); + } + printf("\n"); + uint16_t adc_inputs = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ADC_INPUT_COUNT); + printf("ADC inputs"); + for (unsigned i = 0; i < adc_inputs; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_RAW_ADC_INPUT, i)); + printf("\n"); + + /* setup and state */ + printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); + uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); + printf("arming 0x%04x%s%s%s%s\n", + arming, + ((arming & PX4IO_P_SETUP_ARMING_ARM_OK) ? " ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK) ? " VECTOR_FLIGHT_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); + printf("rates 0x%04x lowrate %u highrate %u relays 0x%04x\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_LOWRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_HIGHRATE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS)); + printf("vbatt scale %u ibatt scale %u ibatt bias %u\n", + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_VBATT_SCALE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_SCALE), + io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_IBATT_BIAS)); + printf("debuglevel %u\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_SET_DEBUG)); + printf("controls"); + for (unsigned i = 0; i < _max_controls; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_CONTROLS, i)); + printf("\n"); + for (unsigned i = 0; i < _max_rc_input; i++) { + unsigned base = PX4IO_P_RC_CONFIG_STRIDE * i; + uint16_t options = io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_OPTIONS); + printf("input %u min %u center %u max %u deadzone %u assigned %u options 0x%04x%s%s\n", + i, + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MIN), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_CENTER), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_MAX), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_DEADZONE), + io_reg_get(PX4IO_PAGE_RC_CONFIG, base + PX4IO_P_RC_CONFIG_ASSIGNMENT), + options, + ((options & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) ? " ENABLED" : ""), + ((options & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) ? " REVERSED" : "")); + } + printf("failsafe"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("\n"); +} + int PX4IO::ioctl(file *filep, int cmd, unsigned long arg) { @@ -1295,7 +1445,7 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) } default: - /* not a recognised value */ + /* not a recognized value */ ret = -ENOTTY; } @@ -1350,7 +1500,7 @@ test(void) servos[i] = pwm_value; ret = write(fd, servos, sizeof(servos)); - if (ret != sizeof(servos)) + if (ret != (int)sizeof(servos)) err(1, "error writing PWM servo data, wrote %u got %d", sizeof(servos), ret); if (direction > 0) { @@ -1422,7 +1572,7 @@ px4io_main(int argc, char *argv[]) errx(1, "already loaded"); /* create the driver - it will set g_dev */ - (void)new PX4IO; + (void)new PX4IO(); if (g_dev == nullptr) errx(1, "driver alloc failed"); @@ -1433,7 +1583,7 @@ px4io_main(int argc, char *argv[]) } /* look for the optional pwm update rate for the supported modes */ - if (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0) { + if ((argc > 2) && (strcmp(argv[2], "-u") == 0 || strcmp(argv[2], "--update-rate") == 0)) { if (argc > 2 + 1) { #warning implement this } else { @@ -1445,24 +1595,41 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "recovery")) { + + if (g_dev != nullptr) { + /* + * Enable in-air restart support. + * We can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ + g_dev->ioctl(NULL, PWM_SERVO_INAIR_RESTART_ENABLE, 0); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "stop")) { - if (g_dev != nullptr) { - /* stop the driver */ - delete g_dev; - } else { - errx(1, "not loaded"); - } - exit(0); + if (g_dev != nullptr) { + /* stop the driver */ + delete g_dev; + } else { + errx(1, "not loaded"); } + exit(0); + } if (!strcmp(argv[1], "status")) { - if (g_dev != nullptr) + if (g_dev != nullptr) { printf("[px4io] loaded\n"); - else + g_dev->print_status(); + } else { printf("[px4io] not loaded\n"); + } exit(0); } @@ -1477,8 +1644,9 @@ px4io_main(int argc, char *argv[]) exit(1); } uint8_t level = atoi(argv[2]); - // we can cheat and call the driver directly, as it - // doesn't reference filp in ioctl() + /* we can cheat and call the driver directly, as it + * doesn't reference filp in ioctl() + */ int ret = g_dev->ioctl(NULL, PWM_SERVO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); diff --git a/apps/drivers/px4io/uploader.h b/apps/drivers/px4io/uploader.h index 915ee9259..f983d1981 100644 --- a/apps/drivers/px4io/uploader.h +++ b/apps/drivers/px4io/uploader.h @@ -46,7 +46,7 @@ class PX4IO_Uploader { public: PX4IO_Uploader(); - ~PX4IO_Uploader(); + virtual ~PX4IO_Uploader(); int upload(const char *filenames[]); diff --git a/apps/drivers/stm32/drv_hrt.c b/apps/drivers/stm32/drv_hrt.c index cd9cb45b1..bb67d5e6d 100644 --- a/apps/drivers/stm32/drv_hrt.c +++ b/apps/drivers/stm32/drv_hrt.c @@ -646,6 +646,36 @@ abstime_to_ts(struct timespec *ts, hrt_abstime abstime) } /* + * Compare a time value with the current time. + */ +hrt_abstime +hrt_elapsed_time(const volatile hrt_abstime *then) +{ + irqstate_t flags = irqsave(); + + hrt_abstime delta = hrt_absolute_time() - *then; + + irqrestore(flags); + + return delta; +} + +/* + * Store the absolute time in an interrupt-safe fashion + */ +hrt_abstime +hrt_store_absolute_time(volatile hrt_abstime *now) +{ + irqstate_t flags = irqsave(); + + hrt_abstime ts = hrt_absolute_time(); + + irqrestore(flags); + + return ts; +} + +/* * Initalise the high-resolution timing module. */ void diff --git a/apps/drivers/stm32/drv_pwm_servo.c b/apps/drivers/stm32/drv_pwm_servo.c index 954b458f5..2b8641f00 100644 --- a/apps/drivers/stm32/drv_pwm_servo.c +++ b/apps/drivers/stm32/drv_pwm_servo.c @@ -299,8 +299,12 @@ up_pwm_servo_arm(bool armed) rCR1(i) |= GTIM_CR1_CEN | GTIM_CR1_ARPE; } else { - /* on disarm, just stop auto-reload so we don't generate runts */ - rCR1(i) &= ~GTIM_CR1_ARPE; + // XXX This leads to FMU PWM being still active + // but uncontrollable. Just disable the timer + // and risk a runt. + ///* on disarm, just stop auto-reload so we don't generate runts */ + //rCR1(i) &= ~GTIM_CR1_ARPE; + rCR1(i) = 0; } } } diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index db851221b..955e77b3e 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -247,8 +247,8 @@ void KalmanNav::update() // output if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; - printf("nav: %4d Hz, miss #: %4d\n", - _navFrames / 10, _miss / 10); + //printf("nav: %4d Hz, miss #: %4d\n", + // _navFrames / 10, _miss / 10); _navFrames = 0; _miss = 0; } diff --git a/apps/mavlink/mavlink_log.h b/apps/mavlink/mavlink_log.h index 62e6f7ca0..233a76cb3 100644 --- a/apps/mavlink/mavlink_log.h +++ b/apps/mavlink/mavlink_log.h @@ -63,7 +63,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_emergency(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#else #define mavlink_log_emergency(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_EMERGENCY, (unsigned long)_text); +#endif /** * Send a mavlink critical message. @@ -71,7 +75,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_critical(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#else #define mavlink_log_critical(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_CRITICAL, (unsigned long)_text); +#endif /** * Send a mavlink info message. @@ -79,7 +87,11 @@ * @param _fd A file descriptor returned from open(MAVLINK_LOG_DEVICE, 0); * @param _text The text to log; */ +#ifdef __cplusplus +#define mavlink_log_info(_fd, _text) ::ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#else #define mavlink_log_info(_fd, _text) ioctl(_fd, MAVLINK_IOC_SEND_TEXT_INFO, (unsigned long)_text); +#endif struct mavlink_logmessage { char text[51]; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 15066010c..10044bb98 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -511,28 +511,12 @@ l_actuator_outputs(struct listener *l) 0); } else { - - /* - * Catch the case where no rudder is in use and throttle is not - * on output four - */ - float rudder, throttle; - - if (act_outputs.noutputs < 4) { - rudder = 0.0f; - throttle = (act_outputs.output[2] - 900.0f) / 1200.0f; - - } else { - rudder = (act_outputs.output[2] - 1500.0f) / 600.0f; - throttle = (act_outputs.output[3] - 900.0f) / 1200.0f; - } - mavlink_msg_hil_controls_send(chan, hrt_absolute_time(), (act_outputs.output[0] - 1500.0f) / 600.0f, (act_outputs.output[1] - 1500.0f) / 600.0f, - rudder, - throttle, + (act_outputs.output[2] - 1500.0f) / 600.0f, + (act_outputs.output[3] - 900.0f) / 1200.0f, (act_outputs.output[4] - 1500.0f) / 600.0f, (act_outputs.output[5] - 1500.0f) / 600.0f, (act_outputs.output[6] - 1500.0f) / 600.0f, diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 057a573b1..408a850d8 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -504,7 +504,7 @@ int mavlink_onboard_main(int argc, char *argv[]) mavlink_task = task_spawn("mavlink_onboard", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 6000 /* XXX probably excessive */, + 2048, mavlink_thread_main, (const char**)argv); exit(0); diff --git a/apps/px4io/adc.c b/apps/px4io/adc.c index e06b269dc..670b8d635 100644 --- a/apps/px4io/adc.c +++ b/apps/px4io/adc.c @@ -154,7 +154,7 @@ adc_measure(unsigned channel) while (!(rSR & ADC_SR_EOC)) { /* never spin forever - this will give a bogus result though */ - if ((hrt_absolute_time() - now) > 1000) { + if (hrt_elapsed_time(&now) > 1000) { debug("adc timeout"); break; } diff --git a/apps/px4io/controls.c b/apps/px4io/controls.c index b4a18bae6..b507078a1 100644 --- a/apps/px4io/controls.c +++ b/apps/px4io/controls.c @@ -39,13 +39,11 @@ #include <nuttx/config.h> #include <stdbool.h> -#include <poll.h> #include <drivers/drv_hrt.h> #include <systemlib/perf_counter.h> #include <systemlib/ppm_decode.h> -//#define DEBUG #include "px4io.h" #define RC_FAILSAFE_TIMEOUT 2000000 /**< two seconds failsafe timeout */ @@ -54,21 +52,18 @@ static bool ppm_input(uint16_t *values, uint16_t *num_values); +static perf_counter_t c_gather_dsm; +static perf_counter_t c_gather_sbus; +static perf_counter_t c_gather_ppm; + void -controls_main(void) +controls_init(void) { - struct pollfd fds[2]; - /* DSM input */ - fds[0].fd = dsm_init("/dev/ttyS0"); - fds[0].events = POLLIN; + dsm_init("/dev/ttyS0"); /* S.bus input */ - fds[1].fd = sbus_init("/dev/ttyS2"); - fds[1].events = POLLIN; - - ASSERT(fds[0].fd >= 0); - ASSERT(fds[1].fd >= 0); + sbus_init("/dev/ttyS2"); /* default to a 1:1 input map, all enabled */ for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { @@ -83,200 +78,218 @@ controls_main(void) r_page_rc_input_config[base + PX4IO_P_RC_CONFIG_OPTIONS] = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; } - for (;;) { - /* run this loop at ~100Hz */ - int result = poll(fds, 2, 10); - - ASSERT(result >= 0); - - /* - * Gather R/C control inputs from supported sources. - * - * Note that if you're silly enough to connect more than - * one control input source, they're going to fight each - * other. Don't do that. - */ - - bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); - if (dsm_updated) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; - - bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); - if (sbus_updated) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; - - /* - * XXX each S.bus frame will cause a PPM decoder interrupt - * storm (lots of edges). It might be sensible to actually - * disable the PPM decoder completely if we have S.bus signal. - */ - bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); - if (ppm_updated) - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; - - ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - - /* - * In some cases we may have received a frame, but input has still - * been lost. - */ - bool rc_input_lost = false; - - /* - * If we received a new frame from any of the RC sources, process it. - */ - if (dsm_updated || sbus_updated || ppm_updated) { - - /* update RC-received timestamp */ - system_state.rc_channels_timestamp = hrt_absolute_time(); - - /* record a bitmask of channels assigned */ - unsigned assigned_channels = 0; - - /* map raw inputs to mapped inputs */ - /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { - - /* map the input channel */ - uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; - - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { - - uint16_t raw = r_raw_rc_values[i]; - - /* implement the deadzone */ - if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) { - raw += conf[PX4IO_P_RC_CONFIG_DEADZONE]; - if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) - raw = conf[PX4IO_P_RC_CONFIG_CENTER]; - } - if (raw > conf[PX4IO_P_RC_CONFIG_CENTER]) { - raw -= conf[PX4IO_P_RC_CONFIG_DEADZONE]; - if (raw < conf[PX4IO_P_RC_CONFIG_CENTER]) - raw = conf[PX4IO_P_RC_CONFIG_CENTER]; - } + c_gather_dsm = perf_alloc(PC_ELAPSED, "c_gather_dsm"); + c_gather_sbus = perf_alloc(PC_ELAPSED, "c_gather_sbus"); + c_gather_ppm = perf_alloc(PC_ELAPSED, "c_gather_ppm"); +} - /* constrain to min/max values */ - if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) - raw = conf[PX4IO_P_RC_CONFIG_MIN]; - if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) - raw = conf[PX4IO_P_RC_CONFIG_MAX]; +void +controls_tick() { - int16_t scaled = raw; + /* + * Gather R/C control inputs from supported sources. + * + * Note that if you're silly enough to connect more than + * one control input source, they're going to fight each + * other. Don't do that. + */ - /* adjust to zero-relative (-500..500) */ - scaled -= 1500; + perf_begin(c_gather_dsm); + bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); + if (dsm_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + perf_end(c_gather_dsm); - /* scale to fixed-point representation (-10000..10000) */ - scaled *= 20; + perf_begin(c_gather_sbus); + bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count); + if (sbus_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS; + perf_end(c_gather_sbus); - ASSERT(scaled >= -15000); - ASSERT(scaled <= 15000); + /* + * XXX each S.bus frame will cause a PPM decoder interrupt + * storm (lots of edges). It might be sensible to actually + * disable the PPM decoder completely if we have S.bus signal. + */ + perf_begin(c_gather_ppm); + bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count); + if (ppm_updated) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM; + perf_end(c_gather_ppm); - if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) - scaled = -scaled; + ASSERT(r_raw_rc_count <= MAX_CONTROL_CHANNELS); - /* and update the scaled/mapped version */ - unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; - ASSERT(mapped < MAX_CONTROL_CHANNELS); + /* + * In some cases we may have received a frame, but input has still + * been lost. + */ + bool rc_input_lost = false; - r_rc_values[mapped] = SIGNED_TO_REG(scaled); - assigned_channels |= (1 << mapped); + /* + * If we received a new frame from any of the RC sources, process it. + */ + if (dsm_updated || sbus_updated || ppm_updated) { + + /* update RC-received timestamp */ + system_state.rc_channels_timestamp = hrt_absolute_time(); + + /* record a bitmask of channels assigned */ + unsigned assigned_channels = 0; + + /* map raw inputs to mapped inputs */ + /* XXX mapping should be atomic relative to protocol */ + for (unsigned i = 0; i < r_raw_rc_count; i++) { + + /* map the input channel */ + uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; + + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + + uint16_t raw = r_raw_rc_values[i]; + + int16_t scaled; + + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) + raw = conf[PX4IO_P_RC_CONFIG_MIN]; + if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) + raw = conf[PX4IO_P_RC_CONFIG_MAX]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * then scale to 20000 range (if center is an actual center, -10000..10000, + * if parameters only support half range, scale to 10000 range, e.g. if + * center == min 0..10000, if center == max -10000..0). + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ + if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])); + + } else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN])); + + } else { + /* in the configured dead zone, output zero */ + scaled = 0; } - } - /* set un-assigned controls to zero */ - for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { - if (!(assigned_channels & (1 << i))) - r_rc_values[i] = 0; - } + /* invert channel if requested */ + if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE) + scaled = -scaled; - /* - * If we got an update with zero channels, treat it as - * a loss of input. - * - * This might happen if a protocol-based receiver returns an update - * that contains no channels that we have mapped. - */ - if (assigned_channels == 0) { - rc_input_lost = true; - } else { - r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + /* and update the scaled/mapped version */ + unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT]; + ASSERT(mapped < MAX_CONTROL_CHANNELS); + + r_rc_values[mapped] = SIGNED_TO_REG(scaled); + assigned_channels |= (1 << mapped); } + } - /* - * Export the valid channel bitmap - */ - r_rc_valid = assigned_channels; + /* set un-assigned controls to zero */ + for (unsigned i = 0; i < MAX_CONTROL_CHANNELS; i++) { + if (!(assigned_channels & (1 << i))) + r_rc_values[i] = 0; } /* - * If we haven't seen any new control data in 200ms, assume we - * have lost input. + * If we got an update with zero channels, treat it as + * a loss of input. + * + * This might happen if a protocol-based receiver returns an update + * that contains no channels that we have mapped. */ - if ((hrt_absolute_time() - system_state.rc_channels_timestamp) > 200000) { + if (assigned_channels == 0) { rc_input_lost = true; - - /* clear the input-kind flags here */ - r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_RC_PPM | - PX4IO_P_STATUS_FLAGS_RC_DSM | - PX4IO_P_STATUS_FLAGS_RC_SBUS); + } else { + /* set RC OK flag and clear RC lost alarm */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK; + r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_RC_LOST; } /* - * Handle losing RC input + * Export the valid channel bitmap */ - if (rc_input_lost) { + r_rc_valid = assigned_channels; + } + + /* + * If we haven't seen any new control data in 200ms, assume we + * have lost input. + */ + if (hrt_elapsed_time(&system_state.rc_channels_timestamp) > 200000) { + rc_input_lost = true; + + /* clear the input-kind flags here */ + r_status_flags &= ~( + PX4IO_P_STATUS_FLAGS_RC_PPM | + PX4IO_P_STATUS_FLAGS_RC_DSM | + PX4IO_P_STATUS_FLAGS_RC_SBUS); + } + + /* + * Handle losing RC input + */ + if (rc_input_lost) { - /* Clear the RC input status flag, clear manual override flag */ - r_status_flags &= ~( - PX4IO_P_STATUS_FLAGS_OVERRIDE | - PX4IO_P_STATUS_FLAGS_RC_OK); + /* Clear the RC input status flag, clear manual override flag */ + r_status_flags &= ~( + PX4IO_P_STATUS_FLAGS_OVERRIDE | + PX4IO_P_STATUS_FLAGS_RC_OK); - /* Set the RC_LOST alarm */ - r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; + /* Set the RC_LOST alarm */ + r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST; - /* Mark the arrays as empty */ - r_raw_rc_count = 0; - r_rc_valid = 0; - } + /* Mark the arrays as empty */ + r_raw_rc_count = 0; + r_rc_valid = 0; + } + + /* + * Check for manual override. + * + * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we + * must have R/C input. + * Override is enabled if either the hardcoded channel / value combination + * is selected, or the AP has requested it. + */ + if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + + bool override = false; /* - * Check for manual override. + * Check mapped channel 5 (can be any remote channel, + * depends on RC_MAP_OVER parameter); + * If the value is 'high' then the pilot has + * requested override. * - * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we - * must have R/C input. - * Override is enabled if either the hardcoded channel / value combination - * is selected, or the AP has requested it. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { - - bool override = false; + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) > RC_CHANNEL_HIGH_THRESH)) + override = true; - /* - * Check mapped channel 5; if the value is 'high' then the pilot has - * requested override. - * - * XXX This should be configurable. - */ - if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (r_rc_values[4] > RC_CHANNEL_HIGH_THRESH)) - override = true; + if (override) { - if (override) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; - r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + /* mix new RC input control values to servos */ + if (dsm_updated || sbus_updated || ppm_updated) + mixer_tick(); - /* mix new RC input control values to servos */ - if (dsm_updated || sbus_updated || ppm_updated) - mixer_tick(); - - } else { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; - } + } else { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } - } } @@ -292,7 +305,7 @@ ppm_input(uint16_t *values, uint16_t *num_values) * If we have received a new PPM frame within the last 200ms, accept it * and then invalidate it. */ - if ((hrt_absolute_time() - ppm_last_valid_decode) < 200000) { + if (hrt_elapsed_time(&ppm_last_valid_decode) < 200000) { /* PPM data exists, copy it */ *num_values = ppm_decoded_channels; diff --git a/apps/px4io/dsm.c b/apps/px4io/dsm.c index a79c9734c..ea35e5513 100644 --- a/apps/px4io/dsm.c +++ b/apps/px4io/dsm.c @@ -249,18 +249,18 @@ dsm_guess_format(bool reset) if ((votes11 == 1) && (votes10 == 0)) { channel_shift = 11; - debug("DSM: detected 11-bit format"); + debug("DSM: 11-bit format"); return; } if ((votes10 == 1) && (votes11 == 0)) { channel_shift = 10; - debug("DSM: detected 10-bit format"); + debug("DSM: 10-bit format"); return; } /* call ourselves to reset our state ... we have to try again */ - debug("DSM: format detector failed, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); + debug("DSM: format detect fail, 10: 0x%08x %d 11: 0x%08x %d", cs10, votes10, cs11, votes11); dsm_guess_format(true); } diff --git a/apps/px4io/mixer.cpp b/apps/px4io/mixer.cpp index 3ae2a3115..ec69cdd64 100644 --- a/apps/px4io/mixer.cpp +++ b/apps/px4io/mixer.cpp @@ -38,6 +38,7 @@ */ #include <nuttx/config.h> +#include <syslog.h> #include <sys/types.h> #include <stdbool.h> @@ -88,15 +89,20 @@ void mixer_tick(void) { /* check that we are receiving fresh data from the FMU */ - if ((hrt_absolute_time() - system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { + if (hrt_elapsed_time(&system_state.fmu_data_received_time) > FMU_INPUT_DROP_LIMIT_US) { /* too long without FMU input, time to go to failsafe */ if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { - debug("AP RX timeout"); + lowsyslog("AP RX timeout"); } - r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; + + /* XXX this is questionable - vehicle may not make sense for direct control */ + r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE; + } else { + r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; } source = MIX_FAILSAFE; @@ -104,9 +110,11 @@ mixer_tick(void) /* * Decide which set of controls we're using. */ - if (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) { + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) || + !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { - /* don't actually mix anything - we already have raw PWM values */ + /* don't actually mix anything - we already have raw PWM values or + not a valid mixer. */ source = MIX_NONE; } else { @@ -119,7 +127,8 @@ mixer_tick(void) } if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; @@ -157,71 +166,6 @@ mixer_tick(void) r_page_servos[i] = 0; } -#if 0 - /* if everything is ok */ - - if (!system_state.mixer_manual_override && system_state.mixer_fmu_available) { - /* we have recent control data from the FMU */ - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - - } else if (system_state.rc_channels > 0) { - /* when override is on or the fmu is not available, but RC is present */ - control_count = system_state.rc_channels; - - sched_lock(); - - /* remap roll, pitch, yaw and throttle from RC specific to internal ordering */ - rc_channel_data[ROLL] = system_state.rc_channel_data[system_state.rc_map[ROLL] - 1]; - rc_channel_data[PITCH] = system_state.rc_channel_data[system_state.rc_map[PITCH] - 1]; - rc_channel_data[YAW] = system_state.rc_channel_data[system_state.rc_map[YAW] - 1]; - rc_channel_data[THROTTLE] = system_state.rc_channel_data[system_state.rc_map[THROTTLE] - 1]; - //rc_channel_data[OVERRIDE] = system_state.rc_channel_data[system_state.rc_map[OVERRIDE] - 1]; - - /* get the remaining channels, no remapping needed */ - for (unsigned i = 4; i < system_state.rc_channels; i++) { - rc_channel_data[i] = system_state.rc_channel_data[i]; - } - - /* scale the control inputs */ - rc_channel_data[THROTTLE] = ((float)(rc_channel_data[THROTTLE] - system_state.rc_min[THROTTLE]) / - (float)(system_state.rc_max[THROTTLE] - system_state.rc_min[THROTTLE])) * 1000.0f + 1000; - - if (rc_channel_data[THROTTLE] > 2000) { - rc_channel_data[THROTTLE] = 2000; - } - - if (rc_channel_data[THROTTLE] < 1000) { - rc_channel_data[THROTTLE] = 1000; - } - - // lowsyslog("Tmin: %d Ttrim: %d Tmax: %d T: %d \n", - // (int)(system_state.rc_min[THROTTLE]), (int)(system_state.rc_trim[THROTTLE]), - // (int)(system_state.rc_max[THROTTLE]), (int)(rc_channel_data[THROTTLE])); - - control_values = &rc_channel_data[0]; - sched_unlock(); - } else { - /* we have no control input (no FMU, no RC) */ - - // XXX builtin failsafe would activate here - control_count = 0; - } - //lowsyslog("R: %d P: %d Y: %d T: %d \n", control_values[0], control_values[1], control_values[2], control_values[3]); - - /* this is for multicopters, etc. where manual override does not make sense */ - } else { - /* if the fmu is available whe are good */ - if (system_state.mixer_fmu_available) { - control_count = PX4IO_CONTROL_CHANNELS; - control_values = &system_state.fmu_channel_data[0]; - /* we better shut everything off */ - } else { - control_count = 0; - } - } -#endif - /* * Decide whether the servos should be armed right now. * @@ -231,9 +175,11 @@ mixer_tick(void) * XXX correct behaviour for failsafe may require an additional case * here. */ - bool should_arm = (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))); + bool should_arm = ( + /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + /* there is valid input */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)); if (should_arm && !mixer_servos_armed) { /* need to arm, but not armed */ @@ -298,10 +244,15 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { + /* do not allow a mixer change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + return; + } px4io_mixdata *msg = (px4io_mixdata *)buffer; - isr_debug(2, "mixer text %u", length); + isr_debug(2, "mix txt %u", length); if (length < sizeof(px4io_mixdata)) return; @@ -311,9 +262,12 @@ mixer_handle_text(const void *buffer, size_t length) switch (msg->action) { case F2I_MIXER_ACTION_RESET: isr_debug(2, "reset"); + + /* FIRST mark the mixer as invalid */ + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; + /* THEN actually delete it */ mixer_group.reset(); mixer_text_length = 0; - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; /* FALLTHROUGH */ case F2I_MIXER_ACTION_APPEND: diff --git a/apps/px4io/protocol.h b/apps/px4io/protocol.h index 0ee6d2c4b..14d221b5b 100644 --- a/apps/px4io/protocol.h +++ b/apps/px4io/protocol.h @@ -103,6 +103,7 @@ #define PX4IO_P_STATUS_FLAGS_RAW_PWM (1 << 7) /* raw PWM from FMU is bypassing the mixer */ #define PX4IO_P_STATUS_FLAGS_MIXER_OK (1 << 8) /* mixer is OK */ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ +#define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 56923a674..9de37e118 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -64,18 +64,11 @@ struct sys_state_s system_state; static struct hrt_call serial_dma_call; -/* global debug level for isr_debug() */ -volatile uint8_t debug_level = 0; +/* store i2c reset count XXX this should be a register, together with other error counters */ volatile uint32_t i2c_loop_resets = 0; -struct hrt_call loop_overtime_call; - -// this allows wakeup of the main task via a signal -static pid_t daemon_pid; - - /* - a set of debug buffers to allow us to send debug information from ISRs + * a set of debug buffers to allow us to send debug information from ISRs */ static volatile uint32_t msg_counter; @@ -83,19 +76,20 @@ static volatile uint32_t last_msg_counter; static volatile uint8_t msg_next_out, msg_next_in; /* - * WARNING too large buffers here consume the memory required + * WARNING: too large buffers here consume the memory required * for mixer handling. Do not allocate more than 80 bytes for * output. */ #define NUM_MSG 2 -static char msg[NUM_MSG][50]; +static char msg[NUM_MSG][40]; /* - add a debug message to be printed on the console + * add a debug message to be printed on the console */ -void isr_debug(uint8_t level, const char *fmt, ...) +void +isr_debug(uint8_t level, const char *fmt, ...) { - if (level > debug_level) { + if (level > r_page_setup[PX4IO_P_SETUP_SET_DEBUG]) { return; } va_list ap; @@ -107,9 +101,10 @@ void isr_debug(uint8_t level, const char *fmt, ...) } /* - show all pending debug messages + * show all pending debug messages */ -static void show_debug_messages(void) +static void +show_debug_messages(void) { if (msg_counter != last_msg_counter) { uint32_t n = msg_counter - last_msg_counter; @@ -122,36 +117,9 @@ static void show_debug_messages(void) } } -/* - catch I2C lockups - */ -static void loop_overtime(void *arg) -{ - debug("RESETTING\n"); - i2c_loop_resets++; - i2c_dump(); - i2c_reset(); - hrt_call_after(&loop_overtime_call, 50000, (hrt_callout)loop_overtime, NULL); -} - -static void wakeup_handler(int signo, siginfo_t *info, void *ucontext) -{ - // nothing to do - we just want poll() to return -} - - -/* - wakeup the main task using a signal - */ -void daemon_wakeup(void) -{ - kill(daemon_pid, SIGUSR1); -} - -int user_start(int argc, char *argv[]) +int +user_start(int argc, char *argv[]) { - daemon_pid = getpid(); - /* run C++ ctors before we go any further */ up_cxxinitialize(); @@ -184,17 +152,8 @@ int user_start(int argc, char *argv[]) /* configure the first 8 PWM outputs (i.e. all of them) */ up_pwm_servo_init(0xff); - /* start the flight control signal handler */ - task_create("FCon", - SCHED_PRIORITY_DEFAULT, - 1024, - (main_t)controls_main, - NULL); - - struct mallinfo minfo = mallinfo(); - lowsyslog("free %u largest %u\n", minfo.mxordblk, minfo.fordblks); - - debug("debug_level=%u\n", (unsigned)debug_level); + /* initialise the control inputs */ + controls_init(); /* start the i2c handler */ i2c_init(); @@ -202,45 +161,69 @@ int user_start(int argc, char *argv[]) /* add a performance counter for mixing */ perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix"); - /* - * setup a null handler for SIGUSR1 - we will use this for wakeup from poll() - */ - struct sigaction sa; - memset(&sa, 0, sizeof(sa)); - sa.sa_sigaction = wakeup_handler; - sigfillset(&sa.sa_mask); - sigdelset(&sa.sa_mask, SIGUSR1); - if (sigaction(SIGUSR1, &sa, NULL) != OK) { - debug("Failed to setup SIGUSR1 handler\n"); + /* add a performance counter for controls */ + perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls"); + + /* and one for measuring the loop rate */ + perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop"); + + struct mallinfo minfo = mallinfo(); + lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks); + +#if 0 + /* not enough memory, lock down */ + if (minfo.mxordblk < 500) { + lowsyslog("ERR: not enough MEM"); + bool phase = false; + + if (phase) { + LED_AMBER(true); + LED_BLUE(false); + } else { + LED_AMBER(false); + LED_BLUE(true); + } + + phase = !phase; + usleep(300000); } +#endif + + /* + * Run everything in a tight loop. + */ - /* - run the mixer at ~50Hz, using signals to run it early if - need be - */ uint64_t last_debug_time = 0; for (;;) { - /* - if we are not scheduled for 30ms then reset the I2C bus - */ - hrt_call_after(&loop_overtime_call, 30000, (hrt_callout)loop_overtime, NULL); - // we use usleep() instead of poll() as poll() is not - // interrupted by signals in nuttx, whereas usleep() is - usleep(20000); + /* track the rate at which the loop is running */ + perf_count(loop_perf); + /* kick the mixer */ perf_begin(mixer_perf); mixer_tick(); perf_end(mixer_perf); + /* kick the control inputs */ + perf_begin(controls_perf); + controls_tick(); + perf_end(controls_perf); + + /* check for debug activity */ show_debug_messages(); - if (hrt_absolute_time() - last_debug_time > 1000000) { - isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u", - (unsigned)debug_level, + + /* post debug state at ~1Hz */ + if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) { + + struct mallinfo minfo = mallinfo(); + + isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x r=%u m=%u", + (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG], (unsigned)r_status_flags, (unsigned)r_setup_arming, (unsigned)r_setup_features, - (unsigned)i2c_loop_resets); + (unsigned)i2c_loop_resets, + (unsigned)minfo.mxordblk); last_debug_time = hrt_absolute_time(); } } diff --git a/apps/px4io/px4io.h b/apps/px4io/px4io.h index cd5977258..7b4b07c2c 100644 --- a/apps/px4io/px4io.h +++ b/apps/px4io/px4io.h @@ -105,12 +105,12 @@ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ */ struct sys_state_s { - uint64_t rc_channels_timestamp; + volatile uint64_t rc_channels_timestamp; /** * Last FMU receive time, in microseconds since system boot */ - uint64_t fmu_data_received_time; + volatile uint64_t fmu_data_received_time; }; @@ -170,7 +170,8 @@ extern uint16_t adc_measure(unsigned channel); * * Input functions return true when they receive an update from the RC controller. */ -extern void controls_main(void); +extern void controls_init(void); +extern void controls_tick(void); extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern int sbus_init(const char *device); @@ -179,11 +180,6 @@ extern bool sbus_input(uint16_t *values, uint16_t *num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; -/** - * Wake up mixer. - */ -void daemon_wakeup(void); - /* send a debug message to the console */ extern void isr_debug(uint8_t level, const char *fmt, ...); diff --git a/apps/px4io/registers.c b/apps/px4io/registers.c index ec1980aaa..645c1565d 100644 --- a/apps/px4io/registers.c +++ b/apps/px4io/registers.c @@ -137,7 +137,8 @@ volatile uint16_t r_page_setup[] = [PX4IO_P_SETUP_RELAYS] = 0, [PX4IO_P_SETUP_VBATT_SCALE] = 10000, [PX4IO_P_SETUP_IBATT_SCALE] = 0, - [PX4IO_P_SETUP_IBATT_BIAS] = 0 + [PX4IO_P_SETUP_IBATT_BIAS] = 0, + [PX4IO_P_SETUP_SET_DEBUG] = 0, }; #define PX4IO_P_SETUP_FEATURES_VALID (0) @@ -165,8 +166,8 @@ volatile uint16_t r_page_controls[PX4IO_CONTROL_CHANNELS]; */ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE]; -/* valid options excluding ENABLE */ -#define PX4IO_P_RC_CONFIG_OPTIONS_VALID PX4IO_P_RC_CONFIG_OPTIONS_REVERSE +/* valid options */ +#define PX4IO_P_RC_CONFIG_OPTIONS_VALID (PX4IO_P_RC_CONFIG_OPTIONS_REVERSE | PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) /* * PAGE 104 uses r_page_servos. @@ -201,10 +202,9 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK; + r_status_alarms &= ~PX4IO_P_STATUS_ALARMS_FMU_LOST; r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RAW_PWM; - // wake up daemon to trigger mixer - daemon_wakeup(); break; /* handle raw PWM input */ @@ -224,8 +224,6 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num system_state.fmu_data_received_time = hrt_absolute_time(); r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM; - // wake up the main thread to trigger mixer - daemon_wakeup(); break; /* handle setup for servo failsafe values */ @@ -355,8 +353,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_P_SETUP_SET_DEBUG: - debug_level = value; - isr_debug(0, "set debug %u\n", (unsigned)debug_level); + r_page_setup[PX4IO_P_SETUP_SET_DEBUG] = value; + isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; default: @@ -365,6 +363,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) break; case PX4IO_PAGE_RC_CONFIG: { + + /* do not allow a RC config change while fully armed */ + if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_ARM_OK) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + break; + } + unsigned channel = offset / PX4IO_P_RC_CONFIG_STRIDE; unsigned index = offset - channel * PX4IO_P_RC_CONFIG_STRIDE; uint16_t *conf = &r_page_rc_input_config[channel * PX4IO_P_RC_CONFIG_STRIDE]; @@ -387,6 +392,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_RC_CONFIG_OPTIONS: value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; @@ -394,27 +400,44 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) /* should the channel be enabled? */ /* this option is normally set last */ if (value & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) { + uint8_t count = 0; + /* assert min..center..max ordering */ - if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) - break; - if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) - break; - if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) - break; - if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) - break; + if (conf[PX4IO_P_RC_CONFIG_MIN] < 500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_MAX] > 2500) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] < conf[PX4IO_P_RC_CONFIG_MIN]) { + count++; + } + if (conf[PX4IO_P_RC_CONFIG_CENTER] > conf[PX4IO_P_RC_CONFIG_MAX]) { + count++; + } + /* assert deadzone is sane */ - if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) - break; - if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) - break; - if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) - break; - if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) - break; + if (conf[PX4IO_P_RC_CONFIG_DEADZONE] > 500) { + count++; + } + // The following check isn't needed as constraint checks in controls.c will catch this. + //if (conf[PX4IO_P_RC_CONFIG_MIN] > (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + // count++; + //} + //if (conf[PX4IO_P_RC_CONFIG_MAX] < (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) { + // count++; + //} + if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) { + count++; + } /* sanity checks pass, enable channel */ - conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + if (count) { + isr_debug(0, "ERROR: %d config error(s) for RC%d.\n", count, (channel + 1)); + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_INIT_OK; + } else { + conf[index] |= PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; + } } break; /* inner switch: case PX4IO_P_RC_CONFIG_OPTIONS */ diff --git a/apps/px4io/safety.c b/apps/px4io/safety.c index 540636e19..5495d5ae1 100644 --- a/apps/px4io/safety.c +++ b/apps/px4io/safety.c @@ -176,12 +176,17 @@ heartbeat_blink(void *arg) static void failsafe_blink(void *arg) { + /* indicate that a serious initialisation error occured */ + if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)) { + LED_AMBER(true); + return; + } + static bool failsafe = false; /* blink the failsafe LED if we don't have FMU input */ if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { failsafe = !failsafe; - } else { failsafe = false; } diff --git a/apps/px4io/sbus.c b/apps/px4io/sbus.c index d199a9361..073ddeaba 100644 --- a/apps/px4io/sbus.c +++ b/apps/px4io/sbus.c @@ -53,7 +53,7 @@ #include "debug.h" #define SBUS_FRAME_SIZE 25 -#define SBUS_INPUT_CHANNELS 18 +#define SBUS_INPUT_CHANNELS 16 static int sbus_fd = -1; @@ -239,7 +239,9 @@ sbus_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) } /* decode switch channels if data fields are wide enough */ - if (chancount > 17) { + if (PX4IO_INPUT_CHANNELS > 17 && chancount > 15) { + chancount = 18; + /* channel 17 (index 16) */ values[16] = (frame[23] & (1 << 0)) * 1000 + 998; /* channel 18 (index 17) */ diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index 4fd5fea1b..df8745d9f 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -683,7 +683,7 @@ int sdlog_thread_main(int argc, char *argv[]) .vbat = buf.batt.voltage_v, .bat_current = buf.batt.current_a, .bat_discharged = buf.batt.discharged_mah, - .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2]}, + .adc = {buf.raw.adc_voltage_v[0], buf.raw.adc_voltage_v[1], buf.raw.adc_voltage_v[2], buf.raw.adc_voltage_v[3]}, .local_position = {buf.local_pos.x, buf.local_pos.y, buf.local_pos.z}, .gps_raw_position = {buf.gps_pos.lat, buf.gps_pos.lon, buf.gps_pos.alt}, .attitude = {buf.att.pitch, buf.att.roll, buf.att.yaw}, diff --git a/apps/sdlog/sdlog_ringbuffer.h b/apps/sdlog/sdlog_ringbuffer.h index f7fc0d450..b65916459 100644 --- a/apps/sdlog/sdlog_ringbuffer.h +++ b/apps/sdlog/sdlog_ringbuffer.h @@ -56,7 +56,7 @@ struct sdlog_sysvector { float vbat; /**< battery voltage in [volt] */ float bat_current; /**< battery discharge current */ float bat_discharged; /**< discharged energy in mAh */ - float adc[3]; /**< remaining auxiliary ADC ports [volt] */ + float adc[4]; /**< ADC ports [volt] */ float local_position[3]; /**< tangent plane mapping into x,y,z [m] */ int32_t gps_raw_position[3]; /**< latitude [degrees] north, longitude [degrees] east, altitude above MSL [millimeter] */ float attitude[3]; /**< roll, pitch, yaw [rad] */ @@ -88,4 +88,4 @@ void sdlog_logbuffer_write(struct sdlog_logbuffer *lb, const struct sdlog_sysvec int sdlog_logbuffer_read(struct sdlog_logbuffer *lb, struct sdlog_sysvector *elem); -#endif
\ No newline at end of file +#endif diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index df7f66116..5987a76fc 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -64,6 +64,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f); PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f); +PARAM_DEFINE_FLOAT(SENS_VAIR_OFF, 2.5f); + PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f); PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f); PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index bfec17657..f48abb4a2 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -187,6 +187,7 @@ private: float mag_scale[3]; float accel_offset[3]; float accel_scale[3]; + float airspeed_offset; int rc_type; @@ -233,6 +234,7 @@ private: param_t accel_scale[3]; param_t mag_offset[3]; param_t mag_scale[3]; + param_t airspeed_offset; param_t rc_map_roll; param_t rc_map_pitch; @@ -475,6 +477,9 @@ Sensors::Sensors() : _parameter_handles.mag_scale[1] = param_find("SENS_MAG_YSCALE"); _parameter_handles.mag_scale[2] = param_find("SENS_MAG_ZSCALE"); + /*Airspeed offset */ + _parameter_handles.airspeed_offset = param_find("SENS_VAIR_OFF"); + _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); /* fetch initial parameter values */ @@ -672,6 +677,9 @@ Sensors::parameters_update() param_get(_parameter_handles.mag_scale[1], &(_parameters.mag_scale[1])); param_get(_parameter_handles.mag_scale[2], &(_parameters.mag_scale[2])); + /* Airspeed offset */ + param_get(_parameter_handles.airspeed_offset, &(_parameters.airspeed_offset)); + /* scaling of ADC ticks to battery voltage */ if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) { warnx("Failed updating voltage scaling param"); @@ -975,12 +983,16 @@ Sensors::adc_poll(struct sensor_combined_s &raw) /* read all channels available */ int ret = read(_fd_adc, &buf_adc, sizeof(buf_adc)); - /* look for battery channel */ - for (unsigned i = 0; i < sizeof(buf_adc) / sizeof(buf_adc[0]); i++) { - + if (ret >= (int)sizeof(buf_adc[0])) { + /* Save raw voltage values */ + if (i < (sizeof(raw.adc_voltage_v)) / sizeof(raw.adc_voltage_v[0])) { + raw.adc_voltage_v[i] = buf_adc[i].am_data / (4096.0f / 3.3f); + } + + /* look for specific channels and process the raw voltage to measurement data */ if (ADC_BATTERY_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* Voltage in volts */ float voltage = (buf_adc[i].am_data * _parameters.battery_voltage_scaling); @@ -1010,8 +1022,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ - - float voltage = buf_adc[i].am_data / 4096.0f; + float voltage = (float)(buf_adc[i].am_data ) * 3.3f / 4096.0f * 2.0f; //V_ref/4096 * (voltage divider factor) /** * The voltage divider pulls the signal down, only act on @@ -1019,24 +1030,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw) */ if (voltage > 0.4f) { - float pres_raw = fabsf(voltage - (3.3f / 2.0f)); - float pres_mbar = pres_raw * (3.3f / 5.0f) * 10.0f; + float diff_pres_pa = (voltage - _parameters.airspeed_offset) * 1000.0f; //for MPXV7002DP sensor - float airspeed_true = calc_true_airspeed(pres_mbar + _barometer.pressure, - _barometer.pressure, _barometer.temperature - 5.0f); + float airspeed_true = calc_true_airspeed(diff_pres_pa + _barometer.pressure*1e2f, + _barometer.pressure*1e2f, _barometer.temperature - 5.0f); //factor 1e2 for conversion from mBar to Pa // XXX HACK - true temperature is much less than indicated temperature in baro, // subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - float airspeed_indicated = calc_indicated_airspeed(pres_mbar + _barometer.pressure, - _barometer.pressure, _barometer.temperature - 5.0f); - // XXX HACK + float airspeed_indicated = calc_indicated_airspeed(diff_pres_pa); + + //printf("voltage: %.4f, diff_pres_pa %.4f, baro press %.4f Pa, v_ind %.4f, v_true %.4f\n", (double)voltage, (double)diff_pres_pa, (double)_barometer.pressure*1e2f, (double)airspeed_indicated, (double)airspeed_true); _differential_pressure.timestamp = hrt_absolute_time(); _differential_pressure.static_pressure_mbar = _barometer.pressure; - _differential_pressure.differential_pressure_mbar = pres_mbar; + _differential_pressure.differential_pressure_mbar = diff_pres_pa*1e-2f; _differential_pressure.temperature_celcius = _barometer.temperature; _differential_pressure.indicated_airspeed_m_s = airspeed_indicated; _differential_pressure.true_airspeed_m_s = airspeed_true; + _differential_pressure.voltage = voltage; /* announce the airspeed if needed, just publish else */ if (_airspeed_pub > 0) { @@ -1058,36 +1069,6 @@ Sensors::adc_poll(struct sensor_combined_s &raw) void Sensors::ppm_poll() { - /* fake low-level driver, directly pulling from driver variables */ - static orb_advert_t rc_input_pub = -1; - struct rc_input_values raw; - - raw.timestamp = ppm_last_valid_decode; - /* we are accepting this message */ - _ppm_last_valid = ppm_last_valid_decode; - - /* - * relying on two decoded channels is very noise-prone, - * in particular if nothing is connected to the pins. - * requiring a minimum of four channels - */ - if (ppm_decoded_channels > 4 && hrt_absolute_time() - _ppm_last_valid < PPM_INPUT_TIMEOUT_INTERVAL) { - - for (unsigned i = 0; i < ppm_decoded_channels; i++) { - raw.values[i] = ppm_buffer[i]; - } - - raw.channel_count = ppm_decoded_channels; - - /* publish to object request broker */ - if (rc_input_pub <= 0) { - rc_input_pub = orb_advertise(ORB_ID(input_rc), &raw); - - } else { - orb_publish(ORB_ID(input_rc), rc_input_pub, &raw); - } - } - /* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */ bool rc_updated; @@ -1133,31 +1114,45 @@ Sensors::ppm_poll() /* Read out values from raw message */ for (unsigned int i = 0; i < channel_limit; i++) { - /* scale around the mid point differently for lower and upper range */ + /* + * 1) Constrain to min/max values, as later processing depends on bounds. + */ + if (rc_input.values[i] < _parameters.min[i]) + rc_input.values[i] = _parameters.min[i]; + if (rc_input.values[i] > _parameters.max[i]) + rc_input.values[i] = _parameters.max[i]; + + /* + * 2) Scale around the mid point differently for lower and upper range. + * + * This is necessary as they don't share the same endpoints and slope. + * + * First normalize to 0..1 range with correct sign (below or above center), + * the total range is 2 (-1..1). + * If center (trim) == min, scale to 0..1, if center (trim) == max, + * scale to -1..0. + * + * As the min and max bounds were enforced in step 1), division by zero + * cannot occur, as for the case of center == min or center == max the if + * statement is mutually exclusive with the arithmetic NaN case. + * + * DO NOT REMOVE OR ALTER STEP 1! + */ if (rc_input.values[i] > (_parameters.trim[i] + _parameters.dz[i])) { - _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i]) / (float)(_parameters.max[i] - _parameters.trim[i]); + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.max[i] - _parameters.trim[i] - _parameters.dz[i]); } else if (rc_input.values[i] < (_parameters.trim[i] - _parameters.dz[i])) { - /* division by zero impossible for trim == min (as for throttle), as this falls in the above if clause */ - _rc.chan[i].scaled = -((_parameters.trim[i] - rc_input.values[i]) / (float)(_parameters.trim[i] - _parameters.min[i])); + _rc.chan[i].scaled = (rc_input.values[i] - _parameters.trim[i] - _parameters.dz[i]) / (float)(_parameters.trim[i] - _parameters.min[i] - _parameters.dz[i]); } else { /* in the configured dead zone, output zero */ _rc.chan[i].scaled = 0.0f; } - /* reverse channel if required */ - if (i == (int)_rc.function[THROTTLE]) { - if ((int)_parameters.rev[i] == -1) { - _rc.chan[i].scaled = 1.0f + -1.0f * _rc.chan[i].scaled; - } - - } else { - _rc.chan[i].scaled *= _parameters.rev[i]; - } + _rc.chan[i].scaled *= _parameters.rev[i]; /* handle any parameter-induced blowups */ - if (isnan(_rc.chan[i].scaled) || isinf(_rc.chan[i].scaled)) + if (!isfinite(_rc.chan[i].scaled)) _rc.chan[i].scaled = 0.0f; } diff --git a/apps/systemcmds/mixer/mixer.c b/apps/systemcmds/mixer/mixer.c index e2f7b5bd5..55c4f0836 100644 --- a/apps/systemcmds/mixer/mixer.c +++ b/apps/systemcmds/mixer/mixer.c @@ -117,7 +117,23 @@ load(const char *devname, const char *fname) if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':')) continue; - /* XXX an optimisation here would be to strip extra whitespace */ + /* compact whitespace in the buffer */ + char *t, *f; + for (f = buf; *f != '\0'; f++) { + /* scan for space characters */ + if (*f == ' ') { + /* look for additional spaces */ + t = f + 1; + while (*t == ' ') + t++; + if (*t == '\0') { + /* strip trailing whitespace */ + *f = '\0'; + } else if (t > (f + 1)) { + memmove(f + 1, t, strlen(t) + 1); + } + } + } /* if the line is too long to fit in the buffer, bail */ if ((strlen(line) + strlen(buf) + 1) >= sizeof(buf)) diff --git a/apps/systemlib/airspeed.c b/apps/systemlib/airspeed.c index 5c68f8ea5..264287b10 100644 --- a/apps/systemlib/airspeed.c +++ b/apps/systemlib/airspeed.c @@ -40,14 +40,31 @@ * */ -#include "math.h" +#include <stdio.h> +#include <math.h> #include "conversions.h" #include "airspeed.h" -float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature) +/** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param differential_pressure total_ pressure - static pressure + * @return indicated airspeed in m/s + */ +float calc_indicated_airspeed(float differential_pressure) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + + if (differential_pressure > 0) { + return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } else { + return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C); + } + } /** @@ -55,14 +72,14 @@ float calc_indicated_airspeed(float pressure_front, float pressure_ambient, floa * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param speed current indicated airspeed + * @param speed_indicated current indicated airspeed * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius + * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature) +float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius) { - return speed * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature)); + return speed_indicated * sqrtf(CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C / get_air_density(pressure_ambient, temperature_celsius)); } /** @@ -70,12 +87,25 @@ float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, flo * * Note that the true airspeed is NOT the groundspeed, because of the effects of wind * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius * @return true airspeed in m/s */ -float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature) +float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius) { - return sqrtf((2.0f*(pressure_front - pressure_ambient)) / get_air_density(pressure_ambient, temperature)); + float density = get_air_density(static_pressure, temperature_celsius); + if (density < 0.0001f || !isfinite(density)) { + density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C; + printf("[airspeed] Invalid air density, using density at sea level\n"); + } + + float pressure_difference = total_pressure - static_pressure; + + if(pressure_difference > 0) { + return sqrtf((2.0f*(pressure_difference)) / density); + } else + { + return -sqrtf((2.0f*fabs(pressure_difference)) / density); + } } diff --git a/apps/systemlib/airspeed.h b/apps/systemlib/airspeed.h index b1beb79ae..def53f0c1 100644 --- a/apps/systemlib/airspeed.h +++ b/apps/systemlib/airspeed.h @@ -48,43 +48,42 @@ __BEGIN_DECLS -/** - * Calculate indicated airspeed. - * - * Note that the indicated airspeed is not the true airspeed because it - * lacks the air density compensation. Use the calc_true_airspeed functions to get - * the true airspeed. - * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return indicated airspeed in m/s - */ -__EXPORT float calc_indicated_airspeed(float pressure_front, float pressure_ambient, float temperature); + /** + * Calculate indicated airspeed. + * + * Note that the indicated airspeed is not the true airspeed because it + * lacks the air density compensation. Use the calc_true_airspeed functions to get + * the true airspeed. + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @return indicated airspeed in m/s + */ + __EXPORT float calc_indicated_airspeed(float differential_pressure); -/** - * Calculate true airspeed from indicated airspeed. - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param speed current indicated airspeed - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return true airspeed in m/s - */ -__EXPORT float calc_true_airspeed_from_indicated(float speed, float pressure_ambient, float temperature); + /** + * Calculate true airspeed from indicated airspeed. + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param speed_indicated current indicated airspeed + * @param pressure_ambient pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_ambient, float temperature_celsius); -/** - * Directly calculate true airspeed - * - * Note that the true airspeed is NOT the groundspeed, because of the effects of wind - * - * @param pressure_front pressure inside the pitot/prandl tube - * @param pressure_ambient pressure at the side of the tube/airplane - * @param temperature air temperature in degrees celcius - * @return true airspeed in m/s - */ -__EXPORT float calc_true_airspeed(float pressure_front, float pressure_ambient, float temperature); + /** + * Directly calculate true airspeed + * + * Note that the true airspeed is NOT the groundspeed, because of the effects of wind + * + * @param total_pressure pressure inside the pitot/prandtl tube + * @param static_pressure pressure at the side of the tube/airplane + * @param temperature_celsius air temperature in degrees celcius + * @return true airspeed in m/s + */ + __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius); __END_DECLS diff --git a/apps/systemlib/conversions.c b/apps/systemlib/conversions.c index 2b8003e45..ac94252c5 100644 --- a/apps/systemlib/conversions.c +++ b/apps/systemlib/conversions.c @@ -150,5 +150,5 @@ void quat2rot(const float Q[4], float R[9]) float get_air_density(float static_pressure, float temperature_celsius) { - return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius + CONSTANTS_ABSOLUTE_NULL_KELVIN)); + return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); } diff --git a/apps/systemlib/conversions.h b/apps/systemlib/conversions.h index c2987709b..5d485b01f 100644 --- a/apps/systemlib/conversions.h +++ b/apps/systemlib/conversions.h @@ -44,10 +44,10 @@ #include <float.h> #include <stdint.h> -#define CONSTANTS_ONE_G 9.80665f -#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f -#define CONSTANTS_AIR_GAS_CONST 8.31432f -#define CONSTANTS_ABSOLUTE_NULL_KELVIN 273.15f +#define CONSTANTS_ONE_G 9.80665f // m/s^2 +#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C 1.225f // kg/m^3 +#define CONSTANTS_AIR_GAS_CONST 287.1f // J/(kg * K) +#define CONSTANTS_ABSOLUTE_NULL_CELSIUS -273.15f // °C __BEGIN_DECLS diff --git a/apps/systemlib/mixer/mixer.cpp b/apps/systemlib/mixer/mixer.cpp index 72f765d90..df0dfe838 100644 --- a/apps/systemlib/mixer/mixer.cpp +++ b/apps/systemlib/mixer/mixer.cpp @@ -54,6 +54,7 @@ #include "mixer.h" Mixer::Mixer(ControlCallback control_cb, uintptr_t cb_handle) : + _next(nullptr), _control_cb(control_cb), _cb_handle(cb_handle) { diff --git a/apps/systemlib/mixer/mixer.h b/apps/systemlib/mixer/mixer.h index 00ddf1581..71386cba7 100644 --- a/apps/systemlib/mixer/mixer.h +++ b/apps/systemlib/mixer/mixer.h @@ -160,7 +160,7 @@ public: * @param control_cb Callback invoked when reading controls. */ Mixer(ControlCallback control_cb, uintptr_t cb_handle); - ~Mixer() {}; + virtual ~Mixer() {}; /** * Perform the mixing function. diff --git a/apps/systemlib/mixer/mixer_group.cpp b/apps/systemlib/mixer/mixer_group.cpp index 60eeff225..1dbc512cd 100644 --- a/apps/systemlib/mixer/mixer_group.cpp +++ b/apps/systemlib/mixer/mixer_group.cpp @@ -93,6 +93,7 @@ MixerGroup::reset() mixer = _first; _first = mixer->_next; delete mixer; + mixer = nullptr; } } diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 3f3476f62..136375140 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -53,6 +53,9 @@ ORB_DEFINE(sensor_gyro, struct gyro_report); #include <drivers/drv_baro.h> ORB_DEFINE(sensor_baro, struct baro_report); +#include <drivers/drv_range_finder.h> +ORB_DEFINE(sensor_range_finder, struct range_finder_report); + #include <drivers/drv_pwm_output.h> ORB_DEFINE(output_pwm, struct pwm_output_values); diff --git a/apps/uORB/topics/differential_pressure.h b/apps/uORB/topics/differential_pressure.h index fd7670cbc..d5e4bf37e 100644 --- a/apps/uORB/topics/differential_pressure.h +++ b/apps/uORB/topics/differential_pressure.h @@ -49,15 +49,16 @@ */ /** - * Battery voltages and status + * Differential pressure and airspeed */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ + uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ float static_pressure_mbar; /**< Static / environment pressure */ float differential_pressure_mbar; /**< Differential pressure reading */ float temperature_celcius; /**< ambient temperature in celcius, -1 if unknown */ float indicated_airspeed_m_s; /**< indicated airspeed in meters per second, -1 if unknown */ - float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float true_airspeed_m_s; /**< true airspeed in meters per second, -1 if unknown */ + float voltage; /**< Voltage from the airspeed sensor (voltage divider already compensated) */ }; /** @@ -67,4 +68,4 @@ struct differential_pressure_s { /* register this as object request broker structure */ ORB_DECLARE(differential_pressure); -#endif
\ No newline at end of file +#endif diff --git a/apps/uORB/topics/subsystem_info.h b/apps/uORB/topics/subsystem_info.h index c3e039d66..c415e832e 100644 --- a/apps/uORB/topics/subsystem_info.h +++ b/apps/uORB/topics/subsystem_info.h @@ -71,7 +71,8 @@ enum SUBSYSTEM_TYPE SUBSYSTEM_TYPE_YAWPOSITION = 4096, SUBSYSTEM_TYPE_ALTITUDECONTROL = 16384, SUBSYSTEM_TYPE_POSITIONCONTROL = 32768, - SUBSYSTEM_TYPE_MOTORCONTROL = 65536 + SUBSYSTEM_TYPE_MOTORCONTROL = 65536, + SUBSYSTEM_TYPE_RANGEFINDER = 131072 }; /** diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index eba5a5047..6d3f8a863 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -191,12 +191,16 @@ struct vehicle_status_s bool condition_auto_mission_available; bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ bool condition_launch_position_valid; /**< indicates a valid launch position */ - bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; + bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + + // bool condition_auto_flight_mode_ok; /**< conditions of auto flight mode apply plus a valid takeoff position lock has been aquired */ + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ bool flag_fmu_armed; /**< true is motors / actuators of FMU are armed */ - bool flag_io_armed; /**< true is motors / actuators of IO are armed */ + bool flag_io_armed; /**< true is motors / actuators of IO are armed */ bool flag_system_emergency; bool flag_preflight_calibration; @@ -208,10 +212,10 @@ struct vehicle_status_s bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ - // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ // bool flag_preflight_accel_calibration; + // bool flag_preflight_airspeed_calibration; bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ @@ -242,10 +246,6 @@ struct vehicle_status_s uint16_t errors_count3; uint16_t errors_count4; - // bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ - // bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - }; /** diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 532e54b8e..7abbf42ae 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -429,6 +429,12 @@ ORBDevNode::appears_updated(SubscriberData *sd) /* avoid racing between interrupt and non-interrupt context calls */ irqstate_t state = irqsave(); + /* check if this topic has been published yet, if not bail out */ + if (_data == nullptr) { + ret = false; + goto out; + } + /* * If the subscriber's generation count matches the update generation * count, there has been no update from their perspective; if they @@ -485,6 +491,7 @@ ORBDevNode::appears_updated(SubscriberData *sd) break; } +out: irqrestore(state); /* consider it updated */ diff --git a/nuttx/configs/px4fmu/nsh/appconfig b/nuttx/configs/px4fmu/nsh/appconfig index 289a721a1..f1f70e228 100644 --- a/nuttx/configs/px4fmu/nsh/appconfig +++ b/nuttx/configs/px4fmu/nsh/appconfig @@ -81,7 +81,7 @@ CONFIGURED_APPS += systemcmds/delay_test # Tutorial code from # https://pixhawk.ethz.ch/px4/dev/debug_values -CONFIGURED_APPS += examples/px4_mavlink_debug +# CONFIGURED_APPS += examples/px4_mavlink_debug # Shared object broker; required by many parts of the system. CONFIGURED_APPS += uORB @@ -125,6 +125,7 @@ CONFIGURED_APPS += drivers/stm32/adc CONFIGURED_APPS += drivers/px4fmu CONFIGURED_APPS += drivers/hil CONFIGURED_APPS += drivers/gps +CONFIGURED_APPS += drivers/mb12xx # Testing stuff CONFIGURED_APPS += px4/sensors_bringup diff --git a/nuttx/configs/px4io/io/defconfig b/nuttx/configs/px4io/io/defconfig index 5db94c6f1..bb937cf4e 100755 --- a/nuttx/configs/px4io/io/defconfig +++ b/nuttx/configs/px4io/io/defconfig @@ -401,11 +401,11 @@ CONFIG_SCHED_ATEXIT=n CONFIG_DISABLE_CLOCK=n CONFIG_DISABLE_POSIX_TIMERS=y CONFIG_DISABLE_PTHREAD=y -CONFIG_DISABLE_SIGNALS=n +CONFIG_DISABLE_SIGNALS=y CONFIG_DISABLE_MQUEUE=y CONFIG_DISABLE_MOUNTPOINT=y CONFIG_DISABLE_ENVIRON=y -CONFIG_DISABLE_POLL=n +CONFIG_DISABLE_POLL=y # # Misc libc settings @@ -475,12 +475,12 @@ CONFIG_ARCH_BZERO=n # timer structures to minimize dynamic allocations. Set to # zero for all dynamic allocations. # -CONFIG_MAX_TASKS=8 +CONFIG_MAX_TASKS=4 CONFIG_MAX_TASK_ARGS=4 -CONFIG_NPTHREAD_KEYS=4 +CONFIG_NPTHREAD_KEYS=2 CONFIG_NFILE_DESCRIPTORS=8 CONFIG_NFILE_STREAMS=0 -CONFIG_NAME_MAX=32 +CONFIG_NAME_MAX=12 CONFIG_STDIO_BUFFER_SIZE=32 CONFIG_STDIO_LINEBUFFER=n CONFIG_NUNGET_CHARS=2 @@ -541,7 +541,7 @@ CONFIG_BOOT_COPYTORAM=n CONFIG_CUSTOM_STACK=n CONFIG_STACK_POINTER= CONFIG_IDLETHREAD_STACKSIZE=1024 -CONFIG_USERMAIN_STACKSIZE=1024 +CONFIG_USERMAIN_STACKSIZE=1200 CONFIG_PTHREAD_STACK_MIN=512 CONFIG_PTHREAD_STACK_DEFAULT=1024 CONFIG_HEAP_BASE= |