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)); }
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); } }
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); } }
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)); }
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); } }
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); } }
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); } }
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); } }
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; } }
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); } }
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)); }
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)); }
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)); }
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); } }
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); } }
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)); }
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)); }
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); }
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)); }
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); } }