diff --git a/Makefile b/Makefile index 201187e021..47fe326b92 100644 --- a/Makefile +++ b/Makefile @@ -48,7 +48,12 @@ GIT_DESC := $(shell git log -1 --pretty=format:%H) ifneq ($(words $(GIT_DESC)),1) GIT_DESC := "unknown_git_version" endif -export GIT_DESC + +GIT_DESC_SHORT := $(shell echo $(GIT_DESC) | cut -c1-16) + +$(shell echo "#include " > $(BUILD_DIR)git_version.c) +$(shell echo "const char* px4_git_version = \"$(GIT_DESC)\";" >> $(BUILD_DIR)git_version.c) +$(shell echo "const uint64_t px4_git_version_binary = 0x$(GIT_DESC_SHORT);" >> $(BUILD_DIR)git_version.c) # # Canned firmware configurations that we (know how to) build. diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index e100e6b4b7..2dfe3d8e30 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -302,6 +302,18 @@ then # Sensors System (start before Commander so Preflight checks are properly run) # sh /etc/init.d/rc.sensors + + if [ $GPS == yes ] + then + if [ $GPS_FAKE == yes ] + then + echo "[i] Faking GPS" + gps start -f + else + gps start + fi + fi + unset GPS_FAKE # Needs to be this early for in-air-restarts commander start @@ -479,22 +491,10 @@ then sh /etc/init.d/rc.uavcan # - # Logging, GPS + # Logging # sh /etc/init.d/rc.logging - if [ $GPS == yes ] - then - if [ $GPS_FAKE == yes ] - then - echo "[i] Faking GPS" - gps start -f - else - gps start - fi - fi - unset GPS_FAKE - # # Start up ARDrone Motor interface # diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 475e7cc434..645a0e1b1e 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 475e7cc4348b207ac3efed45eb61160d23ac7a26 +Subproject commit 645a0e1b1eb09efeeaa14cc18d2474628a31ab53 diff --git a/msg/filtered_bottom_flow.msg b/msg/filtered_bottom_flow.msg new file mode 100644 index 0000000000..815a38414d --- /dev/null +++ b/msg/filtered_bottom_flow.msg @@ -0,0 +1,8 @@ +# Filtered bottom flow in bodyframe. +uint64 timestamp # time of this estimate, in microseconds since system start + +float32 sumx # Integrated bodyframe x flow in meters +float32 sumy # Integrated bodyframe y flow in meters + +float32 vx # Flow bodyframe x speed, m/s +float32 vy # Flow bodyframe y Speed, m/s diff --git a/msg/home_position.msg b/msg/home_position.msg new file mode 100644 index 0000000000..d8aff3f634 --- /dev/null +++ b/msg/home_position.msg @@ -0,0 +1,10 @@ +# GPS home position in WGS84 coordinates. +uint64 timestamp # Timestamp (microseconds since system boot) + +float64 lat # Latitude in degrees +float64 lon # Longitude in degrees +float32 alt # Altitude in meters (AMSL) + +float32 x # X coordinate in meters +float32 y # Y coordinate in meters +float32 z # Z coordinate in meters diff --git a/msg/optical_flow.msg b/msg/optical_flow.msg new file mode 100644 index 0000000000..d34e32b8fd --- /dev/null +++ b/msg/optical_flow.msg @@ -0,0 +1,17 @@ +# Optical flow in NED body frame in SI units. +# @see http://en.wikipedia.org/wiki/International_System_of_Units + +uint64 timestamp # in microseconds since system start +uint8 sensor_id # id of the sensor emitting the flow value +float32 pixel_flow_x_integral # accumulated optical flow in radians around x axis +float32 pixel_flow_y_integral # accumulated optical flow in radians around y axis +float32 gyro_x_rate_integral # accumulated gyro value in radians around x axis +float32 gyro_y_rate_integral # accumulated gyro value in radians around y axis +float32 gyro_z_rate_integral # accumulated gyro value in radians around z axis +float32 ground_distance_m # Altitude / distance to ground in meters +uint32 integration_timespan # accumulation timespan in microseconds +uint32 time_since_last_sonar_update # time since last sonar update in microseconds +uint16 frame_count_since_last_readout # number of accumulated frames in timespan +int16 gyro_temperature # Temperature * 100 in centi-degrees Celsius +uint8 quality # Average of quality of accumulated frames, 0: bad quality, 255: maximum quality + diff --git a/msg/range_finder.msg b/msg/range_finder.msg new file mode 100644 index 0000000000..d2e67543e5 --- /dev/null +++ b/msg/range_finder.msg @@ -0,0 +1,13 @@ +int16 RANGE_FINDER_TYPE_LASER = 0 + +# range finder report structure. Reads from the device must be in multiples of this +# structure. +uint64 timestamp +uint64 error_count +uint16 type # type, following RANGE_FINDER_TYPE enum +float32 distance # in meters +float32 minimum_distance # minimum distance the sensor can measure +float32 maximum_distance # maximum distance the sensor can measure +uint8 valid # 1 == within sensor range, 0 = outside sensor range +float32[12] distance_vector # in meters length should match MB12XX_MAX_RANGEFINDERS +uint8 just_updated # number of the most recent measurement sensor diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg new file mode 100644 index 0000000000..c5e427e194 --- /dev/null +++ b/msg/sensor_combined.msg @@ -0,0 +1,100 @@ +# Definition of the sensor_combined uORB topic. + +int32 MAGNETOMETER_MODE_NORMAL = 0 +int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1 +int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2 + +# Sensor readings in raw and SI-unit form. +# +# These values are read from the sensors. Raw values are in sensor-specific units, +# the scaled values are in SI-units, as visible from the ending of the variable +# or the comments. The use of the SI fields is in general advised, as these fields +# are scaled and offset-compensated where possible and do not change with board +# revisions and sensor updates. +# +# Actual data, this is specific to the type of data which is stored in this struct +# A line containing L0GME will be added by the Python logging code generator to the logged dataset. +# +# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only + +uint64 timestamp # Timestamp in microseconds since boot, from gyro +# +int16[3] gyro_raw # Raw sensor values of angular velocity +float32[3] gyro_rad_s # Angular velocity in radian per seconds +uint32 gyro_errcount # Error counter for gyro 0 +float32 gyro_temp # Temperature of gyro 0 + +int16[3] accelerometer_raw # Raw acceleration in NED body frame +float32[3] accelerometer_m_s2 # Acceleration in NED body frame, in m/s^2 +int16 accelerometer_mode # Accelerometer measurement mode +float32 accelerometer_range_m_s2 # Accelerometer measurement range in m/s^2 +uint64 accelerometer_timestamp # Accelerometer timestamp +uint32 accelerometer_errcount # Error counter for accel 0 +float32 accelerometer_temp # Temperature of accel 0 + +int16[3] magnetometer_raw # Raw magnetic field in NED body frame +float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss +int16 magnetometer_mode # Magnetometer measurement mode +float32 magnetometer_range_ga # measurement range in Gauss +float32 magnetometer_cuttoff_freq_hz # Internal analog low pass frequency of sensor +uint64 magnetometer_timestamp # Magnetometer timestamp +uint32 magnetometer_errcount # Error counter for mag 0 +float32 magnetometer_temp # Temperature of mag 0 + +int16[3] gyro1_raw # Raw sensor values of angular velocity +float32[3] gyro1_rad_s # Angular velocity in radian per seconds +uint64 gyro1_timestamp # Gyro timestamp +uint32 gyro1_errcount # Error counter for gyro 1 +float32 gyro1_temp # Temperature of gyro 1 + +int16[3] accelerometer1_raw # Raw acceleration in NED body frame +float32[3] accelerometer1_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer1_timestamp # Accelerometer timestamp +uint32 accelerometer1_errcount # Error counter for accel 1 +float32 accelerometer1_temp # Temperature of accel 1 + +int16[3] magnetometer1_raw # Raw magnetic field in NED body frame +float32[3] magnetometer1_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer1_timestamp # Magnetometer timestamp +uint32 magnetometer1_errcount # Error counter for mag 1 +float32 magnetometer1_temp # Temperature of mag 1 + +int16[3] gyro2_raw # Raw sensor values of angular velocity +float32[3] gyro2_rad_s # Angular velocity in radian per seconds +uint64 gyro2_timestamp # Gyro timestamp +uint32 gyro2_errcount # Error counter for gyro 1 +float32 gyro2_temp # Temperature of gyro 1 + +int16[3] accelerometer2_raw # Raw acceleration in NED body frame +float32[3] accelerometer2_m_s2 # Acceleration in NED body frame, in m/s^2 +uint64 accelerometer2_timestamp # Accelerometer timestamp +uint32 accelerometer2_errcount # Error counter for accel 2 +float32 accelerometer2_temp # Temperature of accel 2 + +int16[3] magnetometer2_raw # Raw magnetic field in NED body frame +float32[3] magnetometer2_ga # Magnetic field in NED body frame, in Gauss +uint64 magnetometer2_timestamp # Magnetometer timestamp +uint32 magnetometer2_errcount # Error counter for mag 2 +float32 magnetometer2_temp # Temperature of mag 2 + +float32 baro_pres_mbar # Barometric pressure, already temp. comp. +float32 baro_alt_meter # Altitude, already temp. comp. +float32 baro_temp_celcius # Temperature in degrees celsius +uint64 baro_timestamp # Barometer timestamp + +float32 baro1_pres_mbar # Barometric pressure, already temp. comp. +float32 baro1_alt_meter # Altitude, already temp. comp. +float32 baro1_temp_celcius # Temperature in degrees celsius +uint64 baro1_timestamp # Barometer timestamp + +float32[10] adc_voltage_v # ADC voltages of ADC Chan 10/11/12/13 or -1 +uint16[10] adc_mapping # Channel indices of each of these values +float32 mcu_temp_celcius # Internal temperature measurement of MCU + +float32 differential_pressure_pa # Airspeed sensor differential pressure +uint64 differential_pressure_timestamp # Last measurement timestamp +float32 differential_pressure_filtered_pa # Low pass filtered airspeed sensor differential pressure reading + +float32 differential_pressure1_pa # Airspeed sensor differential pressure +uint64 differential_pressure1_timestamp # Last measurement timestamp +float32 differential_pressure1_filtered_pa # Low pass filtered airspeed sensor differential pressure reading diff --git a/msg/vehicle_global_position.msg b/msg/vehicle_global_position.msg new file mode 100644 index 0000000000..6a38e2c9d7 --- /dev/null +++ b/msg/vehicle_global_position.msg @@ -0,0 +1,20 @@ +# Fused global position in WGS84. +# This struct contains global position estimation. It is not the raw GPS +# measurement (@see vehicle_gps_position). This topic is usually published by the position +# estimator, which will take more sources of information into account than just GPS, +# e.g. control inputs of the vehicle in a Kalman-filter implementation. +# +uint64 timestamp # Time of this estimate, in microseconds since system start +uint64 time_utc_usec # GPS UTC timestamp in microseconds +float64 lat # Latitude in degrees +float64 lon # Longitude in degrees +float32 alt # Altitude AMSL in meters +float32 vel_n # Ground north velocity, m/s +float32 vel_e # Ground east velocity, m/s +float32 vel_d # Ground downside velocity, m/s +float32 yaw # Yaw in radians -PI..+PI. +float32 eph # Standard deviation of position estimate horizontally +float32 epv # Standard deviation of position vertically +float32 terrain_alt # Terrain altitude in m, WGS84 +bool terrain_alt_valid # Terrain altitude estimate is valid +bool dead_reckoning # True if this position is estimated through dead-reckoning diff --git a/msg/vehicle_gps_position.msg b/msg/vehicle_gps_position.msg new file mode 100644 index 0000000000..25c532ccaa --- /dev/null +++ b/msg/vehicle_gps_position.msg @@ -0,0 +1,29 @@ +# GPS position in WGS84 coordinates. +uint64 timestamp_position # Timestamp for position information +int32 lat # Latitude in 1E-7 degrees +int32 lon # Longitude in 1E-7 degrees +int32 alt # Altitude in 1E-3 meters (millimeters) above MSL + +uint64 timestamp_variance +float32 s_variance_m_s # speed accuracy estimate m/s +float32 c_variance_rad # course accuracy estimate rad +uint8 fix_type # 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. + +float32 eph # GPS HDOP horizontal dilution of position in m +float32 epv # GPS VDOP horizontal dilution of position in m + +int32 noise_per_ms # GPS noise per millisecond +int32 jamming_indicator # indicates jamming is occurring + +uint64 timestamp_velocity # Timestamp for velocity informations +float32 vel_m_s # GPS ground speed (m/s) +float32 vel_n_m_s # GPS ground speed in m/s +float32 vel_e_m_s # GPS ground speed in m/s +float32 vel_d_m_s # GPS ground speed in m/s +float32 cog_rad # Course over ground (NOT heading, but direction of movement) in rad, -PI..PI +bool vel_ned_valid # Flag to indicate if NED speed is valid + +uint64 timestamp_time # Timestamp for time information +uint64 time_utc_usec # Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 + +uint8 satellites_used # Number of satellites used diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index 430f7dd768..c5d5ee9a16 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -91,6 +91,7 @@ uint8 nav_state # set navigation state machine to specified value uint8 arming_state # current arming state uint8 hil_state # current hil state bool failsafe # true if system is in failsafe state +bool calibration_enabled # true if current calibrating parts of the system. Also sets the system to ARMING_STATE_INIT. int32 system_type # system type, inspired by MAVLink's VEHICLE_TYPE enum int32 system_id # system id, inspired by MAVLink's system ID field diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index ab8f9eef62..19e0f7c632 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -560,8 +560,8 @@ CONFIG_USART1_2STOP=0 # # USART2 Configuration # -CONFIG_USART2_RXBUFSIZE=512 -CONFIG_USART2_TXBUFSIZE=512 +CONFIG_USART2_RXBUFSIZE=600 +CONFIG_USART2_TXBUFSIZE=2200 CONFIG_USART2_BAUD=57600 CONFIG_USART2_BITS=8 CONFIG_USART2_PARITY=0 diff --git a/src/drivers/drv_range_finder.h b/src/drivers/drv_range_finder.h index cc91569cd9..bd72971b94 100644 --- a/src/drivers/drv_range_finder.h +++ b/src/drivers/drv_range_finder.h @@ -48,39 +48,14 @@ #define RANGE_FINDER0_DEVICE_PATH "/dev/range_finder0" #define MB12XX_MAX_RANGEFINDERS 12 //Maximum number of RangeFinders that can be connected -enum RANGE_FINDER_TYPE { - RANGE_FINDER_TYPE_LASER = 0, -}; +#define range_finder_report range_finder_s +#define __orb_sensor_range_finder __orb_range_finder -/** - * @addtogroup topics - * @{ - */ +#include -/** - * range finder report structure. Reads from the device must be in multiples of this - * structure. - */ -struct range_finder_report { - uint64_t timestamp; - uint64_t error_count; - unsigned type; /**< type, following RANGE_FINDER_TYPE enum */ - float distance; /**< in meters */ - float minimum_distance; /**< minimum distance the sensor can measure */ - float maximum_distance; /**< maximum distance the sensor can measure */ - uint8_t valid; /**< 1 == within sensor range, 0 = outside sensor range */ - float distance_vector[MB12XX_MAX_RANGEFINDERS]; /** in meters */ - uint8_t just_updated; /** number of the most recent measurement sensor */ -}; - -/** - * @} - */ - -/* - * ObjDev tag for raw range finder data. - */ -ORB_DECLARE(sensor_range_finder); +#ifndef RANGE_FINDER_TYPE_LASER +#define RANGE_FINDER_TYPE_LASER 0 +#endif /* * ioctl() definitions diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 714c80ded2..e3706cf671 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -354,6 +354,20 @@ GPS::task_main() if (_Helper->configure(_baudrate) == 0) { unlock(); + //Publish initial report that we have access to a GPS + //Make sure to clear any stale data in case driver is reset + memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); + _report_gps_pos.timestamp_position = hrt_absolute_time(); + _report_gps_pos.timestamp_variance = hrt_absolute_time(); + _report_gps_pos.timestamp_velocity = hrt_absolute_time(); + _report_gps_pos.timestamp_time = hrt_absolute_time(); + if (_report_gps_pos_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + // GPS is obviously detected successfully, reset statistics _Helper->reset_update_rates(); @@ -364,13 +378,7 @@ GPS::task_main() if (!(_pub_blocked)) { if (helper_ret & 1) { - - if (_report_gps_pos_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub > 0) { diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 0319b601e7..82c18b5c14 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -195,7 +195,7 @@ static const int ERROR = -1; This time reduction is enough to cope with worst case timing jitter due to other timers */ -#define L3GD20_TIMER_REDUCTION 200 +#define L3GD20_TIMER_REDUCTION 600 extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); } diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 66641d6408..2b100c230a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -260,32 +260,37 @@ SF0X::~SF0X() int SF0X::init() { - /* do regular cdev init */ - if (CDev::init() != OK) { - goto out; - } + /* status */ + int ret = 0; - /* allocate basic report buffers */ - _reports = new RingBuffer(2, sizeof(range_finder_report)); + do { /* create a scope to handle exit conditions using break */ - if (_reports == nullptr) { - warnx("mem err"); - goto out; - } + /* do regular cdev init */ + ret = CDev::init(); + if (ret != OK) break; - /* get a publish handle on the range finder topic */ - struct range_finder_report zero_report; - memset(&zero_report, 0, sizeof(zero_report)); - _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + /* allocate basic report buffers */ + _reports = new RingBuffer(2, sizeof(range_finder_report)); + if (_reports == nullptr) { + warnx("mem err"); + ret = -1; + break; + } - if (_range_finder_topic < 0) { - warnx("advert err"); - } + /* get a publish handle on the range finder topic */ + struct range_finder_report zero_report; + memset(&zero_report, 0, sizeof(zero_report)); + _range_finder_topic = orb_advertise(ORB_ID(sensor_range_finder), &zero_report); + + if (_range_finder_topic < 0) { + warnx("advert err"); + } + } while(0); /* close the fd */ ::close(_fd); _fd = -1; -out: + return OK; } diff --git a/src/include/containers/List.hpp b/src/include/containers/List.hpp index 13cbda9382..1906030abb 100644 --- a/src/include/containers/List.hpp +++ b/src/include/containers/List.hpp @@ -45,6 +45,7 @@ class __EXPORT ListNode public: ListNode() : _sibling(nullptr) { } + virtual ~ListNode() {}; void setSibling(T sibling) { _sibling = sibling; } T getSibling() { return _sibling; } T get() { @@ -52,6 +53,11 @@ public: } protected: T _sibling; +private: + // forbid copy + ListNode(const ListNode& other); + // forbid assignment + ListNode & operator = (const ListNode &); }; template @@ -60,12 +66,18 @@ class __EXPORT List public: List() : _head() { } + virtual ~List() {}; void add(T newNode) { newNode->setSibling(getHead()); setHead(newNode); } T getHead() { return _head; } -private: +protected: void setHead(T &head) { _head = head; } T _head; +private: + // forbid copy + List(const List& other); + // forbid assignment + List& operator = (const List &); }; diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index 4d9bd8ae0c..96e7757da0 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -61,6 +62,7 @@ #include #include +#include #include @@ -269,8 +271,38 @@ out: return success; } +static bool gnssCheck(int mavlink_fd) +{ + bool success = true; + + int gpsSub = orb_subscribe(ORB_ID(vehicle_gps_position)); + + //Wait up to 2000ms to allow the driver to detect a GNSS receiver module + struct pollfd fds[1]; + fds[0].fd = gpsSub; + fds[0].events = POLLIN; + if(poll(fds, 1, 2000) <= 0) { + success = false; + } + else { + struct vehicle_gps_position_s gps; + if ( (OK != orb_copy(ORB_ID(vehicle_gps_position), gpsSub, &gps)) || + (hrt_elapsed_time(&gps.timestamp_position) > 1000000)) { + success = false; + } + } + + //Report failure to detect module + if(!success) { + mavlink_and_console_log_critical(mavlink_fd, "PREFLIGHT FAIL: GPS RECEIVER MISSING"); + } + + close(gpsSub); + return success; +} + bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, - bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic) + bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic) { bool failed = false; @@ -336,6 +368,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro } } + /* ---- Global Navigation Satellite System receiver ---- */ + if(checkGNSS) { + if(!gnssCheck(mavlink_fd)) { + failed = true; + } + } + /* Report status */ return !failed; } diff --git a/src/modules/commander/PreflightCheck.h b/src/modules/commander/PreflightCheck.h index 66947a3470..b6423a4d9a 100644 --- a/src/modules/commander/PreflightCheck.h +++ b/src/modules/commander/PreflightCheck.h @@ -59,11 +59,15 @@ namespace Commander * true if the gyroscopes should be checked * @param checkBaro * true if the barometer should be checked +* @param checkAirspeed +* true if the airspeed sensor should be checked * @param checkRC * true if the Remote Controller should be checked +* @param checkGNSS +* true if the GNSS receiver should be checked **/ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, - bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkDynamic = false); + bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS, bool checkDynamic = false); const unsigned max_mandatory_gyro_count = 1; const unsigned max_optional_gyro_count = 3; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index efd88b3d40..f250eef475 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -133,6 +133,7 @@ #include #include #include +#include #include #include #include @@ -143,6 +144,7 @@ #include #include #include +#include /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -552,3 +554,89 @@ calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref return calibrate_return_ok; } + +int do_level_calibration(int mavlink_fd) { + const unsigned cal_time = 5; + const unsigned cal_hz = 100; + const unsigned settle_time = 30; + int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + struct vehicle_attitude_s att; + memset(&att, 0, sizeof(att)); + + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "level"); + + param_t roll_offset_handler = param_find("SENS_BOARD_X_OFF"); + param_t pitch_offset_handler = param_find("SENS_BOARD_Y_OFF"); + + // save old values if calibration fails + float roll_offset_current; + float pitch_offset_current; + param_get(roll_offset_handler, &roll_offset_current); + param_get(pitch_offset_handler, &pitch_offset_current); + + float zero = 0.0f; + param_set(roll_offset_handler, &zero); + param_set(pitch_offset_handler, &zero); + + struct pollfd fds[1]; + + fds[0].fd = att_sub; + fds[0].events = POLLIN; + + float roll_mean = 0.0f; + float pitch_mean = 0.0f; + unsigned counter = 0; + + // sleep for some time + hrt_abstime start = hrt_absolute_time(); + while(hrt_elapsed_time(&start) < settle_time * 1000000) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, (int)(90*hrt_elapsed_time(&start)/1e6f/(float)settle_time)); + sleep(settle_time / 10); + } + + start = hrt_absolute_time(); + // average attitude for 5 seconds + while(hrt_elapsed_time(&start) < cal_time * 1000000) { + poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100); + orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); + roll_mean += att.roll; + pitch_mean += att.pitch; + counter++; + } + + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100); + + bool success = false; + if (counter > (cal_time * cal_hz / 2 )) { + roll_mean /= counter; + pitch_mean /= counter; + } else { + mavlink_and_console_log_info(mavlink_fd, "not enough measurements taken"); + success = false; + goto out; + } + + if (fabsf(roll_mean) > 0.8f ) { + mavlink_and_console_log_critical(mavlink_fd, "excess roll angle"); + } else if (fabsf(pitch_mean) > 0.8f ) { + mavlink_and_console_log_critical(mavlink_fd, "excess pitch angle"); + } else { + roll_mean *= (float)M_RAD_TO_DEG; + pitch_mean *= (float)M_RAD_TO_DEG; + param_set(roll_offset_handler, &roll_mean); + param_set(pitch_offset_handler, &pitch_mean); + success = true; + } + +out: + if (success) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "level"); + return 0; + } else { + // set old parameters + param_set(roll_offset_handler, &roll_offset_current); + param_set(pitch_offset_handler, &pitch_offset_current); + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "level"); + return 1; + } +} diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 6b7e16d449..05c02e294d 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -45,5 +45,6 @@ #include int do_accel_calibration(int mavlink_fd); +int do_level_calibration(int mavlink_fd); #endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h index 7b4baae623..53775ffe4f 100644 --- a/src/modules/commander/calibration_messages.h +++ b/src/modules/commander/calibration_messages.h @@ -52,6 +52,7 @@ #define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s" #define CAL_QGC_DONE_MSG "[cal] calibration done: %s" #define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s" +#define CAL_QGC_WARNING_MSG "[cal] calibration warning: %s" #define CAL_QGC_CANCELLED_MSG "[cal] calibration cancelled" #define CAL_QGC_PROGRESS_MSG "[cal] progress <%u>" #define CAL_QGC_ORIENTATION_DETECTED_MSG "[cal] %s orientation detected" diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 9676ad53e2..f752a37b98 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -125,6 +125,8 @@ static const int ERROR = -1; extern struct system_load_s system_load; +static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage signal to noise ratio allowed for GPS reception */ + /* Decouple update interval and hysteris counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 50000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) @@ -315,6 +317,10 @@ int commander_main(int argc, char *argv[]) calib_ret = do_accel_calibration(mavlink_fd); } else if (!strcmp(argv[2], "gyro")) { calib_ret = do_gyro_calibration(mavlink_fd); + } else if (!strcmp(argv[2], "level")) { + calib_ret = do_level_calibration(mavlink_fd); + } else if (!strcmp(argv[2], "esc")) { + calib_ret = do_esc_calibration(mavlink_fd, &armed); } else { warnx("argument %s unsupported.", argv[2]); } @@ -916,6 +922,7 @@ int commander_thread_main(int argc, char *argv[]) status.circuit_breaker_engaged_airspd_check = false; status.circuit_breaker_engaged_enginefailure_check = false; status.circuit_breaker_engaged_gpsfailure_check = false; + get_circuit_breaker_params(); /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -1117,8 +1124,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_sys_type, &(status.system_type)); // get system type status.is_rotary_wing = is_rotary_wing(&status) || is_vtol(&status); - get_circuit_breaker_params(); - bool checkAirspeed = false; /* Perform airspeed check only if circuit breaker is not * engaged and it's not a rotary wing */ @@ -1127,7 +1132,7 @@ int commander_thread_main(int argc, char *argv[]) } // Run preflight check - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); if (!status.condition_system_sensors_initialized) { set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune } @@ -1300,7 +1305,7 @@ int commander_thread_main(int argc, char *argv[]) } /* provide RC and sensor status feedback to the user */ - (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true); + (void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; @@ -1601,7 +1606,7 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { + if (!status.calibration_enabled && status.arming_state == vehicle_status_s::ARMING_STATE_INIT) { arming_ret = arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); @@ -1637,19 +1642,31 @@ int commander_thread_main(int argc, char *argv[]) (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } - /* check if GPS fix is ok */ - if (status.circuit_breaker_engaged_gpsfailure_check || - (gps_position.fix_type >= 3 && - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { - /* handle the case where gps was regained */ - if (status.gps_failure) { - status.gps_failure = false; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); + /* check if GPS is ok */ + if (!status.circuit_breaker_engaged_gpsfailure_check) { + bool gpsIsNoisy = gps_position.noise_per_ms > 0 && gps_position.noise_per_ms < COMMANDER_MAX_GPS_NOISE; + + //Check if GPS receiver is too noisy while we are disarmed + if (!armed.armed && gpsIsNoisy) { + if (!status.gps_failure) { + mavlink_log_critical(mavlink_fd, "GPS signal noisy"); + set_tune_override(TONE_GPS_WARNING_TUNE); + + //GPS suffers from signal jamming or excessive noise, disable GPS-aided flight + status.gps_failure = true; + status_changed = true; + } } - } else { - if (!status.gps_failure) { + if (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure && !gpsIsNoisy) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + + } else if (!status.gps_failure) { status.gps_failure = true; status_changed = true; mavlink_log_critical(mavlink_fd, "gps fix lost"); @@ -2674,6 +2691,8 @@ void *commander_low_prio_loop(void *arg) false /* fRunPreArmChecks */, mavlink_fd)) { answer_command(cmd, VEHICLE_CMD_RESULT_DENIED); break; + } else { + status.calibration_enabled = true; } if ((int)(cmd.param1) == 1) { @@ -2707,7 +2726,10 @@ void *commander_low_prio_loop(void *arg) /* accelerometer calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_accel_calibration(mavlink_fd); - + } else if ((int)(cmd.param5) == 2) { + // board offset calibration + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_level_calibration(mavlink_fd); } else if ((int)(cmd.param6) == 1) { /* airspeed calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); @@ -2715,14 +2737,9 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param7) == 1) { /* do esc calibration */ - calib_ret = check_if_batt_disconnected(mavlink_fd); - if(calib_ret == OK) { - answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED); - armed.in_esc_calibration_mode = true; - calib_ret = do_esc_calibration(mavlink_fd); - armed.in_esc_calibration_mode = false; - } - + answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_esc_calibration(mavlink_fd, &armed); + } else if ((int)(cmd.param4) == 0) { /* RC calibration ended - have we been in one worth confirming? */ if (status.rc_input_blocked) { @@ -2730,12 +2747,14 @@ void *commander_low_prio_loop(void *arg) /* enable RC control input */ status.rc_input_blocked = false; mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); - calib_ret = OK; + calib_ret = OK; } /* this always succeeds */ calib_ret = OK; } + status.calibration_enabled = false; + if (calib_ret == OK) { tune_positive(true); @@ -2751,9 +2770,9 @@ void *commander_low_prio_loop(void *arg) checkAirspeed = true; } - status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true); + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status.circuit_breaker_engaged_gpsfailure_check); - arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd); + arming_state_transition(&status, &safety, vehicle_status_s::ARMING_STATE_STANDBY, &armed, false /* fRunPreArmChecks */, mavlink_fd); } else { tune_negative(true); diff --git a/src/modules/commander/esc_calibration.cpp b/src/modules/commander/esc_calibration.cpp index 541aca0531..22f1d4f99f 100644 --- a/src/modules/commander/esc_calibration.cpp +++ b/src/modules/commander/esc_calibration.cpp @@ -40,6 +40,8 @@ */ #include "esc_calibration.h" +#include "calibration_messages.h" + #include #include #include @@ -62,91 +64,122 @@ #endif static const int ERROR = -1; -int check_if_batt_disconnected(int mavlink_fd) { - struct battery_status_s battery; - memset(&battery,0,sizeof(battery)); - int batt_sub = orb_subscribe(ORB_ID(battery_status)); - orb_copy(ORB_ID(battery_status), batt_sub, &battery); - - if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) { - mavlink_log_info(mavlink_fd, "Please disconnect battery and try again!"); - return ERROR; +int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed) +{ + int return_code = OK; + + int fd = -1; + + struct battery_status_s battery; + int batt_sub = -1; + bool batt_updated = false; + bool batt_connected = false; + + hrt_abstime battery_connect_wait_timeout = 20000000; + hrt_abstime pwm_high_timeout = 5000000; + hrt_abstime timeout_start; + + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "esc"); + + batt_sub = orb_subscribe(ORB_ID(battery_status)); + if (batt_sub < 0) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Subscribe to battery"); + goto Error; } - return OK; -} + // Make sure battery is disconnected + orb_copy(ORB_ID(battery_status), batt_sub, &battery); + if (battery.voltage_filtered_v > 3.0f) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Disconnect battery and try again"); + goto Error; + } + + armed->in_esc_calibration_mode = true; + + fd = open(PWM_OUTPUT0_DEVICE_PATH, 0); -int do_esc_calibration(int mavlink_fd) { - - int fd = open(PWM_OUTPUT0_DEVICE_PATH, 0); - int ret; - - if(fd < 0) { - err(1,"Can't open %s", PWM_OUTPUT0_DEVICE_PATH); + if (fd < 0) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Can't open PWM device"); + goto Error; } /* tell IO/FMU that its ok to disable its safety with the switch */ - ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); - if (ret != OK) - err(1, "PWM_SERVO_SET_ARM_OK"); + if (ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to disable safety switch"); + goto Error; + } + /* tell IO/FMU that the system is armed (it will output values if safety is off) */ - ret = ioctl(fd, PWM_SERVO_ARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_ARM"); + if (ioctl(fd, PWM_SERVO_ARM, 0) != OK) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to arm system"); + goto Error; + } + /* tell IO to switch off safety without using the safety switch */ - ret = ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0); - if(ret!=0) { - err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF"); + if (ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to force safety off"); + goto Error; } - mavlink_log_info(mavlink_fd,"Please connect battery now"); + mavlink_and_console_log_info(mavlink_fd, "[cal] Connect battery now"); + + timeout_start = hrt_absolute_time(); - struct battery_status_s battery; - memset(&battery,0,sizeof(battery)); - int batt_sub = orb_subscribe(ORB_ID(battery_status)); - orb_copy(ORB_ID(vehicle_command),batt_sub, &battery); - bool updated = false; - - struct vehicle_command_s cmd; - memset(&cmd, 0, sizeof(cmd)); - int cmd_sub = orb_subscribe(ORB_ID(vehicle_command)); - orb_copy(ORB_ID(vehicle_command),cmd_sub, &cmd); - - /* wait for one of the following events: - 1) user has pressed the button in QGroundControl - 2) timeout of 5 seconds is reached - */ - hrt_abstime start_time = hrt_absolute_time(); - - while(true) { - orb_check(batt_sub,&updated); - if(updated) { - orb_copy(ORB_ID(battery_status), batt_sub, &battery); - } - // user has connected battery - if(battery.voltage_filtered_v > 3.0f) { - orb_check(cmd_sub,&updated); - if(updated) { - orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); - } - if((int)(cmd.param7) == 2 && cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION) { - break; - } else if (hrt_absolute_time() - start_time > 5000000) { - // waited for 5 seconds, switch to low pwm - break; + while (true) { + // We are either waiting for the user to connect the battery. Or we are waiting to let the PWM + // sit high. + hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout; + + if (hrt_absolute_time() - timeout_start > timeout_wait) { + if (!batt_connected) { + mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Timeout waiting for battery"); + goto Error; } + + // PWM was high long enough + break; } - else { - start_time = hrt_absolute_time(); + + if (!batt_connected) { + orb_check(batt_sub, &batt_updated); + if (batt_updated) { + orb_copy(ORB_ID(battery_status), batt_sub, &battery); + if (battery.voltage_filtered_v > 3.0f) { + // Battery is connected, signal to user and start waiting again + batt_connected = true; + timeout_start = hrt_absolute_time(); + mavlink_and_console_log_info(mavlink_fd, "[cal] Battery connected"); + } + } } usleep(50000); } - /* disarm */ - ret = ioctl(fd, PWM_SERVO_DISARM, 0); - if (ret != OK) - err(1, "PWM_SERVO_DISARM"); - - mavlink_log_info(mavlink_fd,"ESC calibration finished"); - return OK; - } \ No newline at end of file +Out: + if (batt_sub != -1) { + orb_unsubscribe(batt_sub); + } + if (fd != -1) { + if (ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still off"); + } + if (ioctl(fd, PWM_SERVO_DISARM, 0) != OK) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Servos still armed"); + } + if (ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still deactivated"); + } + close(fd); + } + armed->in_esc_calibration_mode = false; + + if (return_code == OK) { + mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "esc"); + } + + return return_code; + +Error: + return_code = ERROR; + goto Out; +} diff --git a/src/modules/commander/esc_calibration.h b/src/modules/commander/esc_calibration.h index 5539955ebc..ac37b278c6 100644 --- a/src/modules/commander/esc_calibration.h +++ b/src/modules/commander/esc_calibration.h @@ -41,7 +41,9 @@ #ifndef ESC_CALIBRATION_H_ #define ESC_CALIBRATION_H_ -int check_if_batt_disconnected(int mavlink_fd); -int do_esc_calibration(int mavlink_fd); + +#include + +int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed); #endif \ No newline at end of file diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 73fdb09406..b53eeebeb6 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -216,23 +216,13 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s valid_transition = true; } - // Sensors need to be initialized for STANDBY state, except for HIL - if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && - (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && - (!status->condition_system_sensors_initialized)) { - mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); - feedback_provided = true; - valid_transition = false; - status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; - } - // Check if we are trying to arm, checks look good but we are in STANDBY_ERROR if (status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) { if (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) { if (status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_fd, "Preflight check now OK, power cycle before arming"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot before arming"); } else { mavlink_and_console_log_critical(mavlink_fd, "Preflight check failed, refusing to arm"); } @@ -240,11 +230,24 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } else if ((new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && status->condition_system_sensors_initialized) { - mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, power cycle to complete"); + mavlink_and_console_log_critical(mavlink_fd, "Preflight check resolved, reboot to complete"); feedback_provided = true; } else { - + // Silent ignore + feedback_provided = true; } + + // Sensors need to be initialized for STANDBY state, except for HIL + } else if ((status->hil_state != vehicle_status_s::HIL_STATE_ON) && + (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) && + (status->arming_state != vehicle_status_s::ARMING_STATE_STANDBY_ERROR) && + (!status->condition_system_sensors_initialized)) { + if (!fRunPreArmChecks) { + mavlink_and_console_log_critical(mavlink_fd, "Not ready to fly: Sensors need inspection"); + } + feedback_provided = true; + valid_transition = false; + status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR; } // Finish up the state transition @@ -689,5 +692,5 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) checkAirspeed = true; } - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, true); + return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, true, !status->circuit_breaker_engaged_gpsfailure_check, true); } diff --git a/src/modules/controllib/block/Block.cpp b/src/modules/controllib/block/Block.cpp index f3cd877287..083ab9498e 100644 --- a/src/modules/controllib/block/Block.cpp +++ b/src/modules/controllib/block/Block.cpp @@ -208,4 +208,9 @@ void SuperBlock::updateChildPublications() } } + } // namespace control + +template class List; +template class List; +template class List; diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp index d2f9cdf07e..d169d35c5f 100644 --- a/src/modules/controllib/block/Block.hpp +++ b/src/modules/controllib/block/Block.hpp @@ -43,12 +43,9 @@ #include #include - -// forward declaration -namespace uORB { - class SubscriptionNode; - class PublicationNode; -} +#include +#include +#include namespace control { @@ -60,8 +57,8 @@ static const uint16_t maxPublicationsPerBlock = 100; static const uint8_t blockNameLengthMax = 80; // forward declaration -class BlockParamBase; class SuperBlock; +class BlockParamBase; /** */ @@ -137,4 +134,5 @@ protected: List _children; }; + } // namespace control diff --git a/src/modules/controllib/block/BlockParam.cpp b/src/modules/controllib/block/BlockParam.cpp index 8f98da74fa..532a037d40 100644 --- a/src/modules/controllib/block/BlockParam.cpp +++ b/src/modules/controllib/block/BlockParam.cpp @@ -43,6 +43,8 @@ #include "BlockParam.hpp" +#include + namespace control { diff --git a/src/modules/controllib/block/BlockParam.hpp b/src/modules/controllib/block/BlockParam.hpp index 437e43bfb4..cab28c65fd 100644 --- a/src/modules/controllib/block/BlockParam.hpp +++ b/src/modules/controllib/block/BlockParam.hpp @@ -47,6 +47,8 @@ namespace control { +class Block; + /** * A base class for block params that enables traversing linked list. */ diff --git a/src/modules/controllib/uorb/blocks.cpp b/src/modules/controllib/uorb/blocks.cpp index 454d0db19c..7c520219f1 100644 --- a/src/modules/controllib/uorb/blocks.cpp +++ b/src/modules/controllib/uorb/blocks.cpp @@ -38,6 +38,7 @@ */ #include "blocks.hpp" +#include namespace control { diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp index 491d4681de..e3c0811116 100644 --- a/src/modules/controllib/uorb/blocks.hpp +++ b/src/modules/controllib/uorb/blocks.hpp @@ -61,10 +61,6 @@ #include #include -extern "C" { -#include -} - #include "../blocks.hpp" #include #include diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c index 6b248cbe2e..d9ccd8bac8 100644 --- a/src/modules/fw_att_control/fw_att_control_params.c +++ b/src/modules/fw_att_control/fw_att_control_params.c @@ -1,7 +1,6 @@ /**************************************************************************** * -f * Copyright (c) 2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -37,8 +36,8 @@ f * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Parameters defined by the fixed-wing attitude control task * - * @author Lorenz Meier - * @author Thomas Gubler + * @author Lorenz Meier + * @author Thomas Gubler */ #include @@ -73,6 +72,8 @@ PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f); * This defines how much the elevator input will be commanded depending on the * current body angular rate error. * + * @min 0.005 + * @max 1.0 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f); @@ -144,6 +145,8 @@ PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); * This defines how much the aileron input will be commanded depending on the * current body angular rate error. * + * @min 0.005 + * @max 1.0 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f); @@ -190,6 +193,8 @@ PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f); * This defines how much the rudder input will be commanded depending on the * current body angular rate error. * + * @min 0.005 + * @max 1.0 * @group FW Attitude Control */ PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f); @@ -234,7 +239,9 @@ PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f); /** * Roll rate feed forward * - * Direct feed forward from rate setpoint to control surface output + * Direct feed forward from rate setpoint to control surface output. Use this + * to obtain a tigher response of the controller without introducing + * noise amplification. * * @min 0.0 * @max 10.0 diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index a6c92ac623..233bc2d323 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -381,8 +381,7 @@ MavlinkFTP::_workList(PayloadHeader* payload) } break; case DTYPE_DIRECTORY: - // XXX @DonLakeFlyer: Remove the first condition for the test setup - if ((entry.d_name[0] == '.') || strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back direntType = kDirentSkip; } else { diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index e5cddfafb6..be63ae861c 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -66,6 +66,8 @@ #include #include #include +#include +#include #include #include #include @@ -912,6 +914,47 @@ Mavlink::send_statustext(unsigned char severity, const char *string) mavlink_logbuffer_write(&_logbuffer, &logmsg); } +void Mavlink::send_autopilot_capabilites() { + struct vehicle_status_s status; + + MavlinkOrbSubscription *status_sub = this->add_orb_subscription(ORB_ID(vehicle_status)); + + if (status_sub->update(&status)) { + mavlink_autopilot_version_t msg = {}; + + msg.capabilities = MAV_PROTOCOL_CAPABILITY_MISSION_FLOAT; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_PARAM_FLOAT; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_COMMAND_INT; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_FTP; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ATTITUDE_TARGET; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_POSITION_TARGET_LOCAL_NED; + msg.capabilities |= MAV_PROTOCOL_CAPABILITY_SET_ACTUATOR_TARGET; + msg.flight_sw_version = 0; + msg.middleware_sw_version = 0; + msg.os_sw_version = 0; + msg.board_version = 0; + memcpy(&msg.flight_custom_version, &px4_git_version_binary, sizeof(msg.flight_custom_version)); + memcpy(&msg.middleware_custom_version, &px4_git_version_binary, sizeof(msg.middleware_custom_version)); + memset(&msg.os_custom_version, 0, sizeof(msg.os_custom_version)); + #ifdef CONFIG_CDCACM_VENDORID + msg.vendor_id = CONFIG_CDCACM_VENDORID; + #else + msg.vendor_id = 0; + #endif + #ifdef CONFIG_CDCACM_PRODUCTID + msg.product_id = CONFIG_CDCACM_PRODUCTID; + #else + msg.product_id = 0; + #endif + uint32_t uid[3]; + mcu_unique_id(uid); + msg.uid = (((uint64_t)uid[1]) << 32) | uid[2]; + + this->send_message(MAVLINK_MSG_ID_AUTOPILOT_VERSION, &msg); + } +} + MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance) { /* check if already subscribed to this topic */ @@ -1406,6 +1449,9 @@ Mavlink::task_main(int argc, char *argv[]) case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); + configure_stream("HIGHRES_IMU", 50.0f); + configure_stream("VFR_HUD", 5.0f); + configure_stream("GPS_RAW_INT", 5.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("LOCAL_POSITION_NED", 30.0f); configure_stream("CAMERA_CAPTURE", 2.0f); @@ -1414,6 +1460,7 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); configure_stream("DISTANCE_SENSOR", 10.0f); configure_stream("OPTICAL_FLOW_RAD", 10.0f); + configure_stream("RC_CHANNELS_RAW", 20.0f); configure_stream("VFR_HUD", 10.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("TIMESYNC", 10.0f); @@ -1430,6 +1477,8 @@ Mavlink::task_main(int argc, char *argv[]) /* now the instance is fully initialized and we can bump the instance count */ LL_APPEND(_mavlink_instances, this); + send_autopilot_capabilites(); + while (!_task_should_exit) { /* main loop */ usleep(_main_loop_delay); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 90b84061f6..b732355b44 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -229,6 +229,7 @@ public: * @param severity the log level */ void send_statustext(unsigned char severity, const char *string); + void send_autopilot_capabilites(); MavlinkStream * get_streams() const { return _streams; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 3883cb84a9..1753aaec4e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -59,7 +59,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 40adc6b054..29b4fbcabb 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -278,7 +278,9 @@ MavlinkReceiver::handle_message_command_long(mavlink_message_t *msg) /* terminate other threads and this thread */ _mavlink->_task_should_exit = true; - + } else if (cmd_mavlink.command == MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES) { + /* send autopilot version message */ + _mavlink->send_autopilot_capabilites(); } else { if (msg->sysid == mavlink_system.sysid && msg->compid == mavlink_system.compid) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 887d2f88ed..fe217f3c3b 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -64,7 +64,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index ec512ab56f..34bfa46140 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 @@ -40,7 +40,7 @@ * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. * * @author Tobias Naegeli - * @author Lorenz Meier + * @author Lorenz Meier * @author Anton Babushkin * * The controller has two loops: P loop for angular error and PD loop for angular rate error. diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 3f19a51f06..c9bf3753b4 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 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 diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c1763ba93d..a68f4de012 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -207,6 +207,8 @@ Mission::update_onboard_mission() void Mission::update_offboard_mission() { + bool failed = true; + if (orb_copy(ORB_ID(offboard_mission), _navigator->get_offboard_mission_sub(), &_offboard_mission) == OK) { warnx("offboard mission updated: dataman_id=%d, count=%d, current_seq=%d", _offboard_mission.dataman_id, _offboard_mission.count, _offboard_mission.current_seq); /* determine current index */ @@ -228,12 +230,15 @@ Mission::update_offboard_mission() * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id); - _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, + failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt); } else { warnx("offboard mission update failed"); + } + + if (failed) { _offboard_mission.count = 0; _offboard_mission.current_seq = 0; _current_offboard_mission_index = 0; diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 949231b159..8f1d6f8e85 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -65,6 +65,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { + bool failed = false; /* Init if not done yet */ init(); @@ -74,11 +75,16 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } + // check if all mission item commands are supported + failed |= !checkMissionItemValidity(dm_current, nMissionItems); + if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); + + return !failed; } bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) @@ -163,6 +169,38 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, return true; } +bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) { + // do not allow mission if we find unsupported item + for (size_t i = 0; i < nMissionItems; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + // not supposed to happen unless the datamanager can't access the SD card, etc. + mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Cannot access mission from SD card"); + return false; + } + + // check if we find unsupported item and reject mission if so + if (missionitem.nav_cmd != NAV_CMD_IDLE && + missionitem.nav_cmd != NAV_CMD_WAYPOINT && + missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && + missionitem.nav_cmd != NAV_CMD_LOITER_TURN_COUNT && + missionitem.nav_cmd != NAV_CMD_LOITER_TIME_LIMIT && + missionitem.nav_cmd != NAV_CMD_LAND && + missionitem.nav_cmd != NAV_CMD_TAKEOFF && + missionitem.nav_cmd != NAV_CMD_ROI && + missionitem.nav_cmd != NAV_CMD_PATHPLANNING && + missionitem.nav_cmd != NAV_CMD_DO_JUMP) { + + mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); + return false; + } + } + mavlink_log_critical(_mavlink_fd, "Mission is valid!"); + return true; +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 9a77a6dc25..94b6b29226 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -62,6 +62,7 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); + bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); /* Checks specific to fixedwing airframes */ bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 6310cf6de4..b1bd4e540e 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -110,4 +110,4 @@ PARAM_DEFINE_INT32(MIS_ALTMODE, 0); * @max 3 * @group Mission */ -PARAM_DEFINE_INT32(MIS_YAWMODE, 0); +PARAM_DEFINE_INT32(MIS_YAWMODE, 1); diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index 6964acf339..08dddf219b 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -47,5 +47,3 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os EXTRACFLAGS = -Wframe-larger-than=1400 - -EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"' diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 37234ca5d3..b88400c276 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -104,6 +104,7 @@ #include #include #include +#include #include #include @@ -795,7 +796,7 @@ int write_version(int fd) }; /* fill version message and write it */ - strncpy(log_msg_VER.body.fw_git, GIT_VERSION, sizeof(log_msg_VER.body.fw_git)); + strncpy(log_msg_VER.body.fw_git, px4_git_version, sizeof(log_msg_VER.body.fw_git)); strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch)); return write(fd, &log_msg_VER, sizeof(log_msg_VER)); } diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 153e704809..203564ec97 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -129,7 +129,7 @@ #endif #define BATT_V_LOWPASS 0.001f -#define BATT_V_IGNORE_THRESHOLD 4.8f +#define BATT_V_IGNORE_THRESHOLD 2.5f /** * HACK - true temperature is much less than indicated temperature in baro, @@ -856,7 +856,7 @@ Sensors::parameters_update() M_DEG_TO_RAD_F * _parameters.board_offset[1], M_DEG_TO_RAD_F * _parameters.board_offset[2]); - _board_rotation = _board_rotation * board_rotation_offset; + _board_rotation = board_rotation_offset * _board_rotation; /* update barometer qnh setting */ param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh)); @@ -2066,35 +2066,25 @@ Sensors::task_main() { /* start individual sensors */ - int ret; - ret = accel_init(); + int ret = 0; + do { /* create a scope to handle exit with break */ + ret = accel_init(); + if (ret) break; + ret = gyro_init(); + if (ret) break; + ret = mag_init(); + if (ret) break; + ret = baro_init(); + if (ret) break; + ret = adc_init(); + if (ret) break; + break; + } while (0); if (ret) { - goto exit_immediate; - } - - ret = gyro_init(); - - if (ret) { - goto exit_immediate; - } - - ret = mag_init(); - - if (ret) { - goto exit_immediate; - } - - ret = baro_init(); - - if (ret) { - goto exit_immediate; - } - - ret = adc_init(); - - if (ret) { - goto exit_immediate; + _sensors_task = -1; + _exit(ret); + return; } /* @@ -2237,8 +2227,6 @@ Sensors::task_main() } warnx("exiting."); - -exit_immediate: _sensors_task = -1; _exit(ret); } diff --git a/src/modules/systemlib/circuit_breaker_params.c b/src/modules/systemlib/circuit_breaker_params.c index e5cc034bc9..097903d21f 100644 --- a/src/modules/systemlib/circuit_breaker_params.c +++ b/src/modules/systemlib/circuit_breaker_params.c @@ -120,3 +120,19 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @group Circuit Breaker */ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); + +/** + * Circuit breaker for GPS failure detection + * + * Setting this parameter to 240024 will disable the GPS failure detection. + * If this check is enabled, then the sensor check will fail if the GPS module + * is missing. It will also check for excessive signal noise on the GPS receiver + * and warn the user if detected. + * + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 240024 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); \ No newline at end of file diff --git a/src/modules/uORB/topics/filtered_bottom_flow.h b/src/modules/systemlib/git_version.h similarity index 64% rename from src/modules/uORB/topics/filtered_bottom_flow.h rename to src/modules/systemlib/git_version.h index c5d5455422..9b8f9e860e 100644 --- a/src/modules/uORB/topics/filtered_bottom_flow.h +++ b/src/modules/systemlib/git_version.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Samuel Zihlmann - * Lorenz Meier + * Copyright (c) 2015 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 @@ -34,40 +32,18 @@ ****************************************************************************/ /** - * @file filtered_bottom_flow.h - * Definition of the filtered bottom flow uORB topic. + * @file git_version.h + * + * GIT repository version */ -#ifndef TOPIC_FILTERED_BOTTOM_FLOW_H_ -#define TOPIC_FILTERED_BOTTOM_FLOW_H_ +#pragma once #include -#include -#include "../uORB.h" -/** - * @addtogroup topics - * @{ - */ +__BEGIN_DECLS -/** - * Filtered bottom flow in bodyframe. - */ -struct filtered_bottom_flow_s { - uint64_t timestamp; /**< time of this estimate, in microseconds since system start */ +__EXPORT extern const char* px4_git_version; +__EXPORT extern const uint64_t px4_git_version_binary; - float sumx; /**< Integrated bodyframe x flow in meters */ - float sumy; /**< Integrated bodyframe y flow in meters */ - - float vx; /**< Flow bodyframe x speed, m/s */ - float vy; /**< Flow bodyframe y Speed, m/s */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(filtered_bottom_flow); - -#endif +__END_DECLS diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index f2499bbb13..ff409fa473 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -55,7 +55,8 @@ SRCS = err.c \ pwm_limit/pwm_limit.c \ circuit_breaker.cpp \ circuit_breaker_params.c \ - mcu_version.c + mcu_version.c \ + $(BUILD_DIR)git_version.c MAXOPTIMIZATION = -Os diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index eb2d84726f..0ea8e5db51 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -69,6 +69,14 @@ void * Publication::getDataVoidPtr() { return (void *)(T *)(this); } + +PublicationNode::PublicationNode(const struct orb_metadata *meta, + List * list) : + PublicationBase(meta) { + if (list != nullptr) list->add(this); +} + + template class __EXPORT Publication; template class __EXPORT Publication; template class __EXPORT Publication; diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8cbe51119e..9b4159cdfd 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -94,6 +94,11 @@ protected: // attributes const struct orb_metadata *_meta; orb_advert_t _handle; +private: + // forbid copy + PublicationBase(const PublicationBase&) : _meta(), _handle() {}; + // forbid assignment + PublicationBase& operator = (const PublicationBase &); }; /** @@ -120,10 +125,7 @@ public: * that this should be appended to. */ PublicationNode(const struct orb_metadata *meta, - List * list=nullptr) : - PublicationBase(meta) { - if (list != nullptr) list->add(this); - } + List * list=nullptr); /** * This function is the callback for list traversal @@ -136,7 +138,7 @@ public: * Publication wrapper class */ template -class Publication : +class __EXPORT Publication : public T, // this must be first! public PublicationNode { diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index db47218d9d..0c9433f036 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -53,6 +53,8 @@ #include "topics/vehicle_attitude_setpoint.h" #include "topics/vehicle_rates_setpoint.h" #include "topics/rc_channels.h" +#include "topics/vehicle_control_mode.h" +#include "topics/actuator_armed.h" namespace uORB { diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 12301ce96f..31842ed715 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -106,6 +106,11 @@ protected: // attributes const struct orb_metadata *_meta; int _handle; +private: + // forbid copy + SubscriptionBase(const SubscriptionBase& other); + // forbid assignment + SubscriptionBase& operator = (const SubscriptionBase &); }; /** diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h deleted file mode 100644 index 54d84231f0..0000000000 --- a/src/modules/uORB/topics/actuator_controls_effective.h +++ /dev/null @@ -1,79 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 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 actuator_controls_effective.h - * - * Actuator control topics - mixer inputs. - * - * Values published to these topics are the outputs of the vehicle control - * system and mixing process; they are the control-scale values that are - * then fed to the actual actuator driver. - * - * Each topic can be published by a single controller - */ - -#ifndef TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H -#define TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H - -//#include -//#include "../uORB.h" -//#include "actuator_controls.h" -// -//#define NUM_ACTUATOR_CONTROLS_EFFECTIVE NUM_ACTUATOR_CONTROLS -//#define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE NUM_ACTUATOR_CONTROL_GROUPS /**< for sanity checking */ -// -///** -// * @addtogroup topics -// * @{ -// */ -// -//struct actuator_controls_effective_s { -// uint64_t timestamp; -// float control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE]; -//}; -// -///** -// * @} -// */ -// -///* actuator control sets; this list can be expanded as more controllers emerge */ -//ORB_DECLARE(actuator_controls_effective_0); -//ORB_DECLARE(actuator_controls_effective_1); -//ORB_DECLARE(actuator_controls_effective_2); -//ORB_DECLARE(actuator_controls_effective_3); -// -///* control sets with pre-defined applications */ -//#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE ORB_ID(actuator_controls_effective_0) - -#endif /* TOPIC_ACTUATOR_CONTROLS_EFFECTIVE_H */ diff --git a/src/modules/uORB/topics/home_position.h b/src/modules/uORB/topics/home_position.h deleted file mode 100644 index 02e17cdd7f..0000000000 --- a/src/modules/uORB/topics/home_position.h +++ /dev/null @@ -1,77 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012-2013 PX4 Development Team. All rights reserved. - * Author: Lorenz Meier - * Julian Oes - * - * 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 home_position.h - * Definition of the home position uORB topic. - * - * @author Lorenz Meier - * @author Julian Oes - */ - -#ifndef TOPIC_HOME_POSITION_H_ -#define TOPIC_HOME_POSITION_H_ - -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * GPS home position in WGS84 coordinates. - */ -struct home_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - - double lat; /**< Latitude in degrees */ - double lon; /**< Longitude in degrees */ - float alt; /**< Altitude in meters (AMSL) */ - - float x; /**< X coordinate in meters */ - float y; /**< Y coordinate in meters */ - float z; /**< Z coordinate in meters */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(home_position); - -#endif diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h deleted file mode 100644 index d3dc46ee0b..0000000000 --- a/src/modules/uORB/topics/optical_flow.h +++ /dev/null @@ -1,84 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Lorenz Meier - * - * 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 optical_flow.h - * Definition of the optical flow uORB topic. - */ - -#ifndef TOPIC_OPTICAL_FLOW_H_ -#define TOPIC_OPTICAL_FLOW_H_ - -#include -#include -#include "../uORB.h" - -/** - * @addtogroup topics - */ - -/** - * Optical flow in NED body frame in SI units. - * - * @see http://en.wikipedia.org/wiki/International_System_of_Units - */ -struct optical_flow_s { - - uint64_t timestamp; /**< in microseconds since system start */ - uint8_t sensor_id; /**< id of the sensor emitting the flow value */ - float pixel_flow_x_integral; /**< accumulated optical flow in radians around x axis */ - float pixel_flow_y_integral; /**< accumulated optical flow in radians around y axis */ - float gyro_x_rate_integral; /**< accumulated gyro value in radians around x axis */ - float gyro_y_rate_integral; /**< accumulated gyro value in radians around y axis */ - float gyro_z_rate_integral; /**< accumulated gyro value in radians around z axis */ - float ground_distance_m; /**< Altitude / distance to ground in meters */ - uint32_t integration_timespan; /** - * @author Julian Oes - * @author Lorenz Meier - */ - -#ifndef SENSOR_COMBINED_H_ -#define SENSOR_COMBINED_H_ - -#include -#include -#include "../uORB.h" - -enum MAGNETOMETER_MODE { - MAGNETOMETER_MODE_NORMAL = 0, - MAGNETOMETER_MODE_POSITIVE_BIAS, - MAGNETOMETER_MODE_NEGATIVE_BIAS -}; - -/** - * @addtogroup topics - * @{ - */ - -/** - * Sensor readings in raw and SI-unit form. - * - * These values are read from the sensors. Raw values are in sensor-specific units, - * the scaled values are in SI-units, as visible from the ending of the variable - * or the comments. The use of the SI fields is in general advised, as these fields - * are scaled and offset-compensated where possible and do not change with board - * revisions and sensor updates. - * - */ -struct sensor_combined_s { - - /* - * Actual data, this is specific to the type of data which is stored in this struct - * A line containing L0GME will be added by the Python logging code generator to the - * logged dataset. - */ - - /* NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only */ - - uint64_t timestamp; /**< Timestamp in microseconds since boot, from gyro */ - - int16_t gyro_raw[3]; /**< Raw sensor values of angular velocity */ - float gyro_rad_s[3]; /**< Angular velocity in radian per seconds */ - unsigned gyro_errcount; /**< Error counter for gyro 0 */ - float gyro_temp; /**< Temperature of gyro 0 */ - - int16_t accelerometer_raw[3]; /**< Raw acceleration in NED body frame */ - float accelerometer_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ - int accelerometer_mode; /**< Accelerometer measurement mode */ - float accelerometer_range_m_s2; /**< Accelerometer measurement range in m/s^2 */ - uint64_t accelerometer_timestamp; /**< Accelerometer timestamp */ - unsigned accelerometer_errcount; /**< Error counter for accel 0 */ - float accelerometer_temp; /**< Temperature of accel 0 */ - - int16_t magnetometer_raw[3]; /**< Raw magnetic field in NED body frame */ - float magnetometer_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ - int magnetometer_mode; /**< Magnetometer measurement mode */ - float magnetometer_range_ga; /**< ± measurement range in Gauss */ - float magnetometer_cuttoff_freq_hz; /**< Internal analog low pass frequency of sensor */ - uint64_t magnetometer_timestamp; /**< Magnetometer timestamp */ - unsigned magnetometer_errcount; /**< Error counter for mag 0 */ - float magnetometer_temp; /**< Temperature of mag 0 */ - - int16_t gyro1_raw[3]; /**< Raw sensor values of angular velocity */ - float gyro1_rad_s[3]; /**< Angular velocity in radian per seconds */ - uint64_t gyro1_timestamp; /**< Gyro timestamp */ - unsigned gyro1_errcount; /**< Error counter for gyro 1 */ - float gyro1_temp; /**< Temperature of gyro 1 */ - - int16_t accelerometer1_raw[3]; /**< Raw acceleration in NED body frame */ - float accelerometer1_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ - uint64_t accelerometer1_timestamp; /**< Accelerometer timestamp */ - unsigned accelerometer1_errcount; /**< Error counter for accel 1 */ - float accelerometer1_temp; /**< Temperature of accel 1 */ - - int16_t magnetometer1_raw[3]; /**< Raw magnetic field in NED body frame */ - float magnetometer1_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ - uint64_t magnetometer1_timestamp; /**< Magnetometer timestamp */ - unsigned magnetometer1_errcount; /**< Error counter for mag 1 */ - float magnetometer1_temp; /**< Temperature of mag 1 */ - - int16_t gyro2_raw[3]; /**< Raw sensor values of angular velocity */ - float gyro2_rad_s[3]; /**< Angular velocity in radian per seconds */ - uint64_t gyro2_timestamp; /**< Gyro timestamp */ - unsigned gyro2_errcount; /**< Error counter for gyro 1 */ - float gyro2_temp; /**< Temperature of gyro 1 */ - - int16_t accelerometer2_raw[3]; /**< Raw acceleration in NED body frame */ - float accelerometer2_m_s2[3]; /**< Acceleration in NED body frame, in m/s^2 */ - uint64_t accelerometer2_timestamp; /**< Accelerometer timestamp */ - unsigned accelerometer2_errcount; /**< Error counter for accel 2 */ - float accelerometer2_temp; /**< Temperature of accel 2 */ - - int16_t magnetometer2_raw[3]; /**< Raw magnetic field in NED body frame */ - float magnetometer2_ga[3]; /**< Magnetic field in NED body frame, in Gauss */ - uint64_t magnetometer2_timestamp; /**< Magnetometer timestamp */ - unsigned magnetometer2_errcount; /**< Error counter for mag 2 */ - float magnetometer2_temp; /**< Temperature of mag 2 */ - - float baro_pres_mbar; /**< Barometric pressure, already temp. comp. */ - float baro_alt_meter; /**< Altitude, already temp. comp. */ - float baro_temp_celcius; /**< Temperature in degrees celsius */ - uint64_t baro_timestamp; /**< Barometer timestamp */ - - float baro1_pres_mbar; /**< Barometric pressure, already temp. comp. */ - float baro1_alt_meter; /**< Altitude, already temp. comp. */ - float baro1_temp_celcius; /**< Temperature in degrees celsius */ - uint64_t baro1_timestamp; /**< Barometer timestamp */ - - float adc_voltage_v[10]; /**< ADC voltages of ADC Chan 10/11/12/13 or -1 */ - unsigned adc_mapping[10]; /**< Channel indices of each of these values */ - float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ - - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint64_t differential_pressure_timestamp; /**< Last measurement timestamp */ - float differential_pressure_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ - - float differential_pressure1_pa; /**< Airspeed sensor differential pressure */ - uint64_t differential_pressure1_timestamp; /**< Last measurement timestamp */ - float differential_pressure1_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ - -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(sensor_combined); - -#endif diff --git a/src/modules/uORB/topics/vehicle_global_position.h b/src/modules/uORB/topics/vehicle_global_position.h deleted file mode 100644 index 50c9429616..0000000000 --- a/src/modules/uORB/topics/vehicle_global_position.h +++ /dev/null @@ -1,87 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file vehicle_global_position.h - * Definition of the global fused WGS84 position uORB topic. - * - * @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - */ - -#ifndef VEHICLE_GLOBAL_POSITION_T_H_ -#define VEHICLE_GLOBAL_POSITION_T_H_ - -#include -#include -#include - -/** - * @addtogroup topics - * @{ - */ - - /** - * Fused global position in WGS84. - * - * This struct contains global position estimation. It is not the raw GPS - * measurement (@see vehicle_gps_position). This topic is usually published by the position - * estimator, which will take more sources of information into account than just GPS, - * e.g. control inputs of the vehicle in a Kalman-filter implementation. - */ -struct vehicle_global_position_s { - uint64_t timestamp; /**< Time of this estimate, in microseconds since system start */ - uint64_t time_utc_usec; /**< GPS UTC timestamp in microseconds */ - double lat; /**< Latitude in degrees */ - double lon; /**< Longitude in degrees */ - float alt; /**< Altitude AMSL in meters */ - float vel_n; /**< Ground north velocity, m/s */ - float vel_e; /**< Ground east velocity, m/s */ - float vel_d; /**< Ground downside velocity, m/s */ - float yaw; /**< Yaw in radians -PI..+PI. */ - float eph; /**< Standard deviation of position estimate horizontally */ - float epv; /**< Standard deviation of position vertically */ - float terrain_alt; /**< Terrain altitude in m, WGS84 */ - bool terrain_alt_valid; /**< Terrain altitude estimate is valid */ - bool dead_reckoning; /**< True if this position is estimated through dead-reckoning*/ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_global_position); - -#endif diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h deleted file mode 100644 index 102914bbbb..0000000000 --- a/src/modules/uORB/topics/vehicle_gps_position.h +++ /dev/null @@ -1,94 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Thomas Gubler - * @author Julian Oes - * @author Lorenz Meier - * - * 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 vehicle_gps_position.h - * Definition of the GPS WGS84 uORB topic. - */ - -#ifndef TOPIC_VEHICLE_GPS_H_ -#define TOPIC_VEHICLE_GPS_H_ - -#include -#include "../uORB.h" - -/** - * @addtogroup topics - * @{ - */ - -/** - * GPS position in WGS84 coordinates. - */ -struct vehicle_gps_position_s { - uint64_t timestamp_position; /**< Timestamp for position information */ - int32_t lat; /**< Latitude in 1E-7 degrees */ - int32_t lon; /**< Longitude in 1E-7 degrees */ - int32_t alt; /**< Altitude in 1E-3 meters (millimeters) above MSL */ - - uint64_t timestamp_variance; - float s_variance_m_s; /**< speed accuracy estimate m/s */ - float c_variance_rad; /**< course accuracy estimate rad */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - - float eph; /**< GPS HDOP horizontal dilution of position in m */ - float epv; /**< GPS VDOP horizontal dilution of position in m */ - - unsigned noise_per_ms; /**< */ - unsigned jamming_indicator; /**< */ - - uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ - float vel_m_s; /**< GPS ground speed (m/s) */ - float vel_n_m_s; /**< GPS ground speed in m/s */ - float vel_e_m_s; /**< GPS ground speed in m/s */ - float vel_d_m_s; /**< GPS ground speed in m/s */ - float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ - bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ - - uint64_t timestamp_time; /**< Timestamp for time information */ - uint64_t time_utc_usec; /**< Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0 */ - - uint8_t satellites_used; /**< Number of satellites used */ -}; - -/** - * @} - */ - -/* register this as object request broker structure */ -ORB_DECLARE(vehicle_gps_position); - -#endif diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 437feb3014..e3fd31a9a1 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -63,7 +63,7 @@ SRCS += $(subst $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_SRC)) INCLUDE_DIRS += $(LIBUAVCAN_INC) # Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode. -override EXTRADEFINES := $(EXTRADEFINES) -DGIT_VERSION='"$(GIT_DESC)"' -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS +override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS # diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp index f04ab9f17c..474739bd66 100644 --- a/src/modules/uavcan/uavcan_main.cpp +++ b/src/modules/uavcan/uavcan_main.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -212,7 +213,7 @@ void UavcanNode::fill_node_info() // Extracting the first 8 hex digits of GIT_VERSION and converting them to int char fw_git_short[9] = {}; - std::memmove(fw_git_short, GIT_VERSION, 8); + std::memmove(fw_git_short, px4_git_version, 8); assert(fw_git_short[8] == '\0'); char *end = nullptr; swver.vcs_commit = std::strtol(fw_git_short, &end, 16); diff --git a/src/systemcmds/ver/module.mk b/src/systemcmds/ver/module.mk index 4597b5f110..2eeb80b616 100644 --- a/src/systemcmds/ver/module.mk +++ b/src/systemcmds/ver/module.mk @@ -42,5 +42,3 @@ SRCS = ver.c MODULE_STACKSIZE = 1024 MAXOPTIMIZATION = -Os - -EXTRADEFINES = -DGIT_VERSION='"$(GIT_DESC)"' diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index b794e8b2fa..a248dfb79c 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -45,6 +45,7 @@ #include #include #include +#include /* string constants for version commands */ static const char sz_ver_hw_str[] = "hw"; @@ -101,7 +102,7 @@ int ver_main(int argc, char *argv[]) } if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { - printf("FW git-hash: %s\n", GIT_VERSION); + printf("FW git-hash: %s\n", px4_git_version); ret = 0; }