From 1530aeccae706fe8024b4de2293060539c01ceaa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 16 Aug 2012 11:21:59 +0200 Subject: [PATCH 01/11] Working towards full sensor flexibility --- apps/drivers/mpu6000/mpu6000.cpp | 13 +- apps/sensors/Makefile | 2 +- apps/sensors/sensors.c | 347 ++++++++++++++++++++---------- nuttx/configs/px4fmu/src/up_nsh.c | 13 -- 4 files changed, 246 insertions(+), 129 deletions(-) diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 7576fc1d96..1123ddf3ab 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -288,9 +288,9 @@ MPU6000::MPU6000(int bus, spi_dev_e device) : _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), - _accel_range_scale(1.0f), + _accel_range_scale(0.02f), _accel_topic(-1), - _gyro_range_scale(1.0f), + _gyro_range_scale(0.02f), _gyro_topic(-1), _reads(0), _sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")) @@ -826,7 +826,7 @@ test() fd = open(ACCEL_DEVICE_PATH, O_RDONLY); if (fd < 0) { - reason = "can't open driver"; + reason = "can't open driver, use first"; break; } @@ -841,9 +841,10 @@ test() printf("single read\n"); fflush(stdout); printf("time: %lld\n", report.timestamp); - printf("x: %f\n", report.x); - printf("y: %f\n", report.y); - printf("z: %f\n", report.z); + printf("x: %5.2f\n", (double)report.x); + printf("y: %5.2f\n", (double)report.y); + printf("z: %5.2f\n", (double)report.z); + printf("test: %8.4f\n", 1.5); } while (0); diff --git a/apps/sensors/Makefile b/apps/sensors/Makefile index dadc2993d1..2692ade645 100644 --- a/apps/sensors/Makefile +++ b/apps/sensors/Makefile @@ -37,6 +37,6 @@ APPNAME = sensors PRIORITY = SCHED_PRIORITY_MAX-5 -STACKSIZE = 2048 +STACKSIZE = 3096 include $(APPDIR)/mk/app.mk diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index d847c4ffc1..6caeb06e5b 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -57,6 +57,7 @@ #include #include #include +#include #include #include @@ -105,11 +106,12 @@ static pthread_mutex_t sensors_read_ready_mutex; static int sensors_timer_loop_counter = 0; /* File descriptors for all sensors */ -static int fd_gyro = -1; -static int fd_accelerometer = -1; -static int fd_magnetometer = -1; -static int fd_barometer = -1; -static int fd_adc = -1; +static int fd_gyro = -1; +static int fd_bma180 = -1; +static int fd_magnetometer = -1; +static int fd_barometer = -1; +static int fd_adc = -1; +static int fd_accelerometer = -1; /* Private functions declared static */ static void sensors_timer_loop(void *arg); @@ -169,30 +171,70 @@ static int sensors_init(void) /* open gyro */ fd_gyro = open("/dev/l3gd20", O_RDONLY); + int errno_gyro = (int)*get_errno_ptr(); - if (fd_gyro < 0) { - fprintf(stderr, "[sensors] L3GD20 open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - fflush(stderr); - /* this sensor is critical, exit on failed init */ - errno = ENOSYS; - return ERROR; - - } else { + if (!(fd_gyro < 0)) { printf("[sensors] L3GD20 open ok\n"); } - /* open accelerometer */ - fd_accelerometer = open("/dev/bma180", O_RDONLY); + /* open accelerometer, prefer the MPU-6000 */ + fd_accelerometer = open("/dev/accel", O_RDONLY); + int errno_accelerometer = (int)*get_errno_ptr(); + if (!(fd_accelerometer < 0)) { + printf("[sensors] Accelerometer open ok\n"); + } + + + /* only attempt to use BMA180 if MPU-6000 is not available */ + int errno_bma180 = 0; if (fd_accelerometer < 0) { - fprintf(stderr, "[sensors] BMA180: open fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - fflush(stderr); - /* this sensor is critical, exit on failed init */ + fd_bma180 = open("/dev/bma180", O_RDONLY); + errno_bma180 = (int)*get_errno_ptr(); + + if (!(fd_bma180 < 0)) { + printf("[sensors] Accelerometer (BMA180) open ok\n"); + } + } else { + fd_bma180 = -1; + } + + /* fail if no accelerometer is available */ + if (fd_accelerometer < 0 && fd_bma180 < 0) { + /* print error message only if both failed, discard message else at all to not confuse users */ + if (fd_accelerometer < 0) { + fprintf(stderr, "[sensors] MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer)); + fflush(stderr); + /* this sensor is redundant with BMA180 */ + } + + if (fd_bma180 < 0) { + fprintf(stderr, "[sensors] BMA180: open fail (err #%d): %s\n", errno_bma180, strerror(errno_bma180)); + fflush(stderr); + /* this sensor is redundant with MPU-6000 */ + } + errno = ENOSYS; return ERROR; + } - } else { - printf("[sensors] BMA180 open ok\n"); + /* fail if no gyro is available */ + if (fd_accelerometer < 0 && fd_bma180 < 0) { + /* print error message only if both failed, discard message else at all to not confuse users */ + if (fd_accelerometer < 0) { + fprintf(stderr, "[sensors] MPU-6000: open fail (err #%d): %s\n", errno_accelerometer, strerror(errno_accelerometer)); + fflush(stderr); + /* this sensor is redundant with BMA180 */ + } + + if (fd_gyro < 0) { + fprintf(stderr, "[sensors] L3GD20 open fail (err #%d): %s\n", errno_gyro, strerror(errno_gyro)); + fflush(stderr); + /* this sensor is critical, exit on failed init */ + } + + errno = ENOSYS; + return ERROR; } /* open adc */ @@ -209,16 +251,18 @@ static int sensors_init(void) printf("[sensors] ADC open ok\n"); } - /* configure gyro */ - if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { - fprintf(stderr, "[sensors] L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); - fflush(stderr); - /* this sensor is critical, exit on failed init */ - errno = ENOSYS; - return ERROR; + /* configure gyro - if its not available and we got here the MPU-6000 is for sure available */ + if (fd_gyro > 0) { + if (ioctl(fd_gyro, L3GD20_SETRATE, L3GD20_RATE_760HZ_LP_30HZ) || ioctl(fd_gyro, L3GD20_SETRANGE, L3GD20_RANGE_500DPS)) { + fprintf(stderr, "[sensors] L3GD20 configuration (ioctl) fail (err #%d): %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + fflush(stderr); + /* this sensor is critical, exit on failed init */ + errno = ENOSYS; + return ERROR; - } else { - printf("[sensors] L3GD20 configuration ok\n"); + } else { + printf("[sensors] L3GD20 configuration ok\n"); + } } /* XXX Add IOCTL configuration of remaining sensors */ @@ -255,7 +299,7 @@ int sensors_main(int argc, char *argv[]) fprintf(stderr, "[sensors] ERROR: Failed to initialize all sensors\n"); /* Clean up */ close(fd_gyro); - close(fd_accelerometer); + close(fd_bma180); close(fd_magnetometer); close(fd_barometer); close(fd_adc); @@ -311,6 +355,7 @@ int sensors_main(int argc, char *argv[]) int16_t buf_gyro[3]; int16_t buf_accelerometer[3]; + struct accel_report buf_accel_report; int16_t buf_magnetometer[7]; float buf_barometer[3]; @@ -354,8 +399,9 @@ int sensors_main(int argc, char *argv[]) /* Empty sensor buffers, avoid junk values */ /* Read first two values of each sensor into void */ - (void)read(fd_gyro, buf_gyro, sizeof(buf_gyro)); - (void)read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer)); + if (fd_gyro > 0)(void)read(fd_gyro, buf_gyro, sizeof(buf_gyro)); + if (fd_bma180 > 0)(void)read(fd_bma180, buf_accelerometer, 6); + if (fd_accelerometer > 0)(void)read(fd_accelerometer, buf_accelerometer, 12); (void)read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); if (fd_barometer > 0)(void)read(fd_barometer, buf_barometer, sizeof(buf_barometer)); @@ -455,8 +501,12 @@ int sensors_main(int argc, char *argv[]) if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) { hil_enabled = true; publishing = false; - int ret = close(sensor_pub); - printf("[sensors] Closing sensor pub: %i \n", ret); + int sens_ret = close(sensor_pub); + if (sens_ret == OK) { + printf("[sensors] Closing sensor pub OK\n"); + } else { + printf("[sensors] FAILED Closing sensor pub, result: %i \n", sens_ret); + } /* switching from HIL to non-HIL mode */ @@ -508,79 +558,121 @@ int sensors_main(int argc, char *argv[]) paramcounter++; - /* try reading gyro */ - uint64_t start_gyro = hrt_absolute_time(); - ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro)); - int gyrotime = hrt_absolute_time() - start_gyro; + if (fd_gyro > 0) { + /* try reading gyro */ + uint64_t start_gyro = hrt_absolute_time(); + ret_gyro = read(fd_gyro, buf_gyro, sizeof(buf_gyro)); + int gyrotime = hrt_absolute_time() - start_gyro; - if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime); + if (gyrotime > 500) printf("GYRO (pure read): %d us\n", gyrotime); - /* GYROSCOPE */ - if (ret_gyro != sizeof(buf_gyro)) { - gyro_fail_count++; + /* GYROSCOPE */ + if (ret_gyro != sizeof(buf_gyro)) { + gyro_fail_count++; - if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { - fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + if ((((gyro_fail_count % 20) == 0) || (gyro_fail_count > 20 && gyro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { + fprintf(stderr, "[sensors] L3GD20 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } + + if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) { + // global_data_send_subsystem_info(&gyro_present_enabled); + gyro_healthy = false; + gyro_success_count = 0; + } + + } else { + gyro_success_count++; + + if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) { + // global_data_send_subsystem_info(&gyro_present_enabled_healthy); + gyro_healthy = true; + gyro_fail_count = 0; + + } + + gyro_updated = true; } - if (gyro_healthy && gyro_fail_count >= GYRO_HEALTH_COUNTER_LIMIT_ERROR) { - // global_data_send_subsystem_info(&gyro_present_enabled); - gyro_healthy = false; - gyro_success_count = 0; - } + gyrotime = hrt_absolute_time() - start_gyro; - } else { - gyro_success_count++; - - if (!gyro_healthy && gyro_success_count >= GYRO_HEALTH_COUNTER_LIMIT_OK) { - // global_data_send_subsystem_info(&gyro_present_enabled_healthy); - gyro_healthy = true; - gyro_fail_count = 0; - - } - - gyro_updated = true; + if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime); } - gyrotime = hrt_absolute_time() - start_gyro; + /* read MPU-6000 */ + if (fd_accelerometer > 0) { + /* try reading acc */ + uint64_t start_acc = hrt_absolute_time(); + ret_accelerometer = read(fd_accelerometer, &buf_accel_report, sizeof(struct accel_report)); - if (gyrotime > 500) printf("GYRO (complete): %d us\n", gyrotime); + /* ACCELEROMETER */ + if (ret_accelerometer != sizeof(struct accel_report)) { + acc_fail_count++; - /* try reading acc */ - uint64_t start_acc = hrt_absolute_time(); - ret_accelerometer = read(fd_accelerometer, buf_accelerometer, sizeof(buf_accelerometer)); + if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) { + fprintf(stderr, "[sensors] MPU-6000 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } - /* ACCELEROMETER */ - if (ret_accelerometer != sizeof(buf_accelerometer)) { - acc_fail_count++; + if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) { + // global_data_send_subsystem_info(&acc_present_enabled); + gyro_healthy = false; + acc_success_count = 0; + } - if (acc_fail_count & 0b1000 || (acc_fail_count > 20 && acc_fail_count < 100)) { - fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } else { + acc_success_count++; + + if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) { + + // global_data_send_subsystem_info(&acc_present_enabled_healthy); + acc_healthy = true; + acc_fail_count = 0; + + } + + acc_updated = true; } - if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) { - // global_data_send_subsystem_info(&acc_present_enabled); - gyro_healthy = false; - acc_success_count = 0; - } - - } else { - acc_success_count++; - - if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) { - - // global_data_send_subsystem_info(&acc_present_enabled_healthy); - acc_healthy = true; - acc_fail_count = 0; - - } - - acc_updated = true; + int acctime = hrt_absolute_time() - start_acc; + if (acctime > 500) printf("ACC: %d us\n", acctime); } - int acctime = hrt_absolute_time() - start_acc; + /* read BMA180. If the MPU-6000 is present, the BMA180 file descriptor won't be open */ + if (fd_bma180 > 0) { + /* try reading acc */ + uint64_t start_acc = hrt_absolute_time(); + ret_accelerometer = read(fd_bma180, buf_accelerometer, 6); - if (acctime > 500) printf("ACC: %d us\n", acctime); + /* ACCELEROMETER */ + if (ret_accelerometer != 6) { + acc_fail_count++; + + if ((acc_fail_count % 20) == 0 || (acc_fail_count > 20 && acc_fail_count < 100)) { + fprintf(stderr, "[sensors] BMA180 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); + } + + if (acc_healthy && acc_fail_count >= ACC_HEALTH_COUNTER_LIMIT_ERROR) { + // global_data_send_subsystem_info(&acc_present_enabled); + gyro_healthy = false; + acc_success_count = 0; + } + + } else { + acc_success_count++; + + if (!acc_healthy && acc_success_count >= ACC_HEALTH_COUNTER_LIMIT_OK) { + + // global_data_send_subsystem_info(&acc_present_enabled_healthy); + acc_healthy = true; + acc_fail_count = 0; + + } + + acc_updated = true; + } + + int acctime = hrt_absolute_time() - start_acc; + if (acctime > 500) printf("ACC: %d us\n", acctime); + } /* MAGNETOMETER */ if (magcounter == 4) { /* 120 Hz */ @@ -607,7 +699,7 @@ int sensors_main(int argc, char *argv[]) if (ret_magnetometer != sizeof(buf_magnetometer)) { mag_fail_count++; - if (mag_fail_count & 0b1000 || (mag_fail_count > 20 && mag_fail_count < 100)) { + if ((mag_fail_count % 20) == 0 || (mag_fail_count > 20 && mag_fail_count < 100)) { fprintf(stderr, "[sensors] HMC5883L ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } @@ -650,7 +742,7 @@ int sensors_main(int argc, char *argv[]) if (ret_barometer != sizeof(buf_barometer)) { baro_fail_count++; - if ((baro_fail_count & 0b1000 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { + if (((baro_fail_count % 20) == 0 || (baro_fail_count > 20 && baro_fail_count < 100)) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] MS5611 ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } @@ -687,10 +779,10 @@ int sensors_main(int argc, char *argv[]) ret_adc = read(fd_adc, &buf_adc, adc_readsize); nsamples_adc = ret_adc / sizeof(struct adc_msg_s); - if (ret_adc < 0 || nsamples_adc * sizeof(struct adc_msg_s) != ret_adc) { + if (ret_adc < 0 || ((int)(nsamples_adc * sizeof(struct adc_msg_s))) != ret_adc) { adc_fail_count++; - if ((adc_fail_count & 0b1000 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) { + if (((adc_fail_count % 20) == 0 || adc_fail_count < 10) && (int)*get_errno_ptr() != EAGAIN) { fprintf(stderr, "[sensors] ADC ERROR #%d: %s\n", (int)*get_errno_ptr(), strerror((int)*get_errno_ptr())); } @@ -730,7 +822,7 @@ int sensors_main(int argc, char *argv[]) */ if (ppm_decoded_channels > 1 && (hrt_absolute_time() - ppm_last_valid_decode) < 45000) { /* Read out values from HRT */ - for (int i = 0; i < ppm_decoded_channels; i++) { + for (unsigned int i = 0; i < ppm_decoded_channels; i++) { rc.chan[i].raw = ppm_buffer[i]; /* Set the range to +-, then scale up */ rc.chan[i].scale = (ppm_buffer[i] - rc.chan[i].mid) * rc.chan[i].scaling_factor * 10000; @@ -759,9 +851,9 @@ int sensors_main(int argc, char *argv[]) if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; /* throttle input */ - manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled; + manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled/2.0f; if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; - if (manual_control.throttle > 2.0f) manual_control.throttle = 2.0f; + if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; /* mode switch input */ manual_control.override_mode_switch = rc.chan[rc.function[OVERRIDE]].scaled; @@ -800,19 +892,56 @@ int sensors_main(int argc, char *argv[]) if (acc_updated) { /* copy sensor readings to global data and transform coordinates into px4fmu board frame */ - /* assign negated value, except for -SHORT_MAX, as it would wrap there */ - raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor - raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor - raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor + if (fd_accelerometer > 0) { + /* MPU-6000 values */ - // XXX read range from sensor - float range_g = 4.0f; - /* scale from 14 bit to m/s2 */ - raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f; - raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f; - raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f; + /* scale from 14 bit to m/s2 */ + raw.accelerometer_m_s2[0] = buf_accel_report.x; + raw.accelerometer_m_s2[1] = buf_accel_report.y; + raw.accelerometer_m_s2[2] = buf_accel_report.z; - raw.accelerometer_raw_counter++; + /* assign negated value, except for -SHORT_MAX, as it would wrap there */ + raw.accelerometer_raw[0] = buf_accel_report.x*1000; // x of the board is -y of the sensor + raw.accelerometer_raw[1] = buf_accel_report.y*1000; // y on the board is x of the sensor + raw.accelerometer_raw[2] = buf_accel_report.z*1000; // z of the board is z of the sensor + + raw.accelerometer_raw_counter++; + } else if (fd_bma180 > 0) { + + /* assign negated value, except for -SHORT_MAX, as it would wrap there */ + raw.accelerometer_raw[0] = (buf_accelerometer[1] == -32768) ? 32767 : -buf_accelerometer[1]; // x of the board is -y of the sensor + raw.accelerometer_raw[1] = (buf_accelerometer[0] == -32768) ? -32767 : buf_accelerometer[0]; // y on the board is x of the sensor + raw.accelerometer_raw[2] = (buf_accelerometer[2] == -32768) ? -32767 : buf_accelerometer[2]; // z of the board is z of the sensor + + + // XXX read range from sensor + float range_g = 4.0f; + /* scale from 14 bit to m/s2 */ + raw.accelerometer_m_s2[0] = (((raw.accelerometer_raw[0] - acc_offset[0]) * range_g) / 8192.0f) / 9.81f; + raw.accelerometer_m_s2[1] = (((raw.accelerometer_raw[1] - acc_offset[1]) * range_g) / 8192.0f) / 9.81f; + raw.accelerometer_m_s2[2] = (((raw.accelerometer_raw[2] - acc_offset[2]) * range_g) / 8192.0f) / 9.81f; + + raw.accelerometer_raw_counter++; + } + + /* L3GD20 is not available, use MPU-6000 */ + if (fd_accelerometer > 0 && fd_gyro < 0) { + raw.gyro_raw[0] = ((buf_accelerometer[3] == -32768) ? -32767 : buf_accelerometer[3]); // x of the board is y of the sensor + /* assign negated value, except for -SHORT_MAX, as it would wrap there */ + raw.gyro_raw[1] = ((buf_accelerometer[4] == -32768) ? 32767 : -buf_accelerometer[4]); // y on the board is -x of the sensor + raw.gyro_raw[2] = ((buf_accelerometer[5] == -32768) ? -32767 : buf_accelerometer[5]); // z of the board is -z of the sensor + + /* scale measurements */ + // XXX request scaling from driver instead of hardcoding it + /* scaling calculated as: raw * (1/(32768*(500/180*PI))) */ + raw.gyro_rad_s[0] = (raw.gyro_raw[0] - gyro_offset[0]) * 0.000266316109f; + raw.gyro_rad_s[1] = (raw.gyro_raw[1] - gyro_offset[1]) * 0.000266316109f; + raw.gyro_rad_s[2] = (raw.gyro_raw[2] - gyro_offset[2]) * 0.000266316109f; + + raw.gyro_raw_counter++; + /* mark as updated */ + gyro_updated = true; + } } /* MAGNETOMETER */ @@ -912,7 +1041,7 @@ int sensors_main(int argc, char *argv[]) printf("[sensors] sensor readout stopped\n"); close(fd_gyro); - close(fd_accelerometer); + close(fd_bma180); close(fd_magnetometer); close(fd_barometer); close(fd_adc); diff --git a/nuttx/configs/px4fmu/src/up_nsh.c b/nuttx/configs/px4fmu/src/up_nsh.c index 6e092e3b41..e7b9ce70a4 100644 --- a/nuttx/configs/px4fmu/src/up_nsh.c +++ b/nuttx/configs/px4fmu/src/up_nsh.c @@ -236,19 +236,6 @@ int nsh_archinitialize(void) if (acc_fail) message("[boot] FAILED to attach BMA180 accelerometer\r\n"); - int mpu_attempts = 0; - int mpu_fail = 0; - - while (mpu_attempts < 1) - { - mpu_fail = mpu6000_attach(spi1, PX4_SPIDEV_MPU); - mpu_attempts++; - if (mpu_fail == 0) break; - up_udelay(200); - } - - if (mpu_fail) message("[boot] FAILED to attach MPU 6000 gyro/acc\r\n"); - /* initialize I2C2 bus */ i2c2 = up_i2cinitialize(2); From a0ae2cb175b8f83ee7731335d452cc5ca30e820b Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 20:14:43 -0700 Subject: [PATCH 02/11] Add a set of ORB topics for advertising actuator outputs. This is part of \#7 --- apps/px4/fmu/fmu.cpp | 52 +++++++++++----------- apps/uORB/objects_common.cpp | 6 +++ apps/uORB/topics/actuator_outputs.h | 68 +++++++++++++++++++++++++++++ 3 files changed, 101 insertions(+), 25 deletions(-) create mode 100644 apps/uORB/topics/actuator_outputs.h diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 9d88472462..83bc625099 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -64,6 +64,7 @@ #include #include +#include class FMUServo : public device::CDev @@ -88,6 +89,7 @@ private: int _task; int _t_actuators; int _t_armed; + int _t_outputs; unsigned _num_outputs; volatile bool _task_should_exit; @@ -98,15 +100,12 @@ private: actuator_controls_s _controls; static void task_main_trampoline(int argc, char *argv[]); - void task_main(); + void task_main() __attribute__((noreturn)); - static int control_callback_trampoline(uintptr_t handle, + static int control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input); - int control_callback(uint8_t control_group, - uint8_t control_index, - float &input); }; namespace @@ -212,6 +211,11 @@ FMUServo::task_main() _t_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_armed, 100); /* 10Hz update rate */ + /* advertise the mixed control outputs */ + struct actuator_output_s outputs; + memset(&outputs, 0, sizeof(outputs)); + _t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); + struct pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; @@ -237,7 +241,6 @@ FMUServo::task_main() /* do we have a control update? */ if (fds[0].revents & POLLIN) { - float outputs[num_outputs]; /* get controls - must always do this to avoid spinning */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls); @@ -246,14 +249,20 @@ FMUServo::task_main() if (_mixers != nullptr) { /* do mixing */ - _mixers->mix(&outputs[0], num_outputs); + _mixers->mix(&outputs.output[0], num_outputs); /* iterate actuators */ for (unsigned i = 0; i < num_outputs; i++) { /* scale for PWM output 900 - 2100us */ - up_pwm_servo_set(i, 1500 + (600 * outputs[i])); + outputs.output[i] = 1500 + (600 * outputs.output[i]); + + /* output to the servo */ + up_pwm_servo_set(i, outputs.output[i]); } + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); } } @@ -264,13 +273,14 @@ FMUServo::task_main() /* get new value */ orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); - /* update PMW servo armed status */ + /* update PWM servo armed status */ up_pwm_servo_arm(aa.armed); } } ::close(_t_actuators); ::close(_t_armed); + ::close(_t_outputs); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -285,24 +295,14 @@ FMUServo::task_main() } int -FMUServo::control_callback_trampoline(uintptr_t handle, +FMUServo::control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input) { - return ((FMUServo *)handle)->control_callback(control_group, control_index, input); -} + const actuator_controls_s *controls = (actuator_controls_s *)handle; -int -FMUServo::control_callback(uint8_t control_group, - uint8_t control_index, - float &input) -{ - /* XXX currently only supporting group zero */ - if ((control_group != 0) || (control_index > NUM_ACTUATOR_CONTROLS)) - return -1; - - input = _controls.control[control_index]; + input = controls->control[control_index]; return 0; } @@ -377,7 +377,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) case MIXERIOCADDSIMPLE: { mixer_simple_s *mixinfo = (mixer_simple_s *)arg; - SimpleMixer *mixer = new SimpleMixer(control_callback_trampoline, (uintptr_t)this, mixinfo); + SimpleMixer *mixer = new SimpleMixer(control_callback, + (uintptr_t)&_controls, mixinfo); if (mixer->check()) { delete mixer; @@ -385,7 +386,8 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) } else { if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); + _mixers = new MixerGroup(control_callback, + (uintptr_t)&_controls); _mixers->add_mixer(mixer); } @@ -406,7 +408,7 @@ FMUServo::ioctl(struct file *filp, int cmd, unsigned long arg) _mixers = nullptr; } - _mixers = new MixerGroup(control_callback_trampoline, (uintptr_t)this); + _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); if (_mixers->load_from_file(path) != 0) { delete _mixers; diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index d6b566b6a8..6ff66fceb7 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -114,3 +114,9 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s); + +#include "topics/actuator_outputs.h" +ORB_DEFINE(actuator_outputs_0, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_1, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_2, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_3, struct actuator_output_s); diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h new file mode 100644 index 0000000000..63441a0591 --- /dev/null +++ b/apps/uORB/topics/actuator_outputs.h @@ -0,0 +1,68 @@ +/**************************************************************************** + * + * 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_outputs.h + * + * Actuator output values. + * + * Values published to these topics are the outputs of the control mixing + * system as sent to the actuators (servos, motors, etc.) that operate + * the vehicle. + * + * Each topic can be published by a single output driver. + */ + +#ifndef TOPIC_ACTUATOR_OUTPUTS_H +#define TOPIC_ACTUATOR_OUTPUTS_H + +#include +#include "../uORB.h" + +#define NUM_ACTUATOR_OUTPUTS 16 +#define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ + +struct actuator_output_s { + float output[NUM_ACTUATOR_OUTPUTS]; +}; + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(actuator_outputs_0); +ORB_DECLARE(actuator_outputs_1); +ORB_DECLARE(actuator_outputs_2); +ORB_DECLARE(actuator_outputs_3); + +/* output sets with pre-defined applications */ +#define ORB_ID_VEHICLE_CONTROLS ORB_ID(actuator_outputs_0) + +#endif \ No newline at end of file From 63831fa908f9deb848c20433c0235554b261bec3 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 22:04:38 -0700 Subject: [PATCH 03/11] Fix an issue where the baro ORB topic was advertised from the wrong context and thus could not be published to. --- apps/drivers/ms5611/ms5611.cpp | 32 ++++++++++++++++++++------------ 1 file changed, 20 insertions(+), 12 deletions(-) diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 6c6951d9b5..187de178b5 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -246,6 +246,7 @@ MS5611::MS5611(int bus) : _measure_phase(0), _dT(0), _temp64(0), + _baro_topic(-1), _reads(0), _measure_errors(0), _read_errors(0), @@ -277,18 +278,6 @@ MS5611::init() /* do I2C init (and probe) first */ ret = I2C::init(); - /* assuming we're good, advertise the object */ - if (ret == OK) { - struct baro_report b; - - /* if this fails (e.g. no object in the system) that's OK */ - memset(&b, 0, sizeof(b)); - _baro_topic = orb_advertise(ORB_ID(sensor_baro), &b); - - if (_baro_topic < 0) - debug("failed to create sensor_baro object"); - } - return ret; } @@ -538,6 +527,25 @@ MS5611::cycle_trampoline(void *arg) void MS5611::cycle() { + /* + * We have to publish the baro topic in the context of the workq + * in order to ensure that the descriptor is valid when we go to publish. + * + * @bug We can't really ever be torn down and restarted, since this + * descriptor will never be closed and on the restart we will be + * unable to re-advertise. + */ + if (_baro_topic == -1) { + struct baro_report b; + + /* if this fails (e.g. no object in the system) we will cope */ + memset(&b, 0, sizeof(b)); + _baro_topic = orb_advertise(ORB_ID(sensor_baro), &b); + + if (_baro_topic < 0) + debug("failed to create sensor_baro object"); + } + /* collection phase? */ if (_collect_phase) { From 5f77561ed4fb6b8347a7c848f0d02d7aeaa21c3f Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 22:05:42 -0700 Subject: [PATCH 04/11] Fix output scaling for the hmc5883 driver. Add data checking, and fix an issue where the ORB topic could not be published due to being advertised in the wrong context. --- apps/drivers/drv_mag.h | 8 +++ apps/drivers/hmc5883/hmc5883.cpp | 116 +++++++++++++++++++++++-------- 2 files changed, 96 insertions(+), 28 deletions(-) diff --git a/apps/drivers/drv_mag.h b/apps/drivers/drv_mag.h index fe1addaebe..a592917787 100644 --- a/apps/drivers/drv_mag.h +++ b/apps/drivers/drv_mag.h @@ -48,6 +48,8 @@ /** * mag report structure. Reads from the device must be in multiples of this * structure. + * + * Output values are in gauss. */ struct mag_report { float x; @@ -99,4 +101,10 @@ ORB_DECLARE(sensor_mag); /** set the mag scaling constants to the structure pointed to by (arg) */ #define MAGIOCSSCALE _MAGIOC(5) +/** copy the mag scaling constants to the structure pointed to by (arg) */ +#define MAGIOCGSCALE _MAGIOC(6) + +/** perform self-calibration, update scale factors to canonical units */ +#define MAGIOCCALIBRATE _MAGIOC(7) + #endif /* _DRV_MAG_H */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 64a0a932e7..6ef0716cd9 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -63,7 +64,6 @@ #include - /* * HMC5883 internal constants and data structures. */ @@ -137,14 +137,14 @@ protected: virtual int probe(); private: - struct work_s _work; + work_s _work; unsigned _measure_ticks; unsigned _num_reports; volatile unsigned _next_report; volatile unsigned _oldest_report; - struct mag_report *_reports; - + mag_report *_reports; + mag_scale _scale; bool _collect_phase; int _mag_topic; @@ -257,6 +257,7 @@ HMC5883::HMC5883(int bus) : _next_report(0), _oldest_report(0), _reports(nullptr), + _mag_topic(-1), _reads(0), _measure_errors(0), _read_errors(0), @@ -266,6 +267,14 @@ HMC5883::HMC5883(int bus) : // enable debug() calls _debug_enabled = true; + // default scaling + _scale.x_offset = 0; + _scale.x_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */ + _scale.y_offset = 0; + _scale.y_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */ + _scale.z_offset = 0; + _scale.z_scale = 1.0f / 1090.0f; /* default range scale from counts to gauss */ + // work_cancel in the dtor will explode if we don't do this... _work.worker = nullptr; } @@ -290,18 +299,6 @@ HMC5883::init() if (ret != OK) goto out; - /* assuming we're good, advertise the object */ - struct mag_report m; - - /* if this fails (e.g. no object in the system) that's OK */ - memset(&m, 0, sizeof(m)); - _mag_topic = orb_advertise(ORB_ID(sensor_mag), &m); - - if (_mag_topic < 0) { - debug("failed to create sensor_baro object"); - } else { - ret = 0; - } out: return ret; } @@ -340,7 +337,6 @@ HMC5883::close_last(struct file *filp) int HMC5883::probe() { - uint8_t cmd[] = { ADDR_STATUS }; uint8_t data[3]; if (read_reg(ADDR_ID_A, data[0]) || @@ -434,7 +430,7 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) _measure_ticks = 0; return OK; - /* external signalling not supported */ + /* external signalling (DRDY) not supported */ case MAG_POLLRATE_EXTERNAL: /* zero would be bad */ @@ -486,7 +482,30 @@ HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; } + case MAGIOCSSCALE: + /* set new scale factors */ + memcpy(&_scale, (mag_scale *)arg, sizeof(_scale)); + return 0; + + case MAGIOCGSCALE: + /* copy out scale factors */ + memcpy((mag_scale *)arg, &_scale, sizeof(_scale)); + return 0; + + case MAGIOCCALIBRATE: + /* XXX perform auto-calibration */ + return -EINVAL; + + case MAGIOCSSAMPLERATE: + /* not supported, always 1 sample per poll */ + return -EINVAL; + + case MAGIOCSLOWPASS: + /* not supported, no internal filtering */ + return -EINVAL; + case MAGIOCSREPORTFORMAT: + /* not supported, no custom report format */ return -EINVAL; default: @@ -526,6 +545,25 @@ HMC5883::cycle_trampoline(void *arg) void HMC5883::cycle() { + /* + * We have to publish the mag topic in the context of the workq + * in order to ensure that the descriptor is valid when we go to publish. + * + * @bug We can't really ever be torn down and restarted, since this + * descriptor will never be closed and on the restart we will be + * unable to re-advertise. + */ + if (_mag_topic == -1) { + struct mag_report m; + + /* if this fails (e.g. no object in the system) we will cope */ + memset(&m, 0, sizeof(m)); + _mag_topic = orb_advertise(ORB_ID(sensor_mag), &m); + + if (_mag_topic < 0) + debug("failed to create sensor_mag object"); + } + /* collection phase? */ if (_collect_phase) { @@ -591,33 +629,55 @@ HMC5883::collect() #pragma pack(push, 1) struct { /* status register and data as read back from the device */ uint8_t x[2]; - uint8_t z[2]; uint8_t y[2]; - uint8_t status; + uint8_t z[2]; } hmc_report; #pragma pack(pop) + struct { + int16_t x, y, z; + } report; int ret = -EIO; - uint8_t cmd[1]; + uint8_t cmd; + perf_begin(_sample_perf); /* this should be fairly close to the end of the measurement, so the best approximation of the time */ _reports[_next_report].timestamp = hrt_absolute_time(); + /* + * @note We could read the status register here, which could tell us that + * we were too early and that the output registers are still being + * written. In the common case that would just slow us down, and + * we're better off just never being early. + */ + /* get measurements from the device */ - cmd[0] = ADDR_DATA_OUT_X_MSB; - ret = transfer(&cmd[0], 1, &hmc_report.x[0], sizeof(hmc_report)); + cmd = ADDR_DATA_OUT_X_MSB; + ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); if (ret != OK) { debug("data/status read error"); goto out; } - /* XXX check status? */ + /* swap the data we just received */ + report.x = ((int16_t)hmc_report.x[0] << 8) + hmc_report.x[1]; + report.y = ((int16_t)hmc_report.y[0] << 8) + hmc_report.y[1]; + report.z = ((int16_t)hmc_report.z[0] << 8) + hmc_report.z[1]; - /* XXX scaling */ - _reports[_next_report].x = meas_to_float(hmc_report.x); - _reports[_next_report].y = meas_to_float(hmc_report.y); - _reports[_next_report].z = meas_to_float(hmc_report.z); + /* + * If any of the values are -4096, there was an internal math error in the sensor. + * Generalise this to a simple range check that will also catch some bit errors. + */ + if ((abs(report.x) > 2048) || + (abs(report.y) > 2048) || + (abs(report.z) > 2048)) + goto out; + + /* scale values for output */ + _reports[_next_report].x = report.x * _scale.x_scale + _scale.x_offset; + _reports[_next_report].y = report.y * _scale.y_scale + _scale.y_offset; + _reports[_next_report].z = report.z * _scale.z_scale + _scale.z_offset; /* publish it */ orb_publish(ORB_ID(sensor_mag), _mag_topic, &_reports[_next_report]); From 8c22e2a092a5527e5e4af2a54ecdeac586ffbfa7 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 22:30:04 -0700 Subject: [PATCH 05/11] Whitespace --- apps/drivers/hmc5883/hmc5883.cpp | 15 +++++++++------ apps/drivers/ms5611/ms5611.cpp | 4 ++-- 2 files changed, 11 insertions(+), 8 deletions(-) diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index 6ef0716cd9..a98b30efc9 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -296,6 +296,7 @@ HMC5883::init() /* do I2C init (and probe) first */ ret = I2C::init(); + if (ret != OK) goto out; @@ -344,7 +345,7 @@ HMC5883::probe() read_reg(ADDR_ID_C, data[2])) debug("read_reg fail"); - if ((data[0] != ID_A_WHO_AM_I) || + if ((data[0] != ID_A_WHO_AM_I) || (data[1] != ID_B_WHO_AM_I) || (data[2] != ID_C_WHO_AM_I)) { debug("ID byte mismatch (%02x,%02x,%02x)", data[0], data[1], data[2]); @@ -545,8 +546,8 @@ HMC5883::cycle_trampoline(void *arg) void HMC5883::cycle() { - /* - * We have to publish the mag topic in the context of the workq + /* + * We have to publish the mag topic in the context of the workq * in order to ensure that the descriptor is valid when we go to publish. * * @bug We can't really ever be torn down and restarted, since this @@ -617,6 +618,7 @@ HMC5883::measure() * Send the command to begin a measurement. */ ret = write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE); + if (OK != ret) _measure_errors++; @@ -648,13 +650,14 @@ HMC5883::collect() /* * @note We could read the status register here, which could tell us that * we were too early and that the output registers are still being - * written. In the common case that would just slow us down, and + * written. In the common case that would just slow us down, and * we're better off just never being early. */ /* get measurements from the device */ cmd = ADDR_DATA_OUT_X_MSB; ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report)); + if (ret != OK) { debug("data/status read error"); goto out; @@ -665,8 +668,8 @@ HMC5883::collect() report.y = ((int16_t)hmc_report.y[0] << 8) + hmc_report.y[1]; report.z = ((int16_t)hmc_report.z[0] << 8) + hmc_report.z[1]; - /* - * If any of the values are -4096, there was an internal math error in the sensor. + /* + * If any of the values are -4096, there was an internal math error in the sensor. * Generalise this to a simple range check that will also catch some bit errors. */ if ((abs(report.x) > 2048) || diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index 187de178b5..ee366f6688 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -527,8 +527,8 @@ MS5611::cycle_trampoline(void *arg) void MS5611::cycle() { - /* - * We have to publish the baro topic in the context of the workq + /* + * We have to publish the baro topic in the context of the workq * in order to ensure that the descriptor is valid when we go to publish. * * @bug We can't really ever be torn down and restarted, since this From 88f0080a0ffb299006950c0453eabddb7d17f078 Mon Sep 17 00:00:00 2001 From: px4dev Date: Tue, 21 Aug 2012 23:44:22 -0700 Subject: [PATCH 06/11] Fix an architectural issue with the ORB that prevented publication from interrupt context. ORB topic advertisements are now global handles that can be used in any context. It is still possible to open a topic node as a publisher, but it's not the default. As a consequence, the type of the handle returned from orb_advertise has changed; all other API remains the same. --- .../attitude_estimator_ekf_main.c | 2 +- apps/commander/commander.c | 2 +- apps/drivers/drv_orb_dev.h | 3 + apps/drivers/hmc5883/hmc5883.cpp | 2 +- apps/drivers/mpu6000/mpu6000.cpp | 4 +- apps/drivers/ms5611/ms5611.cpp | 2 +- apps/examples/px4_simple_app/px4_simple_app.c | 4 +- apps/fixedwing_control/fixedwing_control.c | 2 +- apps/gps/mtk.c | 2 +- apps/gps/nmea_helper.c | 2 +- apps/gps/ubx.c | 2 +- apps/mavlink/mavlink.c | 14 +-- .../multirotor_att_control_main.c | 4 +- .../multirotor_pos_control.c | 2 +- .../position_estimator_main.c | 2 +- .../attitude_estimator_bm.c | 2 +- apps/px4/fmu/fmu.cpp | 6 +- apps/px4/px4io/driver/px4io.cpp | 2 +- apps/sensors/sensors.c | 9 +- apps/uORB/uORB.cpp | 88 ++++++++++++------- apps/uORB/uORB.h | 21 ++++- 21 files changed, 110 insertions(+), 67 deletions(-) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index fdf6c9d91d..09cbdd4e94 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -138,7 +138,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* advertise attitude */ - int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 149a662fd4..55d3b82e71 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -96,7 +96,7 @@ static int buzzer; static int mavlink_fd; static bool commander_initialized = false; static struct vehicle_status_s current_status; /**< Main state machine */ -static int stat_pub; +static orb_advert_t stat_pub; static uint16_t nofix_counter = 0; static uint16_t gotfix_counter = 0; diff --git a/apps/drivers/drv_orb_dev.h b/apps/drivers/drv_orb_dev.h index b3fc01a5fa..50288f6901 100644 --- a/apps/drivers/drv_orb_dev.h +++ b/apps/drivers/drv_orb_dev.h @@ -81,4 +81,7 @@ /** Set the minimum interval at which the topic can be seen to be updated for this subscription */ #define ORBIOCSETINTERVAL _ORBIOC(12) +/** Get the global advertiser handle for the topic */ +#define ORBIOCGADVERTISER _ORBIOC(13) + #endif /* _DRV_UORB_H */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index a98b30efc9..0fa1f365dc 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -147,7 +147,7 @@ private: mag_scale _scale; bool _collect_phase; - int _mag_topic; + orb_advert_t _mag_topic; unsigned _reads; unsigned _measure_errors; diff --git a/apps/drivers/mpu6000/mpu6000.cpp b/apps/drivers/mpu6000/mpu6000.cpp index 7576fc1d96..951e1a0621 100644 --- a/apps/drivers/mpu6000/mpu6000.cpp +++ b/apps/drivers/mpu6000/mpu6000.cpp @@ -181,12 +181,12 @@ private: struct accel_report _accel_report; struct accel_scale _accel_scale; float _accel_range_scale; - int _accel_topic; + orb_advert_t _accel_topic; struct gyro_report _gyro_report; struct gyro_scale _gyro_scale; float _gyro_range_scale; - int _gyro_topic; + orb_advert_t _gyro_topic; unsigned _reads; perf_counter_t _sample_perf; diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index ee366f6688..8f8f96217f 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -125,7 +125,7 @@ private: int32_t _dT; int64_t _temp64; - int _baro_topic; + orb_advert_t _baro_topic; unsigned _reads; unsigned _measure_errors; diff --git a/apps/examples/px4_simple_app/px4_simple_app.c b/apps/examples/px4_simple_app/px4_simple_app.c index fd34a5dacb..7f655964d9 100644 --- a/apps/examples/px4_simple_app/px4_simple_app.c +++ b/apps/examples/px4_simple_app/px4_simple_app.c @@ -59,7 +59,7 @@ int px4_simple_app_main(int argc, char *argv[]) /* advertise attitude topic */ struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - int att_pub_fd = orb_advertise(ORB_ID(vehicle_attitude), &att); + orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); /* one could wait for multiple topics with this technique, just using one here */ struct pollfd fds[] = { @@ -103,7 +103,7 @@ int px4_simple_app_main(int argc, char *argv[]) att.roll = raw.accelerometer_m_s2[0]; att.pitch = raw.accelerometer_m_s2[1]; att.yaw = raw.accelerometer_m_s2[2]; - orb_publish(ORB_ID(vehicle_attitude), att_pub_fd, &att); + orb_publish(ORB_ID(vehicle_attitude), att_pub, &att); } /* there could be more file descriptors here, in the form like: * if (fds[1..n].revents & POLLIN) {} diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index bad62edcfc..7ea4101abf 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -675,7 +675,7 @@ int fixedwing_control_main(int argc, char *argv[]) /* Set up to publish fixed wing control messages */ struct fixedwing_control_s control; - int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); + orb_advert_t fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); /* Subscribe to global position, attitude and rc */ struct vehicle_global_position_s global_pos; diff --git a/apps/gps/mtk.c b/apps/gps/mtk.c index cef70601b3..0333c71008 100644 --- a/apps/gps/mtk.c +++ b/apps/gps/mtk.c @@ -311,7 +311,7 @@ void *mtk_loop(void *arg) /* advertise GPS topic */ struct vehicle_gps_position_s mtk_gps_d; mtk_gps = &mtk_gps_d; - int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps); + orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), &mtk_gps); while (1) { /* Parse a message from the gps receiver */ diff --git a/apps/gps/nmea_helper.c b/apps/gps/nmea_helper.c index 4b520d4036..54912b6d37 100644 --- a/apps/gps/nmea_helper.c +++ b/apps/gps/nmea_helper.c @@ -176,7 +176,7 @@ void *nmea_loop(void *arg) /* advertise GPS topic */ struct vehicle_gps_position_s nmea_gps_d = {0}; nmea_gps = &nmea_gps_d; - int gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps); + orb_advert_t gps_handle = orb_advertise(ORB_ID(vehicle_gps_position), nmea_gps); while (1) { /* Parse a message from the gps receiver */ diff --git a/apps/gps/ubx.c b/apps/gps/ubx.c index 771dfbd6c0..a629e904d8 100644 --- a/apps/gps/ubx.c +++ b/apps/gps/ubx.c @@ -842,7 +842,7 @@ void *ubx_loop(void *arg) ubx_gps = &ubx_gps_d; - int gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps); + orb_advert_t gps_pub = orb_advertise(ORB_ID(vehicle_gps_position), &ubx_gps); while (1) { /* Parse a message from the gps receiver */ diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 7fba65cb73..97822510cc 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -113,7 +113,7 @@ static struct vehicle_status_s v_status; static struct rc_channels_s rc; /* HIL publishers */ -static int pub_hil_attitude = -1; +static orb_advert_t pub_hil_attitude = -1; /** HIL attitude */ static struct vehicle_attitude_s hil_attitude; @@ -126,16 +126,16 @@ static struct ardrone_motors_setpoint_s ardrone_motors; static struct vehicle_command_s vcmd; -static int pub_hil_global_pos = -1; -static int ardrone_motors_pub = -1; -static int cmd_pub = -1; +static orb_advert_t pub_hil_global_pos = -1; +static orb_advert_t ardrone_motors_pub = -1; +static orb_advert_t cmd_pub = -1; static int sensor_sub = -1; static int att_sub = -1; static int global_pos_sub = -1; static int local_pos_sub = -1; -static int flow_pub = -1; -static int global_position_setpoint_pub = -1; -static int local_position_setpoint_pub = -1; +static orb_advert_t flow_pub = -1; +static orb_advert_t global_position_setpoint_pub = -1; +static orb_advert_t local_position_setpoint_pub = -1; static bool mavlink_hil_enabled = false; static char mavlink_message_string[51] = {0}; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index baee507b2f..df08ca75fe 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -119,9 +119,9 @@ mc_thread_main(int argc, char *argv[]) /* publish actuator controls */ for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) actuators.control[i] = 0.0f; - int actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); + orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); armed.armed = true; - int armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); diff --git a/apps/multirotor_pos_control/multirotor_pos_control.c b/apps/multirotor_pos_control/multirotor_pos_control.c index ff32fb4607..3f9c725177 100644 --- a/apps/multirotor_pos_control/multirotor_pos_control.c +++ b/apps/multirotor_pos_control/multirotor_pos_control.c @@ -89,7 +89,7 @@ mpc_thread_main(int argc, char *argv[]) int local_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); /* publish attitude setpoint */ - int att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); while (1) { /* get a local copy of the vehicle state */ diff --git a/apps/position_estimator/position_estimator_main.c b/apps/position_estimator/position_estimator_main.c index 773cd87ff3..17482dc0a8 100644 --- a/apps/position_estimator/position_estimator_main.c +++ b/apps/position_estimator/position_estimator_main.c @@ -297,7 +297,7 @@ int position_estimator_main(int argc, char *argv[]) .lon = lon_current * 1e7, .alt = gps.alt }; - int global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); + orb_advert_t global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos); printf("[multirotor position estimator] initialized projection with: lat: %.10f, lon:%.10f\n", lat_current, lon_current); diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index e56ff1c3a8..fd3ed11375 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -154,7 +154,7 @@ int attitude_estimator_bm_main(int argc, char *argv[]) bool publishing = false; /* advertise attitude */ - int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); publishing = true; struct pollfd fds[] = { diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 83bc625099..3016cb01eb 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -89,7 +89,7 @@ private: int _task; int _t_actuators; int _t_armed; - int _t_outputs; + orb_advert_t _t_outputs; unsigned _num_outputs; volatile bool _task_should_exit; @@ -566,15 +566,13 @@ fake(int argc, char *argv[]) ac.control[3] = strtol(argv[4], 0, 0) / 100.0f; - int handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); + orb_advert_t handle = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &ac); if (handle < 0) { puts("advertise failed"); exit(1); } - close(handle); - exit(0); } diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp index a5def874d8..c3371c138b 100644 --- a/apps/px4/px4io/driver/px4io.cpp +++ b/apps/px4/px4io/driver/px4io.cpp @@ -99,7 +99,7 @@ protected: void set_channels(unsigned count, const servo_position_t *data); private: - int _publication; + orb_advert_t _publication; struct rc_input_values _input; }; diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index ef8ff798f1..891167d1c7 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -124,8 +124,8 @@ extern unsigned ppm_decoded_channels; extern uint64_t ppm_last_valid_decode; #endif -/* file handle that will be used for subscribing */ -static int sensor_pub; +/* ORB topic publishing our results */ +static orb_advert_t sensor_pub; /** * Sensor readout and publishing. @@ -404,7 +404,7 @@ int sensors_thread_main(int argc, char *argv[]) .yaw = 0.0f, .throttle = 0.0f }; - int manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); + orb_advert_t manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); if (manual_control_pub < 0) { fprintf(stderr, "[sensors] ERROR: orb_advertise for topic manual_control_setpoint failed.\n"); @@ -413,7 +413,7 @@ int sensors_thread_main(int argc, char *argv[]) /* advertise the rc topic */ struct rc_channels_s rc; memset(&rc, 0, sizeof(rc)); - int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); + orb_advert_t rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); if (rc_pub < 0) { fprintf(stderr, "[sensors] ERROR: orb_advertise for topic rc_channels failed.\n"); @@ -466,7 +466,6 @@ int sensors_thread_main(int argc, char *argv[]) if (vstatus.flag_hil_enabled && !hil_enabled) { hil_enabled = true; publishing = false; - int ret = close(sensor_pub); printf("[sensors] Closing sensor pub: %i \n", ret); /* switching from HIL to non-HIL mode */ diff --git a/apps/uORB/uORB.cpp b/apps/uORB/uORB.cpp index 1e7cdc8db1..c4434e4edd 100644 --- a/apps/uORB/uORB.cpp +++ b/apps/uORB/uORB.cpp @@ -112,6 +112,8 @@ public: virtual ssize_t write(struct file *filp, const char *buffer, size_t buflen); virtual int ioctl(struct file *filp, int cmd, unsigned long arg); + static ssize_t publish(const orb_metadata *meta, orb_advert_t handle, const void *data); + protected: virtual pollevent_t poll_state(struct file *filp); virtual void poll_notify_one(struct pollfd *fds, pollevent_t events); @@ -298,6 +300,8 @@ ORBDevNode::write(struct file *filp, const char *buffer, size_t buflen) * * Writes outside interrupt context will allocate the object * if it has not yet been allocated. + * + * Note that filp will usually be NULL. */ if (nullptr == _data) { if (!up_interrupt_context()) { @@ -353,12 +357,42 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) sd->update_interval = arg; return OK; + case ORBIOCGADVERTISER: + *(uintptr_t *)arg = (uintptr_t)this; + return OK; + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); } } +ssize_t +ORBDevNode::publish(const orb_metadata *meta, orb_advert_t handle, const void *data) +{ + ORBDevNode *devnode = (ORBDevNode *)handle; + int ret; + + /* this is a bit risky, since we are trusting the handle in order to deref it */ + if (devnode->_meta != meta) { + errno = EINVAL; + return ERROR; + } + + /* call the devnode write method with no file pointer */ + ret = devnode->write(nullptr, (const char *)data, meta->o_size); + + if (ret < 0) + return ERROR; + + if (ret != (int)meta->o_size) { + errno = EIO; + return ERROR; + } + + return OK; +} + pollevent_t ORBDevNode::poll_state(struct file *filp) { @@ -614,7 +648,7 @@ test() if (pfd < 0) return test_fail("advertise failed: %d", errno); - test_note("publish fd %d", pfd); + test_note("publish handle 0x%08x", pfd); sfd = orb_subscribe(ORB_ID(orb_test)); if (sfd < 0) @@ -877,29 +911,35 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve return ERROR; } - /* the advertiser must perform an initial publish to initialise the object */ - if (advertiser) { - ret = orb_publish(meta, fd, data); - - if (ret != OK) { - /* save errno across the close */ - ret = errno; - close(fd); - errno = ret; - return ERROR; - } - } - /* everything has been OK, we can return the handle now */ return fd; } } // namespace -int +orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) { - return node_open(PUBSUB, meta, data, true); + int result, fd; + orb_advert_t advertiser; + + /* open the node as an advertiser */ + fd = node_open(PUBSUB, meta, data, true); + if (fd == ERROR) + return ERROR; + + /* get the advertiser handle and close the node */ + result = ioctl(fd, ORBIOCGADVERTISER, (unsigned long)&advertiser); + close(fd); + if (result == ERROR) + return ERROR; + + /* the advertiser must perform an initial publish to initialise the object */ + result= orb_publish(meta, advertiser, data); + if (result == ERROR) + return ERROR; + + return advertiser; } int @@ -915,21 +955,9 @@ orb_unsubscribe(int handle) } int -orb_publish(const struct orb_metadata *meta, int handle, const void *data) +orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) { - int ret; - - ret = write(handle, data, meta->o_size); - - if (ret < 0) - return ERROR; - - if (ret != (int)meta->o_size) { - errno = EIO; - return ERROR; - } - - return OK; + return ORBDevNode::publish(meta, handle, data); } int diff --git a/apps/uORB/uORB.h b/apps/uORB/uORB.h index c36d0044f2..eb068d2b78 100644 --- a/apps/uORB/uORB.h +++ b/apps/uORB/uORB.h @@ -105,12 +105,27 @@ struct orb_metadata { __BEGIN_DECLS +/** + * ORB topic advertiser handle. + * + * Advertiser handles are global; once obtained they can be shared freely + * and do not need to be closed or released. + * + * This permits publication from interrupt context and other contexts where + * a file-descriptor-based handle would not otherwise be in scope for the + * publisher. + */ +typedef intptr_t orb_advert_t; + /** * Advertise as the publisher of a topic. * * This performs the initial advertisement of a topic; it creates the topic * node in /obj if required and publishes the initial data. * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. * @param data A pointer to the initial data to be published. @@ -122,7 +137,7 @@ __BEGIN_DECLS * ORB_DEFINE with no corresponding ORB_DECLARE) * this function will return -1 and set errno to ENOENT. */ -extern int orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; +extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; /** * Publish new data to a topic. @@ -131,13 +146,13 @@ extern int orb_advertise(const struct orb_metadata *meta, const void *data) __EX * will be notified. Subscribers that are not waiting can check the topic * for updates using orb_check and/or orb_stat. * - * @handle The handle returned from orb_advertise. * @param meta The uORB metadata (usually from the ORB_ID() macro) * for the topic. + * @handle The handle returned from orb_advertise. * @param data A pointer to the data to be published. * @return OK on success, ERROR otherwise with errno set accordingly. */ -extern int orb_publish(const struct orb_metadata *meta, int handle, const void *data) __EXPORT; +extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data) __EXPORT; /** * Subscribe to a topic. From 1bea49869b8daadddedf1c78c4eea5281b931d93 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Aug 2012 10:00:46 +0200 Subject: [PATCH 07/11] task file list gdb macro --- Debug/NuttX | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/Debug/NuttX b/Debug/NuttX index b6d4e74f05..5396ab6b1c 100644 --- a/Debug/NuttX +++ b/Debug/NuttX @@ -73,6 +73,29 @@ document showheap . Prints the contents of the malloc heap(s). end +################################################################################ +# Task file listing +################################################################################ + +define showfiles + set $task = (struct _TCB *)$arg0 + set $nfiles = sizeof((*(struct filelist*)0).fl_files) / sizeof(struct file) + printf "%d files\n", $nfiles + set $index = 0 + while $index < $nfiles + set $file = &($task->filelist->fl_files[$index]) + printf "%d: inode %p f_priv %p\n", $index, $file->f_inode, $file->f_priv + if $file->f_inode != 0 + printf " i_name %s i_private %p\n", &$file->f_inode->i_name[0], $file->f_inode->i_private + end + set $index = $index + 1 + end +end + +document showfiles +. showfiles +. Prints the files opened by a task. + ################################################################################ # Task display ################################################################################ From 1eccfb7ccb91b98f53e791e41b917cb05b17d7e5 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 Aug 2012 01:08:43 -0700 Subject: [PATCH 08/11] Add link to the BSON spec. --- apps/systemlib/bson/tinybson.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/systemlib/bson/tinybson.h b/apps/systemlib/bson/tinybson.h index 05bae007b2..6b92e61ef9 100644 --- a/apps/systemlib/bson/tinybson.h +++ b/apps/systemlib/bson/tinybson.h @@ -34,7 +34,7 @@ /** * @file tinybson.h * - * A simple subset SAX-style BSON parser and generator. + * A simple subset SAX-style BSON parser and generator. See http://bsonspec.org * * Some types and defines taken from the standalone BSON parser/generator * in the Mongo C connector. From 44ff4d4ee29f3da53f64b2f24b822f2f2f7032c0 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 22 Aug 2012 01:09:06 -0700 Subject: [PATCH 09/11] Fix parameter change advertisement to conform to API change. --- apps/systemlib/param/param.c | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 7d0c756703..acd5674aba 100644 --- a/apps/systemlib/param/param.c +++ b/apps/systemlib/param/param.c @@ -91,6 +91,9 @@ UT_icd param_icd = {sizeof(struct param_wbuf_s), NULL, NULL, NULL}; /** parameter update topic */ ORB_DEFINE(parameter_update, struct parameter_update_s); +/** parameter update topic handle */ +static orb_advert_t param_topic = -1; + /** lock the parameter store */ static void param_lock(void) @@ -390,13 +393,15 @@ out: struct parameter_update_s pup = { .timestamp = hrt_absolute_time() }; /* - * Because we're a library, we can't keep a persistent advertisement - * around, so if we succeed in updating the topic, we have to toss - * the descriptor straight away. + * If we don't have a handle to our topic, create one now; otherwise + * just publish. */ - int param_topic = orb_advertise(ORB_ID(parameter_update), &pup); - if (param_topic != -1) - close(param_topic); + if (param_topic == -1) { + param_topic = orb_advertise(ORB_ID(parameter_update), &pup); + } else { + orb_publish(ORB_ID(parameter_update), param_topic, &pup); + } + } return result; From fa9f145b08cace0f48c808cf307daf6cd43d9bd6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Aug 2012 14:35:22 +0200 Subject: [PATCH 10/11] Fixed a bunch of issues in the arming state machine for multirotors, arming / disarming works fine now. Porting of various processes needed --- apps/mavlink/mavlink.c | 285 +++++++++++++++--- .../multirotor_att_control_main.c | 43 ++- .../multirotor_attitude_control.c | 2 + apps/px4/fmu/fmu.cpp | 2 +- apps/sensors/sensors.c | 2 +- apps/uORB/objects_common.cpp | 8 +- apps/uORB/topics/actuator_outputs.h | 2 +- 7 files changed, 285 insertions(+), 59 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 84ea0aae2f..1b23f6751b 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -73,6 +73,8 @@ #include #include #include +#include +#include #include "waypoints.h" #include "mavlink_log.h" @@ -129,9 +131,6 @@ static struct vehicle_command_s vcmd; static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t ardrone_motors_pub = -1; static orb_advert_t cmd_pub = -1; -static int sensor_sub = -1; -static int att_sub = -1; -static int global_pos_sub = -1; static int local_pos_sub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t global_position_setpoint_pub = -1; @@ -146,6 +145,30 @@ static enum { MAVLINK_INTERFACE_MODE_ONBOARD } mavlink_link_mode = MAVLINK_INTERFACE_MODE_OFFBOARD; +static struct mavlink_subscriptions { + int sensor_sub; + int att_sub; + int global_pos_sub; + int act_0_sub; + int act_1_sub; + int act_2_sub; + int act_3_sub; + int gps_sub; + int man_control_sp_sub; + bool initialized; +} mavlink_subs = { + .sensor_sub = 0, + .att_sub = 0, + .global_pos_sub = 0, + .act_0_sub = 0, + .act_1_sub = 0, + .act_2_sub = 0, + .act_3_sub = 0, + .gps_sub = 0, + .man_control_sp_sub = 0, + .initialized = false +}; + /* 3: Define waypoint helper functions */ void mavlink_wpm_send_message(mavlink_message_t *msg); @@ -459,23 +482,33 @@ static void *receiveloop(void *arg) return NULL; } -static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) +static int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_msg_id, int min_interval) { int ret = OK; switch (mavlink_msg_id) { case MAVLINK_MSG_ID_SCALED_IMU: /* senser sub triggers scaled IMU */ - orb_set_interval(sensor_sub, min_interval); + if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval); break; case MAVLINK_MSG_ID_RAW_IMU: /* senser sub triggers RAW IMU */ - orb_set_interval(sensor_sub, min_interval); + if (subs->sensor_sub) orb_set_interval(subs->sensor_sub, min_interval); break; case MAVLINK_MSG_ID_ATTITUDE: /* attitude sub triggers attitude */ - orb_set_interval(att_sub, min_interval); + if (subs->att_sub) orb_set_interval(subs->att_sub, min_interval); break; + case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: + /* actuator_outputs triggers this message */ + if (subs->act_0_sub) orb_set_interval(subs->act_0_sub, min_interval); + if (subs->act_1_sub) orb_set_interval(subs->act_1_sub, min_interval); + if (subs->act_2_sub) orb_set_interval(subs->act_2_sub, min_interval); + if (subs->act_3_sub) orb_set_interval(subs->act_3_sub, min_interval); + break; + case MAVLINK_MSG_ID_MANUAL_CONTROL: + /* manual_control_setpoint triggers this message */ + if (subs->man_control_sp_sub) orb_set_interval(subs->man_control_sp_sub, min_interval); default: /* not found */ ret = ERROR; @@ -494,13 +527,16 @@ static int set_mavlink_interval_limit(int mavlink_msg_id, int min_interval) */ static void *uorb_receiveloop(void *arg) { + /* obtain reference to task's subscriptions */ + struct mavlink_subscriptions *subs = (struct mavlink_subscriptions *)arg; + /* Set thread name */ prctl(PR_SET_NAME, "mavlink uORB", getpid()); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 15; + const ssize_t fdsc = 25; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -515,28 +551,30 @@ static void *uorb_receiveloop(void *arg) struct vehicle_local_position_setpoint_s local_sp; struct vehicle_global_position_setpoint_s global_sp; struct vehicle_attitude_setpoint_s att_sp; + struct actuator_outputs_s act_outputs; + struct manual_control_setpoint_s man_control; } buf; /* --- SENSORS RAW VALUE --- */ /* subscribe to ORB for sensors raw */ - sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - fds[fdsc_count].fd = sensor_sub; + subs->sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + fds[fdsc_count].fd = subs->sensor_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- ATTITUDE VALUE --- */ /* subscribe to ORB for attitude */ - att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - orb_set_interval(att_sub, 100); - fds[fdsc_count].fd = att_sub; + subs->att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + orb_set_interval(subs->att_sub, 100); + fds[fdsc_count].fd = subs->att_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; /* --- GPS VALUE --- */ /* subscribe to ORB for attitude */ - int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(gps_sub, 1000); /* 1Hz updates */ - fds[fdsc_count].fd = gps_sub; + subs->gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + orb_set_interval(subs->gps_sub, 1000); /* 1Hz updates */ + fds[fdsc_count].fd = subs->gps_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -577,7 +615,7 @@ static void *uorb_receiveloop(void *arg) /* --- GLOBAL POS VALUE --- */ /* struct already globally allocated and topic already subscribed */ - fds[fdsc_count].fd = global_pos_sub; + fds[fdsc_count].fd = subs->global_pos_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -608,12 +646,39 @@ static void *uorb_receiveloop(void *arg) /* --- ATTITUDE SETPOINT VALUE --- */ /* subscribe to ORB for attitude setpoint */ /* struct already allocated */ - int spa_sub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint)); + int spa_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); orb_set_interval(spa_sub, 2000); /* 0.5 Hz updates */ fds[fdsc_count].fd = spa_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; + /** --- ACTUATOR OUTPUTS --- */ + subs->act_0_sub = orb_subscribe(ORB_ID(actuator_outputs_0)); + fds[fdsc_count].fd = subs->act_0_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + subs->act_1_sub = orb_subscribe(ORB_ID(actuator_outputs_1)); + fds[fdsc_count].fd = subs->act_1_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + subs->act_2_sub = orb_subscribe(ORB_ID(actuator_outputs_2)); + fds[fdsc_count].fd = subs->act_2_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + subs->act_3_sub = orb_subscribe(ORB_ID(actuator_outputs_3)); + fds[fdsc_count].fd = subs->act_3_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /** --- MAPPED MANUAL CONTROL INPUTS --- */ + subs->man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + fds[fdsc_count].fd = subs->man_control_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + + /* all subscriptions initialized, return success */ + subs->initialized = true; + unsigned int sensors_raw_counter = 0; unsigned int attitude_counter = 0; unsigned int gps_counter = 0; @@ -651,7 +716,7 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { /* copy sensors raw data into local buffer */ - orb_copy(ORB_ID(sensor_combined), sensor_sub, &buf.raw); + orb_copy(ORB_ID(sensor_combined), subs->sensor_sub, &buf.raw); /* send raw imu data */ mavlink_msg_raw_imu_send(MAVLINK_COMM_0, buf.raw.timestamp, buf.raw.accelerometer_raw[0], buf.raw.accelerometer_raw[1], buf.raw.accelerometer_raw[2], buf.raw.gyro_raw[0], buf.raw.gyro_raw[1], buf.raw.gyro_raw[2], buf.raw.magnetometer_raw[0], buf.raw.magnetometer_raw[1], buf.raw.magnetometer_raw[2]); @@ -667,7 +732,7 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { /* copy attitude data into local buffer */ - orb_copy(ORB_ID(vehicle_attitude), att_sub, &buf.att); + orb_copy(ORB_ID(vehicle_attitude), subs->att_sub, &buf.att); /* send sensor values */ mavlink_msg_attitude_send(MAVLINK_COMM_0, buf.att.timestamp / 1000, buf.att.roll, buf.att.pitch, buf.att.yaw, buf.att.rollspeed, buf.att.pitchspeed, buf.att.yawspeed); @@ -678,7 +743,7 @@ static void *uorb_receiveloop(void *arg) /* --- GPS VALUE --- */ if (fds[ifds++].revents & POLLIN) { /* copy gps data into local buffer */ - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &buf.gps); + orb_copy(ORB_ID(vehicle_gps_position), subs->gps_sub, &buf.gps); /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, buf.gps.timestamp, buf.gps.fix_type, buf.gps.lat, buf.gps.lon, buf.gps.alt, buf.gps.eph, buf.gps.epv, buf.gps.vel, buf.gps.cog, buf.gps.satellites_visible); @@ -801,7 +866,7 @@ static void *uorb_receiveloop(void *arg) /* --- VEHICLE GLOBAL POSITION --- */ if (fds[ifds++].revents & POLLIN) { /* copy global position data into local buffer */ - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + orb_copy(ORB_ID(vehicle_global_position), subs->global_pos_sub, &global_pos); uint64_t timestamp = global_pos.timestamp; int32_t lat = global_pos.lat; int32_t lon = global_pos.lon; @@ -845,6 +910,126 @@ static void *uorb_receiveloop(void *arg) orb_copy(ORB_ID(vehicle_attitude_setpoint), spa_sub, &buf.att_sp); mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, buf.att_sp.timestamp/1000, buf.att_sp.roll_tait_bryan, buf.att_sp.pitch_tait_bryan, buf.att_sp.yaw_tait_bryan, buf.att_sp.thrust); } + + /* --- ACTUATOR OUTPUTS 0 --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy actuator data into local buffer */ + orb_copy(ORB_ID(actuator_outputs_0), subs->act_0_sub, &buf.act_outputs); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + 0 /* port 0 */, + buf.act_outputs.output[0], + buf.act_outputs.output[1], + buf.act_outputs.output[2], + buf.act_outputs.output[3], + buf.act_outputs.output[4], + buf.act_outputs.output[5], + buf.act_outputs.output[6], + buf.act_outputs.output[7]); + // if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { + // mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + // 1 /* port 1 */, + // buf.act_outputs.output[ 8], + // buf.act_outputs.output[ 9], + // buf.act_outputs.output[10], + // buf.act_outputs.output[11], + // buf.act_outputs.output[12], + // buf.act_outputs.output[13], + // buf.act_outputs.output[14], + // buf.act_outputs.output[15]); + // } + } + + /* --- ACTUATOR OUTPUTS 1 --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy actuator data into local buffer */ + orb_copy(ORB_ID(actuator_outputs_1), subs->act_1_sub, &buf.act_outputs); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 2 : 1 /* port 2 or 1*/, + buf.act_outputs.output[0], + buf.act_outputs.output[1], + buf.act_outputs.output[2], + buf.act_outputs.output[3], + buf.act_outputs.output[4], + buf.act_outputs.output[5], + buf.act_outputs.output[6], + buf.act_outputs.output[7]); + if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + 3 /* port 3 */, + buf.act_outputs.output[ 8], + buf.act_outputs.output[ 9], + buf.act_outputs.output[10], + buf.act_outputs.output[11], + buf.act_outputs.output[12], + buf.act_outputs.output[13], + buf.act_outputs.output[14], + buf.act_outputs.output[15]); + } + } + + /* --- ACTUATOR OUTPUTS 2 --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy actuator data into local buffer */ + orb_copy(ORB_ID(actuator_outputs_2), subs->act_2_sub, &buf.act_outputs); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 4 : 2 /* port 4 or 2 */, + buf.act_outputs.output[0], + buf.act_outputs.output[1], + buf.act_outputs.output[2], + buf.act_outputs.output[3], + buf.act_outputs.output[4], + buf.act_outputs.output[5], + buf.act_outputs.output[6], + buf.act_outputs.output[7]); + if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + 5 /* port 5 */, + buf.act_outputs.output[ 8], + buf.act_outputs.output[ 9], + buf.act_outputs.output[10], + buf.act_outputs.output[11], + buf.act_outputs.output[12], + buf.act_outputs.output[13], + buf.act_outputs.output[14], + buf.act_outputs.output[15]); + } + } + + /* --- ACTUATOR OUTPUTS 3 --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy actuator data into local buffer */ + orb_copy(ORB_ID(actuator_outputs_3), subs->act_3_sub, &buf.act_outputs); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) ? 6 : 3 /* port 6 or 3 */, + buf.act_outputs.output[0], + buf.act_outputs.output[1], + buf.act_outputs.output[2], + buf.act_outputs.output[3], + buf.act_outputs.output[4], + buf.act_outputs.output[5], + buf.act_outputs.output[6], + buf.act_outputs.output[7]); + if (NUM_ACTUATOR_OUTPUTS > 8 && NUM_ACTUATOR_OUTPUTS <= 16) { + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, hrt_absolute_time(), + 7 /* port 7 */, + buf.act_outputs.output[ 8], + buf.act_outputs.output[ 9], + buf.act_outputs.output[10], + buf.act_outputs.output[11], + buf.act_outputs.output[12], + buf.act_outputs.output[13], + buf.act_outputs.output[14], + buf.act_outputs.output[15]); + } + } + + /* --- MAPPED MANUAL CONTROL INPUTS --- */ + if (fds[ifds++].revents & POLLIN) { + /* copy local position data into local buffer */ + orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control); + mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll, + buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 1, 1, 1, 1); + } } } @@ -891,7 +1076,8 @@ void handleMessage(mavlink_message_t *msg) mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); - if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { + if (cmd_mavlink.target_system == mavlink_system.sysid && ((cmd_mavlink.target_component == mavlink_system.compid) + || (cmd_mavlink.target_component == MAV_COMP_ID_ALL))) { //check for MAVLINK terminate command if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN && ((int)cmd_mavlink.param1) == 3) { /* This is the link shutdown command, terminate mavlink */ //TODO: check what happens with global_data buffers that are read by the mavlink app @@ -1299,8 +1485,8 @@ int mavlink_thread_main(int argc, char *argv[]) /* topics to subscribe globally */ /* subscribe to ORB for global position */ - global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - orb_set_interval(global_pos_sub, 1000); /* 1Hz active updates */ + mavlink_subs.global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + orb_set_interval(mavlink_subs.global_pos_sub, 1000); /* 1Hz active updates */ /* subscribe to ORB for local position */ local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); orb_set_interval(local_pos_sub, 1000); /* 1Hz active updates */ @@ -1315,7 +1501,7 @@ int mavlink_thread_main(int argc, char *argv[]) pthread_attr_init(&uorb_attr); /* Set stack size, needs more than 4000 bytes */ pthread_attr_setstacksize(&uorb_attr, 4096); - pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, NULL); + pthread_create(&uorb_receive_thread, &uorb_attr, uorb_receiveloop, &mavlink_subs); /* initialize waypoint manager */ mavlink_wpm_init(wpm); @@ -1323,27 +1509,52 @@ int mavlink_thread_main(int argc, char *argv[]) uint16_t counter = 0; int lowspeed_counter = 0; + /* make sure all threads have registered their subscriptions */ + while (!mavlink_subs.initialized) { + usleep(500); + } + /* all subscriptions are now active, set up initial guess about rate limits */ if (baudrate >= 921600) { /* 500 Hz / 2 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 2); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 2); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 2); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 2); + /* 200 Hz / 5 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 5); + /* 5 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 200); } else if (baudrate >= 460800) { /* 250 Hz / 4 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 5); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 5); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 5); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 5); + /* 50 Hz / 20 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 20); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); } else if (baudrate >= 115200) { /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 50); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); + /* 10 Hz / 100 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 100); + /* 1 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 1000); } else if (baudrate >= 57600) { /* 10 Hz / 100 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 100); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 100); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 100); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 100); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + /* 0.2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 5000); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ - set_mavlink_interval_limit(MAVLINK_MSG_ID_SCALED_IMU, 1000); - set_mavlink_interval_limit(MAVLINK_MSG_ID_ATTITUDE, 1000); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SCALED_IMU, 1000); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000); + /* 0.5 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000); + /* 0.1 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000); } while (!thread_should_exit) { @@ -1351,7 +1562,7 @@ int mavlink_thread_main(int argc, char *argv[]) if (mavlink_exit_requested) break; /* get local and global position */ - orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + orb_copy(ORB_ID(vehicle_global_position), mavlink_subs.global_pos_sub, &global_pos); orb_copy(ORB_ID(vehicle_local_position), local_pos_sub, &local_pos); /* check if waypoint has been reached against the last positions */ diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index df08ca75fe..33e7ab7e78 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -78,13 +78,11 @@ static enum { static bool thread_should_exit; static int mc_task; +static bool motor_test_mode = false; static int mc_thread_main(int argc, char *argv[]) { - bool motor_test_mode = false; - - /* structures */ /* declare and safely initialize all structs */ struct vehicle_status_s state; memset(&state, 0, sizeof(state)); @@ -109,7 +107,7 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + //int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); // int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint)); /* rate-limit the attitude subscription to 200Hz to pace our loop */ @@ -120,8 +118,9 @@ mc_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) actuators.control[i] = 0.0f; orb_advert_t actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); - armed.armed = true; + armed.armed = false; orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); + orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); /* register the perf counter */ perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "multirotor_att_control"); @@ -136,6 +135,8 @@ mc_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); + /* get a local copy of system state */ + orb_copy(ORB_ID(vehicle_status), state_sub, &state); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ @@ -150,7 +151,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.roll_body = 0.0f; att_sp.pitch_body = 0.0f; att_sp.yaw_body = 0.0f; - att_sp.thrust = 0.3f; + att_sp.thrust = 0.1f; } else { if (state.state_machine == SYSTEM_STATE_MANUAL || state.state_machine == SYSTEM_STATE_GROUND_READY || @@ -159,28 +160,30 @@ mc_thread_main(int argc, char *argv[]) state.state_machine == SYSTEM_STATE_MISSION_ABORT || state.state_machine == SYSTEM_STATE_EMCY_LANDING) { att_sp.thrust = manual.throttle; + armed.armed = true; } else if (state.state_machine == SYSTEM_STATE_EMCY_CUTOFF) { /* immediately cut off motors */ - att_sp.thrust = 0.0f; + armed.armed = false; } else { /* limit motor throttle to zero for an unknown mode */ - att_sp.thrust = 0.0f; + armed.armed = false; } } multirotor_control_attitude(&att_sp, &att, &state, &actuator_controls, motor_test_mode); - //ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode); /* publish the result */ + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); perf_end(mc_loop_perf); } - printf("[multirotor att control] stopping.\n"); + printf("[multirotor att control] stopping, disarming motors.\n"); /* kill all outputs */ armed.armed = false; @@ -208,8 +211,9 @@ usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); - fprintf(stderr, "usage: multirotor_att_control [-m ] {start|stop}\n"); + fprintf(stderr, "usage: multirotor_att_control [-m ] [-t] {start|stop}\n"); fprintf(stderr, " is 'rates' or 'attitude'\n"); + fprintf(stderr, " -t enables motor test mode with 10%% thrust\n"); exit(1); } @@ -220,7 +224,7 @@ int multirotor_att_control_main(int argc, char *argv[]) control_mode = CONTROL_MODE_RATES; unsigned int optioncount = 0; - while ((ch = getopt(argc, argv, "m:")) != EOF) { + while ((ch = getopt(argc, argv, "tm:")) != EOF) { switch (ch) { case 'm': if (!strcmp(optarg, "rates")) { @@ -231,24 +235,33 @@ int multirotor_att_control_main(int argc, char *argv[]) usage("unrecognized -m value"); } optioncount += 2; + break; + case 't': + motor_test_mode = true; + optioncount += 1; + break; + case ':': + usage("missing parameter"); + break; default: + fprintf(stderr, "option: -%c\n", ch); usage("unrecognized option"); } } argc -= optioncount; - argv += optioncount; + //argv += optioncount; if (argc < 1) usage("missing command"); - if (!strcmp(argv[1], "start")) { + if (!strcmp(argv[1+optioncount], "start")) { thread_should_exit = false; mc_task = task_create("multirotor_att_control", SCHED_PRIORITY_MAX - 15, 2048, mc_thread_main, NULL); exit(0); } - if (!strcmp(argv[1], "stop")) { + if (!strcmp(argv[1+optioncount], "stop")) { thread_should_exit = true; exit(0); } diff --git a/apps/multirotor_att_control/multirotor_attitude_control.c b/apps/multirotor_att_control/multirotor_attitude_control.c index b21a20ad1f..886ce304ed 100644 --- a/apps/multirotor_att_control/multirotor_attitude_control.c +++ b/apps/multirotor_att_control/multirotor_attitude_control.c @@ -239,4 +239,6 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s actuators->control[1] = pitch_control; actuators->control[2] = yaw_rate_control; actuators->control[3] = motor_thrust; + + motor_skip_counter++; } diff --git a/apps/px4/fmu/fmu.cpp b/apps/px4/fmu/fmu.cpp index 3016cb01eb..b5db823bd4 100644 --- a/apps/px4/fmu/fmu.cpp +++ b/apps/px4/fmu/fmu.cpp @@ -212,7 +212,7 @@ FMUServo::task_main() orb_set_interval(_t_armed, 100); /* 10Hz update rate */ /* advertise the mixed control outputs */ - struct actuator_output_s outputs; + struct actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); _t_outputs = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index b5875cb68a..2739b39710 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -832,7 +832,7 @@ int sensors_thread_main(int argc, char *argv[]) if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f; /* throttle input */ - manual_control.throttle = rc.chan[rc.function[THROTTLE]].scaled/2.0f; + manual_control.throttle = (rc.chan[rc.function[THROTTLE]].scaled+1.0f)/2.0f; if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f; if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f; diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp index 6ff66fceb7..d6c00f9965 100644 --- a/apps/uORB/objects_common.cpp +++ b/apps/uORB/objects_common.cpp @@ -116,7 +116,7 @@ ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s); #include "topics/actuator_outputs.h" -ORB_DEFINE(actuator_outputs_0, struct actuator_output_s); -ORB_DEFINE(actuator_outputs_1, struct actuator_output_s); -ORB_DEFINE(actuator_outputs_2, struct actuator_output_s); -ORB_DEFINE(actuator_outputs_3, struct actuator_output_s); +ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); diff --git a/apps/uORB/topics/actuator_outputs.h b/apps/uORB/topics/actuator_outputs.h index 63441a0591..202f72b549 100644 --- a/apps/uORB/topics/actuator_outputs.h +++ b/apps/uORB/topics/actuator_outputs.h @@ -52,7 +52,7 @@ #define NUM_ACTUATOR_OUTPUTS 16 #define NUM_ACTUATOR_OUTPUT_GROUPS 4 /**< for sanity checking */ -struct actuator_output_s { +struct actuator_outputs_s { float output[NUM_ACTUATOR_OUTPUTS]; }; From 06e18f14e65dd493c4d5215874d8d3cf32440b59 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 22 Aug 2012 16:34:53 +0200 Subject: [PATCH 11/11] Ensure that sensor drivers are loaded --- ROMFS/scripts/rc.sensors | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ROMFS/scripts/rc.sensors b/ROMFS/scripts/rc.sensors index b6b4c65cb4..f913e82af9 100644 --- a/ROMFS/scripts/rc.sensors +++ b/ROMFS/scripts/rc.sensors @@ -7,8 +7,9 @@ # Start sensor drivers here. # -#ms5611 start -#mpu6000 start +ms5611 start +mpu6000 start +hmc5883 start # # Start the sensor collection task.