void
CameraTrigger::cycle_trampoline(void *arg)
{

	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);

	if (trig->_vcommand_sub < 0) {
		trig->_vcommand_sub = orb_subscribe(ORB_ID(vehicle_command));
	}

	bool updated;
	orb_check(trig->_vcommand_sub, &updated);

	// while the trigger is inactive it has to be ready
	// to become active instantaneously
	int poll_interval_usec = 5000;

	if (trig->_mode < 3) {

		if (updated) {

			struct vehicle_command_s cmd;

			orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);

			if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_TRIGGER_CONTROL) {
				// Set trigger rate from command
				if (cmd.param2 > 0) {
					trig->_interval = cmd.param2;
					param_set(trig->_p_interval, &(trig->_interval));
				}

				if (cmd.param1 < 1.0f) {
					trig->control(false);

				} else if (cmd.param1 >= 1.0f) {
					trig->control(true);
					// while the trigger is active there is no
					// need to poll at a very high rate
					poll_interval_usec = 100000;
				}

			} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL) {
				if (cmd.param5 > 0) {
					// One-shot trigger, default 1 ms interval
					trig->_interval = 1000;
					trig->control(true);
				}
			}
		}

	} else {

		// Set trigger based on covered distance
		if (trig->_vlposition_sub < 0) {
			trig->_vlposition_sub = orb_subscribe(ORB_ID(vehicle_local_position));
		}

		struct vehicle_local_position_s pos;

		orb_copy(ORB_ID(vehicle_local_position), trig->_vlposition_sub, &pos);

		if (pos.xy_valid) {

			bool turning_on = false;

			if (updated && trig->_mode == 4) {

				// Check update from command
				struct vehicle_command_s cmd;
				orb_copy(ORB_ID(vehicle_command), trig->_vcommand_sub, &cmd);

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

					// Set trigger to disabled if the set distance is not positive
					if (cmd.param1 > 0.0f && !trig->_trigger_enabled) {
						trig->turnOnOff();
						trig->keepAlive(true);
						// Give the camera time to turn on, before starting to send trigger signals
						poll_interval_usec = 5000000;
						turning_on = true;

					} else if (cmd.param1 <= 0.0f && trig->_trigger_enabled) {
						hrt_cancel(&(trig->_engagecall));
						hrt_cancel(&(trig->_disengagecall));
						trig->keepAlive(false);
						trig->turnOnOff();
					}

					trig->_trigger_enabled = cmd.param1 > 0.0f;
					trig->_distance = cmd.param1;
				}
			}

			if ((trig->_trigger_enabled || trig->_mode < 4) && !turning_on) {

				// Initialize position if not done yet
				math::Vector<2> current_position(pos.x, pos.y);

				if (!trig->_valid_position) {
					// First time valid position, take first shot
					trig->_last_shoot_position = current_position;
					trig->_valid_position = pos.xy_valid;
					trig->shootOnce();
				}

				// Check that distance threshold is exceeded and the time between last shot is large enough
				if ((trig->_last_shoot_position - current_position).length() >= trig->_distance) {
					trig->shootOnce();
					trig->_last_shoot_position = current_position;
				}
			}

		} else {
			poll_interval_usec = 100000;
		}
	}

	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
		   camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
}
Exemple #2
0
int
LPS25H::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	unsigned dummy = arg;

	switch (cmd) {
	case SENSORIOCSPOLLRATE: {
			switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

			/* external signalling (DRDY) not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(LPS25H_CONVERSION_INTERVAL);

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}

			/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					unsigned ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(LPS25H_CONVERSION_INTERVAL)) {
						return -EINVAL;
					}

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0) {
			return SENSOR_POLLRATE_MANUAL;
		}

		return (1000 / _measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 1) || (arg > 100)) {
				return -EINVAL;
			}

			irqstate_t flags = px4_enter_critical_section();

			if (!_reports->resize(arg)) {
				px4_leave_critical_section(flags);
				return -ENOMEM;
			}

			px4_leave_critical_section(flags);
			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return _reports->size();

	case SENSORIOCRESET:
		return reset();

	case BAROIOCSMSLPRESSURE:

		/* range-check for sanity */
		if ((arg < 80000) || (arg > 120000)) {
			return -EINVAL;
		}

		_msl_pressure = arg;
		return OK;

	case BAROIOCGMSLPRESSURE:
		return _msl_pressure;

	case DEVIOCGDEVICEID:
		return _interface->ioctl(cmd, dummy);

	default:
		/* give it to the superclass */
		return CDev::ioctl(filp, cmd, arg);
	}
}
Exemple #3
0
int
MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case BAROIOCSPOLLRATE: {
			switch (arg) {

				/* switching to manual polling */
			case BARO_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

				/* external signalling not supported */
			case BARO_POLLRATE_EXTERNAL:

				/* zero would be bad */
			case 0:
				return -EINVAL;

				/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					unsigned ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(MS5611_CONVERSION_INTERVAL))
						return -EINVAL;

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start)
						start();

					return OK;
				}
			}
		}

	case BAROIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 2) || (arg > 100))
				return -EINVAL;

			/* allocate new buffer */
			struct baro_report *buf = new struct baro_report[arg];

			if (nullptr == buf)
				return -ENOMEM;

			/* reset the measurement state machine with the new buffer, free the old */
			stop();
			delete[] _reports;
			_num_reports = arg;
			_reports = buf;
			start();

			return OK;
		}

	case BAROIOCSREPORTFORMAT:
		return -EINVAL;

	default:
		/* give it to the superclass */
		return I2C::ioctl(filp, cmd, arg);
	}
}
Exemple #4
0
void
SF0X::cycle()
{
	/* fds initialized? */
	if (_fd < 0) {
		/* open fd */
		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_NONBLOCK);

		if (_fd < 0) {
			PX4_ERR("open failed (%i)", errno);
			return;
		}

		struct termios uart_config;

		int termios_state;

		/* fill the struct for the new configuration */
		tcgetattr(_fd, &uart_config);

		/* clear ONLCR flag (which appends a CR for every LF) */
		uart_config.c_oflag &= ~ONLCR;

		/* no parity, one stop bit */
		uart_config.c_cflag &= ~(CSTOPB | PARENB);

		/* if distance sensor model is SF11/C, then set baudrate 115200, else 9600 */
		int hw_model;

		param_get(param_find("SENS_EN_SF0X"), &hw_model);

		unsigned speed;

		if (hw_model == 5) {
			speed = B115200;

		} else {
			speed = B9600;
		}

		/* set baud rate */
		if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) {
			PX4_ERR("CFG: %d ISPD", termios_state);
		}

		if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
			PX4_ERR("CFG: %d OSPD", termios_state);
		}

		if ((termios_state = tcsetattr(_fd, TCSANOW, &uart_config)) < 0) {
			PX4_ERR("baud %d ATTR", termios_state);
		}
	}

	/* collection phase? */
	if (_collect_phase) {

		/* perform collection */
		int collect_ret = collect();

		if (collect_ret == -EAGAIN) {
			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
			work_queue(HPWORK,
				   &_work,
				   (worker_t)&SF0X::cycle_trampoline,
				   this,
				   USEC2TICK(1042 * 8));
			return;
		}

		if (OK != collect_ret) {

			/* we know the sensor needs about four seconds to initialize */
			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
				PX4_ERR("collection error #%u", _consecutive_fail_count);
			}

			_consecutive_fail_count++;

			/* restart the measurement state machine */
			start();
			return;

		} else {
			/* apparently success */
			_consecutive_fail_count = 0;
		}

		/* next phase is measurement */
		_collect_phase = false;

		/*
		 * Is there a collect->measure gap?
		 */
		if (_measure_ticks > USEC2TICK(_conversion_interval)) {

			/* schedule a fresh cycle call when we are ready to measure again */
			work_queue(HPWORK,
				   &_work,
				   (worker_t)&SF0X::cycle_trampoline,
				   this,
				   _measure_ticks - USEC2TICK(_conversion_interval));

			return;
		}
	}

	/* measurement phase */
	if (OK != measure()) {
		PX4_DEBUG("measure error");
	}

	/* next phase is collection */
	_collect_phase = true;

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(HPWORK,
		   &_work,
		   (worker_t)&SF0X::cycle_trampoline,
		   this,
		   USEC2TICK(_conversion_interval));
}
Exemple #5
0
int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	unsigned dummy = arg;

	switch (cmd) {
	case SENSORIOCSPOLLRATE: {
			switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

			/* external signalling (DRDY) not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}

			/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					unsigned ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL)) {
						return -EINVAL;
					}

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0) {
			return SENSOR_POLLRATE_MANUAL;
		}

		return 1000000 / TICK2USEC(_measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 1) || (arg > 100)) {
				return -EINVAL;
			}

			irqstate_t flags = irqsave();

			if (!_reports->resize(arg)) {
				irqrestore(flags);
				return -ENOMEM;
			}

			irqrestore(flags);

			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return _reports->size();

	case SENSORIOCRESET:
		return reset();

	case MAGIOCSSAMPLERATE:
		/* same as pollrate because device is in single measurement mode*/
		return ioctl(filp, SENSORIOCSPOLLRATE, arg);

	case MAGIOCGSAMPLERATE:
		/* same as pollrate because device is in single measurement mode*/
		return 1000000 / TICK2USEC(_measure_ticks);

	case MAGIOCSRANGE:
		return set_range(arg);

	case MAGIOCGRANGE:
		return _range_ga;

	case MAGIOCSLOWPASS:
	case MAGIOCGLOWPASS:
		/* not supported, no internal filtering */
		return -EINVAL;

	case MAGIOCSSCALE:
		/* set new scale factors */
		memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale));
		/* check calibration, but not actually return an error */
		(void)check_calibration();
		return 0;

	case MAGIOCGSCALE:
		/* copy out scale factors */
		memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale));
		return 0;

	case MAGIOCCALIBRATE:
		return calibrate(filp, arg);

	case MAGIOCEXSTRAP:
		return set_excitement(arg);

	case MAGIOCSELFTEST:
		return check_calibration();

	case MAGIOCGEXTERNAL:
		DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver");
		return _interface->ioctl(cmd, dummy);

	case MAGIOCSTEMPCOMP:
		return set_temperature_compensation(arg);

	case DEVIOCGDEVICEID:
		return _interface->ioctl(cmd, dummy);

	default:
		/* give it to the superclass */
		return CDev::ioctl(filp, cmd, arg);
	}
}
Exemple #6
0
int
SRF02_I2C::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
			switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

			/* external signalling (DRDY) not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(_cycling_rate);

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();

					}

					return OK;
				}

			/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					int ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(_cycling_rate)) {
						return -EINVAL;
					}

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0) {
			return SENSOR_POLLRATE_MANUAL;
		}

		return (1000 / _measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 1) || (arg > 100)) {
				return -EINVAL;
			}

			irqstate_t flags = irqsave();

			if (!_reports->resize(arg)) {
				irqrestore(flags);
				return -ENOMEM;
			}

			irqrestore(flags);

			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return _reports->size();

	case SENSORIOCRESET:
		/* XXX implement this */
		return -EINVAL;

	case RANGEFINDERIOCSETMINIUMDISTANCE: {
			set_minimum_distance(*(float *)arg);
			return 0;
		}
		break;

	case RANGEFINDERIOCSETMAXIUMDISTANCE: {
			set_maximum_distance(*(float *)arg);
			return 0;
		}
		break;

	default:
		/* give it to the superclass */
		return I2C::ioctl(filp, cmd, arg);
	}
}
Exemple #7
0
int
LIS3MDL::ioctl(struct file *file_pointer, int cmd, unsigned long arg)
{
	unsigned dummy = 0;

	switch (cmd) {
	case SENSORIOCSPOLLRATE: {
			switch (arg) {

			/* zero would be bad */
			case 0:
				return -EINVAL;

			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool not_started = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(LIS3MDL_CONVERSION_INTERVAL);

					/* if we need to start the poll state machine, do it */
					if (not_started) {
						start();
					}

					return PX4_OK;
				}

			/* Uses arg (hz) for a custom poll rate */
			default: {
					/* do we need to start internal polling? */
					bool not_started = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					unsigned ticks = USEC2TICK(1000000 / arg);

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (not_started) {
						start();
					}

					return PX4_OK;
				}
			}
		}

	case SENSORIOCRESET:
		return reset();

	case MAGIOCSRANGE:
		return set_range(arg);

	case MAGIOCSSCALE:
		/* set new scale factors */
		memcpy(&_scale, (struct mag_calibration_s *)arg, sizeof(_scale));
		return 0;

	case MAGIOCGSCALE:
		/* copy out scale factors */
		memcpy((struct mag_calibration_s *)arg, &_scale, sizeof(_scale));
		return 0;

	case MAGIOCCALIBRATE:
		return calibrate(file_pointer, arg);

	case MAGIOCEXSTRAP:
		return set_excitement(arg);

	case MAGIOCGEXTERNAL:
		DEVICE_DEBUG("MAGIOCGEXTERNAL in main driver");
		return _interface->ioctl(cmd, dummy);

	case DEVIOCGDEVICEID:
		return _interface->ioctl(cmd, dummy);

	default:
		/* give it to the superclass */
		return CDev::ioctl(file_pointer, cmd, arg);
	}
}
Exemple #8
0
int
MB12XX::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
			switch (arg) {

				/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

				/* external signalling (DRDY) not supported */
			case SENSOR_POLLRATE_EXTERNAL:

				/* zero would be bad */
			case 0:
				return -EINVAL;

				/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(MB12XX_CONVERSION_INTERVAL);

					/* if we need to start the poll state machine, do it */
					if (want_start)
						start();

					return OK;
				}

				/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					unsigned ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(MB12XX_CONVERSION_INTERVAL))
						return -EINVAL;

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start)
						start();

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0)
			return SENSOR_POLLRATE_MANUAL;

		return (1000 / _measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* add one to account for the sentinel in the ring */
			arg++;

			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 2) || (arg > 100))
				return -EINVAL;

			/* allocate new buffer */
			struct range_finder_report *buf = new struct range_finder_report[arg];

			if (nullptr == buf)
				return -ENOMEM;

			/* reset the measurement state machine with the new buffer, free the old */
			stop();
			delete[] _reports;
			_num_reports = arg;
			_reports = buf;
			start();

			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return _num_reports - 1;
		
	case SENSORIOCRESET:
		/* XXX implement this */
		return -EINVAL;
	
	case RANGEFINDERIOCSETMINIUMDISTANCE:
	{
		set_minimum_distance(*(float *)arg);
		return 0;
	}
	break;
	case RANGEFINDERIOCSETMAXIUMDISTANCE:
	{
		set_maximum_distance(*(float *)arg);
		return 0;
	}
	break;
	default:
		/* give it to the superclass */
		return I2C::ioctl(filp, cmd, arg);
	}
}
Exemple #9
0
int
AirspeedSim::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
    switch (cmd) {

    case SENSORIOCSPOLLRATE: {
        switch (arg) {

        /* switching to manual polling */
        case SENSOR_POLLRATE_MANUAL:
            stop();
            _measure_ticks = 0;
            return OK;

        /* external signalling (DRDY) not supported */
        case SENSOR_POLLRATE_EXTERNAL:

        /* zero would be bad */
        case 0:
            return -EINVAL;

        /* set default/max polling rate */
        case SENSOR_POLLRATE_MAX:
        case SENSOR_POLLRATE_DEFAULT: {
            /* do we need to start internal polling? */
            bool want_start = (_measure_ticks == 0);

            /* set interval for next measurement to minimum legal value */
            _measure_ticks = USEC2TICK(_conversion_interval);

            /* if we need to start the poll state machine, do it */
            if (want_start)
                start();

            return OK;
        }

        /* adjust to a legal polling interval in Hz */
        default: {
            /* do we need to start internal polling? */
            bool want_start = (_measure_ticks == 0);

            /* convert hz to tick interval via microseconds */
            unsigned long ticks = USEC2TICK(1000000 / arg);

            /* check against maximum rate */
            if (ticks < USEC2TICK(_conversion_interval))
                return -EINVAL;

            /* update interval for next measurement */
            _measure_ticks = ticks;

            /* if we need to start the poll state machine, do it */
            if (want_start)
                start();

            return OK;
        }
        }
    }

    case SENSORIOCGPOLLRATE:
        if (_measure_ticks == 0)
            return SENSOR_POLLRATE_MANUAL;

        return (1000 / _measure_ticks);

    case SENSORIOCSQUEUEDEPTH: {
        /* lower bound is mandatory, upper bound is a sanity check */
        if ((arg < 1) || (arg > 100))
            return -EINVAL;

        //irqstate_t flags = irqsave();
        if (!_reports->resize(arg)) {
            //irqrestore(flags);
            return -ENOMEM;
        }
        //irqrestore(flags);

        return OK;
    }

    case SENSORIOCGQUEUEDEPTH:
        return _reports->size();

    case SENSORIOCRESET:
        /* XXX implement this */
        return -EINVAL;

    case AIRSPEEDIOCSSCALE: {
        struct airspeed_scale *s = (struct airspeed_scale*)arg;
        _diff_pres_offset = s->offset_pa;
        return OK;
    }

    case AIRSPEEDIOCGSCALE: {
        struct airspeed_scale *s = (struct airspeed_scale*)arg;
        s->offset_pa = _diff_pres_offset;
        s->scale = 1.0f;
        return OK;
    }

    default:
        /* give it to the superclass */
        //return I2C::ioctl(filp, cmd, arg);	XXX SIM should be super class
        return 0;
    }
}
Exemple #10
0
int
HMC5883::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
			switch (arg) {

				/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

				/* external signalling (DRDY) not supported */
			case SENSOR_POLLRATE_EXTERNAL:

				/* zero would be bad */
			case 0:
				return -EINVAL;

				/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(HMC5883_CONVERSION_INTERVAL);

					/* if we need to start the poll state machine, do it */
					if (want_start)
						start();

					return OK;
				}

				/* adjust to a legal polling interval in Hz */
			default: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					unsigned ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(HMC5883_CONVERSION_INTERVAL))
						return -EINVAL;

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start)
						start();

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0)
			return SENSOR_POLLRATE_MANUAL;

		return (1000 / _measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* add one to account for the sentinel in the ring */
			arg++;

			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 2) || (arg > 100))
				return -EINVAL;

			/* allocate new buffer */
			struct mag_report *buf = new struct mag_report[arg];

			if (nullptr == buf)
				return -ENOMEM;

			/* reset the measurement state machine with the new buffer, free the old */
			stop();
			delete[] _reports;
			_num_reports = arg;
			_reports = buf;
			start();

			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return _num_reports - 1;

	case SENSORIOCRESET:
		/* XXX implement this */
		return -EINVAL;

	case MAGIOCSSAMPLERATE:
		/* not supported, always 1 sample per poll */
		return -EINVAL;

	case MAGIOCSRANGE:
		return set_range(arg);

	case MAGIOCSLOWPASS:
		/* not supported, no internal filtering */
		return -EINVAL;

	case MAGIOCSSCALE:
		/* set new scale factors */
		memcpy(&_scale, (mag_scale *)arg, sizeof(_scale));
		/* check calibration, but not actually return an error */
		(void)check_calibration();
		return 0;

	case MAGIOCGSCALE:
		/* copy out scale factors */
		memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
		return 0;

	case MAGIOCCALIBRATE:
		return calibrate(filp, arg);

	case MAGIOCEXSTRAP:
		return set_excitement(arg);

	case MAGIOCSELFTEST:
		return check_calibration();

	default:
		/* give it to the superclass */
		return I2C::ioctl(filp, cmd, arg);
	}
}
Exemple #11
0
void
CameraTrigger::cycle_trampoline(void *arg)
{

	CameraTrigger *trig = reinterpret_cast<CameraTrigger *>(arg);

	// default loop polling interval
	int poll_interval_usec = 5000;

	if (trig->_command_sub < 0) {
		trig->_command_sub = orb_subscribe(ORB_ID(vehicle_command));
	}

	bool updated = false;
	orb_check(trig->_command_sub, &updated);

	struct vehicle_command_s cmd;
	unsigned cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
	bool need_ack = false;

	// this flag is set when the polling loop is slowed down to allow the camera to power on
	trig->_turning_on = false;

	// these flags are used to detect state changes in the command loop
	bool main_state = trig->_trigger_enabled;
	bool pause_state = trig->_trigger_paused;

	// Command handling
	if (updated) {

		orb_copy(ORB_ID(vehicle_command), trig->_command_sub, &cmd);

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

			need_ack = true;

			if (commandParamToInt(cmd.param7) == 1) {
				// test shots are not logged or forwarded to GCS for geotagging
				trig->_test_shot = true;

			}

			if (commandParamToInt((float)cmd.param5) == 1) {
				// Schedule shot
				trig->_one_shot = true;

			}

			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;

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

			need_ack = true;

			if (commandParamToInt(cmd.param3) == 1) {
				// pause triggger
				trig->_trigger_paused = true;

			} else if (commandParamToInt(cmd.param3) == 0) {
				trig->_trigger_paused = false;

			}

			if (commandParamToInt(cmd.param2) == 1) {
				// reset trigger sequence
				trig->_trigger_seq = 0;

			}

			if (commandParamToInt(cmd.param1) == 1) {
				trig->_trigger_enabled = true;

			} else if (commandParamToInt(cmd.param1) == 0) {
				trig->_trigger_enabled = false;

			}

			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;

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

			need_ack = true;

			/*
			 * TRANSITIONAL SUPPORT ADDED AS OF 11th MAY 2017 (v1.6 RELEASE)
			*/

			if (cmd.param1 > 0.0f) {
				trig->_distance = cmd.param1;
				param_set(trig->_p_distance, &(trig->_distance));

				trig->_trigger_enabled = true;
				trig->_trigger_paused = false;

			} else if (commandParamToInt(cmd.param1) == 0) {
				trig->_trigger_paused = true;

			} else if (commandParamToInt(cmd.param1) == -1) {
				trig->_trigger_enabled = false;
			}

			// We can only control the shutter integration time of the camera in GPIO mode (for now)
			if (cmd.param2 > 0.0f) {
				if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
					trig->_activation_time = cmd.param2;
					param_set(trig->_p_activation_time, &(trig->_activation_time));
				}
			}

			// Trigger once immediately if param is set
			if (cmd.param3 > 0.0f) {
				// Schedule shot
				trig->_one_shot = true;
			}

			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;

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

			need_ack = true;

			if (cmd.param1 > 0.0f) {
				trig->_interval = cmd.param1;
				param_set(trig->_p_interval, &(trig->_interval));
			}

			// We can only control the shutter integration time of the camera in GPIO mode
			if (cmd.param2 > 0.0f) {
				if (trig->_camera_interface_mode == CAMERA_INTERFACE_MODE_GPIO) {
					trig->_activation_time = cmd.param2;
					param_set(trig->_p_activation_time, &(trig->_activation_time));
				}
			}

			cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;

		}

	}

	// State change handling
	if ((main_state != trig->_trigger_enabled) ||
	    (pause_state != trig->_trigger_paused) ||
	    trig->_one_shot) {

		if (trig->_trigger_enabled || trig->_one_shot) { // Just got enabled via a command

			// If camera isn't already powered on, we enable power to it
			if (!trig->_camera_interface->is_powered_on() &&
			    trig->_camera_interface->has_power_control()) {

				trig->toggle_power();
				trig->enable_keep_alive(true);

				// Give the camera time to turn on before starting to send trigger signals
				poll_interval_usec = 3000000;
				trig->_turning_on = true;
			}

		} else if (!trig->_trigger_enabled || trig->_trigger_paused) { // Just got disabled/paused via a command

			// Power off the camera if we are disabled
			if (trig->_camera_interface->is_powered_on() &&
			    trig->_camera_interface->has_power_control() &&
			    !trig->_trigger_enabled) {

				trig->enable_keep_alive(false);
				trig->toggle_power();
			}

			// cancel all calls for both disabled and paused
			hrt_cancel(&trig->_engagecall);
			hrt_cancel(&trig->_disengagecall);

			// ensure that the pin is off
			hrt_call_after(&trig->_disengagecall, 0,
				       (hrt_callout)&CameraTrigger::disengage, trig);

			// reset distance counter if needed
			if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
			    trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {

				// this will force distance counter reinit on getting enabled/unpaused
				trig->_valid_position = false;

			}

		}

		// only run on state changes, not every loop iteration
		if (trig->_trigger_mode == TRIGGER_MODE_INTERVAL_ON_CMD) {

			// update intervalometer state and reset calls
			trig->update_intervalometer();

		}

	}

	// run every loop iteration and trigger if needed
	if (trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ON_CMD ||
	    trig->_trigger_mode == TRIGGER_MODE_DISTANCE_ALWAYS_ON) {

		// update distance counter and trigger
		trig->update_distance();

	}

	// One shot command-based capture handling
	if (trig->_one_shot && !trig->_turning_on) {

		// One-shot trigger
		trig->shoot_once();
		trig->_one_shot = false;

		if (trig->_test_shot) {
			trig->_test_shot = false;
		}

	}

	// Command ACK handling
	if (updated && need_ack) {
		vehicle_command_ack_s command_ack = {
			.timestamp = 0,
			.result_param2 = 0,
			.command = cmd.command,
			.result = (uint8_t)cmd_result,
			.from_external = 0,
			.result_param1 = 0,
			.target_system = cmd.source_system,
			.target_component = cmd.source_component
		};

		if (trig->_cmd_ack_pub == nullptr) {
			trig->_cmd_ack_pub = orb_advertise_queue(ORB_ID(vehicle_command_ack), &command_ack,
					     vehicle_command_ack_s::ORB_QUEUE_LENGTH);

		} else {
			orb_publish(ORB_ID(vehicle_command_ack), trig->_cmd_ack_pub, &command_ack);

		}
	}

	work_queue(LPWORK, &_work, (worker_t)&CameraTrigger::cycle_trampoline,
		   camera_trigger::g_camera_trigger, USEC2TICK(poll_interval_usec));
}
Exemple #12
0
void
OREOLED::cycle()
{
	/* check time since startup */
	uint64_t now = hrt_absolute_time();
	bool startup_timeout = (now - _start_time > OREOLED_TIMEOUT_MS);

	/* if not leds found during start-up period, exit without rescheduling */
	if (startup_timeout && _num_healthy == 0) {
		warnx("did not find oreoled");
		return;
	}

	/* during startup period keep searching for unhealthy LEDs */
	if (!startup_timeout && _num_healthy < OREOLED_NUM_LEDS) {
		/* prepare command to turn off LED*/
		uint8_t msg[] = {OREOLED_PATTERN_OFF};

		/* attempt to contact each unhealthy LED */
		for (uint8_t i = 0; i < OREOLED_NUM_LEDS; i++) {
			if (!_healthy[i]) {
				/* set I2C address */
				set_address(OREOLED_BASE_I2C_ADDR + i);

				/* send I2C command and record health*/
				if (transfer(msg, sizeof(msg), nullptr, 0) == OK) {
					_healthy[i] = true;
					_num_healthy++;
					warnx("oreoled %d ok", (unsigned)i);
				}
			}
		}

		/* schedule another attempt in 0.1 sec */
		work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
			   USEC2TICK(OREOLED_STARTUP_INTERVAL_US));
		return;
	}

	/* get next command from queue */
	oreoled_cmd_t next_cmd;

	while (_cmd_queue->get(&next_cmd, sizeof(oreoled_cmd_t))) {
		/* send valid messages to healthy LEDs */
		if ((next_cmd.led_num < OREOLED_NUM_LEDS) && _healthy[next_cmd.led_num]
		    && (next_cmd.num_bytes <= OREOLED_CMD_LENGTH_MAX)) {
			/* set I2C address */
			set_address(OREOLED_BASE_I2C_ADDR + next_cmd.led_num);
			/* send I2C command */
			transfer(next_cmd.buff, next_cmd.num_bytes, nullptr, 0);
		}
	}

	/* send general call every 4 seconds*/
	if ((now - _last_gencall) > OREOLED_GENERALCALL_US) {
		send_general_call();
	}

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(HPWORK, &_work, (worker_t)&OREOLED::cycle_trampoline, this,
		   USEC2TICK(OREOLED_UPDATE_INTERVAL_US));
}
Exemple #13
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));
}
Exemple #14
0
int
AK8975::ioctl(struct file *filp, int cmd, unsigned long arg)
{
	switch (cmd) {
	case SENSORIOCSPOLLRATE: {
		switch (arg) {

			/* switching to manual polling */
		case SENSOR_POLLRATE_MANUAL:
			stop();
			_measure_ticks = 0;
			return OK;

			/* external signalling (DRDY) not supported */
		case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
		case 0:
			return -EINVAL;

			/* set default/max polling rate */
		case SENSOR_POLLRATE_MAX:
		case SENSOR_POLLRATE_DEFAULT: {
				/* do we need to start internal polling? */
				bool want_start = (_measure_ticks == 0);

				/* set interval for next measurement to minimum legal value */
				_measure_ticks = USEC2TICK(AK8975_CONVERSION_INTERVAL);

				/* if we need to start the poll state machine, do it */
				if (want_start)
					start();

				return OK;
			}

			/* adjust to a legal polling interval in Hz */
		default: {
				/* do we need to start internal polling? */
				bool want_start = (_measure_ticks == 0);

				/* convert hz to tick interval via microseconds */
				unsigned ticks = USEC2TICK(1000000 / arg);

				/* check against maximum rate */
				if (ticks < USEC2TICK(AK8975_CONVERSION_INTERVAL))
					return -EINVAL;

				/* update interval for next measurement */
				_measure_ticks = ticks;

				/* if we need to start the poll state machine, do it */
				if (want_start)
					start();

				return OK;
			}
		}
	}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0)
			return SENSOR_POLLRATE_MANUAL;

		return 1000000/TICK2USEC(_measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 1) || (arg > 100))
				return -EINVAL;

			irqstate_t flags = irqsave();
			if (!_reports->resize(arg)) {
				irqrestore(flags);
				return -ENOMEM;
			}
			irqrestore(flags);

			return OK;
		}

	case SENSORIOCGQUEUEDEPTH:
		return _reports->size();

	case SENSORIOCRESET:
		return reset();

	case MAGIOCSSAMPLERATE:
		/* same as pollrate because device is in single measurement mode*/
		return ioctl(filp, SENSORIOCSPOLLRATE, arg);

	case MAGIOCGSAMPLERATE:
		/* same as pollrate because device is in single measurement mode*/
		return 1000000/TICK2USEC(_measure_ticks);

	case MAGIOCSRANGE:
		// not allowed on this sensor
		return -EINVAL;

	case MAGIOCGRANGE:
		return _range_ga;

	case MAGIOCSLOWPASS:
	case MAGIOCGLOWPASS:
		/* not supported, no internal filtering */
		return -EINVAL;

	case MAGIOCSSCALE:
		/* not supported on this sensor */
		return -EINVAL;

	case MAGIOCGSCALE:
		/* copy out scale factors */
		memcpy((mag_scale *)arg, &_scale, sizeof(_scale));
		return 0;

	case MAGIOCCALIBRATE:
		/* not supported */
		return -EINVAL;

	case MAGIOCEXSTRAP:
		/* not supported */
		return -EINVAL;

	case MAGIOCSELFTEST:
		return check_calibration();

	case MAGIOCGEXTERNAL:
		if (_bus == PX4_I2C_BUS_EXPANSION)
			return 1;
		else
			return 0;

	default:
		/* give it to the superclass */
		return I2C::ioctl(filp, cmd, arg);
	}
}
Exemple #15
0
int
TFMINI::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
	switch (cmd) {

	case SENSORIOCSPOLLRATE: {
			switch (arg) {

			/* switching to manual polling */
			case SENSOR_POLLRATE_MANUAL:
				stop();
				_measure_ticks = 0;
				return OK;

			/* external signalling (DRDY) not supported */
			case SENSOR_POLLRATE_EXTERNAL:

			/* zero would be bad */
			case 0:
				return -EINVAL;

			/* set default/max polling rate */
			case SENSOR_POLLRATE_MAX:
			case SENSOR_POLLRATE_DEFAULT: {
					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* set interval for next measurement to minimum legal value */
					_measure_ticks = USEC2TICK(_conversion_interval);

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}

			/* adjust to a legal polling interval in Hz */
			default: {

					/* do we need to start internal polling? */
					bool want_start = (_measure_ticks == 0);

					/* convert hz to tick interval via microseconds */
					unsigned ticks = USEC2TICK(1000000 / arg);

					/* check against maximum rate */
					if (ticks < USEC2TICK(_conversion_interval)) {
						return -EINVAL;
					}

					/* update interval for next measurement */
					_measure_ticks = ticks;

					/* if we need to start the poll state machine, do it */
					if (want_start) {
						start();
					}

					return OK;
				}
			}
		}

	case SENSORIOCGPOLLRATE:
		if (_measure_ticks == 0) {
			return SENSOR_POLLRATE_MANUAL;
		}

		return (1000 / _measure_ticks);

	case SENSORIOCSQUEUEDEPTH: {
			/* lower bound is mandatory, upper bound is a sanity check */
			if ((arg < 1) || (arg > 100)) {
				return -EINVAL;
			}

			ATOMIC_ENTER;

			if (!_reports->resize(arg)) {
				ATOMIC_LEAVE;
				return -ENOMEM;
			}

			ATOMIC_LEAVE;

			return OK;
		}

	case SENSORIOCRESET:
		/* XXX implement this */
		return -EINVAL;

	default:
		/* give it to the superclass */
		return CDev::ioctl(filp, cmd, arg);
	}
}
Exemple #16
0
void
BATT_SMBUS::cycle()
{
	// get current time
	uint64_t now = hrt_absolute_time();

	// exit without rescheduling if we have failed to find a battery after 10 seconds
	if (!_enabled && (now - _start_time > BATT_SMBUS_TIMEOUT_MS)) {
		warnx("did not find smart battery");
		return;
	}

	// read data from sensor
	struct battery_status_s new_report;

	// set time of reading
	new_report.timestamp = now;

	// read voltage
	uint16_t tmp;

	if (read_reg(BATT_SMBUS_VOLTAGE, tmp) == OK) {
		// initialise new_report
		memset(&new_report, 0, sizeof(new_report));

		// convert millivolts to volts
		new_report.voltage_v = ((float)tmp) / 1000.0f;

		// read current
		uint8_t buff[4];

		if (read_block(BATT_SMBUS_CURRENT, buff, 4, false) == 4) {
			new_report.current_a = -(float)((int32_t)((uint32_t)buff[3] << 24 | (uint32_t)buff[2] << 16 | (uint32_t)buff[1] << 8 |
							(uint32_t)buff[0])) / 1000.0f;
		}

		// read battery design capacity
		if (_batt_capacity == 0) {
			if (read_reg(BATT_SMBUS_FULL_CHARGE_CAPACITY, tmp) == OK) {
				_batt_capacity = tmp;
			}
		}

		// read remaining capacity
		if (_batt_capacity > 0) {
			if (read_reg(BATT_SMBUS_REMAINING_CAPACITY, tmp) == OK) {
				if (tmp < _batt_capacity) {
					new_report.discharged_mah = _batt_capacity - tmp;
				}
			}
		}

		// publish to orb
		if (_batt_topic != nullptr) {
			orb_publish(_batt_orb_id, _batt_topic, &new_report);

		} else {
			_batt_topic = orb_advertise(_batt_orb_id, &new_report);

			if (_batt_topic == nullptr) {
				errx(1, "ADVERT FAIL");
			}
		}

		// copy report for test()
		_last_report = new_report;

		// post a report to the ring
		_reports->force(&new_report);

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

		// record we are working
		_enabled = true;
	}

	// schedule a fresh cycle call when the measurement is done
	work_queue(HPWORK, &_work, (worker_t)&BATT_SMBUS::cycle_trampoline, this,
		   USEC2TICK(BATT_SMBUS_MEASUREMENT_INTERVAL_MS));
}
Exemple #17
0
void
TFMINI::cycle()
{
	/* fds initialized? */
	if (_fd < 0) {
		/* open fd */
		_fd = ::open(_port, O_RDWR | O_NOCTTY | O_SYNC);
	}

	/* collection phase? */
	if (_collect_phase) {

		/* perform collection */
		int collect_ret = collect();

		if (collect_ret == -EAGAIN) {
			/* reschedule to grab the missing bits, time to transmit 8 bytes @ 9600 bps */
			work_queue(HPWORK,
				   &_work,
				   (worker_t)&TFMINI::cycle_trampoline,
				   this,
				   USEC2TICK(1042 * 8));
			return;
		}

		if (OK != collect_ret) {

			/* we know the sensor needs about four seconds to initialize */
			if (hrt_absolute_time() > 5 * 1000 * 1000LL && _consecutive_fail_count < 5) {
				DEVICE_LOG("collection error #%u", _consecutive_fail_count);
			}

			_consecutive_fail_count++;

			/* restart the measurement state machine */
			start();
			return;

		} else {
			/* apparently success */
			_consecutive_fail_count = 0;
		}

		/* next phase is measurement */
		_collect_phase = false;

		/*
		 * Is there a collect->measure gap?
		 */
		if (_measure_ticks > USEC2TICK(_conversion_interval)) {

			/* schedule a fresh cycle call when we are ready to measure again */
			work_queue(HPWORK,
				   &_work,
				   (worker_t)&TFMINI::cycle_trampoline,
				   this,
				   _measure_ticks - USEC2TICK(_conversion_interval));

			return;
		}
	}

	/* next phase is collection */
	_collect_phase = true;

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(HPWORK,
		   &_work,
		   (worker_t)&TFMINI::cycle_trampoline,
		   this,
		   USEC2TICK(_conversion_interval));
}
Exemple #18
0
static void work_process(struct wqueue_s *wqueue, int lock_id)
{
	volatile struct work_s *work;
	worker_t  worker;
	void *arg;
	uint64_t elapsed;
	uint32_t remaining;
	uint32_t next;

	/* Then process queued work.  We need to keep interrupts disabled while
	 * we process items in the work list.
	 */

	next  = CONFIG_SCHED_WORKPERIOD;

	work_lock(lock_id);

	work  = (struct work_s *)wqueue->q.head;

	while (work) {
		/* Is this work ready?  It is ready if there is no delay or if
		 * the delay has elapsed. qtime is the time that the work was added
		 * to the work queue.  It will always be greater than or equal to
		 * zero.  Therefore a delay of zero will always execute immediately.
		 */

		elapsed = USEC2TICK(clock_systimer() - work->qtime);

		//printf("work_process: in ticks elapsed=%lu delay=%u\n", elapsed, work->delay);
		if (elapsed >= work->delay) {
			/* Remove the ready-to-execute work from the list */

			(void)dq_rem((struct dq_entry_s *)work, &wqueue->q);

			/* Extract the work description from the entry (in case the work
			 * instance by the re-used after it has been de-queued).
			 */

			worker = work->worker;
			arg    = work->arg;

			/* Mark the work as no longer being queued */

			work->worker = NULL;

			/* Do the work.  Re-enable interrupts while the work is being
			 * performed... we don't have any idea how long that will take!
			 */

			work_unlock(lock_id);

			if (!worker) {
				PX4_WARN("MESSED UP: worker = 0\n");

			} else {
				worker(arg);
			}

			/* Now, unfortunately, since we re-enabled interrupts we don't
			 * know the state of the work list and we will have to start
			 * back at the head of the list.
			 */

			work_lock(lock_id);
			work  = (struct work_s *)wqueue->q.head;

		} else {
			/* This one is not ready.. will it be ready before the next
			 * scheduled wakeup interval?
			 */

			/* Here: elapsed < work->delay */
			remaining = USEC_PER_TICK * (work->delay - elapsed);

			if (remaining < next) {
				/* Yes.. Then schedule to wake up when the work is ready */

				next = remaining;
			}

			/* Then try the next in the list. */

			work = (struct work_s *)work->dq.flink;
		}
	}

	/* Wait awhile to check the work list.  We will wait here until either
	 * the time elapses or until we are awakened by a signal.
	 */
	work_unlock(lock_id);

	usleep(next);
}
Exemple #19
0
void
SRF02_I2C::cycle()
{
	if (_collect_phase) {
		_index_counter = addr_ind[_cycle_counter]; /*sonar from previous iteration collect is now read out */
		set_address(_index_counter);

		/* perform collection */
		if (OK != collect()) {
			DEVICE_DEBUG("collection error");
			/* if error restart the measurement state machine */
			start();
			return;
		}

		/* next phase is measurement */
		_collect_phase = false;

		/* change i2c adress to next sonar */
		_cycle_counter = _cycle_counter + 1;

		if (_cycle_counter >= addr_ind.size()) {
			_cycle_counter = 0;
		}

		/* Is there a collect->measure gap? Yes, and the timing is set equal to the cycling_rate
		   Otherwise the next sonar would fire without the first one having received its reflected sonar pulse */

		if (_measure_ticks > USEC2TICK(_cycling_rate)) {

			/* schedule a fresh cycle call when we are ready to measure again */
			work_queue(HPWORK,
				   &_work,
				   (worker_t)&SRF02_I2C::cycle_trampoline,
				   this,
				   _measure_ticks - USEC2TICK(_cycling_rate));
			return;
		}
	}

	/* Measurement (firing) phase */

	/* ensure sonar i2c adress is still correct */
	_index_counter = addr_ind[_cycle_counter];
	set_address(_index_counter);

	/* Perform measurement */
	if (OK != measure()) {
		DEVICE_DEBUG("measure error sonar adress %d", _index_counter);
	}

	/* next phase is collection */
	_collect_phase = true;

	/* schedule a fresh cycle call when the measurement is done */
	work_queue(HPWORK,
		   &_work,
		   (worker_t)&SRF02_I2C::cycle_trampoline,
		   this,
		   USEC2TICK(_cycling_rate));

}
Exemple #20
0
void gpio_led_cycle(FAR void *arg)
{
	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;

	/* check for status updates*/
	bool updated;
	orb_check(priv->vehicle_status_sub, &updated);

	if (updated) {
		orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status);
	}

	/* select pattern for current status */
	int pattern = 0;

	if (priv->status.arming_state == ARMING_STATE_ARMED_ERROR) {
		pattern = 0x2A;	// *_*_*_ fast blink (armed, error)

	} else if (priv->status.arming_state == ARMING_STATE_ARMED) {
		if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE && !priv->status.failsafe) {
			pattern = 0x3f;	// ****** solid (armed)

		} else {
			pattern = 0x3e;	// *****_ slow blink (armed, battery low or failsafe)
		}

	} else if (priv->status.arming_state == ARMING_STATE_STANDBY) {
		pattern = 0x38;	// ***___ slow blink (disarmed, ready)

	} else if (priv->status.arming_state == ARMING_STATE_STANDBY_ERROR) {
		pattern = 0x28;	// *_*___ slow double blink (disarmed, error)

	}

	/* blink pattern */
	bool led_state_new = (pattern & (1 << priv->counter)) != 0;

	if (led_state_new != priv->led_state) {
		priv->led_state = led_state_new;

		if (led_state_new) {
			ioctl(priv->gpio_fd, GPIO_SET, priv->pin);

		} else {
			ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
		}
	}

	priv->counter++;

	if (priv->counter > 5) {
		priv->counter = 0;
	}

	/* repeat cycle at 5 Hz */
	if (gpio_led_started) {
		work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));

	} else {
		/* switch off LED on stop */
		ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
	}
}