示例#1
0
int fimg2d4x_bitblt(struct fimg2d_control *ctrl)
{
	int ret = 0;
	enum addr_space addr_type;
	struct fimg2d_context *ctx;
	struct fimg2d_bltcmd *cmd;
	unsigned long *pgd;

	fimg2d_debug("%s : enter blitter\n", __func__);

	while (1) {
		cmd = fimg2d_get_command(ctrl);
		if (!cmd)
			break;

		ctx = cmd->ctx;
		ctx->state = CTX_READY;

#ifdef CONFIG_PM_RUNTIME
		if (fimg2d4x_get_clk_cnt(ctrl->clock) == false)
			fimg2d_err("2D clock is not set\n");
#endif

		addr_type = cmd->image[IDST].addr.type;

		atomic_set(&ctrl->busy, 1);
		perf_start(cmd, PERF_SFR);
		ret = ctrl->configure(ctrl, cmd);
		perf_end(cmd, PERF_SFR);
		if (IS_ERR_VALUE(ret)) {
			fimg2d_err("failed to configure\n");
			ctx->state = CTX_ERROR;
			goto fail_n_del;
		}

		ctx->vma_lock = vma_lock_mapping(ctx->mm, prefbuf, MAX_IMAGES - 1);

		if (fimg2d_check_pgd(ctx->mm, cmd)) {
			ret = -EFAULT;
			goto fail_n_unmap;
		}

		if (addr_type == ADDR_USER || addr_type == ADDR_USER_CONTIG) {
			if (!ctx->mm || !ctx->mm->pgd) {
				atomic_set(&ctrl->busy, 0);
				fimg2d_err("ctx->mm:0x%p or ctx->mm->pgd:0x%p\n",
					       ctx->mm,
					       (ctx->mm) ? ctx->mm->pgd : NULL);
				ret = -EPERM;
				goto fail_n_unmap;
			}
			pgd = (unsigned long *)ctx->mm->pgd;
#ifdef FIMG2D_IOVMM_PAGETABLE
			if (iovmm_activate(ctrl->dev)) {
				fimg2d_err("failed to iovmm activate\n");
				ret = -EPERM;
				goto fail_n_unmap;
			}
#else
			if (exynos_sysmmu_enable(ctrl->dev,
					(unsigned long)virt_to_phys(pgd))) {
				fimg2d_err("failed to sysmme enable\n");
				ret = -EPERM;
				goto fail_n_unmap;
			}
#endif
			fimg2d_debug("%s : sysmmu enable: pgd %p ctx %p seq_no(%u)\n",
				__func__, pgd, ctx, cmd->blt.seq_no);

			//exynos_sysmmu_set_pbuf(ctrl->dev, nbufs, prefbuf);
			fimg2d_debug("%s : set smmu prefbuf\n", __func__);
		}

		fimg2d4x_pre_bitblt(ctrl, cmd);

		perf_start(cmd, PERF_BLIT);
		/* start blit */
		fimg2d_debug("%s : start blit\n", __func__);
		ctrl->run(ctrl);
		ret = fimg2d4x_blit_wait(ctrl, cmd);
		perf_end(cmd, PERF_BLIT);

#ifdef FIMG2D_IOVMM_PAGETABLE
		if (addr_type == ADDR_USER || addr_type == ADDR_USER_CONTIG)
			iovmm_deactivate(ctrl->dev);
#else
		if (addr_type == ADDR_USER || addr_type == ADDR_USER_CONTIG)
			exynos_sysmmu_disable(ctrl->dev);
#endif

fail_n_unmap:
		perf_start(cmd, PERF_UNMAP);
		if (addr_type == ADDR_USER || addr_type == ADDR_USER_CONTIG) {
			fimg2d4x_cleanup_pgtable(ctrl, cmd, ISRC, true);
			fimg2d4x_cleanup_pgtable(ctrl, cmd, IMSK, false);
			fimg2d4x_cleanup_pgtable(ctrl, cmd, IDST, true);
			fimg2d_debug("sysmmu disable\n");
		}
		perf_end(cmd, PERF_UNMAP);
fail_n_del:
		vma_unlock_mapping(ctx->vma_lock);
		fimg2d_del_command(ctrl, cmd);
	}

	fimg2d_debug("%s : exit blitter\n", __func__);

	return ret;
}
示例#2
0
void
controls_tick() {

	/*
	 * Gather R/C control inputs from supported sources.
	 *
	 * Note that if you're silly enough to connect more than
	 * one control input source, they're going to fight each
	 * other.  Don't do that.
	 */

	/* receive signal strenght indicator (RSSI). 0 = no connection, 255: perfect connection */
	uint16_t rssi = 0;

#ifdef ADC_RSSI
	if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
		unsigned counts = adc_measure(ADC_RSSI);
		if (counts != 0xffff) {
			/* use 1:1 scaling on 3.3V ADC input */
			unsigned mV = counts * 3300 / 4096;

			/* scale to 0..253 */
			rssi = mV / 13;
		}
	}
#endif

	perf_begin(c_gather_dsm);
	uint16_t temp_count = r_raw_rc_count;
	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
	if (dsm_updated) {
		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
		r_raw_rc_count = temp_count & 0x7fff;
		if (temp_count & 0x8000)
			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
		else
			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;

		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);

	}
	perf_end(c_gather_dsm);

	perf_begin(c_gather_sbus);

	bool sbus_status = (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_SBUS);

	bool sbus_failsafe, sbus_frame_drop;
	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);

	if (sbus_updated) {
		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;

		rssi = 255;

		if (sbus_frame_drop) {
			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
			rssi = 100;
		} else {
			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
		}

		if (sbus_failsafe) {
			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
			rssi = 0;
		} else {
			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
		}

	}

	perf_end(c_gather_sbus);

	/*
	 * XXX each S.bus frame will cause a PPM decoder interrupt
	 * storm (lots of edges).  It might be sensible to actually
	 * disable the PPM decoder completely if we have S.bus signal.
	 */
	perf_begin(c_gather_ppm);
	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
	if (ppm_updated) {

		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
	}
	perf_end(c_gather_ppm);

	/* limit number of channels to allowable data size */
	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;

	/* store RSSI */
	r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;

	/*
	 * In some cases we may have received a frame, but input has still
	 * been lost.
	 */
	bool rc_input_lost = false;

	/*
	 * If we received a new frame from any of the RC sources, process it.
	 */
	if (dsm_updated || sbus_updated || ppm_updated) {

		/* record a bitmask of channels assigned */
		unsigned assigned_channels = 0;

		/* update RC-received timestamp */
		system_state.rc_channels_timestamp_received = hrt_absolute_time();

		/* do not command anything in failsafe, kick in the RC loss counter */
		if (!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {

			/* update RC-received timestamp */
			system_state.rc_channels_timestamp_valid = system_state.rc_channels_timestamp_received;

			/* map raw inputs to mapped inputs */
			/* XXX mapping should be atomic relative to protocol */
			for (unsigned i = 0; i < r_raw_rc_count; i++) {

				/* map the input channel */
				uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE];

				if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_ENABLED) {

					uint16_t raw = r_raw_rc_values[i];

					int16_t scaled;

					/*
					 * 1) Constrain to min/max values, as later processing depends on bounds.
					 */
					if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
						raw = conf[PX4IO_P_RC_CONFIG_MIN];
					if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
						raw = conf[PX4IO_P_RC_CONFIG_MAX];

					/*
					 * 2) Scale around the mid point differently for lower and upper range.
					 *
					 * This is necessary as they don't share the same endpoints and slope.
					 *
					 * First normalize to 0..1 range with correct sign (below or above center),
					 * then scale to 20000 range (if center is an actual center, -10000..10000,
					 * if parameters only support half range, scale to 10000 range, e.g. if
					 * center == min 0..10000, if center == max -10000..0).
					 *
					 * As the min and max bounds were enforced in step 1), division by zero
					 * cannot occur, as for the case of center == min or center == max the if
					 * statement is mutually exclusive with the arithmetic NaN case.
					 *
					 * DO NOT REMOVE OR ALTER STEP 1!
					 */
					if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));

					} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
						scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));

					} else {
						/* in the configured dead zone, output zero */
						scaled = 0;
					}

					/* invert channel if requested */
					if (conf[PX4IO_P_RC_CONFIG_OPTIONS] & PX4IO_P_RC_CONFIG_OPTIONS_REVERSE)
						scaled = -scaled;

					/* and update the scaled/mapped version */
					unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
					if (mapped < PX4IO_CONTROL_CHANNELS) {

						/* invert channel if pitch - pulling the lever down means pitching up by convention */
						if (mapped == 1) /* roll, pitch, yaw, throttle, override is the standard order */
							scaled = -scaled;

						r_rc_values[mapped] = SIGNED_TO_REG(scaled);
						assigned_channels |= (1 << mapped);

					}
				}
			}

			/* set un-assigned controls to zero */
			for (unsigned i = 0; i < PX4IO_CONTROL_CHANNELS; i++) {
				if (!(assigned_channels & (1 << i)))
					r_rc_values[i] = 0;
			}

			/* set RC OK flag, as we got an update */
			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;

			/* if we have enough channels (5) to control the vehicle, the mapping is ok */
			if (assigned_channels > 4) {
				r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
			} else {
				r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
			}
		}

		/*
		 * Export the valid channel bitmap
		 */
		r_rc_valid = assigned_channels;
	}

	/*
	 * If we haven't seen any new control data in 200ms, assume we
	 * have lost input.
	 */
	if (hrt_elapsed_time(&system_state.rc_channels_timestamp_received) > 200000) {
		rc_input_lost = true;

		/* clear the input-kind flags here */
		r_status_flags &= ~(
			PX4IO_P_STATUS_FLAGS_RC_PPM |
			PX4IO_P_STATUS_FLAGS_RC_DSM |
			PX4IO_P_STATUS_FLAGS_RC_SBUS);

	}

	/*
	 * Handle losing RC input
	 */

	/* this kicks in if the receiver is gone or the system went to failsafe */
	if (rc_input_lost || (r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
		/* Clear the RC input status flag, clear manual override flag */
		r_status_flags &= ~(
			PX4IO_P_STATUS_FLAGS_OVERRIDE |
			PX4IO_P_STATUS_FLAGS_RC_OK);

		/* Mark all channels as invalid, as we just lost the RX */
		r_rc_valid = 0;

		/* Set the RC_LOST alarm */
		r_status_alarms |= PX4IO_P_STATUS_ALARMS_RC_LOST;
	}

	/* this kicks in if the receiver is completely gone */
	if (rc_input_lost) {

		/* Set channel count to zero */
		r_raw_rc_count = 0;
	}

	/*
	 * Check for manual override.
	 *
	 * The PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK flag must be set, and we
	 * must have R/C input.
	 * Override is enabled if either the hardcoded channel / value combination
	 * is selected, or the AP has requested it.
	 */
	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK)) {

		bool override = false;

		/*
		 * Check mapped channel 5 (can be any remote channel,
		 * depends on RC_MAP_OVER parameter);
		 * If the value is 'high' then the pilot has
		 * requested override.
		 *
		 */
		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(r_rc_values[4]) < RC_CHANNEL_LOW_THRESH))
			override = true;

		if (override) {

			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;

			/* mix new RC input control values to servos */
			if (dsm_updated || sbus_updated || ppm_updated)
				mixer_tick();

		} else {
示例#3
0
void
BMI160::measure()
{
	if (hrt_absolute_time() < _reset_wait) {
		// we're waiting for a reset to complete
		return;
	}

	struct BMIReport bmi_report;

	struct Report {
		int16_t		accel_x;
		int16_t		accel_y;
		int16_t		accel_z;
		int16_t		temp;
		int16_t		gyro_x;
		int16_t		gyro_y;
		int16_t		gyro_z;
	} report;

	/* start measuring */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the BMI160 in one pass.
	 */
	bmi_report.cmd = BMIREG_GYR_X_L | DIR_READ;

	set_frequency(BMI160_LOW_BUS_SPEED);

	uint8_t		status = read_reg(BMIREG_STATUS, BMI160_LOW_BUS_SPEED);

	if (OK != transfer((uint8_t *)&bmi_report, ((uint8_t *)&bmi_report), sizeof(bmi_report))) {
		return;
	}

	check_registers();

	if ((!(status && (0x80)))  && (!(status && (0x04)))) {
		perf_end(_sample_perf);
		perf_count(_duplicates);
		_got_duplicate = true;
		return;
	}

	_last_accel[0] = bmi_report.accel_x;
	_last_accel[1] = bmi_report.accel_y;
	_last_accel[2] = bmi_report.accel_z;
	_got_duplicate = false;

	uint8_t temp_l = read_reg(BMIREG_TEMP_0, BMI160_LOW_BUS_SPEED);
	uint8_t temp_h = read_reg(BMIREG_TEMP_1, BMI160_LOW_BUS_SPEED);

	report.temp = ((temp_h << 8) + temp_l);

	report.accel_x = bmi_report.accel_x;
	report.accel_y = bmi_report.accel_y;
	report.accel_z = bmi_report.accel_z;

	report.gyro_x = bmi_report.gyro_x;
	report.gyro_y = bmi_report.gyro_y;
	report.gyro_z = bmi_report.gyro_z;

	if (report.accel_x == 0 &&
	    report.accel_y == 0 &&
	    report.accel_z == 0 &&
	    report.temp == 0 &&
	    report.gyro_x == 0 &&
	    report.gyro_y == 0 &&
	    report.gyro_z == 0) {
		// all zero data - probably a SPI bus error
		perf_count(_bad_transfers);
		perf_end(_sample_perf);
		// note that we don't call reset() here as a reset()
		// costs 20ms with interrupts disabled. That means if
		// the bmi160 does go bad it would cause a FMU failure,
		// regardless of whether another sensor is available,
		return;
	}

	perf_count(_good_transfers);

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using
		// the sensor again. We still increment
		// _good_transfers, but don't return any data yet
		_register_wait--;
		return;
	}

	/*
	 * Report buffers.
	 */
	accel_report		arb;
	gyro_report		grb;

	/*
	 * Adjust and scale results to m/s^2.
	 */
	grb.timestamp = arb.timestamp = hrt_absolute_time();

	// report the error count as the sum of the number of bad
	// transfers and bad register reads. This allows the higher
	// level code to decide if it should use this sensor based on
	// whether it has had failures
	grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */


	/* NOTE: Axes have been swapped to match the board a few lines above. */

	arb.x_raw = report.accel_x;
	arb.y_raw = report.accel_y;
	arb.z_raw = report.accel_z;

	float xraw_f = report.accel_x;
	float yraw_f = report.accel_y;
	float zraw_f = report.accel_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
	float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
	float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

	arb.x = _accel_filter_x.apply(x_in_new);
	arb.y = _accel_filter_y.apply(y_in_new);
	arb.z = _accel_filter_z.apply(z_in_new);

	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
	math::Vector<3> aval_integrated;

	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
	arb.x_integral = aval_integrated(0);
	arb.y_integral = aval_integrated(1);
	arb.z_integral = aval_integrated(2);

	arb.scaling = _accel_range_scale;
	arb.range_m_s2 = _accel_range_m_s2;

	_last_temperature = 23 + report.temp * 1.0f / 512.0f;

	arb.temperature_raw = report.temp;
	arb.temperature = _last_temperature;

	grb.x_raw = report.gyro_x;
	grb.y_raw = report.gyro_y;
	grb.z_raw = report.gyro_z;

	xraw_f = report.gyro_x;
	yraw_f = report.gyro_y;
	zraw_f = report.gyro_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
	grb.z = _gyro_filter_z.apply(z_gyro_in_new);

	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
	math::Vector<3> gval_integrated;

	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
	grb.x_integral = gval_integrated(0);
	grb.y_integral = gval_integrated(1);
	grb.z_integral = gval_integrated(2);

	grb.scaling = _gyro_range_scale;
	grb.range_rad_s = _gyro_range_rad_s;

	grb.temperature_raw = report.temp;
	grb.temperature = _last_temperature;

	_accel_reports->force(&arb);
	_gyro_reports->force(&grb);

	/* notify anyone waiting for data */
	if (accel_notify) {
		poll_notify(POLLIN);
	}

	if (gyro_notify) {
		_gyro->parent_poll_notify();
	}

	if (accel_notify && !(_pub_blocked)) {
		/* log the time of this report */
		perf_begin(_controller_latency_perf);
		/* publish it */
		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
	}

	if (gyro_notify && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
	}

	/* stop measuring */
	perf_end(_sample_perf);
}
示例#4
0
void DataFlash_File::_io_timer(void)
{
    uint16_t _tail;
    if (_write_fd == -1 || !_initialised) {
        return;
    }

    uint16_t nbytes = BUF_AVAILABLE(_writebuf);
    if (nbytes == 0) {
        return;
    }
    uint32_t tnow = hal.scheduler->micros();
    if (nbytes < _writebuf_chunk && 
        tnow - _last_write_time < 2000000UL) {
        // write in 512 byte chunks, but always write at least once
        // per 2 seconds if data is available
        return;
    }

    perf_begin(_perf_write);

    _last_write_time = tnow;
    if (nbytes > _writebuf_chunk) {
        // be kind to the FAT PX4 filesystem
        nbytes = _writebuf_chunk;
    }
    if (_writebuf_head > _tail) {
        // only write to the end of the buffer
        nbytes = min(nbytes, _writebuf_size - _writebuf_head);
    }

    // try to align writes on a 512 byte boundary to avoid filesystem
    // reads
    if ((nbytes + _write_offset) % 512 != 0) {
        uint32_t ofs = (nbytes + _write_offset) % 512;
        if (ofs < nbytes) {
            nbytes -= ofs;
        }
    }

    assert(_writebuf_head+nbytes <= _writebuf_size);
    ssize_t nwritten = ::write(_write_fd, &_writebuf[_writebuf_head], nbytes);
    if (nwritten <= 0) {
        perf_count(_perf_errors);
        close(_write_fd);
        _write_fd = -1;
        _initialised = false;
    } else {
        _write_offset += nwritten;
        /*
          the best strategy for minimising corruption on microSD cards
          seems to be to write in 4k chunks and fsync the file on each
          chunk, ensuring the directory entry is updated after each
          write.
         */
#if CONFIG_HAL_BOARD != HAL_BOARD_AVR_SITL
        ::fsync(_write_fd);
#endif
        BUF_ADVANCEHEAD(_writebuf, nwritten);
    }
    perf_end(_perf_write);
}
示例#5
0
void
MulticopterAttitudeControl::task_main()
{

    /*
     * do subscriptions
     */
    _v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
    _v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
    _v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
    _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
    _params_sub = orb_subscribe(ORB_ID(parameter_update));
    _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
    _armed_sub = orb_subscribe(ORB_ID(actuator_armed));
    _vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
    _motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));

    /* initialize parameters cache */
    parameters_update();

    /* wakeup source: vehicle attitude */
    px4_pollfd_struct_t fds[1];

    fds[0].fd = _v_att_sub;
    fds[0].events = POLLIN;

    while (!_task_should_exit) {

        /* wait for up to 100ms for data */
        int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

        /* timed out - periodic check for _task_should_exit */
        if (pret == 0)
            continue;

        /* this is undesirable but not much we can do - might want to flag unhappy status */
        if (pret < 0) {
            warn("poll error %d, %d", pret, errno);
            /* sleep a bit before next try */
            usleep(100000);
            continue;
        }

        perf_begin(_loop_perf);

        /* run controller on attitude changes */
        if (fds[0].revents & POLLIN) {
            static uint64_t last_run = 0;
            float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
            last_run = hrt_absolute_time();

            /* guard against too small (< 2ms) and too large (> 20ms) dt's */
            if (dt < 0.002f) {
                dt = 0.002f;

            } else if (dt > 0.02f) {
                dt = 0.02f;
            }

            /* copy attitude topic */
            orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);

            /* check for updates in other topics */
            parameter_update_poll();
            vehicle_control_mode_poll();
            arming_status_poll();
            vehicle_manual_poll();
            vehicle_status_poll();
            vehicle_motor_limits_poll();

            if (_v_control_mode.flag_control_attitude_enabled) {
                control_attitude(dt);

                /* publish attitude rates setpoint */
                _v_rates_sp.roll = _rates_sp(0);
                _v_rates_sp.pitch = _rates_sp(1);
                _v_rates_sp.yaw = _rates_sp(2);
                _v_rates_sp.thrust = _thrust_sp;
                _v_rates_sp.timestamp = hrt_absolute_time();

                if (_v_rates_sp_pub != nullptr) {
                    orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

                } else if (_rates_sp_id) {
                    _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
                }

            } else {
                /* attitude controller disabled, poll rates setpoint topic */
                if (_v_control_mode.flag_control_manual_enabled) {
                    /* manual rates control - ACRO mode */
                    _rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
                    _thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);

                    /* publish attitude rates setpoint */
                    _v_rates_sp.roll = _rates_sp(0);
                    _v_rates_sp.pitch = _rates_sp(1);
                    _v_rates_sp.yaw = _rates_sp(2);
                    _v_rates_sp.thrust = _thrust_sp;
                    _v_rates_sp.timestamp = hrt_absolute_time();

                    if (_v_rates_sp_pub != nullptr) {
                        orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

                    } else if (_rates_sp_id) {
                        _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
                    }

                } else {
                    /* attitude controller disabled, poll rates setpoint topic */
                    vehicle_rates_setpoint_poll();
                    _rates_sp(0) = _v_rates_sp.roll;
                    _rates_sp(1) = _v_rates_sp.pitch;
                    _rates_sp(2) = _v_rates_sp.yaw;
                    _thrust_sp = _v_rates_sp.thrust;
                }
            }

            if (_v_control_mode.flag_control_rates_enabled) {
                control_attitude_rates(dt);

                /* publish actuator controls */
                _actuators.control[0] = (PX4_ISFINITE(_att_control(0))) ? _att_control(0) : 0.0f;
                _actuators.control[1] = (PX4_ISFINITE(_att_control(1))) ? _att_control(1) : 0.0f;
                _actuators.control[2] = (PX4_ISFINITE(_att_control(2))) ? _att_control(2) : 0.0f;
                _actuators.control[3] = (PX4_ISFINITE(_thrust_sp)) ? _thrust_sp : 0.0f;
                _actuators.timestamp = hrt_absolute_time();
                _actuators.timestamp_sample = _v_att.timestamp;

                _controller_status.roll_rate_integ = _rates_int(0);
                _controller_status.pitch_rate_integ = _rates_int(1);
                _controller_status.yaw_rate_integ = _rates_int(2);
                _controller_status.timestamp = hrt_absolute_time();

                if (!_actuators_0_circuit_breaker_enabled) {
                    if (_actuators_0_pub != nullptr) {
                        orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
                        perf_end(_controller_latency_perf);

                    } else if (_actuators_id) {
                        _actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
                    }

                }

                /* publish controller status */
                if(_controller_status_pub != nullptr) {
                    orb_publish(ORB_ID(mc_att_ctrl_status),_controller_status_pub, &_controller_status);
                } else {
                    _controller_status_pub = orb_advertise(ORB_ID(mc_att_ctrl_status), &_controller_status);
                }
            }
        }

        perf_end(_loop_perf);
    }

    _control_task = -1;
    return;
}
示例#6
0
int
TFMINI::collect()
{
	perf_begin(_sample_perf);

	/* clear buffer if last read was too long ago */
	int64_t read_elapsed = hrt_elapsed_time(&_last_read);

	/* the buffer for read chars is buflen minus null termination */
	char readbuf[sizeof(_linebuf)];
	unsigned readlen = sizeof(readbuf) - 1;

	int ret = 0;
	float distance_m = -1.0f;

	/* Check the number of bytes available in the buffer*/
	int bytes_available = 0;
	::ioctl(_fd, FIONREAD, (unsigned long)&bytes_available);

	if (!bytes_available) {
		return -EAGAIN;
	}

	/* parse entire buffer */
	do {
		/* read from the sensor (uart buffer) */
		ret = ::read(_fd, &readbuf[0], readlen);

		if (ret < 0) {
			PX4_ERR("read err: %d", ret);
			perf_count(_comms_errors);
			perf_end(_sample_perf);

			/* only throw an error if we time out */
			if (read_elapsed > (_conversion_interval * 2)) {
				/* flush anything in RX buffer */
				tcflush(_fd, TCIFLUSH);
				return ret;

			} else {
				return -EAGAIN;
			}
		}

		_last_read = hrt_absolute_time();

		/* parse buffer */
		for (int i = 0; i < ret; i++) {
			tfmini_parse(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m);
		}

		/* bytes left to parse */
		bytes_available -= ret;

	} while (bytes_available > 0);

	/* no valid measurement after parsing buffer */
	if (distance_m < 0.0f) {
		return -EAGAIN;
	}

	/* publish most recent valid measurement from buffer */
	distance_sensor_s report{};

	report.timestamp = hrt_absolute_time();
	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
	report.orientation = _rotation;
	report.current_distance = distance_m;
	report.min_distance = get_minimum_distance();
	report.max_distance = get_maximum_distance();
	report.covariance = 0.0f;
	report.signal_quality = -1;
	/* TODO: set proper ID */
	report.id = 0;

	/* publish it */
	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);

	_reports->force(&report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}
示例#7
0
void
FXAS21002C::measure()
{
	/* status register and data as read back from the device */

#pragma pack(push, 1)
	struct {
		uint8_t		cmd;
		uint8_t		status;
		int16_t		x;
		int16_t		y;
		int16_t		z;
	} raw_gyro_report;
#pragma pack(pop)

	struct gyro_report gyro_report;

	/* start the performance counter */
	perf_begin(_sample_perf);

	check_registers();

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using
		// the sensor again.
		_register_wait--;
		perf_end(_sample_perf);
		return;
	}

	/* fetch data from the sensor */
	memset(&raw_gyro_report, 0, sizeof(raw_gyro_report));
	raw_gyro_report.cmd = DIR_READ(FXAS21002C_STATUS);
	transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report));

	if (!(raw_gyro_report.status & DR_STATUS_ZYXDR)) {
		perf_end(_sample_perf);
		perf_count(_duplicates);
		return;
	}

	/*
	 * The TEMP register contains an 8-bit 2's complement temperature value with a range
	 * of –128 °C to +127 °C and a scaling of 1 °C/LSB. The temperature data is only
	 * compensated (factory trim values applied) when the device is operating in the Active
	 * mode and actively measuring the angular rate.
	 */

	if ((_read % _current_rate) == 0) {
		_last_temperature = read_reg(FXAS21002C_TEMP) * 1.0f;
		gyro_report.temperature = _last_temperature;
	}

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */

	gyro_report.timestamp = hrt_absolute_time();

	// report the error count as the number of bad
	// register reads. This allows the higher level
	// code to decide if it should use this sensor based on
	// whether it has had failures
	gyro_report.error_count = perf_event_count(_bad_registers);

	gyro_report.x_raw = swap16(raw_gyro_report.x);
	gyro_report.y_raw = swap16(raw_gyro_report.y);
	gyro_report.z_raw = swap16(raw_gyro_report.z);

	float xraw_f = gyro_report.x_raw;
	float yraw_f = gyro_report.y_raw;
	float zraw_f = gyro_report.z_raw;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	gyro_report.x = _gyro_filter_x.apply(x_in_new);
	gyro_report.y = _gyro_filter_y.apply(y_in_new);
	gyro_report.z = _gyro_filter_z.apply(z_in_new);

	math::Vector<3> gval(x_in_new, y_in_new, z_in_new);
	math::Vector<3> gval_integrated;

	bool gyro_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt);
	gyro_report.x_integral = gval_integrated(0);
	gyro_report.y_integral = gval_integrated(1);
	gyro_report.z_integral = gval_integrated(2);

	gyro_report.scaling = _gyro_range_scale;
	gyro_report.range_rad_s = _gyro_range_rad_s;

	/* return device ID */
	gyro_report.device_id = _device_id.devid;


	_reports->force(&gyro_report);

	/* notify anyone waiting for data */
	if (gyro_notify) {
		poll_notify(POLLIN);

		if (!(_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report);
		}
	}

	_read++;

	/* stop the perf counter */
	perf_end(_sample_perf);
}
示例#8
0
void
Navigator::task_main()
{
	/* inform about start */
	warnx("Initializing..");

	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
	_geofence.setMavlinkFd(_mavlink_fd);

	/* Try to load the geofence:
	 * if /fs/microsd/etc/geofence.txt load from this file
	 * else clear geofence data in datamanager */
	struct stat buffer;

	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
		warnx("Try to load geofence.txt");
		_geofence.loadFromFile(GEOFENCE_FILENAME);

	} else {
		mavlink_log_critical(_mavlink_fd, "#audio: No geofence file");
		if (_geofence.clearDm() > 0)
			warnx("Geofence cleared");
		else
			warnx("Could not clear geofence");
	}

	/* do subscriptions */
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));

	/* copy all topics first time */
	vehicle_status_update();
	vehicle_control_mode_update();
	global_position_update();
	gps_position_update();
	sensor_combined_update();
	home_position_update();
	navigation_capabilities_update();
	params_update();

	/* rate limit position and sensor updates to 50 Hz */
	orb_set_interval(_global_pos_sub, 20);
	orb_set_interval(_sensor_combined_sub, 20);

	hrt_abstime mavlink_open_time = 0;
	const hrt_abstime mavlink_open_interval = 500000;

	/* wakeup source(s) */
	struct pollfd fds[8];

	/* Setup of loop */
	fds[0].fd = _global_pos_sub;
	fds[0].events = POLLIN;
	fds[1].fd = _home_pos_sub;
	fds[1].events = POLLIN;
	fds[2].fd = _capabilities_sub;
	fds[2].events = POLLIN;
	fds[3].fd = _vstatus_sub;
	fds[3].events = POLLIN;
	fds[4].fd = _control_mode_sub;
	fds[4].events = POLLIN;
	fds[5].fd = _param_update_sub;
	fds[5].events = POLLIN;
	fds[6].fd = _sensor_combined_sub;
	fds[6].events = POLLIN;
	fds[7].fd = _gps_pos_sub;
	fds[7].events = POLLIN;

	while (!_task_should_exit) {

		/* wait for up to 100ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		if (pret == 0) {
			/* timed out - periodic check for _task_should_exit, etc. */
			continue;

		} else if (pret < 0) {
			/* this is undesirable but not much we can do - might want to flag unhappy status */
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);

		if (_mavlink_fd < 0 && hrt_absolute_time() > mavlink_open_time) {
			/* try to reopen the mavlink log device with specified interval */
			mavlink_open_time = hrt_abstime() + mavlink_open_interval;
			_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
		}

		static bool have_geofence_position_data = false;

		/* gps updated */
		if (fds[7].revents & POLLIN) {
			gps_position_update();
			if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
				have_geofence_position_data = true;
			}
		}

		/* sensors combined updated */
		if (fds[6].revents & POLLIN) {
			sensor_combined_update();
		}

		/* parameters updated */
		if (fds[5].revents & POLLIN) {
			params_update();
			updateParams();
		}

		/* vehicle control mode updated */
		if (fds[4].revents & POLLIN) {
			vehicle_control_mode_update();
		}

		/* vehicle status updated */
		if (fds[3].revents & POLLIN) {
			vehicle_status_update();
		}

		/* navigation capabilities updated */
		if (fds[2].revents & POLLIN) {
			navigation_capabilities_update();
		}

		/* home position updated */
		if (fds[1].revents & POLLIN) {
			home_position_update();
		}

		/* global position updated */
		if (fds[0].revents & POLLIN) {
			global_position_update();
			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
				have_geofence_position_data = true;
			}
		}

		/* Check geofence violation */
		static hrt_abstime last_geofence_check = 0;
		if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) {
			bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter);
			last_geofence_check = hrt_absolute_time();
			have_geofence_position_data = false;
			if (!inside) {
				/* inform other apps via the mission result */
				_mission_result.geofence_violated = true;
				publish_mission_result();

				/* Issue a warning about the geofence violation once */
				if (!_geofence_violation_warning_sent) {
					mavlink_log_critical(_mavlink_fd, "#audio: Geofence violation");
					_geofence_violation_warning_sent = true;
				}
			} else {
				/* inform other apps via the mission result */
				_mission_result.geofence_violated = false;
				publish_mission_result();
				/* Reset the _geofence_violation_warning_sent field */
				_geofence_violation_warning_sent = false;
			}
		}

		/* Do stuff according to navigation state set by commander */
		switch (_vstatus.nav_state) {
			case NAVIGATION_STATE_MANUAL:
			case NAVIGATION_STATE_ACRO:
			case NAVIGATION_STATE_ALTCTL:
			case NAVIGATION_STATE_POSCTL:
			case NAVIGATION_STATE_LAND:
			case NAVIGATION_STATE_TERMINATION:
			case NAVIGATION_STATE_OFFBOARD:
				_navigation_mode = nullptr;
				_can_loiter_at_sp = false;
				break;
			case NAVIGATION_STATE_AUTO_MISSION:
				_navigation_mode = &_mission;
				break;
			case NAVIGATION_STATE_AUTO_LOITER:
				_navigation_mode = &_loiter;
				break;
			case NAVIGATION_STATE_AUTO_RCRECOVER:
				if (_param_rcloss_obc.get() != 0) {
					_navigation_mode = &_rcLoss;
				} else {
					_navigation_mode = &_rtl;
				}
				break;
			case NAVIGATION_STATE_AUTO_RTL:
					_navigation_mode = &_rtl;
				break;
			case NAVIGATION_STATE_AUTO_RTGS:
				/* Use complex data link loss mode only when enabled via param
				* otherwise use rtl */
				if (_param_datalinkloss_obc.get() != 0) {
					_navigation_mode = &_dataLinkLoss;
				} else {
					_navigation_mode = &_rtl;
				}
				break;
			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
				_navigation_mode = &_engineFailure;
				break;
			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
				_navigation_mode = &_gpsFailure;
				break;
			default:
				_navigation_mode = nullptr;
				_can_loiter_at_sp = false;
				break;
		}

		/* iterate through navigation modes and set active/inactive for each */
		for(unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
		}

		/* if nothing is running, set position setpoint triplet invalid */
		if (_navigation_mode == nullptr) {
			// TODO publish empty sp only once
			_pos_sp_triplet.previous.valid = false;
			_pos_sp_triplet.current.valid = false;
			_pos_sp_triplet.next.valid = false;
			_pos_sp_triplet_updated = true;
		}

		if (_pos_sp_triplet_updated) {
			publish_position_setpoint_triplet();
			_pos_sp_triplet_updated = false;
		}

		perf_end(_loop_perf);
	}
	warnx("exiting.");

	_navigator_task = -1;
	_exit(0);
}
示例#9
0
void
Navigator::task_main()
{
	bool have_geofence_position_data = false;

	/* Try to load the geofence:
	 * if /fs/microsd/etc/geofence.txt load from this file */
	struct stat buffer;

	if (stat(GEOFENCE_FILENAME, &buffer) == 0) {
		PX4_INFO("Loading geofence from %s", GEOFENCE_FILENAME);
		_geofence.loadFromFile(GEOFENCE_FILENAME);
	}

	/* do subscriptions */
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
	_gps_pos_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	_fw_pos_ctrl_status_sub = orb_subscribe(ORB_ID(fw_pos_ctrl_status));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
	_home_pos_sub = orb_subscribe(ORB_ID(home_position));
	_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
	_offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
	_param_update_sub = orb_subscribe(ORB_ID(parameter_update));
	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
	_traffic_sub = orb_subscribe(ORB_ID(transponder_report));

	/* copy all topics first time */
	vehicle_status_update();
	vehicle_land_detected_update();
	global_position_update();
	local_position_update();
	gps_position_update();
	sensor_combined_update();
	home_position_update(true);
	fw_pos_ctrl_status_update(true);
	params_update();

	/* wakeup source(s) */
	px4_pollfd_struct_t fds[1] = {};

	/* Setup of loop */
	fds[0].fd = _local_pos_sub;
	fds[0].events = POLLIN;

	/* rate-limit position subscription to 20 Hz / 50 ms */
	orb_set_interval(_local_pos_sub, 50);

	while (!_task_should_exit) {

		/* wait for up to 1000ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 1000);

		if (pret == 0) {
			/* Let the loop run anyway, don't do `continue` here. */

		} else if (pret < 0) {
			/* this is undesirable but not much we can do - might want to flag unhappy status */
			PX4_ERR("poll error %d, %d", pret, errno);
			usleep(10000);
			continue;

		} else {
			if (fds[0].revents & POLLIN) {
				/* success, local pos is available */
				local_position_update();
			}
		}

		perf_begin(_loop_perf);

		bool updated;

		/* gps updated */
		orb_check(_gps_pos_sub, &updated);

		if (updated) {
			gps_position_update();

			if (_geofence.getSource() == Geofence::GF_SOURCE_GPS) {
				have_geofence_position_data = true;
			}
		}

		/* global position updated */
		orb_check(_global_pos_sub, &updated);

		if (updated) {
			global_position_update();

			if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) {
				have_geofence_position_data = true;
			}
		}

		/* sensors combined updated */
		orb_check(_sensor_combined_sub, &updated);

		if (updated) {
			sensor_combined_update();
		}

		/* parameters updated */
		orb_check(_param_update_sub, &updated);

		if (updated) {
			params_update();
		}

		/* vehicle status updated */
		orb_check(_vstatus_sub, &updated);

		if (updated) {
			vehicle_status_update();
		}

		/* vehicle land detected updated */
		orb_check(_land_detected_sub, &updated);

		if (updated) {
			vehicle_land_detected_update();
		}

		/* navigation capabilities updated */
		orb_check(_fw_pos_ctrl_status_sub, &updated);

		if (updated) {
			fw_pos_ctrl_status_update();
		}

		/* home position updated */
		orb_check(_home_pos_sub, &updated);

		if (updated) {
			home_position_update();
		}

		/* vehicle_command updated */
		orb_check(_vehicle_command_sub, &updated);

		if (updated) {
			vehicle_command_s cmd;
			orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_GO_AROUND) {

				// DO_GO_AROUND is currently handled by the position controller (unacknowledged)
				// TODO: move DO_GO_AROUND handling to navigator
				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_REPOSITION) {

				position_setpoint_triplet_s *rep = get_reposition_triplet();
				position_setpoint_triplet_s *curr = get_position_setpoint_triplet();

				// store current position as previous position and goal as next
				rep->previous.yaw = get_global_position()->yaw;
				rep->previous.lat = get_global_position()->lat;
				rep->previous.lon = get_global_position()->lon;
				rep->previous.alt = get_global_position()->alt;

				rep->current.loiter_radius = get_loiter_radius();
				rep->current.loiter_direction = 1;
				rep->current.type = position_setpoint_s::SETPOINT_TYPE_LOITER;
				rep->current.cruising_speed = get_cruising_speed();
				rep->current.cruising_throttle = get_cruising_throttle();

				// Go on and check which changes had been requested
				if (PX4_ISFINITE(cmd.param4)) {
					rep->current.yaw = cmd.param4;

				} else {
					rep->current.yaw = NAN;
				}

				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {

					// Position change with optional altitude change
					rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
					rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;

					if (PX4_ISFINITE(cmd.param7)) {
						rep->current.alt = cmd.param7;

					} else {
						rep->current.alt = get_global_position()->alt;
					}

				} else if (PX4_ISFINITE(cmd.param7) && curr->current.valid
					   && PX4_ISFINITE(curr->current.lat)
					   && PX4_ISFINITE(curr->current.lon)) {

					// Altitude without position change
					rep->current.lat = curr->current.lat;
					rep->current.lon = curr->current.lon;
					rep->current.alt = cmd.param7;

				} else {
					// All three set to NaN - hold in current position
					rep->current.lat = get_global_position()->lat;
					rep->current.lon = get_global_position()->lon;
					rep->current.alt = get_global_position()->alt;
				}

				rep->previous.valid = true;
				rep->current.valid = true;
				rep->next.valid = false;

				// CMD_DO_REPOSITION is acknowledged by commander

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_TAKEOFF) {
				position_setpoint_triplet_s *rep = get_takeoff_triplet();

				// store current position as previous position and goal as next
				rep->previous.yaw = get_global_position()->yaw;
				rep->previous.lat = get_global_position()->lat;
				rep->previous.lon = get_global_position()->lon;
				rep->previous.alt = get_global_position()->alt;

				rep->current.loiter_radius = get_loiter_radius();
				rep->current.loiter_direction = 1;
				rep->current.type = position_setpoint_s::SETPOINT_TYPE_TAKEOFF;

				if (home_position_valid()) {
					rep->current.yaw = cmd.param4;
					rep->previous.valid = true;

				} else {
					rep->current.yaw = get_local_position()->yaw;
					rep->previous.valid = false;
				}

				if (PX4_ISFINITE(cmd.param5) && PX4_ISFINITE(cmd.param6)) {
					rep->current.lat = (cmd.param5 < 1000) ? cmd.param5 : cmd.param5 / (double)1e7;
					rep->current.lon = (cmd.param6 < 1000) ? cmd.param6 : cmd.param6 / (double)1e7;

				} else {
					// If one of them is non-finite, reset both
					rep->current.lat = NAN;
					rep->current.lon = NAN;
				}

				rep->current.alt = cmd.param7;

				rep->current.valid = true;
				rep->next.valid = false;

				// CMD_NAV_TAKEOFF is acknowledged by commander

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_LAND_START) {

				/* find NAV_CMD_DO_LAND_START in the mission and
				 * use MAV_CMD_MISSION_START to start the mission there
				 */
				int land_start = _mission.find_offboard_land_start();

				if (land_start != -1) {
					vehicle_command_s vcmd = {};
					vcmd.command = vehicle_command_s::VEHICLE_CMD_MISSION_START;
					vcmd.param1 = land_start;
					publish_vehicle_cmd(&vcmd);

				} else {
					PX4_WARN("planned landing not available");
				}

				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_MISSION_START) {
				if (get_mission_result()->valid &&
				    PX4_ISFINITE(cmd.param1) && (cmd.param1 >= 0) && (cmd.param1 < _mission_result.seq_total)) {

					_mission.set_current_offboard_mission_index(cmd.param1);
				}

				// CMD_MISSION_START is acknowledged by commander

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
				if (cmd.param2 > FLT_EPSILON) {
					// XXX not differentiating ground and airspeed yet
					set_cruising_speed(cmd.param2);

				} else {
					set_cruising_speed();

					/* if no speed target was given try to set throttle */
					if (cmd.param3 > FLT_EPSILON) {
						set_cruising_throttle(cmd.param3 / 100);

					} else {
						set_cruising_throttle();
					}
				}

				// TODO: handle responses for supported DO_CHANGE_SPEED options?
				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_SET_ROI
				   || cmd.command == vehicle_command_s::VEHICLE_CMD_NAV_ROI) {
				_vroi = {};
				_vroi.mode = cmd.param1;

				switch (_vroi.mode) {
				case vehicle_command_s::VEHICLE_ROI_NONE:
					break;

				case vehicle_command_s::VEHICLE_ROI_WPNEXT:
					// TODO: implement point toward next MISSION
					break;

				case vehicle_command_s::VEHICLE_ROI_WPINDEX:
					_vroi.mission_seq = cmd.param2;
					break;

				case vehicle_command_s::VEHICLE_ROI_LOCATION:
					_vroi.lat = cmd.param5;
					_vroi.lon = cmd.param6;
					_vroi.alt = cmd.param7;
					break;

				case vehicle_command_s::VEHICLE_ROI_TARGET:
					_vroi.target_seq = cmd.param2;
					break;

				default:
					_vroi.mode = vehicle_command_s::VEHICLE_ROI_NONE;
					break;
				}

				_vroi.timestamp = hrt_absolute_time();

				if (_vehicle_roi_pub != nullptr) {
					orb_publish(ORB_ID(vehicle_roi), _vehicle_roi_pub, &_vroi);

				} else {
					_vehicle_roi_pub = orb_advertise(ORB_ID(vehicle_roi), &_vroi);
				}

				publish_vehicle_command_ack(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
			}
		}

		/* Check for traffic */
		check_traffic();

		/* Check geofence violation */
		static hrt_abstime last_geofence_check = 0;

		if (have_geofence_position_data &&
		    (_geofence.getGeofenceAction() != geofence_result_s::GF_ACTION_NONE) &&
		    (hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL)) {

			bool inside = _geofence.check(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter, _home_pos,
						      home_position_valid());
			last_geofence_check = hrt_absolute_time();
			have_geofence_position_data = false;

			_geofence_result.timestamp = hrt_absolute_time();
			_geofence_result.geofence_action = _geofence.getGeofenceAction();
			_geofence_result.home_required = _geofence.isHomeRequired();

			if (!inside) {
				/* inform other apps via the mission result */
				_geofence_result.geofence_violated = true;

				/* Issue a warning about the geofence violation once */
				if (!_geofence_violation_warning_sent) {
					mavlink_log_critical(&_mavlink_log_pub, "Geofence violation");
					_geofence_violation_warning_sent = true;
				}

			} else {
				/* inform other apps via the mission result */
				_geofence_result.geofence_violated = false;

				/* Reset the _geofence_violation_warning_sent field */
				_geofence_violation_warning_sent = false;
			}

			publish_geofence_result();
		}

		/* Do stuff according to navigation state set by commander */
		NavigatorMode *navigation_mode_new{nullptr};

		switch (_vstatus.nav_state) {
		case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_mission;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_loiter;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_rcLoss;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_rtl;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_takeoff;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_LAND:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_land;
			break;

		case vehicle_status_s::NAVIGATION_STATE_DESCEND:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_land;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_dataLinkLoss;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_engineFailure;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_gpsFailure;
			break;

		case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = &_follow_target;
			break;

		case vehicle_status_s::NAVIGATION_STATE_MANUAL:
		case vehicle_status_s::NAVIGATION_STATE_ACRO:
		case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
		case vehicle_status_s::NAVIGATION_STATE_POSCTL:
		case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
		case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
		case vehicle_status_s::NAVIGATION_STATE_STAB:
		default:
			_pos_sp_triplet_published_invalid_once = false;
			navigation_mode_new = nullptr;
			_can_loiter_at_sp = false;
			break;
		}

		/* we have a new navigation mode: reset triplet */
		if (_navigation_mode != navigation_mode_new) {
			reset_triplets();
		}

		_navigation_mode = navigation_mode_new;

		/* iterate through navigation modes and set active/inactive for each */
		for (unsigned int i = 0; i < NAVIGATOR_MODE_ARRAY_SIZE; i++) {
			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
		}

		/* if we landed and have not received takeoff setpoint then stay in idle */
		if (_land_detected.landed &&
		    !((_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF)
		      || (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) {

			_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
			_pos_sp_triplet.current.valid = true;
			_pos_sp_triplet.previous.valid = false;
			_pos_sp_triplet.next.valid = false;

		}

		/* if nothing is running, set position setpoint triplet invalid once */
		if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
			_pos_sp_triplet_published_invalid_once = true;
			reset_triplets();
		}

		if (_pos_sp_triplet_updated) {
			_pos_sp_triplet.timestamp = hrt_absolute_time();
			publish_position_setpoint_triplet();
			_pos_sp_triplet_updated = false;
		}

		if (_mission_result_updated) {
			publish_mission_result();
			_mission_result_updated = false;
		}

		perf_end(_loop_perf);
	}

	orb_unsubscribe(_global_pos_sub);
	orb_unsubscribe(_local_pos_sub);
	orb_unsubscribe(_gps_pos_sub);
	orb_unsubscribe(_sensor_combined_sub);
	orb_unsubscribe(_fw_pos_ctrl_status_sub);
	orb_unsubscribe(_vstatus_sub);
	orb_unsubscribe(_land_detected_sub);
	orb_unsubscribe(_home_pos_sub);
	orb_unsubscribe(_onboard_mission_sub);
	orb_unsubscribe(_offboard_mission_sub);
	orb_unsubscribe(_param_update_sub);
	orb_unsubscribe(_vehicle_command_sub);

	PX4_INFO("exiting");

	_navigator_task = -1;
}
示例#10
0
static int fimg2d_check_dma_sync(struct fimg2d_bltcmd *cmd)
{
	struct mm_struct *mm = cmd->ctx->mm;
	struct fimg2d_param *p = &cmd->param;
	struct fimg2d_image *img;
	struct fimg2d_clip *clp;
	struct fimg2d_rect *r;
	struct fimg2d_dma *c;
	enum pt_status pt;
	int clip_x, clip_w, clip_h, y, dir, i;
	unsigned long clip_start;

	clp = &p->clipping;

	for (i = 0; i < MAX_IMAGES; i++) {
		img = &cmd->image[i];
		c = &cmd->dma[i];
		r = &img->rect;

		if (!img->addr.type)
			continue;

		/* caculate horizontally clipped region */
		if (i == IMAGE_DST && clp->enable) {
			c->addr = img->addr.start + (img->stride * clp->y1);
			c->size = img->stride * (clp->y2 - clp->y1);
		} else {
			c->addr = img->addr.start + (img->stride * r->y1);
			c->size = img->stride * (r->y2 - r->y1);
		}

		/* check pagetable */
		if (img->addr.type == ADDR_USER) {
			pt = fimg2d_check_pagetable(mm, c->addr, c->size);
			if (pt == PT_FAULT)
				return -1;
		}

		if (img->need_cacheopr && i != IMAGE_TMP) {
			c->cached = c->size;
			cmd->dma_all += c->cached;
		}
	}

#ifdef PERF_PROFILE
	perf_start(cmd->ctx, PERF_INNERCACHE);
#endif

	if (is_inner_flushall(cmd->dma_all))
		flush_all_cpu_caches();
	else {
		for (i = 0; i < MAX_IMAGES; i++) {
			img = &cmd->image[i];
			c = &cmd->dma[i];
			r = &img->rect;

			if (!img->addr.type || !c->cached)
				continue;

			if (i == IMAGE_DST)
				dir = DMA_BIDIRECTIONAL;
			else
				dir = DMA_TO_DEVICE;

			if (i == IDST && clp->enable) {
				clip_w = width2bytes(clp->x2 - clp->x1,
							img->fmt);
				clip_x = pixel2offset(clp->x1, img->fmt);
				clip_h = clp->y2 - clp->y1;
			} else {
				clip_w = width2bytes(r->x2 - r->x1, img->fmt);
				clip_x = pixel2offset(r->x1, img->fmt);
				clip_h = r->y2 - r->y1;
			}

			if (is_inner_flushrange(img->stride - clip_w))
				fimg2d_dma_sync_inner(c->addr, c->cached, dir);
			else {
				for (y = 0; y < clip_h; y++) {
					clip_start = c->addr +
						(img->stride * y) + clip_x;
					fimg2d_dma_sync_inner(clip_start,
								clip_w, dir);
				}
			}
		}
	}
#ifdef PERF_PROFILE
	perf_end(cmd->ctx, PERF_INNERCACHE);
#endif

#ifdef CONFIG_OUTER_CACHE
#ifdef PERF_PROFILE
	perf_start(cmd->ctx, PERF_OUTERCACHE);
#endif
	if (is_outer_flushall(cmd->dma_all))
		outer_flush_all();
	else {
		for (i = 0; i < MAX_IMAGES; i++) {
			img = &cmd->image[i];
			c = &cmd->dma[i];
			r = &img->rect;

			if (!img->addr.type)
				continue;

			/* clean pagetable */
			if (img->addr.type == ADDR_USER)
				fimg2d_clean_outer_pagetable(mm, c->addr, c->size);

			if (!c->cached)
				continue;

			if (i == IMAGE_DST)
				dir = CACHE_FLUSH;
			else
				dir = CACHE_CLEAN;

			if (i == IDST && clp->enable) {
				clip_w = width2bytes(clp->x2 - clp->x1,
							img->fmt);
				clip_x = pixel2offset(clp->x1, img->fmt);
				clip_h = clp->y2 - clp->y1;
			} else {
				clip_w = width2bytes(r->x2 - r->x1, img->fmt);
				clip_x = pixel2offset(r->x1, img->fmt);
				clip_h = r->y2 - r->y1;
			}

			if (is_outer_flushrange(img->stride - clip_w))
				fimg2d_dma_sync_outer(mm, c->addr,
							c->cached, dir);
			else {
				for (y = 0; y < clip_h; y++) {
					clip_start = c->addr +
						(img->stride * y) + clip_x;
					fimg2d_dma_sync_outer(mm, clip_start,
								clip_w, dir);
				}
			}
		}
	}
#ifdef PERF_PROFILE
	perf_end(cmd->ctx, PERF_OUTERCACHE);
#endif
#endif

	return 0;
}
示例#11
0
int
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];
	}	hmc_report;
#pragma pack(pop)
	struct {
		int16_t		x, y, z;
	} report;

	int	ret;
	uint8_t check_counter;

	perf_begin(_sample_perf);
	struct mag_report new_report;
	bool sensor_is_onboard = false;

	float xraw_f;
	float yraw_f;
	float zraw_f;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	new_report.timestamp = hrt_absolute_time();
	new_report.error_count = perf_event_count(_comms_errors);

	/*
	 * @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 */
	ret = _interface->read(ADDR_DATA_OUT_X_MSB, (uint8_t *)&hmc_report, sizeof(hmc_report));

	if (ret != OK) {
		perf_count(_comms_errors);
		DEVICE_DEBUG("data/status read error");
		goto out;
	}

	/* 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];

	/*
	 * 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)) {
		perf_count(_comms_errors);
		goto out;
	}

	/* get measurements from the device */
	new_report.temperature = 0;

	if (_conf_reg & HMC5983_TEMP_SENSOR_ENABLE) {
		/*
		  if temperature compensation is enabled read the
		  temperature too.

		  We read the temperature every 10 samples to avoid
		  excessive I2C traffic
		 */
		if (_temperature_counter++ == 10) {
			uint8_t raw_temperature[2];

			_temperature_counter = 0;

			ret = _interface->read(ADDR_TEMP_OUT_MSB,
					       raw_temperature, sizeof(raw_temperature));

			if (ret == OK) {
				int16_t temp16 = (((int16_t)raw_temperature[0]) << 8) +
						 raw_temperature[1];
				new_report.temperature = 25 + (temp16 / (16 * 8.0f));
				_temperature_error_count = 0;

			} else {
				_temperature_error_count++;

				if (_temperature_error_count == 10) {
					/*
					  it probably really is an old HMC5883,
					  and can't do temperature. Disable it
					*/
					_temperature_error_count = 0;
					DEVICE_DEBUG("disabling temperature compensation");
					set_temperature_compensation(0);
				}
			}

		} else {
			new_report.temperature = _last_report.temperature;
		}
	}

	/*
	 * RAW outputs
	 *
	 * to align the sensor axes with the board, x and y need to be flipped
	 * and y needs to be negated
	 */
	new_report.x_raw = report.y;
	new_report.y_raw = -report.x;
	/* z remains z */
	new_report.z_raw = report.z;

	/* scale values for output */

	// XXX revisit for SPI part, might require a bus type IOCTL
	unsigned dummy;
	sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);

	if (sensor_is_onboard) {
		// convert onboard so it matches offboard for the
		// scaling below
		report.y = -report.y;
		report.x = -report.x;
	}

	/* the standard external mag by 3DR has x pointing to the
	 * right, y pointing backwards, and z down, therefore switch x
	 * and y and invert y */
	xraw_f = -report.y;
	yraw_f = report.x;
	zraw_f = report.z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
	/* flip axes and negate value for y */
	new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
	/* z remains z */
	new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;

	if (!(_pub_blocked)) {

		if (_mag_topic != nullptr) {
			/* publish it */
			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);

		} else {
			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
							 &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);

			if (_mag_topic == nullptr) {
				DEVICE_DEBUG("ADVERT FAIL");
			}
		}
	}

	_last_report = new_report;

	/* post a report to the ring */
	if (_reports->force(&new_report)) {
		perf_count(_buffer_overflows);
	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	/*
	  periodically check the range register and configuration
	  registers. With a bad I2C cable it is possible for the
	  registers to become corrupt, leading to bad readings. It
	  doesn't happen often, but given the poor cables some
	  vehicles have it is worth checking for.
	 */
	check_counter = perf_event_count(_sample_perf) % 256;

	if (check_counter == 0) {
		check_range();
	}

	if (check_counter == 128) {
		check_conf();
	}

	ret = OK;

out:
	perf_end(_sample_perf);
	return ret;
}
示例#12
0
void
BMA180::measure()
{
	/* BMA180 measurement registers */
// #pragma pack(push, 1)
// 	struct {
// 		uint8_t		cmd;
// 		int16_t	x;
// 		int16_t	y;
// 		int16_t	z;
// 	} raw_report;
// #pragma pack(pop)

	struct accel_report report;

	/* start the performance counter */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the BMA180 in one pass;
	 * starting from the X LSB.
	 */
	//raw_report.cmd = ADDR_ACC_X_LSB;
	// XXX PX4DEV transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));

	/*
	 * Adjust and scale results to SI units.
	 *
	 * Note that we ignore the "new data" bits.  At any time we read, each
	 * of the axis measurements are the "most recent", even if we've seen
	 * them before.  There is no good way to synchronise with the internal
	 * measurement flow without using the external interrupt.
	 */
	report.timestamp = hrt_absolute_time();
        report.error_count = 0;
	/*
	 * y of board is x of sensor and x of board is -y of sensor
	 * perform only the axis assignment here.
	 * Two non-value bits are discarded directly
	 */
	report.y_raw  = read_reg(ADDR_ACC_X_LSB + 0);
	report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
	report.x_raw  = read_reg(ADDR_ACC_X_LSB + 2);
	report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
	report.z_raw  = read_reg(ADDR_ACC_X_LSB + 4);
	report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;

	/* discard two non-value bits in the 16 bit measurement */
	report.x_raw = (report.x_raw / 4);
	report.y_raw = (report.y_raw / 4);
	report.z_raw = (report.z_raw / 4);

	/* invert y axis, due to 14 bit data no overflow can occur in the negation */
	report.y_raw = -report.y_raw;

	report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
	report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
	report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
	report.scaling = _accel_range_scale;
	report.range_m_s2 = _accel_range_m_s2;

	_reports->force(&report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	/* publish for subscribers */
	orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);

	/* stop the perf counter */
	perf_end(_sample_perf);
}
示例#13
0
int
user_start(int argc, char *argv[])
{
	/* configure the first 8 PWM outputs (i.e. all of them) */
	up_pwm_servo_init(0xff);

#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)

	/* run C++ ctors before we go any further */

	up_cxxinitialize();

#	if defined(CONFIG_EXAMPLES_NSH_CXXINITIALIZE)
#  		error CONFIG_EXAMPLES_NSH_CXXINITIALIZE Must not be defined! Use CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE.
#	endif

#else
#  error platform is dependent on c++ both CONFIG_HAVE_CXX and CONFIG_HAVE_CXXINITIALIZE must be defined.
#endif

	/* reset all to zero */
	memset(&system_state, 0, sizeof(system_state));

	/* configure the high-resolution time/callout interface */
	hrt_init();

	/* calculate our fw CRC so FMU can decide if we need to update */
	calculate_fw_crc();

	/*
	 * Poll at 1ms intervals for received bytes that have not triggered
	 * a DMA event.
	 */
#ifdef CONFIG_ARCH_DMA
	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif

	/* print some startup info */
	syslog(LOG_INFO, "\nPX4IO: starting\n");

	/* default all the LEDs to off while we start */
	LED_AMBER(false);
	LED_BLUE(false);
	LED_SAFETY(false);
#ifdef GPIO_LED4
	LED_RING(false);
#endif

	/* turn on servo power (if supported) */
#ifdef POWER_SERVO
	POWER_SERVO(true);
#endif

	/* turn off S.Bus out (if supported) */
#ifdef ENABLE_SBUS_OUT
	ENABLE_SBUS_OUT(false);
#endif

	/* start the safety switch handler */
	safety_init();

	/* initialise the control inputs */
	controls_init();

	/* set up the ADC */
	adc_init();

	/* start the FMU interface */
	interface_init();

	/* add a performance counter for mixing */
	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");

	/* add a performance counter for controls */
	perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");

	/* and one for measuring the loop rate */
	perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");

	struct mallinfo minfo = mallinfo();
	r_page_status[PX4IO_P_STATUS_FREEMEM] = minfo.mxordblk;
	syslog(LOG_INFO, "MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);

	/* initialize PWM limit lib */
	pwm_limit_init(&pwm_limit);

	/*
	 *    P O L I C E    L I G H T S
	 *
	 * Not enough memory, lock down.
	 *
	 * We might need to allocate mixers later, and this will
	 * ensure that a developer doing a change will notice
	 * that he just burned the remaining RAM with static
	 * allocations. We don't want him to be able to
	 * get past that point. This needs to be clearly
	 * documented in the dev guide.
	 *
	 */
	if (minfo.mxordblk < 600) {

		syslog(LOG_ERR, "ERR: not enough MEM");
		bool phase = false;

		while (true) {

			if (phase) {
				LED_AMBER(true);
				LED_BLUE(false);

			} else {
				LED_AMBER(false);
				LED_BLUE(true);
			}

			up_udelay(250000);

			phase = !phase;
		}
	}

	/* Start the failsafe led init */
	failsafe_led_init();

	/*
	 * Run everything in a tight loop.
	 */

	uint64_t last_debug_time = 0;
	uint64_t last_heartbeat_time = 0;
	uint64_t last_loop_time = 0;

	for (;;) {
		dt = (hrt_absolute_time() - last_loop_time) / 1000000.0f;
		last_loop_time = hrt_absolute_time();

		if (dt < 0.0001f) {
			dt = 0.0001f;

		} else if (dt > 0.02f) {
			dt = 0.02f;
		}

		/* track the rate at which the loop is running */
		perf_count(loop_perf);

		/* kick the mixer */
		perf_begin(mixer_perf);
		mixer_tick();
		perf_end(mixer_perf);

		/* kick the control inputs */
		perf_begin(controls_perf);
		controls_tick();
		perf_end(controls_perf);

		/* some boards such as Pixhawk 2.1 made
		   the unfortunate choice to combine the blue led channel with
		   the IMU heater. We need a software hack to fix the hardware hack
		   by allowing to disable the LED / heater.
		 */
		if (r_page_setup[PX4IO_P_SETUP_THERMAL] == PX4IO_THERMAL_IGNORE) {
			/*
			  blink blue LED at 4Hz in normal operation. When in
			  override blink 4x faster so the user can clearly see
			  that override is happening. This helps when
			  pre-flight testing the override system
			 */
			uint32_t heartbeat_period_us = 250 * 1000UL;

			if (r_status_flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) {
				heartbeat_period_us /= 4;
			}

			if ((hrt_absolute_time() - last_heartbeat_time) > heartbeat_period_us) {
				last_heartbeat_time = hrt_absolute_time();
				heartbeat_blink();
			}

		} else if (r_page_setup[PX4IO_P_SETUP_THERMAL] < PX4IO_THERMAL_FULL) {
			/* switch resistive heater off */
			LED_BLUE(false);

		} else {
			/* switch resistive heater hard on */
			LED_BLUE(true);
		}

		update_mem_usage();

		ring_blink();

		check_reboot();

		/* check for debug activity (default: none) */
		show_debug_messages();

		/* post debug state at ~1Hz - this is via an auxiliary serial port
		 * DEFAULTS TO OFF!
		 */
		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {

			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u",
				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
				  (unsigned)r_status_flags,
				  (unsigned)r_setup_arming,
				  (unsigned)r_setup_features,
				  (unsigned)mallinfo().mxordblk);
			last_debug_time = hrt_absolute_time();
		}
	}
}
示例#14
0
int
SF0X::collect()
{
	int	ret;

	perf_begin(_sample_perf);

	/* clear buffer if last read was too long ago */
	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);

	/* the buffer for read chars is buflen minus null termination */
	char readbuf[sizeof(_linebuf)];
	unsigned readlen = sizeof(readbuf) - 1;

	/* read from the sensor (uart buffer) */
	ret = ::read(_fd, &readbuf[0], readlen);

	if (ret < 0) {
		DEVICE_DEBUG("read err: %d", ret);
		perf_count(_comms_errors);
		perf_end(_sample_perf);

		/* only throw an error if we time out */
		if (read_elapsed > (_conversion_interval * 2)) {
			return ret;

		} else {
			return -EAGAIN;
		}

	} else if (ret == 0) {
		return -EAGAIN;
	}

	_last_read = hrt_absolute_time();

	float distance_m = -1.0f;
	bool valid = false;

	for (int i = 0; i < ret; i++) {
		if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &distance_m)) {
			valid = true;
		}
	}

	if (!valid) {
		return -EAGAIN;
	}

	DEVICE_DEBUG("val (float): %8.4f, raw: %s, valid: %s", (double)distance_m, _linebuf, ((valid) ? "OK" : "NO"));

	struct distance_sensor_s report;

	report.timestamp = hrt_absolute_time();
	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
	report.orientation = _rotation;
	report.current_distance = distance_m;
	report.min_distance = get_minimum_distance();
	report.max_distance = get_maximum_distance();
	report.covariance = 0.0f;
	/* TODO: set proper ID */
	report.id = 0;

	/* publish it */
	orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);

	_reports->force(&report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);
	return ret;
}
示例#15
0
void
Gimbal::cycle()
{
	if (!_initialized) {
		/* get a subscription handle on the vehicle command topic */
		_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));

		/* get a publication handle on actuator output topic */
		struct actuator_controls_s zero_report;
		memset(&zero_report, 0, sizeof(zero_report));
		zero_report.timestamp = hrt_absolute_time();
		_actuator_controls_2_topic = orb_advertise(ORB_ID(actuator_controls_2), &zero_report);

		if (_actuator_controls_2_topic == nullptr) {
			warnx("advert err");
		}

		_initialized = true;
	}

	bool	updated = false;

	perf_begin(_sample_perf);

	float roll = 0.0f;
	float pitch = 0.0f;
	float yaw = 0.0f;


	if (_att_sub < 0) {
		_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	}

	vehicle_attitude_s att;

	orb_copy(ORB_ID(vehicle_attitude), _att_sub, &att);

	if (_attitude_compensation_roll) {
		roll = 1.0f / M_PI_F * -att.roll;
		updated = true;
	}

	if (_attitude_compensation_pitch) {
		pitch = 1.0f / M_PI_F * -att.pitch;
		updated = true;
	}

	if (_attitude_compensation_yaw) {
		yaw = 1.0f / M_PI_F * att.yaw;
		updated = true;
	}


	struct vehicle_command_s cmd;

	bool cmd_updated;

	orb_check(_vehicle_command_sub, &cmd_updated);

	if (cmd_updated) {

		orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &cmd);

		if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL
				|| cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT) {

			_control_cmd = cmd;
			_control_cmd_set = true;

		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONFIGURE) {

			_config_cmd = cmd;
			_config_cmd_set = true;

		}

	}

	if (_config_cmd_set) {

		_config_cmd_set = false;

		_attitude_compensation_roll = (int)_config_cmd.param2 == 1;
		_attitude_compensation_pitch = (int)_config_cmd.param3 == 1;
		_attitude_compensation_yaw = (int)_config_cmd.param4 == 1;

	}

	if (_control_cmd_set) {

		unsigned mountMode = _control_cmd.param7;
		DEVICE_DEBUG("control_cmd: %d, mountMode %d | param1: %8.4f param2: %8.4f", _control_cmd.command, mountMode, (double)_control_cmd.param1, (double)_control_cmd.param2);

		if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL &&
			mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
			/* Convert to range -1 ... 1, which corresponds to -180deg ... 180deg */
			roll += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param1;
			pitch += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param2;
			yaw += 1.0f / M_PI_F * M_DEG_TO_RAD_F * _control_cmd.param3;
			
			updated = true;
		}

		if (_control_cmd.command == vehicle_command_s::VEHICLE_CMD_DO_MOUNT_CONTROL_QUAT &&
			mountMode == vehicle_command_s::VEHICLE_MOUNT_MODE_MAVLINK_TARGETING) {
			float gimbalDirectionQuat[] = {_control_cmd.param1, _control_cmd.param2, _control_cmd.param3, _control_cmd.param4};
			math::Vector<3> gimablDirectionEuler = math::Quaternion(gimbalDirectionQuat).to_dcm().to_euler();

			roll += gimablDirectionEuler(0);
			pitch += gimablDirectionEuler(1);
			yaw += gimablDirectionEuler(2);

			updated = true;
		}

	}

	if (updated) {

		struct actuator_controls_s controls;

		// DEVICE_DEBUG("publishing | roll: %8.4f pitch: %8.4f yaw: %8.4f", (double)roll, (double)pitch, (double)yaw);

		/* fill in the final control values */
		controls.timestamp = hrt_absolute_time();
		controls.control[0] = roll;
		controls.control[1] = pitch;
		controls.control[2] = yaw;

		/* publish it */
		orb_publish(ORB_ID(actuator_controls_2), _actuator_controls_2_topic, &controls);

	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	perf_end(_sample_perf);

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(LPWORK,
		   &_work,
		   (worker_t)&Gimbal::cycle_trampoline,
		   this,
		   USEC2TICK(GIMBAL_UPDATE_INTERVAL));
}
示例#16
0
int
MEASAirspeedSim::collect()
{
	int	ret = -EIO;

	/* read from the sensor */
#pragma pack(push, 1)
	struct {
		float		temperature;
		float		diff_pressure;
	} airspeed_report;
#pragma pack(pop)


	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, (uint8_t *)&airspeed_report, sizeof(airspeed_report));

	if (ret < 0) {
		perf_count(_comms_errors);
		perf_end(_sample_perf);
		return ret;
	}

	uint8_t status = 0;

	switch (status) {
	case 0:
		break;

	case 1:

	/* fallthrough */
	case 2:

	/* fallthrough */
	case 3:
		perf_count(_comms_errors);
		perf_end(_sample_perf);
		return -EAGAIN;
	}

	float temperature = airspeed_report.temperature;
	float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar

	struct differential_pressure_s report;

	/* track maximum differential pressure measured (so we can work out top speed). */
	if (diff_press_pa_raw > _max_differential_pressure_pa) {
		_max_differential_pressure_pa = diff_press_pa_raw;
	}

	report.timestamp = hrt_absolute_time();
	report.error_count = perf_event_count(_comms_errors);
	report.temperature = temperature;
	report.differential_pressure_filtered_pa =  _filter.apply(diff_press_pa_raw);

	report.differential_pressure_raw_pa = diff_press_pa_raw;
	report.max_differential_pressure_pa = _max_differential_pressure_pa;

	if (_airspeed_pub != nullptr && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
	}

	new_report(report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);

	return ret;
}
示例#17
0
void
L3G4200D::measure()
{
#if L3G4200D_USE_DRDY
	// if the gyro doesn't have any data ready then re-schedule
	// for 100 microseconds later. This ensures we don't double
	// read a value and then miss the next value
	if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
		perf_count(_reschedules);
		hrt_call_delay(&_call, 100);
		return;
	}
#endif

	/* status register and data as read back from the device */
#pragma pack(push, 1)
	struct {
		uint8_t		cmd;
		uint8_t		temp;
		uint8_t		status;
		int16_t		x;
		int16_t		y;
		int16_t		z;
	} raw_report;
#pragma pack(pop)

	gyro_report report;

	/* start the performance counter */
	perf_begin(_sample_perf);

	/* fetch data from the sensor */
	memset(&raw_report, 0, sizeof(raw_report));
	raw_report.cmd = ADDR_OUT_TEMP | DIR_READ | ADDR_INCREMENT;
	transfer((uint8_t *)&raw_report, (uint8_t *)&raw_report, sizeof(raw_report));

#if L3G4200D_USE_DRDY
        if ((raw_report.status & 0xF) != 0xF) {
            /*
              we waited for DRDY, but did not see DRDY on all axes
              when we captured. That means a transfer error of some sort
             */
            perf_count(_errors);            
            return;
        }
#endif
	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */
	report.timestamp = hrt_absolute_time();
        report.error_count = 0; // not recorded
	
	switch (_orientation) {

		case SENSOR_BOARD_ROTATION_000_DEG:
			/* keep axes in place */
			report.x_raw = raw_report.x;
			report.y_raw = raw_report.y;
			break;

		case SENSOR_BOARD_ROTATION_090_DEG:
			/* swap x and y */
			report.x_raw = raw_report.y;
			report.y_raw = raw_report.x;
			break;

		case SENSOR_BOARD_ROTATION_180_DEG:
			/* swap x and y and negate both */
			/* since sensor is mounted on top, need to rotate and then negate y, z */
			report.x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
			// report.y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y); /* leave as is, 180 rotate + negate */
			report.y_raw = raw_report.y;
			report.z_raw = ((raw_report.z == -32768) ? 32767 : -raw_report.z);
			break;

		case SENSOR_BOARD_ROTATION_270_DEG:
			/* swap x and y and negate y */
			report.x_raw = raw_report.y;
			report.y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
			break;
	}

//	report.z_raw = raw_report.z;

	report.x = ((report.x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	report.y = ((report.y_raw * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	report.z = ((report.z_raw * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	report.x = _gyro_filter_x.apply(report.x);
	report.y = _gyro_filter_y.apply(report.y);
	report.z = _gyro_filter_z.apply(report.z);

	report.scaling = _gyro_range_scale;
	report.range_rad_s = _gyro_range_rad_s;

	_reports->force(&report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	/* publish for subscribers */
	if (_gyro_topic > 0)
		orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);

	_read++;

	/* stop the perf counter */
	perf_end(_sample_perf);
}
int flow_position_estimator_thread_main(int argc, char *argv[])
{
	/* welcome user */
	thread_running = true;
	printf("[flow position estimator] starting\n");

	/* rotation matrix for transformation of optical flow speed vectors */
	static const int8_t rotM_flow_sensor[3][3] =   {{  0, -1, 0 },
													{ 1, 0, 0 },
													{  0, 0, 1 }}; // 90deg rotated
	const float time_scale = powf(10.0f,-6.0f);
	static float speed[3] = {0.0f, 0.0f, 0.0f};
	static float flow_speed[3] = {0.0f, 0.0f, 0.0f};
	static float global_speed[3] = {0.0f, 0.0f, 0.0f};
	static uint32_t counter = 0;
	static uint64_t time_last_flow = 0; // in ms
	static float dt = 0.0f; // seconds
	static float sonar_last = 0.0f;
	static bool sonar_valid = false;
	static float sonar_lp = 0.0f;

	/* subscribe to vehicle status, attitude, sensors and flow*/
	struct actuator_armed_s armed;
	memset(&armed, 0, sizeof(armed));
	struct vehicle_control_mode_s control_mode;
	memset(&control_mode, 0, sizeof(control_mode));
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	struct vehicle_attitude_setpoint_s att_sp;
	memset(&att_sp, 0, sizeof(att_sp));
	struct optical_flow_s flow;
	memset(&flow, 0, sizeof(flow));

	/* subscribe to parameter changes */
	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));

	/* subscribe to armed topic */
	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));

	/* subscribe to safety topic */
	int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));

	/* subscribe to attitude */
	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));

	/* subscribe to attitude setpoint */
	int vehicle_attitude_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));

	/* subscribe to optical flow*/
	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));

	/* init local position and filtered flow struct */
	struct vehicle_local_position_s local_pos = {
			.x = 0.0f,
			.y = 0.0f,
			.z = 0.0f,
			.vx = 0.0f,
			.vy = 0.0f,
			.vz = 0.0f
	};
	struct filtered_bottom_flow_s filtered_flow = {
			.sumx = 0.0f,
			.sumy = 0.0f,
			.vx = 0.0f,
			.vy = 0.0f
	};

	/* advert pub messages */
	orb_advert_t local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos);
	orb_advert_t filtered_flow_pub = orb_advertise(ORB_ID(filtered_bottom_flow), &filtered_flow);

	/* vehicle flying status parameters */
	bool vehicle_liftoff = false;
	bool sensors_ready = false;

	/* parameters init*/
	struct flow_position_estimator_params params;
	struct flow_position_estimator_param_handles param_handles;
	parameters_init(&param_handles);
	parameters_update(&param_handles, &params);

	perf_counter_t mc_loop_perf = perf_alloc(PC_ELAPSED, "flow_position_estimator_runtime");
	perf_counter_t mc_interval_perf = perf_alloc(PC_INTERVAL, "flow_position_estimator_interval");
	perf_counter_t mc_err_perf = perf_alloc(PC_COUNT, "flow_position_estimator_err");

	while (!thread_should_exit)
	{

		if (sensors_ready)
		{
			/*This runs at the rate of the sensors */
			struct pollfd fds[2] = {
					{ .fd = optical_flow_sub, .events = POLLIN },
					{ .fd = parameter_update_sub,   .events = POLLIN }
			};

			/* wait for a sensor update, check for exit condition every 500 ms */
			int ret = poll(fds, 2, 500);

			if (ret < 0)
			{
				/* poll error, count it in perf */
				perf_count(mc_err_perf);

			}
			else if (ret == 0)
			{
				/* no return value, ignore */
//				printf("[flow position estimator] no bottom flow.\n");
			}
			else
			{

				/* parameter update available? */
				if (fds[1].revents & POLLIN)
				{
					/* read from param to clear updated flag */
					struct parameter_update_s update;
					orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);

					parameters_update(&param_handles, &params);
					printf("[flow position estimator] parameters updated.\n");
				}

				/* only if flow data changed */
				if (fds[0].revents & POLLIN)
				{
					perf_begin(mc_loop_perf);

					orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);
					/* got flow, updating attitude and status as well */
					orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
					orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp);
					orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
					orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode);

					/* vehicle state estimation */
					float sonar_new = flow.ground_distance_m;

					/* set liftoff boolean
					 * -> at bottom sonar sometimes does not work correctly, and has to be calibrated (distance higher than 0.3m)
					 * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time)
					 * -> minimum sonar value 0.3m
					 */

					if (!vehicle_liftoff)
					{
						if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f)
							vehicle_liftoff = true;
					}
					else
					{
						if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f))
							vehicle_liftoff = false;
					}

					/* calc dt between flow timestamps */
					/* ignore first flow msg */
					if(time_last_flow == 0)
					{
						time_last_flow = flow.timestamp;
						continue;
					}
					dt = (float)(flow.timestamp - time_last_flow) * time_scale ;
					time_last_flow = flow.timestamp;

					/* only make position update if vehicle is lift off or DEBUG is activated*/
					if (vehicle_liftoff || params.debug)
					{
						/* copy flow */
						flow_speed[0] = flow.flow_comp_x_m;
						flow_speed[1] = flow.flow_comp_y_m;
						flow_speed[2] = 0.0f;

						/* convert to bodyframe velocity */
						for(uint8_t i = 0; i < 3; i++)
						{
							float sum = 0.0f;
							for(uint8_t j = 0; j < 3; j++)
								sum = sum + flow_speed[j] * rotM_flow_sensor[j][i];

							speed[i] = sum;
						}

						/* update filtered flow */
						filtered_flow.sumx = filtered_flow.sumx + speed[0] * dt;
						filtered_flow.sumy = filtered_flow.sumy + speed[1] * dt;
						filtered_flow.vx = speed[0];
						filtered_flow.vy = speed[1];

						// TODO add yaw rotation correction (with distance to vehicle zero)

						/* convert to globalframe velocity
						 * -> local position is currently not used for position control
						 */
						for(uint8_t i = 0; i < 3; i++)
						{
							float sum = 0.0f;
							for(uint8_t j = 0; j < 3; j++)
								sum = sum + speed[j] * att.R[i][j];

							global_speed[i] = sum;
						}

						local_pos.x = local_pos.x + global_speed[0] * dt;
						local_pos.y = local_pos.y + global_speed[1] * dt;
						local_pos.vx = global_speed[0];
						local_pos.vy = global_speed[1];
					}
					else
					{
						/* set speed to zero and let position as it is */
						filtered_flow.vx = 0;
						filtered_flow.vy = 0;
						local_pos.vx = 0;
						local_pos.vy = 0;
					}

					/* filtering ground distance */
					if (!vehicle_liftoff || !armed.armed)
					{
						/* not possible to fly */
						sonar_valid = false;
						local_pos.z = 0.0f;
					}
					else
					{
						sonar_valid = true;
					}

					if (sonar_valid || params.debug)
					{
						/* simple lowpass sonar filtering */
						/* if new value or with sonar update frequency */
						if (sonar_new != sonar_last || counter % 10 == 0)
						{
							sonar_lp = 0.05f * sonar_new + 0.95f * sonar_lp;
							sonar_last = sonar_new;
						}

						float height_diff = sonar_new - sonar_lp;

						/* if over 1/2m spike follow lowpass */
						if (height_diff < -params.sonar_lower_lp_threshold || height_diff > params.sonar_upper_lp_threshold)
						{
							local_pos.z = -sonar_lp;
						}
						else
						{
							local_pos.z = -sonar_new;
						}
					}

					filtered_flow.timestamp = hrt_absolute_time();
					local_pos.timestamp = hrt_absolute_time();

					/* publish local position */
					if(isfinite(local_pos.x) && isfinite(local_pos.y) && isfinite(local_pos.z)
							&& isfinite(local_pos.vx) && isfinite(local_pos.vy))
					{
						orb_publish(ORB_ID(vehicle_local_position), local_pos_pub, &local_pos);
					}

					/* publish filtered flow */
					if(isfinite(filtered_flow.sumx) && isfinite(filtered_flow.sumy) && isfinite(filtered_flow.vx) && isfinite(filtered_flow.vy))
					{
						orb_publish(ORB_ID(filtered_bottom_flow), filtered_flow_pub, &filtered_flow);
					}

					/* measure in what intervals the position estimator runs */
					perf_count(mc_interval_perf);
					perf_end(mc_loop_perf);

				}
			}

		}
		else
		{
			/* sensors not ready waiting for first attitude msg */

			/* polling */
			struct pollfd fds[1] = {
				{ .fd = vehicle_attitude_sub, .events = POLLIN },
			};

			/* wait for a attitude message, check for exit condition every 5 s */
			int ret = poll(fds, 1, 5000);

			if (ret < 0)
			{
				/* poll error, count it in perf */
				perf_count(mc_err_perf);
			}
			else if (ret == 0)
			{
				/* no return value, ignore */
				printf("[flow position estimator] no attitude received.\n");
			}
			else
			{
				if (fds[0].revents & POLLIN){
					sensors_ready = true;
					printf("[flow position estimator] initialized.\n");
				}
			}
		}
int
ETSAirspeed::collect()
{
	int	ret = -EIO;

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 2);

	if (ret < 0) {
		perf_count(_comms_errors);
		return ret;
	}

	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
	uint16_t diff_pres_pa;
        if (diff_pres_pa_raw == 0) {
		// a zero value means the pressure sensor cannot give us a
		// value. We need to return, and not report a value or the
		// caller could end up using this value as part of an
		// average
		perf_count(_comms_errors);
		log("zero value from sensor"); 
		return -1;
        }

	if (diff_pres_pa_raw < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
		diff_pres_pa = 0;
	} else {
		diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset;
	}

	// Track maximum differential pressure measured (so we can work out top speed).
	if (diff_pres_pa > _max_differential_pressure_pa) {
		_max_differential_pressure_pa = diff_pres_pa;
	}

	// XXX we may want to smooth out the readings to remove noise.
	differential_pressure_s report;
	report.timestamp = hrt_absolute_time();
        report.error_count = perf_event_count(_comms_errors);
	report.differential_pressure_pa = (float)diff_pres_pa;
	report.differential_pressure_raw_pa = (float)diff_pres_pa_raw;
	report.voltage = 0;
	report.max_differential_pressure_pa = _max_differential_pressure_pa;

	if (_airspeed_pub > 0 && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
	}

	new_report(report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);

	return ret;
}
示例#20
0
void
FixedwingAttitudeControl::task_main()
{
	/*
	 * do subscriptions
	 */
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));

	parameters_update();

	/* get an initial update for all sensor and status data */
	vehicle_setpoint_poll();
	vehicle_accel_poll();
	vehicle_control_mode_poll();
	vehicle_manual_poll();
	vehicle_status_poll();
	vehicle_land_detected_poll();

	/* wakeup source */
	px4_pollfd_struct_t fds[2];

	/* Setup of loop */
	fds[0].fd = _params_sub;
	fds[0].events = POLLIN;
	fds[1].fd = _ctrl_state_sub;
	fds[1].events = POLLIN;

	_task_running = true;

	while (!_task_should_exit) {
		static int loop_counter = 0;

		/* wait for up to 500ms for data */
		int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		/* timed out - periodic check for _task_should_exit, etc. */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);

		/* only update parameters if they changed */
		if (fds[0].revents & POLLIN) {
			/* read from param to clear updated flag */
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), _params_sub, &update);

			/* update parameters from storage */
			parameters_update();
		}

		/* only run controller if attitude changed */
		if (fds[1].revents & POLLIN) {
			static uint64_t last_run = 0;
			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();

			/* guard against too large deltaT's */
			if (deltaT > 1.0f) {
				deltaT = 0.01f;
			}

			/* load local copies */
			orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);


			/* get current rotation matrix and euler angles from control state quaternions */
			math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
			_R = q_att.to_dcm();

			math::Vector<3> euler_angles;
			euler_angles = _R.to_euler();
			_roll    = euler_angles(0);
			_pitch   = euler_angles(1);
			_yaw     = euler_angles(2);

			if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
				/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
				 *
				 * Since the VTOL airframe is initialized as a multicopter we need to
				 * modify the estimated attitude for the fixed wing operation.
				 * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
				 * the pitch axis compared to the neutral position of the vehicle in multicopter mode
				 * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
				 * Additionally, in order to get the correct sign of the pitch, we need to multiply
				 * the new x axis of the rotation matrix with -1
				 *
				 * original:			modified:
				 *
				 * Rxx  Ryx  Rzx		-Rzx  Ryx  Rxx
				 * Rxy	Ryy  Rzy		-Rzy  Ryy  Rxy
				 * Rxz	Ryz  Rzz		-Rzz  Ryz  Rxz
				 * */
				math::Matrix<3, 3> R_adapted = _R;		//modified rotation matrix

				/* move z to x */
				R_adapted(0, 0) = _R(0, 2);
				R_adapted(1, 0) = _R(1, 2);
				R_adapted(2, 0) = _R(2, 2);

				/* move x to z */
				R_adapted(0, 2) = _R(0, 0);
				R_adapted(1, 2) = _R(1, 0);
				R_adapted(2, 2) = _R(2, 0);

				/* change direction of pitch (convert to right handed system) */
				R_adapted(0, 0) = -R_adapted(0, 0);
				R_adapted(1, 0) = -R_adapted(1, 0);
				R_adapted(2, 0) = -R_adapted(2, 0);
				euler_angles = R_adapted.to_euler();  //adapted euler angles for fixed wing operation

				/* fill in new attitude data */
				_R = R_adapted;
				_roll    = euler_angles(0);
				_pitch   = euler_angles(1);
				_yaw     = euler_angles(2);

				/* lastly, roll- and yawspeed have to be swaped */
				float helper = _ctrl_state.roll_rate;
				_ctrl_state.roll_rate = -_ctrl_state.yaw_rate;
				_ctrl_state.yaw_rate = helper;
			}

			vehicle_setpoint_poll();

			vehicle_accel_poll();

			vehicle_control_mode_poll();

			vehicle_manual_poll();

			global_pos_poll();

			vehicle_status_poll();

			vehicle_land_detected_poll();

			// the position controller will not emit attitude setpoints in some modes
			// we need to make sure that this flag is reset
			_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;

			/* lock integrator until control is started */
			bool lock_integrator;

			if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
				lock_integrator = false;

			} else {
				lock_integrator = true;
			}

			/* Simple handling of failsafe: deploy parachute if failsafe is on */
			if (_vcontrol_mode.flag_control_termination_enabled) {
				_actuators_airframe.control[7] = 1.0f;
				//warnx("_actuators_airframe.control[1] = 1.0f;");

			} else {
				_actuators_airframe.control[7] = 0.0f;
				//warnx("_actuators_airframe.control[1] = -1.0f;");
			}

			/* if we are in rotary wing mode, do nothing */
			if (_vehicle_status.is_rotary_wing && !_vehicle_status.is_vtol) {
				continue;
			}

			/* default flaps to center */
			float flaps_control = 0.0f;

			static float delta_flaps = 0;

			/* map flaps by default to manual if valid */
			if (PX4_ISFINITE(_manual.flaps) && _vcontrol_mode.flag_control_manual_enabled) {
				flaps_control = 0.5f * (_manual.flaps + 1.0f) * _parameters.flaps_scale;

			} else if (_vcontrol_mode.flag_control_auto_enabled) {
				flaps_control = _att_sp.apply_flaps ? 1.0f * _parameters.flaps_scale : 0.0f;
			}

			// move the actual control value continuous with time
			static hrt_abstime t_flaps_changed = 0;

			if (fabsf(flaps_control - _flaps_cmd_last) > 0.01f) {
				t_flaps_changed = hrt_absolute_time();
				delta_flaps = flaps_control - _flaps_cmd_last;
				_flaps_cmd_last = flaps_control;
			}

			static float flaps_applied = 0.0f;

			if (fabsf(flaps_applied - flaps_control) > 0.01f) {
				flaps_applied = (flaps_control - delta_flaps) + (float)hrt_elapsed_time(&t_flaps_changed) * (delta_flaps) / 1000000;
			}

			/* default flaperon to center */
			float flaperon = 0.0f;

			static float delta_flaperon = 0.0f;

			/* map flaperons by default to manual if valid */
			if (PX4_ISFINITE(_manual.aux2) && _vcontrol_mode.flag_control_manual_enabled) {
				flaperon = 0.5f * (_manual.aux2 + 1.0f) * _parameters.flaperon_scale;

			} else if (_vcontrol_mode.flag_control_auto_enabled) {
				flaperon = _att_sp.apply_flaps ? 1.0f * _parameters.flaperon_scale : 0.0f;
			}

			// move the actual control value continuous with time
			static hrt_abstime t_flaperons_changed = 0;

			if (fabsf(flaperon - _flaperons_cmd_last) > 0.01f) {
				t_flaperons_changed = hrt_absolute_time();
				delta_flaperon = flaperon - _flaperons_cmd_last;
				_flaperons_cmd_last = flaperon;
			}

			static float flaperon_applied = 0.0f;

			if (fabsf(flaperon_applied - flaperon) > 0.01f) {
				flaperon_applied = (flaperon - delta_flaperon) + (float)hrt_elapsed_time(&t_flaperons_changed) *
						   (delta_flaperon) / 1000000;
			}

			/* decide if in stabilized or full manual control */
			if (_vcontrol_mode.flag_control_attitude_enabled) {
				/* scale around tuning airspeed */
				float airspeed;

				/* if airspeed is not updating, we assume the normal average speed */
				if (bool nonfinite = !PX4_ISFINITE(_ctrl_state.airspeed) || !_ctrl_state.airspeed_valid) {
					airspeed = _parameters.airspeed_trim;

					if (nonfinite) {
						perf_count(_nonfinite_input_perf);
					}

				} else {
					/* prevent numerical drama by requiring 0.5 m/s minimal speed */
					airspeed = math::max(0.5f, _ctrl_state.airspeed);
				}

				/*
				 * For scaling our actuators using anything less than the min (close to stall)
				 * speed doesn't make any sense - its the strongest reasonable deflection we
				 * want to do in flight and its the baseline a human pilot would choose.
				 *
				 * Forcing the scaling to this value allows reasonable handheld tests.
				 */
				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min :
							 airspeed);

				/* Use min airspeed to calculate ground speed scaling region.
				 * Don't scale below gspd_scaling_trim
				 */
				float groundspeed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
							  _global_pos.vel_e * _global_pos.vel_e);
				float gspd_scaling_trim = (_parameters.airspeed_min * 0.6f);
				float groundspeed_scaler = gspd_scaling_trim / ((groundspeed < gspd_scaling_trim) ? gspd_scaling_trim : groundspeed);

				float roll_sp = _parameters.rollsp_offset_rad;
				float pitch_sp = _parameters.pitchsp_offset_rad;
				float yaw_sp = 0.0f;
				float yaw_manual = 0.0f;
				float throttle_sp = 0.0f;

				/* Read attitude setpoint from uorb if
				 * - velocity control or position control is enabled (pos controller is running)
				 * - manual control is disabled (another app may send the setpoint, but it should
				 *   for sure not be set from the remote control values)
				 */
				if (_vcontrol_mode.flag_control_auto_enabled ||
				    !_vcontrol_mode.flag_control_manual_enabled) {
					/* read in attitude setpoint from attitude setpoint uorb topic */
					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
					yaw_sp = _att_sp.yaw_body;
					throttle_sp = _att_sp.thrust;

					/* reset integrals where needed */
					if (_att_sp.roll_reset_integral) {
						_roll_ctrl.reset_integrator();
					}

					if (_att_sp.pitch_reset_integral) {
						_pitch_ctrl.reset_integrator();
					}

					if (_att_sp.yaw_reset_integral) {
						_yaw_ctrl.reset_integrator();
						_wheel_ctrl.reset_integrator();
					}

				} else if (_vcontrol_mode.flag_control_velocity_enabled) {

					/* the pilot does not want to change direction,
					 * take straight attitude setpoint from position controller
					 */
					if (fabsf(_manual.y) < 0.01f && fabsf(_roll) < 0.2f) {
						roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;

					} else {
						roll_sp = (_manual.y * _parameters.man_roll_max)
							  + _parameters.rollsp_offset_rad;
					}

					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
					throttle_sp = _att_sp.thrust;

					/* reset integrals where needed */
					if (_att_sp.roll_reset_integral) {
						_roll_ctrl.reset_integrator();
					}

					if (_att_sp.pitch_reset_integral) {
						_pitch_ctrl.reset_integrator();
					}

					if (_att_sp.yaw_reset_integral) {
						_yaw_ctrl.reset_integrator();
						_wheel_ctrl.reset_integrator();
					}

				} else if (_vcontrol_mode.flag_control_altitude_enabled) {
					/*
					 * Velocity should be controlled and manual is enabled.
					*/
					roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
					throttle_sp = _att_sp.thrust;

					/* reset integrals where needed */
					if (_att_sp.roll_reset_integral) {
						_roll_ctrl.reset_integrator();
					}

					if (_att_sp.pitch_reset_integral) {
						_pitch_ctrl.reset_integrator();
					}

					if (_att_sp.yaw_reset_integral) {
						_yaw_ctrl.reset_integrator();
						_wheel_ctrl.reset_integrator();
					}

				} else {
					/*
					 * Scale down roll and pitch as the setpoints are radians
					 * and a typical remote can only do around 45 degrees, the mapping is
					 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
					 *
					 * With this mapping the stick angle is a 1:1 representation of
					 * the commanded attitude.
					 *
					 * The trim gets subtracted here from the manual setpoint to get
					 * the intended attitude setpoint. Later, after the rate control step the
					 * trim is added again to the control signal.
					 */
					roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
					pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
					/* allow manual control of rudder deflection */
					yaw_manual = _manual.r;
					throttle_sp = _manual.z;

					/*
					 * in manual mode no external source should / does emit attitude setpoints.
					 * emit the manual setpoint here to allow attitude controller tuning
					 * in attitude control mode.
					 */
					struct vehicle_attitude_setpoint_s att_sp;
					att_sp.timestamp = hrt_absolute_time();
					att_sp.roll_body = roll_sp;
					att_sp.pitch_body = pitch_sp;
					att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
					att_sp.thrust = throttle_sp;

					/* lazily publish the setpoint only once available */
					if (_attitude_sp_pub != nullptr) {
						/* publish the attitude setpoint */
						orb_publish(_attitude_setpoint_id, _attitude_sp_pub, &att_sp);

					} else if (_attitude_setpoint_id) {
						/* advertise and publish */
						_attitude_sp_pub = orb_advertise(_attitude_setpoint_id, &att_sp);
					}
				}

				/* If the aircraft is on ground reset the integrators */
				if (_vehicle_land_detected.landed || _vehicle_status.is_rotary_wing) {
					_roll_ctrl.reset_integrator();
					_pitch_ctrl.reset_integrator();
					_yaw_ctrl.reset_integrator();
					_wheel_ctrl.reset_integrator();
				}

				/* Prepare speed_body_u and speed_body_w */
				float speed_body_u = _R(0, 0) * _global_pos.vel_n + _R(1, 0) * _global_pos.vel_e + _R(2, 0) * _global_pos.vel_d;
				float speed_body_v = _R(0, 1) * _global_pos.vel_n + _R(1, 1) * _global_pos.vel_e + _R(2, 1) * _global_pos.vel_d;
				float speed_body_w = _R(0, 2) * _global_pos.vel_n + _R(1, 2) * _global_pos.vel_e + _R(2, 2) * _global_pos.vel_d;

				/* Prepare data for attitude controllers */
				struct ECL_ControlData control_input = {};
				control_input.roll = _roll;
				control_input.pitch = _pitch;
				control_input.yaw = _yaw;
				control_input.roll_rate = _ctrl_state.roll_rate;
				control_input.pitch_rate = _ctrl_state.pitch_rate;
				control_input.yaw_rate = _ctrl_state.yaw_rate;
				control_input.speed_body_u = speed_body_u;
				control_input.speed_body_v = speed_body_v;
				control_input.speed_body_w = speed_body_w;
				control_input.acc_body_x = _accel.x;
				control_input.acc_body_y = _accel.y;
				control_input.acc_body_z = _accel.z;
				control_input.roll_setpoint = roll_sp;
				control_input.pitch_setpoint = pitch_sp;
				control_input.yaw_setpoint = yaw_sp;
				control_input.airspeed_min = _parameters.airspeed_min;
				control_input.airspeed_max = _parameters.airspeed_max;
				control_input.airspeed = airspeed;
				control_input.scaler = airspeed_scaling;
				control_input.lock_integrator = lock_integrator;
				control_input.groundspeed = groundspeed;
				control_input.groundspeed_scaler = groundspeed_scaler;

				_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);

				/* Run attitude controllers */
				if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
					_roll_ctrl.control_attitude(control_input);
					_pitch_ctrl.control_attitude(control_input);
					_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
					_wheel_ctrl.control_attitude(control_input);

					/* Update input data for rate controllers */
					control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
					control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
					control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();

					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
					float roll_u = _roll_ctrl.control_bodyrate(control_input);
					_actuators.control[0] = (PX4_ISFINITE(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;

					if (!PX4_ISFINITE(roll_u)) {
						_roll_ctrl.reset_integrator();
						perf_count(_nonfinite_output_perf);

						if (_debug && loop_counter % 10 == 0) {
							warnx("roll_u %.4f", (double)roll_u);
						}
					}

					float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
					_actuators.control[1] = (PX4_ISFINITE(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;

					if (!PX4_ISFINITE(pitch_u)) {
						_pitch_ctrl.reset_integrator();
						perf_count(_nonfinite_output_perf);

						if (_debug && loop_counter % 10 == 0) {
							warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
							      " airspeed %.4f, airspeed_scaling %.4f,"
							      " roll_sp %.4f, pitch_sp %.4f,"
							      " _roll_ctrl.get_desired_rate() %.4f,"
							      " _pitch_ctrl.get_desired_rate() %.4f"
							      " att_sp.roll_body %.4f",
							      (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
							      (double)airspeed, (double)airspeed_scaling,
							      (double)roll_sp, (double)pitch_sp,
							      (double)_roll_ctrl.get_desired_rate(),
							      (double)_pitch_ctrl.get_desired_rate(),
							      (double)_att_sp.roll_body);
						}
					}

					float yaw_u = 0.0f;

					if (_att_sp.fw_control_yaw == true) {
						yaw_u = _wheel_ctrl.control_bodyrate(control_input);
					}

					else {
						yaw_u = _yaw_ctrl.control_bodyrate(control_input);
					}

					_actuators.control[2] = (PX4_ISFINITE(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;

					/* add in manual rudder control */
					_actuators.control[2] += yaw_manual;

					if (!PX4_ISFINITE(yaw_u)) {
						_yaw_ctrl.reset_integrator();
						_wheel_ctrl.reset_integrator();
						perf_count(_nonfinite_output_perf);

						if (_debug && loop_counter % 10 == 0) {
							warnx("yaw_u %.4f", (double)yaw_u);
						}
					}

					/* throttle passed through if it is finite and if no engine failure was
					 * detected */
					_actuators.control[3] = (PX4_ISFINITE(throttle_sp) &&
								 !(_vehicle_status.engine_failure ||
								   _vehicle_status.engine_failure_cmd)) ?
								throttle_sp : 0.0f;

					if (!PX4_ISFINITE(throttle_sp)) {
						if (_debug && loop_counter % 10 == 0) {
							warnx("throttle_sp %.4f", (double)throttle_sp);
						}
					}

				} else {
					perf_count(_nonfinite_input_perf);

					if (_debug && loop_counter % 10 == 0) {
						warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
					}
				}

				/*
				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
				 * only once available
				 */
				_rates_sp.roll = _roll_ctrl.get_desired_rate();
				_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
				_rates_sp.yaw = _yaw_ctrl.get_desired_rate();

				_rates_sp.timestamp = hrt_absolute_time();

				if (_rate_sp_pub != nullptr) {
					/* publish the attitude rates setpoint */
					orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);

				} else if (_rates_sp_id) {
					/* advertise the attitude rates setpoint */
					_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
				}

			} else {
				/* manual/direct control */
				_actuators.control[actuator_controls_s::INDEX_ROLL] = _manual.y + _parameters.trim_roll;
				_actuators.control[actuator_controls_s::INDEX_PITCH] = -_manual.x + _parameters.trim_pitch;
				_actuators.control[actuator_controls_s::INDEX_YAW] = _manual.r + _parameters.trim_yaw;
				_actuators.control[actuator_controls_s::INDEX_THROTTLE] = _manual.z;
			}

			_actuators.control[actuator_controls_s::INDEX_FLAPS] = flaps_applied;
			_actuators.control[5] = _manual.aux1;
			_actuators.control[actuator_controls_s::INDEX_AIRBRAKES] = flaperon_applied;
			_actuators.control[7] = _manual.aux3;

			/* lazily publish the setpoint only once available */
			_actuators.timestamp = hrt_absolute_time();
			_actuators.timestamp_sample = _ctrl_state.timestamp;
			_actuators_airframe.timestamp = hrt_absolute_time();
			_actuators_airframe.timestamp_sample = _ctrl_state.timestamp;

			/* Only publish if any of the proper modes are enabled */
			if (_vcontrol_mode.flag_control_rates_enabled ||
			    _vcontrol_mode.flag_control_attitude_enabled ||
			    _vcontrol_mode.flag_control_manual_enabled) {
				/* publish the actuator controls */
				if (_actuators_0_pub != nullptr) {
					orb_publish(_actuators_id, _actuators_0_pub, &_actuators);

				} else if (_actuators_id) {
					_actuators_0_pub = orb_advertise(_actuators_id, &_actuators);
				}

				if (_actuators_2_pub != nullptr) {
					/* publish the actuator controls*/
					orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);

				} else {
					/* advertise and publish */
					_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
				}
			}
		}

		loop_counter++;
		perf_end(_loop_perf);
	}

	warnx("exiting.\n");

	_control_task = -1;
	_task_running = false;
}
示例#21
0
int
user_start(int argc, char *argv[])
{
	/* run C++ ctors before we go any further */
	up_cxxinitialize();

	/* reset all to zero */
	memset(&system_state, 0, sizeof(system_state));

	/* configure the high-resolution time/callout interface */
	hrt_init();

	/*
	 * Poll at 1ms intervals for received bytes that have not triggered
	 * a DMA event.
	 */
#ifdef CONFIG_ARCH_DMA
	hrt_call_every(&serial_dma_call, 1000, 1000, (hrt_callout)stm32_serial_dma_poll, NULL);
#endif

	/* print some startup info */
	lowsyslog("\nPX4IO: starting\n");

	/* default all the LEDs to off while we start */
	LED_AMBER(false);
	LED_BLUE(false);
	LED_SAFETY(false);

	/* turn on servo power (if supported) */
	POWER_SERVO(true);

	/* start the safety switch handler */
	safety_init();

	/* configure the first 8 PWM outputs (i.e. all of them) */
	up_pwm_servo_init(0xff);

	/* initialise the control inputs */
	controls_init();

	/* start the FMU interface */
	interface_init();

	/* add a performance counter for the interface */
	perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");

	/* add a performance counter for mixing */
	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");

	/* add a performance counter for controls */
	perf_counter_t controls_perf = perf_alloc(PC_ELAPSED, "controls");

	/* and one for measuring the loop rate */
	perf_counter_t loop_perf = perf_alloc(PC_INTERVAL, "loop");

	struct mallinfo minfo = mallinfo();
	lowsyslog("MEM: free %u, largest %u\n", minfo.mxordblk, minfo.fordblks);

#if 0
	/* not enough memory, lock down */
	if (minfo.mxordblk < 500) {
		lowsyslog("ERR: not enough MEM");
		bool phase = false;

		if (phase) {
			LED_AMBER(true);
			LED_BLUE(false);
		} else {
			LED_AMBER(false);
			LED_BLUE(true);
		}

		phase = !phase;
		usleep(300000);
	}
#endif

	/*
	 * Run everything in a tight loop.
	 */

	uint64_t last_debug_time = 0;
	for (;;) {

		/* track the rate at which the loop is running */
		perf_count(loop_perf);

		/* kick the interface */
		perf_begin(interface_perf);
		interface_tick();
		perf_end(interface_perf);

		/* kick the mixer */
		perf_begin(mixer_perf);
		mixer_tick();
		perf_end(mixer_perf);

		/* kick the control inputs */
		perf_begin(controls_perf);
		controls_tick();
		perf_end(controls_perf);

		/* check for debug activity */
		show_debug_messages();

		/* post debug state at ~1Hz */
		if (hrt_absolute_time() - last_debug_time > (1000 * 1000)) {

			struct mallinfo minfo = mallinfo();

			isr_debug(1, "d:%u s=0x%x a=0x%x f=0x%x m=%u", 
				  (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG],
				  (unsigned)r_status_flags,
				  (unsigned)r_setup_arming,
				  (unsigned)r_setup_features,
				  (unsigned)minfo.mxordblk);
			last_debug_time = hrt_absolute_time();
		}
	}
}
示例#22
0
int
ETSAirspeed::collect()
{
	int	ret = -EIO;

	/* read from the sensor */
	uint8_t val[2] = {0, 0};

	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 2);

	if (ret < 0) {
		perf_count(_comms_errors);
		return ret;
	}

	uint16_t diff_pres_pa_raw = val[1] << 8 | val[0];
        if (diff_pres_pa_raw == 0) {
		// a zero value means the pressure sensor cannot give us a
		// value. We need to return, and not report a value or the
		// caller could end up using this value as part of an
		// average
		perf_count(_comms_errors);
		DEVICE_LOG("zero value from sensor"); 
		return -1;
        }

	// The raw value still should be compensated for the known offset
	diff_pres_pa_raw -= _diff_pres_offset;

	// Track maximum differential pressure measured (so we can work out top speed).
	if (diff_pres_pa_raw > _max_differential_pressure_pa) {
		_max_differential_pressure_pa = diff_pres_pa_raw;
	}

	differential_pressure_s report;
	report.timestamp = hrt_absolute_time();
        report.error_count = perf_event_count(_comms_errors);

	// XXX we may want to smooth out the readings to remove noise.
	report.differential_pressure_filtered_pa = diff_pres_pa_raw;
	report.differential_pressure_raw_pa = diff_pres_pa_raw;
	report.temperature = -1000.0f;
	report.max_differential_pressure_pa = _max_differential_pressure_pa;

	if (_airspeed_pub != nullptr && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
	}

	new_report(report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);

	return ret;
}
void
FixedwingEstimator::task_main()
{
	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);

	_ekf = new AttPosEKF();
	float dt = 0.0f; // time lapsed since last covariance prediction
	_filter_start_time = hrt_absolute_time();

	if (!_ekf) {
		errx(1, "OUT OF MEM!");
	}

	/*
	 * do subscriptions
	 */
	_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
	_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_home_sub = orb_subscribe(ORB_ID(home_position));

	/* rate limit vehicle status updates to 5Hz */
	orb_set_interval(_vstatus_sub, 200);

#ifndef SENSOR_COMBINED_SUB

	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));

	/* rate limit gyro updates to 50 Hz */
	/* XXX remove this!, BUT increase the data buffer size! */
	orb_set_interval(_gyro_sub, 4);
#else
	_sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
	/* XXX remove this!, BUT increase the data buffer size! */
	orb_set_interval(_sensor_combined_sub, 9);
#endif

	/* sets also parameters in the EKF object */
	parameters_update();

	Vector3f lastAngRate;
	Vector3f lastAccel;

	/* wakeup source(s) */
	struct pollfd fds[2];

	/* Setup of loop */
	fds[0].fd = _params_sub;
	fds[0].events = POLLIN;
#ifndef SENSOR_COMBINED_SUB
	fds[1].fd = _gyro_sub;
	fds[1].events = POLLIN;
#else
	fds[1].fd = _sensor_combined_sub;
	fds[1].events = POLLIN;
#endif

	bool newDataGps = false;
	bool newHgtData = false;
	bool newAdsData = false;
	bool newDataMag = false;

	float posNED[3] = {0.0f, 0.0f, 0.0f}; // North, East Down position (m)

	uint64_t last_gps = 0;
	_gps.vel_n_m_s = 0.0f;
	_gps.vel_e_m_s = 0.0f;
	_gps.vel_d_m_s = 0.0f;

	while (!_task_should_exit) {

		/* wait for up to 500ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		/* timed out - periodic check for _task_should_exit, etc. */
		if (pret == 0)
			continue;

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("POLL ERR %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);

		/* only update parameters if they changed */
		if (fds[0].revents & POLLIN) {
			/* read from param to clear updated flag */
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), _params_sub, &update);

			/* update parameters from storage */
			parameters_update();
		}

		/* only run estimator if gyro updated */
		if (fds[1].revents & POLLIN) {

			/* check vehicle status for changes to publication state */
			bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON);
			vehicle_status_poll();

			bool accel_updated;
			bool mag_updated;

			perf_count(_perf_gyro);

			/* Reset baro reference if switching to HIL, reset sensor states */
			if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) {
				/* system is in HIL now, wait for measurements to come in one last round */
				usleep(60000);

#ifndef SENSOR_COMBINED_SUB
				orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);
				orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
				orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);
#else
				/* now read all sensor publications to ensure all real sensor data is purged */
				orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);
#endif

				/* set sensors to de-initialized state */
				_gyro_valid = false;
				_accel_valid = false;
				_mag_valid = false;

				_baro_init = false;
				_gps_initialized = false;
				_last_sensor_timestamp = hrt_absolute_time();
				_last_run = _last_sensor_timestamp;

				_ekf->ZeroVariables();
				_ekf->dtIMU = 0.01f;
				_filter_start_time = _last_sensor_timestamp;

				/* now skip this loop and get data on the next one, which will also re-init the filter */
				continue;
			}

			/**
			 *    PART ONE: COLLECT ALL DATA
			 **/

			/* load local copies */
#ifndef SENSOR_COMBINED_SUB
			orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &_gyro);


			orb_check(_accel_sub, &accel_updated);

			if (accel_updated) {
				orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel);
			}

			_last_sensor_timestamp = _gyro.timestamp;
			IMUmsec = _gyro.timestamp / 1e3f;

			float deltaT = (_gyro.timestamp - _last_run) / 1e6f;
			_last_run = _gyro.timestamp;

			/* guard against too large deltaT's */
			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
				deltaT = 0.01f;
			}


			// Always store data, independent of init status
			/* fill in last data set */
			_ekf->dtIMU = deltaT;

			if (isfinite(_gyro.x) &&
				isfinite(_gyro.y) &&
				isfinite(_gyro.z)) {
				_ekf->angRate.x = _gyro.x;
				_ekf->angRate.y = _gyro.y;
				_ekf->angRate.z = _gyro.z;

				if (!_gyro_valid) {
					lastAngRate = _ekf->angRate;
				}

				_gyro_valid = true;
			}

			if (accel_updated) {
				_ekf->accel.x = _accel.x;
				_ekf->accel.y = _accel.y;
				_ekf->accel.z = _accel.z;

				if (!_accel_valid) {
					lastAccel = _ekf->accel;
				}

				_accel_valid = true;
			}

			_ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU;
			_ekf->lastAngRate = angRate;
			_ekf->dVelIMU = 0.5f * (accel + lastAccel) * dtIMU;
			_ekf->lastAccel = accel;


#else
			orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined);

			static hrt_abstime last_accel = 0;
			static hrt_abstime last_mag = 0;

			if (last_accel != _sensor_combined.accelerometer_timestamp) {
				accel_updated = true;
			} else {
				accel_updated = false;
			}

			last_accel = _sensor_combined.accelerometer_timestamp;


			// Copy gyro and accel
			_last_sensor_timestamp = _sensor_combined.timestamp;
			IMUmsec = _sensor_combined.timestamp / 1e3f;

			float deltaT = (_sensor_combined.timestamp - _last_run) / 1e6f;

			/* guard against too large deltaT's */
			if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) {
				deltaT = 0.01f;
			}

			_last_run = _sensor_combined.timestamp;

			// Always store data, independent of init status
			/* fill in last data set */
			_ekf->dtIMU = deltaT;

			if (isfinite(_sensor_combined.gyro_rad_s[0]) &&
				isfinite(_sensor_combined.gyro_rad_s[1]) &&
				isfinite(_sensor_combined.gyro_rad_s[2])) {
				_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
				_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
				_ekf->angRate.z = _sensor_combined.gyro_rad_s[2];

				if (!_gyro_valid) {
					lastAngRate = _ekf->angRate;
				}

				_gyro_valid = true;
				perf_count(_perf_gyro);
			}

			if (accel_updated) {
				_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
				_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
				_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];

				if (!_accel_valid) {
					lastAccel = _ekf->accel;
				}

				_accel_valid = true;
			}

			_ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU;
			lastAngRate = _ekf->angRate;
			_ekf->dVelIMU = 0.5f * (_ekf->accel + lastAccel) * _ekf->dtIMU;
			lastAccel = _ekf->accel;

			if (last_mag != _sensor_combined.magnetometer_timestamp) {
				mag_updated = true;
				newDataMag = true;

			} else {
				newDataMag = false;
			}

			last_mag = _sensor_combined.magnetometer_timestamp;

#endif

			//warnx("dang: %8.4f %8.4f dvel: %8.4f %8.4f", _ekf->dAngIMU.x, _ekf->dAngIMU.z, _ekf->dVelIMU.x, _ekf->dVelIMU.z);

			bool airspeed_updated;
			orb_check(_airspeed_sub, &airspeed_updated);

			if (airspeed_updated) {
				orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
				perf_count(_perf_airspeed);

				_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
				newAdsData = true;

			} else {
				newAdsData = false;
			}

			bool gps_updated;
			orb_check(_gps_sub, &gps_updated);

			if (gps_updated) {

				last_gps = _gps.timestamp_position;

				orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &_gps);
				perf_count(_perf_gps);

				if (_gps.fix_type < 3) {
					newDataGps = false;

				} else {

					/* store time of valid GPS measurement */
					_gps_start_time = hrt_absolute_time();

					/* check if we had a GPS outage for a long time */
					if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) {
						_ekf->ResetPosition();
						_ekf->ResetVelocity();
						_ekf->ResetStoredStates();
					}

					/* fuse GPS updates */

					//_gps.timestamp / 1e3;
					_ekf->GPSstatus = _gps.fix_type;
					_ekf->velNED[0] = _gps.vel_n_m_s;
					_ekf->velNED[1] = _gps.vel_e_m_s;
					_ekf->velNED[2] = _gps.vel_d_m_s;

					// warnx("GPS updated: status: %d, vel: %8.4f %8.4f %8.4f", (int)GPSstatus, velNED[0], velNED[1], velNED[2]);

					_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
					_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
					_ekf->gpsHgt = _gps.alt / 1e3f;

					// if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) {
					// 	_ekf->vneSigma = sqrtf(_gps.s_variance_m_s);
					// } else {
					// 	_ekf->vneSigma = _parameters.velne_noise;
					// }

					// if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) {
					// 	_ekf->posNeSigma = sqrtf(_gps.p_variance_m);
					// } else {
					// 	_ekf->posNeSigma = _parameters.posne_noise;
					// }

					// warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m);

					newDataGps = true;

				}

			}

			bool baro_updated;
			orb_check(_baro_sub, &baro_updated);

			if (baro_updated) {
				orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);

				_ekf->baroHgt = _baro.altitude;

				if (!_baro_init) {
					_baro_ref = _baro.altitude;
					_baro_init = true;
					warnx("ALT REF INIT");
				}

				perf_count(_perf_baro);

				newHgtData = true;
			} else {
				newHgtData = false;
			}

#ifndef SENSOR_COMBINED_SUB
			orb_check(_mag_sub, &mag_updated);
#endif

			if (mag_updated) {

				_mag_valid = true;

				perf_count(_perf_mag);

#ifndef SENSOR_COMBINED_SUB
				orb_copy(ORB_ID(sensor_mag), _mag_sub, &_mag);

				// XXX we compensate the offsets upfront - should be close to zero.
				// 0.001f
				_ekf->magData.x = _mag.x;
				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset

				_ekf->magData.y = _mag.y;
				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset

				_ekf->magData.z = _mag.z;
				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset

#else

				// XXX we compensate the offsets upfront - should be close to zero.
				// 0.001f
				_ekf->magData.x = _sensor_combined.magnetometer_ga[0];
				_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset

				_ekf->magData.y = _sensor_combined.magnetometer_ga[1];
				_ekf->magBias.y = 0.000001f; // _mag_offsets.y_offset

				_ekf->magData.z = _sensor_combined.magnetometer_ga[2];
				_ekf->magBias.z = 0.000001f; // _mag_offsets.y_offset

#endif

				newDataMag = true;

			} else {
				newDataMag = false;
			}

			/*
			 *    CHECK IF ITS THE RIGHT TIME TO RUN THINGS ALREADY
			 */
			if (hrt_elapsed_time(&_filter_start_time) < FILTER_INIT_DELAY) {
				continue;
			}

			/**
			 *    PART TWO: EXECUTE THE FILTER
			 *
			 *    We run the filter only once all data has been fetched
			 **/

			if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {

				float initVelNED[3];

				/* Initialize the filter first */
				if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph < _parameters.pos_stddev_threshold && _gps.epv < _parameters.pos_stddev_threshold) {

					// GPS is in scaled integers, convert
					double lat = _gps.lat / 1.0e7;
					double lon = _gps.lon / 1.0e7;
					float gps_alt = _gps.alt / 1e3f;

					initVelNED[0] = _gps.vel_n_m_s;
					initVelNED[1] = _gps.vel_e_m_s;
					initVelNED[2] = _gps.vel_d_m_s;

					// Set up height correctly
					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
					_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
					_baro_gps_offset = _baro.altitude - gps_alt;
					_ekf->baroHgt = _baro.altitude;
					_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));

					// Set up position variables correctly
					_ekf->GPSstatus = _gps.fix_type;

					_ekf->gpsLat = math::radians(lat);
					_ekf->gpsLon = math::radians(lon) - M_PI;
					_ekf->gpsHgt = gps_alt;

					// Look up mag declination based on current position
					float declination = math::radians(get_mag_declination(lat, lon));

					_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);

					// Initialize projection
					_local_pos.ref_lat = lat;
					_local_pos.ref_lon = lon;
					_local_pos.ref_alt = gps_alt;
					_local_pos.ref_timestamp = _gps.timestamp_position;

					map_projection_init(&_pos_ref, lat, lon);
					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);

					#if 0
					warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
						(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
					warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_ref_offset);
					warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv, (double)math::degrees(declination));
					#endif

					_gps_initialized = true;

				} else if (!_ekf->statesInitialised) {

					initVelNED[0] = 0.0f;
					initVelNED[1] = 0.0f;
					initVelNED[2] = 0.0f;
					_ekf->posNE[0] = posNED[0];
					_ekf->posNE[1] = posNED[1];

					_local_pos.ref_alt = _baro_ref;
					_baro_ref_offset = 0.0f;
					_baro_gps_offset = 0.0f;

					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
				} else if (_ekf->statesInitialised) {

					// We're apparently initialized in this case now

					int check = check_filter_state();

					if (check) {
						// Let the system re-initialize itself
						continue;
					}


					// Run the strapdown INS equations every IMU update
					_ekf->UpdateStrapdownEquationsNED();
	#if 0
					// debug code - could be tunred into a filter mnitoring/watchdog function
					float tempQuat[4];

					for (uint8_t j = 0; j <= 3; j++) tempQuat[j] = states[j];

					quat2eul(eulerEst, tempQuat);

					for (uint8_t j = 0; j <= 2; j++) eulerDif[j] = eulerEst[j] - ahrsEul[j];

					if (eulerDif[2] > pi) eulerDif[2] -= 2 * pi;

					if (eulerDif[2] < -pi) eulerDif[2] += 2 * pi;

	#endif
					// store the predicted states for subsequent use by measurement fusion
					_ekf->StoreStates(IMUmsec);
					// Check if on ground - status is used by covariance prediction
					_ekf->OnGroundCheck();
					// sum delta angles and time used by covariance prediction
					_ekf->summedDelAng = _ekf->summedDelAng + _ekf->correctedDelAng;
					_ekf->summedDelVel = _ekf->summedDelVel + _ekf->dVelIMU;
					dt += _ekf->dtIMU;

					// perform a covariance prediction if the total delta angle has exceeded the limit
					// or the time limit will be exceeded at the next IMU update
					if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) {
						_ekf->CovariancePrediction(dt);
						_ekf->summedDelAng.zero();
						_ekf->summedDelVel.zero();
						dt = 0.0f;
					}

					// Fuse GPS Measurements
					if (newDataGps && _gps_initialized) {
						// Convert GPS measurements to Pos NE, hgt and Vel NED

						float gps_dt = (_gps.timestamp_position - last_gps) / 1e6f;

						// Calculate acceleration predicted by GPS velocity change
						if (((fabsf(_ekf->velNED[0] - _gps.vel_n_m_s) > FLT_EPSILON) ||
							(fabsf(_ekf->velNED[1] - _gps.vel_e_m_s) > FLT_EPSILON) ||
							(fabsf(_ekf->velNED[2] - _gps.vel_d_m_s) > FLT_EPSILON)) && (gps_dt > 0.00001f)) {

							_ekf->accelGPSNED[0] = (_ekf->velNED[0] - _gps.vel_n_m_s) / gps_dt;
							_ekf->accelGPSNED[1] = (_ekf->velNED[1] - _gps.vel_e_m_s) / gps_dt;
							_ekf->accelGPSNED[2] = (_ekf->velNED[2] - _gps.vel_d_m_s) / gps_dt;
						}

						_ekf->velNED[0] = _gps.vel_n_m_s;
						_ekf->velNED[1] = _gps.vel_e_m_s;
						_ekf->velNED[2] = _gps.vel_d_m_s;
						_ekf->calcposNED(posNED, _ekf->gpsLat, _ekf->gpsLon, _ekf->gpsHgt, _ekf->latRef, _ekf->lonRef, _ekf->hgtRef);

						_ekf->posNE[0] = posNED[0];
						_ekf->posNE[1] = posNED[1];
						// set fusion flags
						_ekf->fuseVelData = true;
						_ekf->fusePosData = true;
						// recall states stored at time of measurement after adjusting for delays
						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
						// run the fusion step
						_ekf->FuseVelposNED();

					} else if (_ekf->statesInitialised) {
						// Convert GPS measurements to Pos NE, hgt and Vel NED
						_ekf->velNED[0] = 0.0f;
						_ekf->velNED[1] = 0.0f;
						_ekf->velNED[2] = 0.0f;

						_ekf->posNE[0] = 0.0f;
						_ekf->posNE[1] = 0.0f;
						// set fusion flags
						_ekf->fuseVelData = true;
						_ekf->fusePosData = true;
						// recall states stored at time of measurement after adjusting for delays
						_ekf->RecallStates(_ekf->statesAtVelTime, (IMUmsec - _parameters.vel_delay_ms));
						_ekf->RecallStates(_ekf->statesAtPosTime, (IMUmsec - _parameters.pos_delay_ms));
						// run the fusion step
						_ekf->FuseVelposNED();

					} else {
						_ekf->fuseVelData = false;
						_ekf->fusePosData = false;
					}

					if (newHgtData && _ekf->statesInitialised) {
						// Could use a blend of GPS and baro alt data if desired
						_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
						_ekf->fuseHgtData = true;
						// recall states stored at time of measurement after adjusting for delays
						_ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms));
						// run the fusion step
						_ekf->FuseVelposNED();

					} else {
						_ekf->fuseHgtData = false;
					}

					// Fuse Magnetometer Measurements
					if (newDataMag && _ekf->statesInitialised) {
						_ekf->fuseMagData = true;
						_ekf->RecallStates(_ekf->statesAtMagMeasTime, (IMUmsec - _parameters.mag_delay_ms)); // Assume 50 msec avg delay for magnetometer data

						_ekf->magstate.obsIndex = 0;
						_ekf->FuseMagnetometer();
						_ekf->FuseMagnetometer();
						_ekf->FuseMagnetometer();

					} else {
						_ekf->fuseMagData = false;
					}

					// Fuse Airspeed Measurements
					if (newAdsData && _ekf->statesInitialised && _ekf->VtasMeas > 8.0f) {
						_ekf->fuseVtasData = true;
						_ekf->RecallStates(_ekf->statesAtVtasMeasTime, (IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
						_ekf->FuseAirspeed();

					} else {
						_ekf->fuseVtasData = false;
					}


					// Output results
					math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
					math::Matrix<3, 3> R = q.to_dcm();
					math::Vector<3> euler = R.to_euler();

					for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
							_att.R[i][j] = R(i, j);

					_att.timestamp = _last_sensor_timestamp;
					_att.q[0] = _ekf->states[0];
					_att.q[1] = _ekf->states[1];
					_att.q[2] = _ekf->states[2];
					_att.q[3] = _ekf->states[3];
					_att.q_valid = true;
					_att.R_valid = true;

					_att.timestamp = _last_sensor_timestamp;
					_att.roll = euler(0);
					_att.pitch = euler(1);
					_att.yaw = euler(2);

					_att.rollspeed = _ekf->angRate.x - _ekf->states[10];
					_att.pitchspeed = _ekf->angRate.y - _ekf->states[11];
					_att.yawspeed = _ekf->angRate.z - _ekf->states[12];
					// gyro offsets
					_att.rate_offsets[0] = _ekf->states[10];
					_att.rate_offsets[1] = _ekf->states[11];
					_att.rate_offsets[2] = _ekf->states[12];

					/* lazily publish the attitude only once available */
					if (_att_pub > 0) {
						/* publish the attitude setpoint */
						orb_publish(ORB_ID(vehicle_attitude), _att_pub, &_att);

					} else {
						/* advertise and publish */
						_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &_att);
					}

					if (_gps_initialized) {
						_local_pos.timestamp = _last_sensor_timestamp;
						_local_pos.x = _ekf->states[7];
						_local_pos.y = _ekf->states[8];
						// XXX need to announce change of Z reference somehow elegantly
						_local_pos.z = _ekf->states[9] - _baro_ref_offset;

						_local_pos.vx = _ekf->states[4];
						_local_pos.vy = _ekf->states[5];
						_local_pos.vz = _ekf->states[6];

						_local_pos.xy_valid = _gps_initialized;
						_local_pos.z_valid = true;
						_local_pos.v_xy_valid = _gps_initialized;
						_local_pos.v_z_valid = true;
						_local_pos.xy_global = true;

						_velocity_xy_filtered = 0.95f*_velocity_xy_filtered + 0.05f*sqrtf(_local_pos.vx*_local_pos.vx + _local_pos.vy*_local_pos.vy);
						_velocity_z_filtered = 0.95f*_velocity_z_filtered + 0.05f*fabsf(_local_pos.vz);
						_airspeed_filtered = 0.95f*_airspeed_filtered + + 0.05f*_airspeed.true_airspeed_m_s;


						/* crude land detector for fixedwing only,
						* TODO: adapt so that it works for both, maybe move to another location
						*/
						if (_velocity_xy_filtered < 5
							&& _velocity_z_filtered < 10
							&& _airspeed_filtered < 10) {
							_local_pos.landed = true;
						} else {
							_local_pos.landed = false;
						}

						_local_pos.z_global = false;
						_local_pos.yaw = _att.yaw;

						/* lazily publish the local position only once available */
						if (_local_pos_pub > 0) {
							/* publish the attitude setpoint */
							orb_publish(ORB_ID(vehicle_local_position), _local_pos_pub, &_local_pos);

						} else {
							/* advertise and publish */
							_local_pos_pub = orb_advertise(ORB_ID(vehicle_local_position), &_local_pos);
						}

						_global_pos.timestamp = _local_pos.timestamp;

						if (_local_pos.xy_global) {
							double est_lat, est_lon;
							map_projection_reproject(&_pos_ref, _local_pos.x, _local_pos.y, &est_lat, &est_lon);
							_global_pos.lat = est_lat;
							_global_pos.lon = est_lon;
							_global_pos.time_gps_usec = _gps.time_gps_usec;
							_global_pos.eph = _gps.eph;
							_global_pos.epv = _gps.epv;
						}

						if (_local_pos.v_xy_valid) {
							_global_pos.vel_n = _local_pos.vx;
							_global_pos.vel_e = _local_pos.vy;
						} else {
							_global_pos.vel_n = 0.0f;
							_global_pos.vel_e = 0.0f;
						}

						/* local pos alt is negative, change sign and add alt offsets */
						_global_pos.alt = _baro_ref + (-_local_pos.z) - _baro_gps_offset;

						if (_local_pos.v_z_valid) {
							_global_pos.vel_d = _local_pos.vz;
						}


						_global_pos.yaw = _local_pos.yaw;

						_global_pos.eph = _gps.eph;
						_global_pos.epv = _gps.epv;

						_global_pos.timestamp = _local_pos.timestamp;

						/* lazily publish the global position only once available */
						if (_global_pos_pub > 0) {
							/* publish the global position */
							orb_publish(ORB_ID(vehicle_global_position), _global_pos_pub, &_global_pos);

						} else {
							/* advertise and publish */
							_global_pos_pub = orb_advertise(ORB_ID(vehicle_global_position), &_global_pos);
						}

						if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
							_wind.timestamp = _global_pos.timestamp;
							_wind.windspeed_north = _ekf->states[14];
							_wind.windspeed_east = _ekf->states[15];
							_wind.covariance_north = 0.0f; // XXX get form filter
							_wind.covariance_east = 0.0f;

							/* lazily publish the wind estimate only once available */
							if (_wind_pub > 0) {
								/* publish the wind estimate */
								orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);

							} else {
								/* advertise and publish */
								_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
							}

						}

					}

				}

				if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
					_wind.timestamp = _global_pos.timestamp;
					_wind.windspeed_north = _ekf->states[14];
					_wind.windspeed_east = _ekf->states[15];
					_wind.covariance_north = _ekf->P[14][14];
					_wind.covariance_east = _ekf->P[15][15];

					/* lazily publish the wind estimate only once available */
					if (_wind_pub > 0) {
						/* publish the wind estimate */
						orb_publish(ORB_ID(wind_estimate), _wind_pub, &_wind);

					} else {
						/* advertise and publish */
						_wind_pub = orb_advertise(ORB_ID(wind_estimate), &_wind);
					}
				}
			}

		}

		perf_end(_loop_perf);
	}

	warnx("exiting.\n");

	_estimator_task = -1;
	_exit(0);
}
示例#24
0
void
MPU9250::measure()
{

	if (hrt_absolute_time() < _reset_wait) {
		// we're waiting for a reset to complete
		return;
	}

	struct MPUReport mpu_report;

	struct Report {
		int16_t		accel_x;
		int16_t		accel_y;
		int16_t		accel_z;
		int16_t		temp;
		int16_t		gyro_x;
		int16_t		gyro_y;
		int16_t		gyro_z;
	} report;

	/* start measuring */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the MPU9250 in one pass
	 */

	if ((!_magnetometer_only || _mag->is_passthrough()) && _register_wait == 0) {
		if (_whoami == MPU_WHOAMI_9250 || _whoami == MPU_WHOAMI_6500) {
			if (OK != read_reg_range(MPUREG_INT_STATUS, MPU9250_HIGH_BUS_SPEED, (uint8_t *)&mpu_report, sizeof(mpu_report))) {
				perf_end(_sample_perf);
				return;
			}

		}

		check_registers();

		if (check_duplicate(&mpu_report.accel_x[0])) {
			return;
		}
	}

	/*
	 * In case of a mag passthrough read, hand the magnetometer data over to _mag. Else,
	 * try to read a magnetometer report.
	 */

#   ifdef USE_I2C

	if (_mag->is_passthrough()) {
#   endif

		if (_register_wait == 0) {
			_mag->_measure(mpu_report.mag);
		}

#   ifdef USE_I2C

	} else {
		_mag->measure();
	}

#   endif

	/*
	 * Continue evaluating gyro and accelerometer results
	 */
	if (!_magnetometer_only && _register_wait == 0) {

		/*
		 * Convert from big to little endian
		 */
		report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
		report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
		report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
		report.temp    = int16_t_from_bytes(mpu_report.temp);
		report.gyro_x  = int16_t_from_bytes(mpu_report.gyro_x);
		report.gyro_y  = int16_t_from_bytes(mpu_report.gyro_y);
		report.gyro_z  = int16_t_from_bytes(mpu_report.gyro_z);

		if (check_null_data((uint16_t *)&report, sizeof(report) / 2)) {
			return;
		}
	}

	if (_register_wait != 0) {
		/*
		 * We are waiting for some good transfers before using the sensor again.
		 * We still increment _good_transfers, but don't return any data yet.
		 *
		*/
		_register_wait--;
		return;
	}

	/*
	 * Get sensor temperature
	 */
	_last_temperature = (report.temp) / 333.87f + 21.0f;


	/*
	 * Convert and publish accelerometer and gyrometer data.
	 */

	if (!_magnetometer_only) {

		/*
		 * Swap axes and negate y
		 */

		int16_t accel_xt = report.accel_y;
		int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);

		int16_t gyro_xt = report.gyro_y;
		int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);

		/*
		 * Apply the swap
		 */
		report.accel_x = accel_xt;
		report.accel_y = accel_yt;
		report.gyro_x = gyro_xt;
		report.gyro_y = gyro_yt;

		/*
		 * Report buffers.
		 */
		sensor_accel_s		arb;
		sensor_gyro_s			grb;

		/*
		 * Adjust and scale results to m/s^2.
		 */
		grb.timestamp = arb.timestamp = hrt_absolute_time();

		// report the error count as the sum of the number of bad
		// transfers and bad register reads. This allows the higher
		// level code to decide if it should use this sensor based on
		// whether it has had failures
		grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);

		/*
		 * 1) Scale raw value to SI units using scaling from datasheet.
		 * 2) Subtract static offset (in SI units)
		 * 3) Scale the statically calibrated values with a linear
		 *    dynamically obtained factor
		 *
		 * Note: the static sensor offset is the number the sensor outputs
		 * 	 at a nominally 'zero' input. Therefore the offset has to
		 * 	 be subtracted.
		 *
		 *	 Example: A gyro outputs a value of 74 at zero angular rate
		 *	 	  the offset is 74 from the origin and subtracting
		 *		  74 from all measurements centers them around zero.
		 */

		/* NOTE: Axes have been swapped to match the board a few lines above. */

		arb.x_raw = report.accel_x;
		arb.y_raw = report.accel_y;
		arb.z_raw = report.accel_z;

		float xraw_f = report.accel_x;
		float yraw_f = report.accel_y;
		float zraw_f = report.accel_z;

		// apply user specified rotation
		rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

		float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
		float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
		float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

		arb.x = _accel_filter_x.apply(x_in_new);
		arb.y = _accel_filter_y.apply(y_in_new);
		arb.z = _accel_filter_z.apply(z_in_new);

		matrix::Vector3f aval(x_in_new, y_in_new, z_in_new);
		matrix::Vector3f aval_integrated;

		bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
		arb.x_integral = aval_integrated(0);
		arb.y_integral = aval_integrated(1);
		arb.z_integral = aval_integrated(2);

		arb.scaling = _accel_range_scale;

		arb.temperature = _last_temperature;

		/* return device ID */
		arb.device_id = _accel->_device_id.devid;

		grb.x_raw = report.gyro_x;
		grb.y_raw = report.gyro_y;
		grb.z_raw = report.gyro_z;

		xraw_f = report.gyro_x;
		yraw_f = report.gyro_y;
		zraw_f = report.gyro_z;

		// apply user specified rotation
		rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

		float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
		float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
		float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

		grb.x = _gyro_filter_x.apply(x_gyro_in_new);
		grb.y = _gyro_filter_y.apply(y_gyro_in_new);
		grb.z = _gyro_filter_z.apply(z_gyro_in_new);

		matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
		matrix::Vector3f gval_integrated;

		bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
		grb.x_integral = gval_integrated(0);
		grb.y_integral = gval_integrated(1);
		grb.z_integral = gval_integrated(2);

		grb.scaling = _gyro_range_scale;

		grb.temperature = _last_temperature;

		/* return device ID */
		grb.device_id = _gyro->_device_id.devid;

		_accel_reports->force(&arb);
		_gyro_reports->force(&grb);

		/* notify anyone waiting for data */
		if (accel_notify) {
			_accel->poll_notify(POLLIN);
		}

		if (gyro_notify) {
			_gyro->parent_poll_notify();
		}

		if (accel_notify && !(_accel->_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
		}

		if (gyro_notify && !(_gyro->_pub_blocked)) {
			/* publish it */
			orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
		}
	}

	/* stop measuring */
	perf_end(_sample_perf);
}
示例#25
0
void
FixedwingAttitudeControl::task_main()
{

	/* inform about start */
	warnx("Initializing..");
	fflush(stdout);

	/*
	 * do subscriptions
	 */
	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
	_accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0);
	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));

	/* rate limit vehicle status updates to 5Hz */
	orb_set_interval(_vcontrol_mode_sub, 200);
	/* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */
	orb_set_interval(_att_sub, 17);

	parameters_update();

	/* get an initial update for all sensor and status data */
	vehicle_airspeed_poll();
	vehicle_setpoint_poll();
	vehicle_accel_poll();
	vehicle_control_mode_poll();
	vehicle_manual_poll();
	vehicle_status_poll();

	/* wakeup source(s) */
	struct pollfd fds[2];

	/* Setup of loop */
	fds[0].fd = _params_sub;
	fds[0].events = POLLIN;
	fds[1].fd = _att_sub;
	fds[1].events = POLLIN;

	_task_running = true;

	while (!_task_should_exit) {

		static int loop_counter = 0;

		/* wait for up to 500ms for data */
		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

		/* timed out - periodic check for _task_should_exit, etc. */
		if (pret == 0)
			continue;

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("poll error %d, %d", pret, errno);
			continue;
		}

		perf_begin(_loop_perf);

		/* only update parameters if they changed */
		if (fds[0].revents & POLLIN) {
			/* read from param to clear updated flag */
			struct parameter_update_s update;
			orb_copy(ORB_ID(parameter_update), _params_sub, &update);

			/* update parameters from storage */
			parameters_update();
		}

		/* only run controller if attitude changed */
		if (fds[1].revents & POLLIN) {


			static uint64_t last_run = 0;
			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();

			/* guard against too large deltaT's */
			if (deltaT > 1.0f)
				deltaT = 0.01f;

			/* load local copies */
			orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);

			if (_vehicle_status.is_vtol) {
				/* vehicle type is VTOL, need to modify attitude!
				 * The following modification to the attitude is vehicle specific and in this case applies
				 *  to tail-sitter models !!!
				 *
				 * Since the VTOL airframe is initialized as a multicopter we need to
				 * modify the estimated attitude for the fixed wing operation.
				 * Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
				 * the pitch axis compared to the neutral position of the vehicle in multicopter mode
				 * we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
				 * Additionally, in order to get the correct sign of the pitch, we need to multiply
				 * the new x axis of the rotation matrix with -1
				 *
				 * original:			modified:
				 *
				 * Rxx  Ryx  Rzx		-Rzx  Ryx  Rxx
				 * Rxy	Ryy  Rzy		-Rzy  Ryy  Rxy
				 * Rxz	Ryz  Rzz		-Rzz  Ryz  Rxz
				 * */
				math::Matrix<3, 3> R;				//original rotation matrix
				math::Matrix<3, 3> R_adapted;		//modified rotation matrix
				R.set(_att.R);
				R_adapted.set(_att.R);

				/* move z to x */
				R_adapted(0, 0) = R(0, 2);
				R_adapted(1, 0) = R(1, 2);
				R_adapted(2, 0) = R(2, 2);

				/* move x to z */
				R_adapted(0, 2) = R(0, 0);
				R_adapted(1, 2) = R(1, 0);
				R_adapted(2, 2) = R(2, 0);

				/* change direction of pitch (convert to right handed system) */
				R_adapted(0, 0) = -R_adapted(0, 0);
				R_adapted(1, 0) = -R_adapted(1, 0);
				R_adapted(2, 0) = -R_adapted(2, 0);
				math::Vector<3> euler_angles;		//adapted euler angles for fixed wing operation
				euler_angles = R_adapted.to_euler();

				/* fill in new attitude data */
				_att.roll    = euler_angles(0);
				_att.pitch   = euler_angles(1);
				_att.yaw     = euler_angles(2);
				PX4_R(_att.R, 0, 0) = R_adapted(0, 0);
				PX4_R(_att.R, 0, 1) = R_adapted(0, 1);
				PX4_R(_att.R, 0, 2) = R_adapted(0, 2);
				PX4_R(_att.R, 1, 0) = R_adapted(1, 0);
				PX4_R(_att.R, 1, 1) = R_adapted(1, 1);
				PX4_R(_att.R, 1, 2) = R_adapted(1, 2);
				PX4_R(_att.R, 2, 0) = R_adapted(2, 0);
				PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
				PX4_R(_att.R, 2, 2) = R_adapted(2, 2);

				/* lastly, roll- and yawspeed have to be swaped */
				float helper = _att.rollspeed;
				_att.rollspeed = -_att.yawspeed;
				_att.yawspeed = helper;
			}

			vehicle_airspeed_poll();

			vehicle_setpoint_poll();

			vehicle_accel_poll();

			vehicle_control_mode_poll();

			vehicle_manual_poll();

			global_pos_poll();

			vehicle_status_poll();

			/* lock integrator until control is started */
			bool lock_integrator;

			if (_vcontrol_mode.flag_control_attitude_enabled && !_vehicle_status.is_rotary_wing) {
				lock_integrator = false;

			} else {
				lock_integrator = true;
			}

			/* Simple handling of failsafe: deploy parachute if failsafe is on */
			if (_vcontrol_mode.flag_control_termination_enabled) {
				_actuators_airframe.control[7] = 1.0f;
				//warnx("_actuators_airframe.control[1] = 1.0f;");
			} else {
				_actuators_airframe.control[7] = 0.0f;
				//warnx("_actuators_airframe.control[1] = -1.0f;");
			}

			/* decide if in stabilized or full manual control */

			if (_vcontrol_mode.flag_control_attitude_enabled) {

				/* scale around tuning airspeed */

				float airspeed;

				/* if airspeed is not updating, we assume the normal average speed */
				if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
					airspeed = _parameters.airspeed_trim;
					if (nonfinite) {
						perf_count(_nonfinite_input_perf);
					}
				} else {
					/* prevent numerical drama by requiring 0.5 m/s minimal speed */
					airspeed = math::max(0.5f, _airspeed.true_airspeed_m_s);
				}

				/*
				 * For scaling our actuators using anything less than the min (close to stall)
				 * speed doesn't make any sense - its the strongest reasonable deflection we
				 * want to do in flight and its the baseline a human pilot would choose.
				 *
				 * Forcing the scaling to this value allows reasonable handheld tests.
				 */

				float airspeed_scaling = _parameters.airspeed_trim / ((airspeed < _parameters.airspeed_min) ? _parameters.airspeed_min : airspeed);

				float roll_sp = _parameters.rollsp_offset_rad;
				float pitch_sp = _parameters.pitchsp_offset_rad;
				float yaw_manual = 0.0f;
				float throttle_sp = 0.0f;

				/* Read attitude setpoint from uorb if
				 * - velocity control or position control is enabled (pos controller is running)
				 * - manual control is disabled (another app may send the setpoint, but it should
				 *   for sure not be set from the remote control values)
				 */
				if (_vcontrol_mode.flag_control_auto_enabled ||
						!_vcontrol_mode.flag_control_manual_enabled) {
					/* read in attitude setpoint from attitude setpoint uorb topic */
					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
					throttle_sp = _att_sp.thrust;

					/* reset integrals where needed */
					if (_att_sp.roll_reset_integral) {
						_roll_ctrl.reset_integrator();
					}
					if (_att_sp.pitch_reset_integral) {
						_pitch_ctrl.reset_integrator();
					}
					if (_att_sp.yaw_reset_integral) {
						_yaw_ctrl.reset_integrator();
					}
				} else if (_vcontrol_mode.flag_control_velocity_enabled) {
 					/*
					 * Velocity should be controlled and manual is enabled.
					*/
					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
											+ _parameters.rollsp_offset_rad;
					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
					throttle_sp = _att_sp.thrust;

					/* reset integrals where needed */
					if (_att_sp.roll_reset_integral) {
						_roll_ctrl.reset_integrator();
					}
					if (_att_sp.pitch_reset_integral) {
						_pitch_ctrl.reset_integrator();
					}
					if (_att_sp.yaw_reset_integral) {
						_yaw_ctrl.reset_integrator();
					}
				} else {
					/*
					 * Scale down roll and pitch as the setpoints are radians
					 * and a typical remote can only do around 45 degrees, the mapping is
					 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
					 *
					 * With this mapping the stick angle is a 1:1 representation of
					 * the commanded attitude.
					 *
					 * The trim gets subtracted here from the manual setpoint to get
					 * the intended attitude setpoint. Later, after the rate control step the
					 * trim is added again to the control signal.
					 */
					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
						+ _parameters.rollsp_offset_rad;
					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
						+ _parameters.pitchsp_offset_rad;
					/* allow manual control of rudder deflection */
					yaw_manual = _manual.r;
					throttle_sp = _manual.z;
					_actuators.control[4] = _manual.flaps;

					/*
					 * in manual mode no external source should / does emit attitude setpoints.
					 * emit the manual setpoint here to allow attitude controller tuning
					 * in attitude control mode.
					 */
					struct vehicle_attitude_setpoint_s att_sp;
					att_sp.timestamp = hrt_absolute_time();
					att_sp.roll_body = roll_sp;
					att_sp.pitch_body = pitch_sp;
					att_sp.yaw_body = 0.0f - _parameters.trim_yaw;
					att_sp.thrust = throttle_sp;

					/* lazily publish the setpoint only once available */
					if (_attitude_sp_pub > 0 && !_vehicle_status.is_rotary_wing) {
						/* publish the attitude setpoint */
						orb_publish(ORB_ID(vehicle_attitude_setpoint), _attitude_sp_pub, &att_sp);

					} else if (_attitude_sp_pub < 0 && !_vehicle_status.is_rotary_wing) {
						/* advertise and publish */
						_attitude_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
					}
				}

				/* If the aircraft is on ground reset the integrators */
				if (_vehicle_status.condition_landed || _vehicle_status.is_rotary_wing) {
					_roll_ctrl.reset_integrator();
					_pitch_ctrl.reset_integrator();
					_yaw_ctrl.reset_integrator();
				}

				/* Prepare speed_body_u and speed_body_w */
				float speed_body_u = 0.0f;
				float speed_body_v = 0.0f;
				float speed_body_w = 0.0f;
				if(_att.R_valid) 	{
					speed_body_u = PX4_R(_att.R, 0, 0) * _global_pos.vel_n + PX4_R(_att.R, 1, 0) * _global_pos.vel_e + PX4_R(_att.R, 2, 0) * _global_pos.vel_d;
					speed_body_v = PX4_R(_att.R, 0, 1) * _global_pos.vel_n + PX4_R(_att.R, 1, 1) * _global_pos.vel_e + PX4_R(_att.R, 2, 1) * _global_pos.vel_d;
					speed_body_w = PX4_R(_att.R, 0, 2) * _global_pos.vel_n + PX4_R(_att.R, 1, 2) * _global_pos.vel_e + PX4_R(_att.R, 2, 2) * _global_pos.vel_d;
				} else	{
					if (_debug && loop_counter % 10 == 0) {
						warnx("Did not get a valid R\n");
					}
				}

				/* Prepare data for attitude controllers */
				struct ECL_ControlData control_input = {};
				control_input.roll = _att.roll;
				control_input.pitch = _att.pitch;
				control_input.yaw = _att.yaw;
				control_input.roll_rate = _att.rollspeed;
				control_input.pitch_rate = _att.pitchspeed;
				control_input.yaw_rate = _att.yawspeed;
				control_input.speed_body_u = speed_body_u;
				control_input.speed_body_v = speed_body_v;
				control_input.speed_body_w = speed_body_w;
				control_input.acc_body_x = _accel.x;
				control_input.acc_body_y = _accel.y;
				control_input.acc_body_z = _accel.z;
				control_input.roll_setpoint = roll_sp;
				control_input.pitch_setpoint = pitch_sp;
				control_input.airspeed_min = _parameters.airspeed_min;
				control_input.airspeed_max = _parameters.airspeed_max;
				control_input.airspeed = airspeed;
				control_input.scaler = airspeed_scaling;
				control_input.lock_integrator = lock_integrator;

				/* Run attitude controllers */
				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
					_roll_ctrl.control_attitude(control_input);
					_pitch_ctrl.control_attitude(control_input);
					_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude

					/* Update input data for rate controllers */
					control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
					control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();
					control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();

					/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
					float roll_u = _roll_ctrl.control_bodyrate(control_input);
					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
					if (!isfinite(roll_u)) {
						_roll_ctrl.reset_integrator();
						perf_count(_nonfinite_output_perf);

						if (_debug && loop_counter % 10 == 0) {
							warnx("roll_u %.4f", (double)roll_u);
						}
					}

					float pitch_u = _pitch_ctrl.control_bodyrate(control_input);
					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
					if (!isfinite(pitch_u)) {
						_pitch_ctrl.reset_integrator();
						perf_count(_nonfinite_output_perf);
						if (_debug && loop_counter % 10 == 0) {
							warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
								" airspeed %.4f, airspeed_scaling %.4f,"
								" roll_sp %.4f, pitch_sp %.4f,"
								" _roll_ctrl.get_desired_rate() %.4f,"
								" _pitch_ctrl.get_desired_rate() %.4f"
								" att_sp.roll_body %.4f",
								(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
								(double)airspeed, (double)airspeed_scaling,
								(double)roll_sp, (double)pitch_sp,
								(double)_roll_ctrl.get_desired_rate(),
								(double)_pitch_ctrl.get_desired_rate(),
								(double)_att_sp.roll_body);
						}
					}

					float yaw_u = _yaw_ctrl.control_bodyrate(control_input);
					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;

					/* add in manual rudder control */
					_actuators.control[2] += yaw_manual;
					if (!isfinite(yaw_u)) {
						_yaw_ctrl.reset_integrator();
						perf_count(_nonfinite_output_perf);
						if (_debug && loop_counter % 10 == 0) {
							warnx("yaw_u %.4f", (double)yaw_u);
						}
					}

					/* throttle passed through if it is finite and if no engine failure was
					 * detected */
					_actuators.control[3] = (isfinite(throttle_sp) &&
							!(_vehicle_status.engine_failure ||
								_vehicle_status.engine_failure_cmd)) ?
						throttle_sp : 0.0f;
					if (!isfinite(throttle_sp)) {
						if (_debug && loop_counter % 10 == 0) {
							warnx("throttle_sp %.4f", (double)throttle_sp);
						}
					}
				} else {
					perf_count(_nonfinite_input_perf);
					if (_debug && loop_counter % 10 == 0) {
						warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
					}
				}

				/*
				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
				 * only once available
				 */
				_rates_sp.roll = _roll_ctrl.get_desired_rate();
				_rates_sp.pitch = _pitch_ctrl.get_desired_rate();
				_rates_sp.yaw = _yaw_ctrl.get_desired_rate();

				_rates_sp.timestamp = hrt_absolute_time();

				if (_rate_sp_pub > 0) {
					/* publish the attitude rates setpoint */
					orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
				} else if (_rates_sp_id) {
					/* advertise the attitude rates setpoint */
					_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
				}

			} else {
				/* manual/direct control */
				_actuators.control[0] = _manual.y;
				_actuators.control[1] = -_manual.x;
				_actuators.control[2] = _manual.r;
				_actuators.control[3] = _manual.z;
				_actuators.control[4] = _manual.flaps;
			}

			_actuators.control[5] = _manual.aux1;
			_actuators.control[6] = _manual.aux2;
			_actuators.control[7] = _manual.aux3;

			/* lazily publish the setpoint only once available */
			_actuators.timestamp = hrt_absolute_time();
			_actuators.timestamp_sample = _att.timestamp;
			_actuators_airframe.timestamp = hrt_absolute_time();
			_actuators_airframe.timestamp_sample = _att.timestamp;

			/* Only publish if any of the proper modes are enabled */
			if(_vcontrol_mode.flag_control_rates_enabled ||
               _vcontrol_mode.flag_control_attitude_enabled ||
               _vcontrol_mode.flag_control_manual_enabled)
			{
				/* publish the actuator controls */
				if (_actuators_0_pub > 0) {
					orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
				} else if (_actuators_id) {
					_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
				}

				if (_actuators_2_pub > 0) {
					/* publish the actuator controls*/
					orb_publish(ORB_ID(actuator_controls_2), _actuators_2_pub, &_actuators_airframe);

				} else {
					/* advertise and publish */
					_actuators_2_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators_airframe);
				}
			}
		}

		loop_counter++;
		perf_end(_loop_perf);
	}

	warnx("exiting.\n");

	_control_task = -1;
	_task_running = false;
	_exit(0);
}
示例#26
0
int
AK8975::collect()
{
#pragma pack(push, 1)
	struct { /* status register and data as read back from the device */
		uint8_t		x[2];
		uint8_t		y[2];
		uint8_t		z[2];
	}	ak8975_report;
#pragma pack(pop)
	struct {
		int16_t		x, y, z;
	} report;
	int	ret = -EIO;
	uint8_t	cmd;


	perf_begin(_sample_perf);
	struct mag_report new_report;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	new_report.timestamp = hrt_absolute_time();
        new_report.error_count = perf_event_count(_comms_errors);

	/*
	 * @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.
	 */
	/* check data ready bit */	
	if (read_reg(ADDR_DRDY, cmd)) {
		perf_count(_comms_errors);
		debug("data ready read error");
		goto out;	
	}
	if (!(cmd & STATUS_REG_DATA_READY)) {
		/* data not ready */
		perf_count(_comms_errors);
		debug("data not ready [%02X]", cmd);
		goto out;
	}

	/* get measurements from the device */
	cmd = ADDR_DATA_OUT_X_LSB;
	ret = transfer(&cmd, 1, (uint8_t *)&ak8975_report, sizeof(ak8975_report));

	if (ret != OK) {
		perf_count(_comms_errors);
		debug("data/status read error");
		goto out;
	}
	/* check for overflow and not ready error */
	if (read_reg(ADDR_STATUS, cmd)) {
		perf_count(_comms_errors);
		debug("data status read error");
		goto out;	
	}
	if (cmd & (STATUS_REG_DATA_ERROR | STATUS_REG_HOFL)) {
		/* data value error  */
		perf_count(_comms_errors);
		debug("data value error");
		goto out;
	}

	/* swap the data we just received */
	report.x = (((int16_t)ak8975_report.x[1]) << 8) + ak8975_report.x[0];
	report.y = (((int16_t)ak8975_report.y[1]) << 8) + ak8975_report.y[0];
	report.z = (((int16_t)ak8975_report.z[1]) << 8) + ak8975_report.z[0];

	/*
	 * 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) > 4096) ||
	    (abs(report.y) > 4096) ||
	    (abs(report.z) > 4096)) {
		perf_count(_comms_errors);
		goto out;
	}

	/*
	 * RAW outputs
	 *
	 * to align the sensor axes with the board, x and y need to be flipped
	 * and y needs to be negated
	 */
	new_report.x_raw = report.x;
	new_report.y_raw = report.y;
	/* z remains z */
	new_report.z_raw = report.z;

	/* scale values for output */

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */

	/* the standard external mag by 3DR has x pointing to the right, y pointing backwards, and z down,
	 * therefore switch x and y and invert y */
	//new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
	new_report.x = ((report.x * _scale.x_scale) - _scale.x_offset) * _range_scale;
	/* flip axes and negate value for y */
	//new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
	new_report.y = ((report.y * _scale.y_scale) - _scale.y_offset) * _range_scale;
	/* z remains z */
	//new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;
	new_report.z = ((report.z * _scale.z_scale) - _scale.z_offset) * _range_scale;

	/* publish it */
	orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);

	/* post a report to the ring */
	if (_reports->force(&new_report)) {
		perf_count(_buffer_overflows);
	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

out:
	perf_end(_sample_perf);
	return ret;
}
示例#27
0
int
MEASAirspeed::collect()
{
	int	ret = -EIO;

	/* read from the sensor */
	uint8_t val[4] = {0, 0, 0, 0};


	perf_begin(_sample_perf);

	ret = transfer(nullptr, 0, &val[0], 4);

	if (ret < 0) {
                perf_count(_comms_errors);
                perf_end(_sample_perf);
		return ret;
	}

	uint8_t status = val[0] & 0xC0;

	if (status == 2) {
		log("err: stale data");
                perf_count(_comms_errors);
                perf_end(_sample_perf);
		return ret;
	} else if (status == 3) {
		log("err: fault");
                perf_count(_comms_errors);
                perf_end(_sample_perf);
		return ret;                
	}

	//uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8);
	uint16_t temp = (val[3] & 0xE0) << 8 | val[2];

	// XXX leaving this in until new calculation method has been cross-checked
	//diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f));
	//diff_pres_pa -= _diff_pres_offset;
	int16_t dp_raw = 0, dT_raw = 0;
	dp_raw = (val[0] << 8) + val[1];
	dp_raw = 0x3FFF & dp_raw; //mask the used bits
	dT_raw = (val[2] << 8) + val[3];
	dT_raw = (0xFFE0 & dT_raw) >> 5;
	float temperature = ((200 * dT_raw) / 2047) - 50;

	// XXX we may want to smooth out the readings to remove noise.

	// Calculate differential pressure. As its centered around 8000
	// and can go positive or negative, enforce absolute value
//	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
	const float P_min = -1.0f;
	const float P_max = 1.0f;
	float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f)   - _diff_pres_offset);

	struct differential_pressure_s report;

	// Track maximum differential pressure measured (so we can work out top speed).
	if (diff_press_pa > _max_differential_pressure_pa) {
	    _max_differential_pressure_pa = diff_press_pa;
	}

	report.timestamp = hrt_absolute_time();
        report.error_count = perf_event_count(_comms_errors);
	report.temperature = temperature;
	report.differential_pressure_pa = diff_press_pa;
	report.voltage = 0;
	report.max_differential_pressure_pa = _max_differential_pressure_pa;

	/* announce the airspeed if needed, just publish else */
	orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);

	new_report(report);

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	ret = OK;

	perf_end(_sample_perf);

	return ret;
}
示例#28
0
int
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];
	}	hmc_report;
#pragma pack(pop)
	struct {
		int16_t		x, y, z;
	} report;

	int	ret;
	uint8_t	cmd;
	uint8_t check_counter;

	perf_begin(_sample_perf);
	struct mag_report new_report;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	new_report.timestamp = hrt_absolute_time();
        new_report.error_count = perf_event_count(_comms_errors);

	/*
	 * @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 = ADDR_DATA_OUT_X_MSB;
	ret = transfer(&cmd, 1, (uint8_t *)&hmc_report, sizeof(hmc_report));

	if (ret != OK) {
		perf_count(_comms_errors);
		debug("data/status read error");
		goto out;
	}

	/* 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];

	/*
	 * 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)) {
		perf_count(_comms_errors);
		goto out;
	}

	/*
	 * RAW outputs
	 *
	 * to align the sensor axes with the board, x and y need to be flipped
	 * and y needs to be negated
	 */
	new_report.x_raw = report.y;
	new_report.y_raw = -report.x;
	/* z remains z */
	new_report.z_raw = report.z;

	/* scale values for output */

#ifdef PX4_I2C_BUS_ONBOARD
	if (_bus == PX4_I2C_BUS_ONBOARD) {
		// convert onboard so it matches offboard for the
		// scaling below
		report.y = -report.y;
		report.x = -report.x;
        }
#endif

	/* the standard external mag by 3DR has x pointing to the
	 * right, y pointing backwards, and z down, therefore switch x
	 * and y and invert y */
	new_report.x = ((-report.y * _range_scale) - _scale.x_offset) * _scale.x_scale;
	/* flip axes and negate value for y */
	new_report.y = ((report.x * _range_scale) - _scale.y_offset) * _scale.y_scale;
	/* z remains z */
	new_report.z = ((report.z * _range_scale) - _scale.z_offset) * _scale.z_scale;

	// apply user specified rotation
	rotate_3f(_rotation, new_report.x, new_report.y, new_report.z);

	if (!(_pub_blocked)) {

		if (_mag_topic != -1) {
			/* publish it */
			orb_publish(_mag_orb_id, _mag_topic, &new_report);
		} else {
			_mag_topic = orb_advertise(_mag_orb_id, &new_report);

			if (_mag_topic < 0)
				debug("ADVERT FAIL");
		}
	}

	_last_report = new_report;

	/* post a report to the ring */
	if (_reports->force(&new_report)) {
		perf_count(_buffer_overflows);
	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	/*
	  periodically check the range register and configuration
	  registers. With a bad I2C cable it is possible for the
	  registers to become corrupt, leading to bad readings. It
	  doesn't happen often, but given the poor cables some
	  vehicles have it is worth checking for.
	 */
	check_counter = perf_event_count(_sample_perf) % 256;
	if (check_counter == 0) {
		check_range();
	}
	if (check_counter == 128) {
		check_conf();
	}

	ret = OK;

out:
	perf_end(_sample_perf);
	return ret;
}
示例#29
0
int
LIS3MDL::collect()
{
#pragma pack(push, 1)
	struct { /* status register and data as read back from the device */
		uint8_t		x[2];
		uint8_t		y[2];
		uint8_t		z[2];
		uint8_t		t[2];
	}	lis_report;

	struct {
		int16_t		x;
		int16_t		y;
		int16_t		z;
		int16_t		t;
	} report;
#pragma pack(pop)

	int	ret;
//	uint8_t check_counter;

	perf_begin(_sample_perf);
	struct mag_report new_report;
	bool sensor_is_onboard = false;

	float xraw_f;
	float yraw_f;
	float zraw_f;

	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
	new_report.timestamp = hrt_absolute_time();
	new_report.error_count = perf_event_count(_comms_errors);

	/*
	 * @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 */
	ret = _interface->read(ADDR_OUT_X_L, (uint8_t *)&lis_report, sizeof(lis_report));

	if (ret != OK) {
		perf_count(_comms_errors);
		DEVICE_DEBUG("data/status read error");
		goto out;
	}

	/* convert the data we just received */
	report.x = (((int16_t)lis_report.x[1]) << 8) + lis_report.x[0];
	report.y = (((int16_t)lis_report.y[1]) << 8) + lis_report.y[0];
	report.z = (((int16_t)lis_report.z[1]) << 8) + lis_report.z[0];
	report.t = (((int16_t)lis_report.t[1]) << 8) + lis_report.t[0];

	/* get measurements from the device */
	new_report.temperature = report.t;
	new_report.temperature = 25 + (report.t / (16 * 8.0f));

	// XXX revisit for SPI part, might require a bus type IOCTL

	unsigned dummy;
	sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);

	/*
	 * RAW outputs
	 *
	 */
	new_report.x_raw = report.x;
	new_report.y_raw = report.y;
	new_report.z_raw = report.z;

	/* the LIS3MDL mag on Pixhawk Pro by Drotek has x pointing towards,
	 * y pointing to the right, and z down, therefore no switch needed,
	 * it is better to have no artificial rotation inside the
	 * driver and then use the startup script with -R command with the
	 * real rotation between the sensor and body frame */
	xraw_f = report.x;
	yraw_f = report.y;
	zraw_f = report.z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	new_report.x = ((xraw_f * _range_scale) - _scale.x_offset) * _scale.x_scale;
	/* flip axes and negate value for y */
	new_report.y = ((yraw_f * _range_scale) - _scale.y_offset) * _scale.y_scale;
	/* z remains z */
	new_report.z = ((zraw_f * _range_scale) - _scale.z_offset) * _scale.z_scale;

	if (!(_pub_blocked)) {

		if (_mag_topic != nullptr) {
			/* publish it */
			orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);

		} else {
			_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report,
							 &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX);

			if (_mag_topic == nullptr) {
				DEVICE_DEBUG("ADVERT FAIL");
			}
		}
	}

	_last_report = new_report;

	/* post a report to the ring */
	if (_reports->force(&new_report)) {
		perf_count(_buffer_overflows);
	}

	/* notify anyone waiting for data */
	poll_notify(POLLIN);

	check_conf();

	ret = OK;

out:
	perf_end(_sample_perf);
	return ret;
}
示例#30
0
void
MPU9250::measure()
{
	if (hrt_absolute_time() < _reset_wait) {
		// we're waiting for a reset to complete
		return;
	}

	struct MPUReport mpu_report;

	struct Report {
		int16_t		accel_x;
		int16_t		accel_y;
		int16_t		accel_z;
		int16_t		temp;
		int16_t		gyro_x;
		int16_t		gyro_y;
		int16_t		gyro_z;
	} report;

	/* start measuring */
	perf_begin(_sample_perf);

	/*
	 * Fetch the full set of measurements from the MPU9250 in one pass.
	 */
	mpu_report.cmd = DIR_READ | MPUREG_INT_STATUS;

	// sensor transfer at high clock speed
	set_frequency(MPU9250_HIGH_BUS_SPEED);

	if (OK != transfer((uint8_t *)&mpu_report, ((uint8_t *)&mpu_report), sizeof(mpu_report))) {
		return;
	}

	//check_registers();

	if (check_duplicate(&mpu_report.accel_x[0])) {
		return;
	}

	/* remember the time, to use later */
	uint64_t mpu_time = hrt_absolute_time();

	_mag->measure(mpu_report.mag);

	/*
	 * Convert from big to little endian
	 */
	report.accel_x = int16_t_from_bytes(mpu_report.accel_x);
	report.accel_y = int16_t_from_bytes(mpu_report.accel_y);
	report.accel_z = int16_t_from_bytes(mpu_report.accel_z);
	report.temp    = int16_t_from_bytes(mpu_report.temp);
	report.gyro_x  = int16_t_from_bytes(mpu_report.gyro_x);
	report.gyro_y  = int16_t_from_bytes(mpu_report.gyro_y);
	report.gyro_z  = int16_t_from_bytes(mpu_report.gyro_z);

	if (check_null_data((uint32_t *)&report, sizeof(report) / 4)) {
		return;
	}

	if (_register_wait != 0) {
		// we are waiting for some good transfers before using the sensor again
		// We still increment _good_transfers, but don't return any data yet
		_register_wait--;
		return;
	}

	/*
	 * Swap axes and negate y
	 */
	int16_t accel_xt = report.accel_y;
	int16_t accel_yt = ((report.accel_x == -32768) ? 32767 : -report.accel_x);

	int16_t gyro_xt = report.gyro_y;
	int16_t gyro_yt = ((report.gyro_x == -32768) ? 32767 : -report.gyro_x);

	/*
	 * Apply the swap
	 */
	report.accel_x = accel_xt;
	report.accel_y = accel_yt;
	report.gyro_x = gyro_xt;
	report.gyro_y = gyro_yt;

	/*
	 * Report buffers.
	 */
	accel_report		arb;
	gyro_report		grb;

	/*
	 * Adjust and scale results to m/s^2.
	 */
	//grb.timestamp = arb.timestamp = hrt_absolute_time();
	grb.timestamp = arb.timestamp = mpu_time;

	// report the error count as the sum of the number of bad
	// transfers and bad register reads. This allows the higher
	// level code to decide if it should use this sensor based on
	// whether it has had failures
	grb.error_count = arb.error_count = perf_event_count(_bad_transfers) + perf_event_count(_bad_registers);

	/*
	 * 1) Scale raw value to SI units using scaling from datasheet.
	 * 2) Subtract static offset (in SI units)
	 * 3) Scale the statically calibrated values with a linear
	 *    dynamically obtained factor
	 *
	 * Note: the static sensor offset is the number the sensor outputs
	 * 	 at a nominally 'zero' input. Therefore the offset has to
	 * 	 be subtracted.
	 *
	 *	 Example: A gyro outputs a value of 74 at zero angular rate
	 *	 	  the offset is 74 from the origin and subtracting
	 *		  74 from all measurements centers them around zero.
	 */

	/* NOTE: Axes have been swapped to match the board a few lines above. */

	arb.x_raw = report.accel_x;
	arb.y_raw = report.accel_y;
	arb.z_raw = report.accel_z;

	float xraw_f = report.accel_x;
	float yraw_f = report.accel_y;
	float zraw_f = report.accel_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
	float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
	float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;

	arb.x = _accel_filter_x.apply(x_in_new);
	arb.y = _accel_filter_y.apply(y_in_new);
	arb.z = _accel_filter_z.apply(z_in_new);

	math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
	math::Vector<3> aval_integrated;

	bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
	arb.x_integral = aval_integrated(0);
	arb.y_integral = aval_integrated(1);
	arb.z_integral = aval_integrated(2);

	arb.scaling = _accel_range_scale;
	arb.range_m_s2 = _accel_range_m_s2;

	_last_temperature = (report.temp) / 361.0f + 35.0f;

	arb.temperature_raw = report.temp;
	arb.temperature = _last_temperature;

	grb.x_raw = report.gyro_x;
	grb.y_raw = report.gyro_y;
	grb.z_raw = report.gyro_z;

	xraw_f = report.gyro_x;
	yraw_f = report.gyro_y;
	zraw_f = report.gyro_z;

	// apply user specified rotation
	rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);

	float x_gyro_in_new = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
	float y_gyro_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
	float z_gyro_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;

	grb.x = _gyro_filter_x.apply(x_gyro_in_new);
	grb.y = _gyro_filter_y.apply(y_gyro_in_new);
	grb.z = _gyro_filter_z.apply(z_gyro_in_new);

	math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
	math::Vector<3> gval_integrated;

	bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
	grb.x_integral = gval_integrated(0);
	grb.y_integral = gval_integrated(1);
	grb.z_integral = gval_integrated(2);

	grb.scaling = _gyro_range_scale;
	grb.range_rad_s = _gyro_range_rad_s;

	grb.temperature_raw = report.temp;
	grb.temperature = _last_temperature;

	_accel_reports->force(&arb);
	_gyro_reports->force(&grb);

	/* notify anyone waiting for data */
	if (accel_notify) {
		poll_notify(POLLIN);
	}

	if (gyro_notify) {
		_gyro->parent_poll_notify();
	}

	if (accel_notify && !(_pub_blocked)) {
		/* log the time of this report */
		perf_begin(_controller_latency_perf);
		/* publish it */
		orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
	}

	if (gyro_notify && !(_pub_blocked)) {
		/* publish it */
		orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
	}

	/* stop measuring */
	perf_end(_sample_perf);
}