diff options
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mavlink/module.mk | 1 | ||||
-rw-r--r-- | src/modules/px4iofirmware/mixer.cpp | 3 | ||||
-rw-r--r-- | src/modules/px4iofirmware/protocol.h | 3 | ||||
-rw-r--r-- | src/modules/px4iofirmware/registers.c | 12 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 33 | ||||
-rw-r--r-- | src/modules/systemlib/mavlink_log.c (renamed from src/modules/mavlink/mavlink_log.c) | 21 | ||||
-rw-r--r-- | src/modules/systemlib/module.mk | 3 |
7 files changed, 46 insertions, 30 deletions
diff --git a/src/modules/mavlink/module.mk b/src/modules/mavlink/module.mk index bfccb2d38..5d3d6a73c 100644 --- a/src/modules/mavlink/module.mk +++ b/src/modules/mavlink/module.mk @@ -39,7 +39,6 @@ MODULE_COMMAND = mavlink SRCS += mavlink.c \ missionlib.c \ mavlink_parameters.c \ - mavlink_log.c \ mavlink_receiver.cpp \ orb_listener.c \ waypoints.c diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 38727c68c..deed25836 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -148,7 +148,8 @@ mixer_tick(void) if ( (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) && (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && - (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) && + !(r_setup_arming & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED)) { /* if allowed, mix from RC inputs directly */ source = MIX_OVERRIDE; diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 97809d9c2..f5fa0ba75 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -74,7 +74,7 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) -#define PX4IO_PROTOCOL_VERSION 3 +#define PX4IO_PROTOCOL_VERSION 4 /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 @@ -159,6 +159,7 @@ #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ +#define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED (1 << 6) /* Disable the IO-internal evaluation of the RC */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3ff9307cd..655a0c7a8 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -158,9 +158,10 @@ volatile uint16_t r_page_setup[] = #define PX4IO_P_SETUP_ARMING_VALID (PX4IO_P_SETUP_ARMING_FMU_ARMED | \ PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ - PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \ + PX4IO_P_SETUP_ARMING_IO_ARM_OK | \ PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ - PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \ + PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) #define PX4IO_P_SETUP_RATES_VALID ((1 << PX4IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -432,6 +433,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; // } + if (value & PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + } + r_setup_arming = value; break; @@ -523,6 +528,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) value &= PX4IO_P_RC_CONFIG_OPTIONS_VALID; r_status_flags |= PX4IO_P_STATUS_FLAGS_INIT_OK; + /* clear any existing RC disabled flag */ + r_setup_arming &= ~(PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED); + /* set all options except the enabled option */ conf[index] = value & ~PX4IO_P_RC_CONFIG_OPTIONS_ENABLED; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b237bd059..ded39c465 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -912,11 +912,11 @@ Sensors::gyro_init() #else - /* set the gyro internal sampling rate up to at leat 800Hz */ - ioctl(fd, GYROIOCSSAMPLERATE, 800); + /* set the gyro internal sampling rate up to at least 760Hz */ + ioctl(fd, GYROIOCSSAMPLERATE, 760); - /* set the driver to poll at 800Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 800); + /* set the driver to poll at 760Hz */ + ioctl(fd, SENSORIOCSPOLLRATE, 760); #endif @@ -929,6 +929,7 @@ void Sensors::mag_init() { int fd; + int ret; fd = open(MAG_DEVICE_PATH, 0); @@ -937,13 +938,27 @@ Sensors::mag_init() errx(1, "FATAL: no magnetometer found"); } - /* set the mag internal poll rate to at least 150Hz */ - ioctl(fd, MAGIOCSSAMPLERATE, 150); + /* try different mag sampling rates */ - /* set the driver to poll at 150Hz */ - ioctl(fd, SENSORIOCSPOLLRATE, 150); - int ret = ioctl(fd, MAGIOCGEXTERNAL, 0); + ret = ioctl(fd, MAGIOCSSAMPLERATE, 150); + if (ret == OK) { + /* set the pollrate accordingly */ + ioctl(fd, SENSORIOCSPOLLRATE, 150); + } else { + ret = ioctl(fd, MAGIOCSSAMPLERATE, 100); + /* if the slower sampling rate still fails, something is wrong */ + if (ret == OK) { + /* set the driver to poll also at the slower rate */ + ioctl(fd, SENSORIOCSPOLLRATE, 100); + } else { + errx(1, "FATAL: mag sampling rate could not be set"); + } + } + + + + ret = ioctl(fd, MAGIOCGEXTERNAL, 0); if (ret < 0) errx(1, "FATAL: unknown if magnetometer is external or onboard"); else if (ret == 1) diff --git a/src/modules/mavlink/mavlink_log.c b/src/modules/systemlib/mavlink_log.c index 192195856..27608bdbf 100644 --- a/src/modules/mavlink/mavlink_log.c +++ b/src/modules/systemlib/mavlink_log.c @@ -46,28 +46,25 @@ #include <mavlink/mavlink_log.h> -static FILE* text_recorder_fd = NULL; - -void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) +__EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size) { lb->size = size; lb->start = 0; lb->count = 0; lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage)); - text_recorder_fd = fopen("/fs/microsd/text_recorder.txt", "w"); } -int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) +__EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb) { return lb->count == (int)lb->size; } -int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) +__EXPORT int mavlink_logbuffer_is_empty(struct mavlink_logbuffer *lb) { return lb->count == 0; } -void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) +__EXPORT void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_logmessage *elem) { int end = (lb->start + lb->count) % lb->size; memcpy(&(lb->elems[end]), elem, sizeof(struct mavlink_logmessage)); @@ -80,19 +77,13 @@ void mavlink_logbuffer_write(struct mavlink_logbuffer *lb, const struct mavlink_ } } -int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) +__EXPORT int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem) { if (!mavlink_logbuffer_is_empty(lb)) { memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage)); lb->start = (lb->start + 1) % lb->size; --lb->count; - if (text_recorder_fd) { - fwrite(elem->text, 1, strnlen(elem->text, 50), text_recorder_fd); - fputc("\n", text_recorder_fd); - fsync(text_recorder_fd); - } - return 0; } else { @@ -100,7 +91,7 @@ int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessa } } -void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) +__EXPORT void mavlink_logbuffer_vasprintf(struct mavlink_logbuffer *lb, int severity, const char *fmt, ...) { va_list ap; va_start(ap, fmt); diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index b470c1227..cbf829122 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -48,4 +48,5 @@ SRCS = err.c \ geo/geo.c \ systemlib.c \ airspeed.c \ - system_params.c + system_params.c \ + mavlink_log.c |